For an overview on wholeBodyDynamics and to understand how to run the device, please see Force Control on iCub. Below, you will find the description of the features/parameters included by the wholeBodyDynamics device.

A device that takes a list of axes and estimates the joint torques for each one of this axes.

The parameters taken in input by this device are:

The axes contained in the axesNames parameter are then mapped to the wrapped controlboard in the attachAll method, using controlBoardRemapper class. Furthermore are also used to match the yarp axes to the joint names found in the passed URDF file.

By default, the wholebodydynamics device takes the list of forcetorque sensors from the multipleAnalogSensorsNames group that signals which FT sensors are used.

In particular, the SixAxisForceTorqueSensorsNames parameter from the multipleAnalogSensorsNames group specify the sensor names that are used.

Example:

<group name="multipleAnalogSensorsNames">
    <param name="SixAxisForceTorqueSensorsNames">(l_leg_ft,r_leg_ft,l_foot_ft,r_foot_ft)</param>
</group>

The sensors specified in this way need to be:

• Available in the URDF sensor used for the robot, using the format supported by iDynTree to specify FT sensors (see https://github.com/robotology/idyntree/blob/master/doc/model_loading.md#force_torque)
• Exposed as YARP devices that expose the yarp::dev::ISixAxisForceTorqueSensors YARP interface in one of the devices passed to the device via the attach xml list or the attachAll C++ method.

If the FT_TEMPERATURE_COEFFICIENTS parameter is specified, then it is assumed that for each FT sensors there is a temperature sensor with the same name the measure of which is available via the yarp::dev::ITemperatureSensors interface. If that is not the case, an error is raised. To use the temperature compensation, all used FT sensors must support the temperature compensation.

This device also provides gravity compensation torques (using the IImpedanceControl::setImpedanceOffset method) for axis that are in compliant interaction mode and in position/position direct/velocity control mode.

This estimates are obtained just with the model, assuming that there is a link (the gravityCompensationRootLink) at which all external forces are exerted.

Typically this estimates are provided only for the upper joints (arms and torso) of the robots, as the gravity compensation terms for the legs depends on the support state of the robot.

This device support to specify a secondary calibration matrix to apply on the top of the (already calibrated) measure coming from the F/T sensors. This feature is meant to be experimental, and will be removed at any time.

The group of options that regulated this group is the following:

All sensors not specified will use a 6x6 identity as a secondary calibration matrix.

Example of part of a configuration file using .xml yarprobotinterface format (remember to put the fractional dot!).

     <group name="FT_SECONDARY_CALIBRATION">
            <param name="l_arm_ft">(1.0,0.0,0.0,0.0,0.0,0.0,
                                           0.0,1.0,0.0,0.0,0.0,0.0,
                                           0.0,0.0,1.0,0.0,0.0,0.0,
                                           0.0,0.0,0.0,1.0,0.0,0.0,
                                           0.0,0.0,0.0,0.0,1.0,0.0,
                                           0.0,0.0,0.0,0.0,0.0,1.0)                   </param>
            <param name="r_arm_ft">(1.0,0.0,0.0,0.0,0.0,0.0,
                                           0.0,1.0,0.0,0.0,0.0,0.0,
                                           0.0,0.0,1.0,0.0,0.0,0.0,
                                           0.0,0.0,0.0,0.001,0.0,0.0,
                                           0.0,0.0,0.0,0.0,0.001,0.0,
                                           0.0,0.0,0.0,0.0,0.0,0.001)                 </param>
     </group>

This device support to specify a set of temperature coefficients matrix to apply on the top of the (already calibrated) measure coming from the F/T sensors. :warning: This feature is meant to be experimental, and will be removed at any time.

The group of options that regulates this group is the following:

All sensors not specified will use a 1x7 zero vector as a temperature coefficients matrix, where the first 6 are temperature correction for each axis and the 7th value is the temperature offset.

Example of part of a configuration file using .xml yarprobotinterface format (remember to put the fractional dot!).

     <group name="FT_TEMPERATURE_COEFFICIENTS">
            <param name="l_arm_ft">(0.0,0.0,0.0,0.0,0.0,0.0,0.0)</param>
            <param name="r_arm_ft">(0.0,0.0,0.0,0.0,0.0,0.0,0.0)</param>
     </group>

This device support to specify a set an constant offset vector to apply on the top of the (already calibrated) measure coming from the F/T sensors. :warning: This feature is meant to be experimental, and will be removed at any time.

The group of options that regulates this group is the following:

All sensors not specified will use a 1x6 zero vector as a constant offset, where the 6 are offset for each axis.

Example of part of a configuration file using .xml yarprobotinterface format (remember to put the fractional dot!).

     <group name="FT_OFFSET">
            <param name="l_arm_ft">(0.0,0.0,0.0,0.0,0.0,0.0)</param>
            <param name="r_arm_ft">(0.0,0.0,0.0,0.0,0.0,0.0)</param>
     </group>

All the filters used for the input measurements are using the iCub::ctrl::realTime::FirstOrderLowPassFilter class.

Example configuration file using .ini format.

 device wholebodydynamics
 axesNames (joint1 joint2 joint3)

...

Example configuration file using .xml yarprobotinterface format.

<devices robot="iCubGenova02" build="1">
    <device name="wholebodydynamics" type="wholebodydynamics">
        <param name="axesNames">(torso_pitch,torso_roll,torso_yaw,neck_pitch, neck_roll,neck_yaw,l_shoulder_pitch,l_shoulder_roll,l_shoulder_yaw,l_elbow,r_shoulder_pitch,r_shoulder_roll,r_shoulder_yaw,r_elbow,l_hip_pitch,l_hip_roll,l_hip_yaw,l_knee,l_ankle_pitch,l_ankle_roll,r_hip_pitch,r_hip_roll,r_hip_yaw,r_knee,r_ankle_pitch,r_ankle_roll)</param>
        <param name="modelFile">model.urdf</param>
        <param name="fixedFrameGravity">(0,0,-9.81)</param>
        <param name="defaultContactFrames">(l_hand,r_hand,root_link,l_sole,r_sole,l_lower_leg,r_lower_leg,l_elbow_1,r_elbow_1)</param>
        <param name="imuFrameName">imu_frame</param>
        <param name="useJointVelocity">true</param>
        <param name="useJointAcceleration">true</param>
        <!-- map between iDynTree links (identified by a name)
             and skinDynLib links (identified by their frame name, a BodyPart enum
             and a local (to the body part) index -->
        <group name="IDYNTREE_SKINDYNLIB_LINKS">
            <param name="root_link">(root_link,1,0)</param>
            <param name="chest"> (chest,1,2)</param>
            <param name="l_upper_arm">(l_upper_arm,3,2)</param>
            <param name="l_elbow_1">(l_elbow_1, 3, 4)</param>
            <param name="r_upper_arm">(r_upper_arm,4,2)</param>
            <param name="r_elbow_1">(r_elbow_1, 4, 4)</param>
            <param name="l_lower_leg">(l_lower_leg,5,3)</param>
            <param name="l_ankle_1">(l_ankle_1,5,4)</param>
            <param name="l_foot">(l_foot_dh_frame,5,5)</param>
            <param name="r_lower_leg">(r_lower_leg,6,3)</param>
            <param name="r_ankle_1">(r_ankle_1,6,4)</param>
            <param name="r_foot">(r_foot_dh_frame,6,5)</param>
        </group>

        <group name="WBD_OUTPUT_EXTERNAL_WRENCH_PORTS">
            <param name="/wholeBodyDynamics/left_leg/cartesianEndEffectorWrench:o">(l_foot,l_sole,root_link)</param>
            <param name="/wholeBodyDynamics/right_leg/cartesianEndEffectorWrench:o">(r_foot,r_sole,root_link)</param>
        </group>

        <action phase="startup" level="15" type="attach">
            <paramlist name="networks">
                <!-- motorcontrol and virtual torque sensors -->
                <elem name="left_lower_leg">left_lower_leg_mc</elem>
                <elem name="right_lower_leg">right_lower_leg_mc</elem>
                <elem name="left_upper_leg">left_upper_leg_mc</elem>
                <elem name="right_upper_leg">right_upper_leg_mc</elem>
                <elem name="torso">torso_mc</elem>
                <elem name="right_lower_arm">right_lower_arm_mc</elem>
                <elem name="left_lower_arm">left_lower_arm_mc</elem>
                <elem name="right_upper_arm">right_upper_arm_mc</elem>
                <elem name="left_upper_arm">left_upper_arm_mc</elem>
                <elem name="head">head_mc</elem>
                <!-- imu -->
                <elem name="imu">inertial</elem>
                <!-- ft -->
                <elem name="l_arm_ft">left_upper_arm_strain</elem>
                <elem name="r_arm_ft">right_upper_arm_strain</elem>
                <elem name="l_leg_ft">left_upper_leg_strain</elem>
                <elem name="r_leg_ft">right_upper_leg_strain</elem>
                <elem name="l_foot_ft">left_lower_leg_strain</elem>
                <elem name="r_foot_ft">right_lower_leg_strain</elem>
            </paramlist>
        </action>

        <action phase="shutdown" level="2" type="detach" />

    </device>
</devices>

You can access the device while running via YARP RPC. You can run the following command to access into the RPC mode of the device.

yarp rpc <portPrefix>/rpc

In particular, you can execute the following commands inside the RPC mode:

• calib: Calibrate the force/torque sensors, assuming only external forces acting on the robot are on the torso/waist.

calib <code> <noOfSamples>

Where <code> is an argument to specify the sensors to calibrate (all,arms,legs,feet) AT THE MOMENT IT'S IGNORED, and <noOfSamples> is the number of samples (if not set, it will assume a default value of 100).

• calibStanding: Calibrate the force/torque sensors, assuming only external forces acting on the robot are on the soles. It assumes the symmetry of the robot around a vertical central line.

calibStanding <code> <noOfSamples>

• calibStandingWithJetsiRonCub: Works for iRonCub models only, it calibrates the force/torque sensors when on double support and with jet engines turned ON and on idle thrust. It also assumes robot symmetry.

calibStandingWithJetsiRonCub <ironcub_model> <code> <noOfSamples>

Where <ironcub_model> specifies the particular model of iRonCub (mk1, mk1.1).

• calibStandingLeftFoot: Calibrate the force/torque sensors when on single support on left foot.

calibStandingLeftFoot <code> <noOfSamples>

• calibStandingRightFoot: Calibrate the force/torque sensors when on single support on right foot.

calibStandingRightFoot <code> <noOfSamples>

• calibStandingOnOneLink: Calibrate the force/torque sensors offsets when the external forces are acting on only one link. This method is typically used when the robot is standing on only one feet, or when it is attached to a fixture that is acting on a single link (typically the chest or the waist).

calibStandingOnOneLink <standing_frame> <noOfSamples>

Where standing_frame Is a frame belonging to the link on which it is assumed that external forces are acting.

• calibStandingOnTwoLinks: Calibrate the force/torque sensors offsets when the external forces are acting on only two links.

calibStandingOnTwoLinks <first_standing_frame> <second_standing_frame> <noOfSamples>

Where first_standing_frame and second_standing_frame are frames belonging to the two links on which it is assumed that the external forces are acting.

• resetOffset: Reset the sensor offset to 0 0 0 0 0 0 (six zeros).

resetOffset <code>

• usePreEstimatedOffset: Use the offline estimated offset of the sensor.

usePreEstimatedOffset

• quit: Quit the module.

quit

• resetSimpleLeggedOdometry: Reset the odometry world to be (initially) a frame specified in the robot model, and specify a link that is assumed to be fixed in the odometry.

resetSimpleLeggedOdometry <initial_world_frame> <initial_fixed_link>

Where initial_world_frame is the frame of the robot model that is assume to be initially coincident with the world/inertial frame, and initial_fixed_link is the name of the link that should be initially fixed.

• changeFixedLinkSimpleLeggedOdometry: Change the link that is considered fixed by the odometry.

changeFixedLinkSimpleLeggedOdometry <new_fixed_link>

Where new_fixed_link is the name of the new link that should be considered fixed.

• set_imuFilterCutoffInHz: Set the cutoff frequency (in Hz) for IMU measurements.

set_imuFilterCutoffInHz <newCutoff>

• get_imuFilterCutoffInHz: Get the cutoff frequency (in Hz) for IMU measurements.

get_imuFilterCutoffInHz

• set_forceTorqueFilterCutoffInHz: Set the cutoff frequency (in Hz) for FT measurements.

set_forceTorqueFilterCutoffInHz <newCutoff>

• get_forceTorqueFilterCutoffInHz: Get the cutoff frequency (in Hz) for FT measurements.

get_forceTorqueFilterCutoffInHz

• set_jointVelFilterCutoffInHz: Set the cutoff frequency (in Hz) for joint velocities measurements.

set_jointVelFilterCutoffInHz <newCutoff>

• get_jointVelFilterCutoffInHz: Get the cutoff frequency (in Hz) for joint velocities measurements.

get_jointVelFilterCutoffInHz

• set_jointAccFilterCutoffInHz: Set the cutoff frequency (in Hz) for joint acceleration measurements.

set_jointAccFilterCutoffInHz <newCutoff>

• get_jointAccFilterCutoffInHz: Get the cutoff frequency (in Hz) for joint acceleration measurements.

get_jointAccFilterCutoffInHz

• useIMUAsKinematicSource: Use the IMU as the kinematic source of information for the acceleration of one link.

useIMUAsKinematicSource

• useFixedFrameAsKinematicSource: Use a fixed frame (tipically root_link, l_sole or r_sole) as the source of kinematic information. The assumption is that the specified frame will remain fixed until the kinematic source is changing, and the gravity on this link is specified by the fixedFrameGravity (typically set to (0,0,-9.81) .

useFixedFrameAsKinematicSource <fixedFrame>

• setUseOfJointVelocities: Set if to use or not the joint velocities in estimation.

setUseOfJointVelocities <enable>

Where enable is a boolean variable, true for enable the using of joint velocities, and false for disabling it.

• setUseOfJointAccelerations: Set if to use or not the joint accelerations in estimation.

setUseOfJointAccelerations <enable>

Where enable is a boolean variable, true for enable the using of joint accelerations, and false for disabling it.

• getCurrentSettingsString: Get the current settings in the form of a string. Returns the current settings as a human readable string.

getCurrentSettingsString