Skip to content

Add robotstateprovider configuration for using iFeel BLE devices #429

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
S-Dafarra merged 1 commit into from
Mar 25, 2026
Merged

Add robotstateprovider configuration for using iFeel BLE devices #429

Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
206 changes: 206 additions & 0 deletions conf/xml/RobotStateProvider_ergoCub_openxr_ifeel_ble.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -0,0 +1,206 @@
<?xml version="1.0" encoding="UTF-8" ?>
<!DOCTYPE robot PUBLIC "-//YARP//DTD yarprobotinterface 3.0//EN" "http://www.yarp.it/DTD/yarprobotinterfaceV3.0.dtd">
<robot name="ergoCub-Retargeting" build=0 portprefix="">

<device type="frameTransformClient" name="TransformClient">
<param name="period">0.01</param>
<param name="filexml_option">ftc_yarp_only.xml</param>
<param name="ft_client_prefix">/IFrameTransformToIWear/tf</param>
<param name="local_rpc">/IFrameTransformToIWear/tf/local_rpc</param>
<param name="ftc_storage_timeout">10.0</param>
</device>

<device type="iframetransform_to_iwear" name="IFrameTransformToIWear">
<param name="wearableName">TransformServer</param>
<param extern-name="rootFrame" name="rootFrameID">root_link_desired</param>
<param name="wearableSensorType">PoseSensor</param>
<param extern-name="frames" name="frameIDs">(root_link_desired
openxr_head
right_glove
left_glove)</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="IFrameTransformToIWearLabel">TransformClient</elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<device type="iwear_remapper" name="XSenseIWearRemapper">
<param name="wearableDataPorts">(/iFeelSuit/WearableData/data:o)</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="IFrameTransformToIWear">IFrameTransformToIWear</elem>
</paramlist>
</action>
</device>

