Skip to content
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.

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

Support specifying the FT sensors to use in the estimation via multipleAnalogSensorsNames::SixAxisForceTorqueSensorsNames without the need to modify the loaded URDF #194

Merged
merged 4 commits into from
Jul 12, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
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
21 changes: 20 additions & 1 deletion devices/wholeBodyDynamics/README.md
Original file line number Diff line number Diff line change
Expand Up @@ -61,6 +61,26 @@ For an overview on `wholeBodyDynamics` and to understand how to run the device,
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.

#### Specifying the Force Torque Sensors to use

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`](https://www.yarp.it/latest/classyarp_1_1dev_1_1ISixAxisForceTorqueSensors.html) 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`](https://www.yarp.it/latest/classyarp_1_1dev_1_1ITemperatureSensors.html) 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.


#### Gravity 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.

Expand Down Expand Up @@ -149,7 +169,6 @@ Typically this estimates are provided only for the upper joints (arms and torso)
</group>
```

For a detailed explanation on their usage, please see the document [Using temperature coefficients and pre-estimated FT offsets](../../doc/howto/useTemperatureCoefficientsAndOffsetCompensationInWholeBodyDynamics.md).

#### Filters

Expand Down
393 changes: 170 additions & 223 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.cpp

Large diffs are not rendered by default.

8 changes: 4 additions & 4 deletions devices/wholeBodyDynamics/WholeBodyDynamicsDevice.h
Original file line number Diff line number Diff line change
Expand Up @@ -366,6 +366,8 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver,
bool loadSettingsFromConfig(yarp::os::Searchable& config);
bool loadSecondaryCalibrationSettingsFromConfig(yarp::os::Searchable& config);
bool loadGravityCompensationSettingsFromConfig(yarp::os::Searchable & config);
bool m_temperatureCompensationEnabled{false};
bool checkIfTemperatureCompensationIsSetFromConfig(yarp::os::Searchable & config);
bool loadTemperatureCoefficientsSettingsFromConfig(yarp::os::Searchable& config);
bool loadFTSensorOffsetFromConfig(yarp::os::Searchable& config);
bool applyLPFSettingsFromConfig(const yarp::os::Property& config, const std::string& setting_name);
Expand Down Expand Up @@ -446,11 +448,9 @@ class WholeBodyDynamicsDevice : public yarp::dev::DeviceDriver,
std::vector<wholeBodyDynamics::SixAxisForceTorqueMeasureProcessor> ftProcessors;

/**
* Vector of Analog FT Sensor names
* Vector of FT Sensor names considered for estimation
*/
std::vector<std::string> ftAnalogSensorNames;
std::vector<std::string> ftMultipleAnalogSensorNames;
std::vector<int> ftMultipleAnalogSensorIdxMapping;
std::vector<std::string> ftSensorNames;

/***
* RPC Calibration related methods
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
<param name="forceTorqueEstimateConfidence">2</param>
<param name="useJointVelocity">true</param>
<param name="useJointAcceleration">false</param>
<param name="estimateJointVelocityAcceleration">false</param>
<param name="streamFilteredFT">true</param>
<param name="imuFilterCutoffInHz">3.0</param>
<param name="forceTorqueFilterCutoffInHz">3.0</param>
Expand Down

This file was deleted.

Loading