CoDyCo Whole-body Compliant Dynamical Contacts in Cognitive Humanoids
wholeBodyDynamicsTree

Estimates the external wrenches and internal torques of a robot by fusing the information coming from the dynamic model and the measurements coming from the sensors of the robot. More...

Estimates the external wrenches and internal torques of a robot by fusing the information coming from the dynamic model and the measurements coming from the sensors of the robot.

# Description

This module estimates the external wrenches acting at any link of robot, through a model based compensation of the 6-axis force/torque (FT) sensor's measurements, which are acquired through an input YARP port and provides them to an output YARP ports.

The estimation is perfomed relying on rigid body dynamics using CAD parameters.

The intrinsic offsets of the sensors, which are due to the stresses generated during mounting, are defined by the initial FT sensor data. Another source of offsets is the thermal drift of the sensors, to recalibrate the sensors you can send (assuming that the robot is attached to a fixed base) the calib all command to the RPC port of wholeBodyDynamicsTree (see Ports Created section to get more information on how to access the RPC port).

# Libraries

• YARP library.
• iDynTree library.
• yarpWholeBodyInterface library

# Parameters

–robot name

• The parameter name identifies the robot name. If not specified icub is assumed.

–period period The parameter period identifies the rate the estimation thread will work. If not specified 10ms is assumed.

–cutoff_imu cutoff_imu The parameter cutoff_imu specifies the cutoff frequency (in Hz) of the IMU low pass filter used in wholeBodyDynamicsTree. A typical values is 3.0 Hz. For more details on the filters used in wholeBodyDynamicsTree, plese check the filters section.

–cutoff_ft cutoff_ft The parameter cutoff_ft specifies the cutoff frequency (in Hz) of the six axis F/T low pass filter used in wholeBodyDynamicsTree. If cutoff_ft is not preset, not filtering is performed on the F/T measurements. For more details on the filters used in wholeBodyDynamicsTree, plese check the filters section.

–cutoff_velacc cutoff_velacc The parameter cutoff_velacc specifies the cutoff frequency (in Hz) of the joint velocity and acceleration low pass filter used in wholeBodyDynamicsTree. If cutoff_velacc is not preset, not filtering is performed on the joint velocity and acceleration measurements. For more details on the filters used in wholeBodyDynamicsTree, plese check the filters section.

Coming soon.

• /${name}/rpc:i This port enable to communicate via rpc with the module. To access it through the command line, open a terminal and write: yarp rpc /${name}/rpc:i Where ${name} is the name of the module (usually wholeBodyDynamics or wholeBodyDynamicsTree) After that, you can type help to get an overview of the available functions. If your robot is attached to the fixed base, the: calib all command will be used to calibrate the FT sensors. # Input Data Files None. # Output Data Files None. # Configuration Files Configuration files of wholeBodyDynamicsTree load several groups, to separate concerns about the different functionalities of the wholeBodyDynamicsTree. Parameter name Type Units Default Value Required Description Notes initial_world_frame string - - Yes Name of the frame of the model that is supposed to be coincident with the world/inertial at start - initial_fixed_link string - - Yes Name of the link that is assumed to be fixed at start - floating_base_frame string - - Yes Name of the frame assume to be the floating base - stream_com - - - No If present, open a port /${name}/com:o where you stream the COM position in world coordinates -
additional_frames list of strings - - No If present, open a port /${name}/frames:o where a Bottle of states of additional frames is streamed - Consider that this values are just initialization values, but you can always reset/change fixed link of the simple legged odometry using the RPC port. It the odometry is correctly configured, the /${name}/floatingbasestate:o port will stream a bottle with three lists, that are: the 4x4 Transform yarp::sig::Matrix representing the world_H_floatingbase transform (i.e. the matrix that transforms position homogenous vectors expressed in the floatingbase frame to vector expressed in the world frame) The 6x1 yarp::sig::Vector of the coordinates of the base link twist

Linux and OS X.

# Example

If you properly setted the YARP_ROBOT_NAME enviromental variable, and if you provide a proper wholeBodyDynamicsTree.ini configuration file you can simply launch wholeBodyDynamicsTree using the following command:

wholeBodyDynamicsTree --autoconnect

# Filters

wholeBodyDynamicsTree has optional support for filtering the measurement that reads from sensors. In particular it supports filtering of:

• the IMU readings. If the assume_fixed option is not used, wholeBodyDynamicsTree get the information about the gravity, the linear acceleration and the angular velocity and acceleration on a link in which a IMU is mounted. The default setup is to filter this readings with a first order low pass filter (implemented in iCub::ctrl::realtime::FirstOrderLowPassFilter) with a cutoff frequency of 3hz . This cutoff frequency can be changed with the cutoff_imu configuration parameter.
• the six axis F/T sensors readings. By default wholeBodyDynamicsTree does not filter this readings, but an optional first order low pass filter can be specified using the cutoff_ft configuration parameter. In the case the cutoff_ft parameter is specified, the cutoff frequency (in Hz) of the filter is given by the value of the cutoff_ft parametes.
• the encoder velocity and acceleration By default wholeBodyDynamicsTree does not filter this readings (that are normally obtained from low level numerical differentiation), but an optional first order low pass filter can be specified using the cutoff_velacc configuration parameter. In the case the cutoff_velacc parameter is specified, the cutoff frequency (in Hz) of the filter is given by the value of the cutoff_velacc parameter.