<device type="human_state_provider" name="RobotStateProvider">
<param name="period">0.01</param>
<param name="urdf">model.urdf</param>
<param name="floatingBaseFrame">root_link</param>
<!-- ikSolver options: pairwised, global, dynamical -->
<param name="ikSolver">dynamical</param>
<param name="allowIKFailures">true</param>
<param name="useDirectBaseMeasurement">false</param>
<!-- optimization parameters -->
<param name="maxIterationsIK">300</param>
<param name="ikLinearSolver">ma27</param>
<param name="ikPoolSizeOption">2</param>
<param name="posTargetWeight">0.0</param>
<param name="rotTargetWeight">1.0</param>
<param name="costRegularization">1.0</param>
<param name="costTolerance">0.001</param>
<param name="rpcPortPrefix">ergoCub</param>
<!-- inverse velocity kinematics parameters -->
<!-- inverseVelocityKinematicsSolver values:
QP
moorePenrose,
completeOrthogonalDecomposition,
leastSquare,
choleskyDecomposition,
sparseCholeskyDecomposition,
robustCholeskyDecomposition,
sparseRobustCholeskyDecomposition -->
<param name='inverseVelocityKinematicsSolver'>QP</param>
<param name="linVelTargetWeight">1.0</param>
<param name="angVelTargetWeight">1.0</param>
<!-- integration based IK parameters -->
<param name='dynamicalIKJointVelocityLimit'>10.0</param> <!-- comment or -1.0 for no limits -->
<param name="dynamicalIKMeasuredVelocityGainLinRot">(1.0 1.0)</param>
<param name="dynamicalIKCorrectionGainsLinRot">(200.0 20.0)</param>
<param name="dynamicalIKIntegralCorrectionGainsLinRot">(0.0 0.0)</param>
<group name="WEARABLE_SENSOR_TARGETS">
<!-- <param name="target_Pelvis">(root_link, TransformServer::pose::root_link_desired, pose)</param> -->
<param name="target_Chest">(chest, iFeelSuit::vLink::Node#6, gravity)</param>
<param name="target_RightHand">(r_hand_palm, TransformServer::pose::right_glove, pose)</param>
<param name="target_LeftHand">(l_hand_palm, TransformServer::pose::left_glove, pose)</param>
<param name="target_Head">(head, TransformServer::pose::openxr_head, pose)</param>
<param name="target_l_upper_arm">(l_upper_arm iFeelSuit::vLink::Node#5, gravity)</param>
<param name="target_l_forearm">(l_forearm, iFeelSuit::vLink::Node#4, gravity)</param>
<param name="target_r_upper_arm">(r_upper_arm iFeelSuit::vLink::Node#7, gravity)</param>
<param name="target_r_forearm">(r_forearm, iFeelSuit::vLink::Node#8, gravity)</param>
</group>
<param name="jointList">("l_shoulder_pitch",
"l_shoulder_roll",
"l_shoulder_yaw",
"l_elbow",
"l_wrist_roll",
"l_wrist_pitch",
"l_wrist_yaw",
"r_shoulder_pitch",
"r_shoulder_roll",
"r_shoulder_yaw",
"r_elbow",
"r_wrist_roll",
"r_wrist_pitch",
"r_wrist_yaw",
"torso_pitch",
"torso_roll",
"torso_yaw",
"neck_pitch",
"neck_roll",
"neck_yaw",
"camera_tilt")
</param>
<group name="MEASUREMENT_TO_LINK_TRANSFORMS">
<param name="target_LeftHand">( 1.0 0.0 0.0 0.0
0.0 0.0 1.0 -0.02
0.0 -1.0 0.0 0.05
0.0 0.0 0.0 1.0)</param>
<param name="target_RightHand">(-1.0 0.0 0.0 0.0
0.0 0.0 1.0 -0.02
0.0 1.0 0.0 0.05
0.0 0.0 0.0 1.0)</param>
<param name="target_Head">( 0.0 -1.0 0.0 0.0
0.0 0.0 1.0 0.0
-1.0 0.0 0.0 0.1
0.0 0.0 0.0 1.0)</param>
<param name="target_Chest">( 0.0 1.0 0.0 0.0
0.0 0.0 -1.0 0.0
-1.0 0.0 0.0 0.0
0.0 0.0 0.0 1.0)</param>
<param name="target_r_upper_arm">( -1.0 0.0 0.0 0.0
0.0 0.0 1.0 0.0
0.0 1.0 0.0 0.0
0.0 0.0 0.0 1.0)</param>
<param name="target_l_upper_arm">( -1.0 0.0 0.0 0.0
0.0 0.0 -1.0 0.0
0.0 -1.0 0.0 0.0
0.0 0.0 0.0 1.0)</param>
<param name="target_r_forearm">( -1.0 0.0 0.0 0.0
0.0 0.0 1.0 0.0
0.0 1.0 0.0 0.0
0.0 0.0 0.0 1.0)</param>
<param name="target_l_forearm">( -1.0 0.0 0.0 0.0
0.0 0.0 -1.0 0.0
0.0 -1.0 0.0 0.0
0.0 0.0 0.0 1.0)</param>
</group>
<group name="MEASUREMENT_POSITION_SCALE_FACTOR">
<param name="target_Head">( 0.7 0.7 0.6 )</param>
<param name="target_LeftHand">( 0.7 0.7 0.6 )</param>
<param name="target_RightHand">( 0.7 0.7 0.6 )</param>
<param name="x_scale_factor_all" extern-name="xy_scale">0.7</param>
<param name="y_scale_factor_all" extern-name="xy_scale">0.7</param>
<param name="z_scale_factor_all" extern-name="z_scale">0.7</param>
</group>
<group name="CUSTOM_CONSTRAINTS">
<!-- check issue https://github.com/robotology/human-dynamics-estimation/issues/132 for more info-->
<!-- note that a group can not be empty, otherwise it returns error-->
<!-- custom joint limits velocities-->
<!--param name="custom_joints_velocity_limits_names">(neck_roll, neck_pitch)</param-->
<param name="custom_joints_velocity_limits_names">()</param>
<!-- the upper bound is "+", while the lower bounds are "-" -->
<!--param name="custom_joints_velocity_limits_values">(10.0, 15.0)</param-->
<param name="custom_joints_velocity_limits_values">()</param>
<!-- **** base velocity limit: x, y, z, roll, pitch, yaw ****-->
<param name="base_velocity_limit_upper_buond">(0.0, 0.0, 0.0, 0.0, 0.0, 0.0)</param>
<param name="base_velocity_limit_lower_buond">(0.0, 0.0, -0.0, 0.0, 0.0, 0.0)</param>
<!-- Custom joint Configuration constraints-->
<!-- if the boudary value is inf, I will use -1000.0 rad, or +1000.0 rad-->
<param name="custom_constraint_variables">(
l_shoulder_roll, r_shoulder_roll, camera_tilt)</param>
<param name="custom_constraint_matrix"> (
(1.0, 0.0, 0.0),
(0.0, 1.0, 0.0),
(0.0, 0.0, 1.0))</param>
<param name="custom_constraint_lower_bound"> (
-100.0, -100.0, -0.1)</param>
<param name="custom_constraint_upper_bound"> (
1.4, 1.4, 0.1)</param>
<param name="k_u">0.5</param>
<param name="k_l">0.5</param>
</group>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="HumanStateProviderLabel">XSenseIWearRemapper</elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<!-- Uncomment to stream the output of HumanStateProvider on a YARP port -->
<device type="human_state_nws_yarp" name="RobotState_nws_yarp">
<param name="period">0.01</param>
<param name="outputPort">/ergoCub/RobotStateServer/state:o</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="HumanStateServerLabel">RobotStateProvider</elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

<device type="wearable_targets_nws_yarp" name="WearableTargets_nws_yarp">
<param name="period">0.01</param>
<param name="outputPort">/HDE/WearableTargetsServer/state:o</param>
<action phase="startup" level="5" type="attach">
<paramlist name="networks">
<elem name="HumanStateServerLabel">RobotStateProvider</elem>
</paramlist>
</action>
<action phase="shutdown" level="5" type="detach"/>
</device>

</robot>
Loading