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.

Author
Silvio Traversaro

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

Parameters

–robot name

–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.

Ports Accessed

Coming soon.

Ports Created

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

Tested OS

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:

Author
Silvio Traversaro