iDynTree::ExtWrenchesAndJointTorquesEstimator class
#include <iDynTree/ExtWrenchesAndJointTorquesEstimator.h>

Estimator for external wrenches and joint torques using internal F/T sensors.

This is a class for estimating external wrenches and joint torques using as an input the robot velocities and the accelerations and the measurement of internal six axis F/T sensors.

The kinematic information (position,velocity and acceleration) necessary for the estimation can be provided in two ways:

  • using the updateKinematicsFromFloatingBase
  • using the updateKinematicsFromFixedBase

Note that in both ways there is no need (for the estimation) to provide the absolute position and linear velocity of the robot with respect to the inertial frame. The effect of gravity is considered by directly using the proper acceleration in the case of the floating frame (proper acceleration can be directly measured by an accelerometer) or by directly providing the gravity vector in the fixed frame case.

Beside its main goal of estimation of external wrenches and joint torques, the class also provide methods that can be useful to calibrate the six-axis FT sensors of the robot. These methods are:

Constructors, destructors, conversion operators

ExtWrenchesAndJointTorquesEstimator()
Constructor.
~ExtWrenchesAndJointTorquesEstimator()
Destructor.

Public functions

auto setModel(const Model& _model) -> bool
Set model and sensors used for the estimation.
auto setModelAndSensors(const Model& _model, const SensorsList& _sensors) -> bool
Set model and sensors used for the estimation.
auto loadModelAndSensorsFromFile(const std::string filename, const std::string filetype = "") -> bool deprecated
Load model and sensors from file.
auto loadModelAndSensorsFromFileWithSpecifiedDOFs(const std::string filename, const std::vector<std::string>& consideredDOFs, const std::string filetype = "") -> bool
Load model and sensors from file, specifieng the dof considered for the estimation.
auto model() const -> const Model&
Get used model.
auto sensors() const -> const SensorsList&
Get used sensors.
auto submodels() const -> const SubModelDecomposition&
Get the used submodel decomposition.
auto updateKinematicsFromFloatingBase(const VectorDynSize& jointPos, const VectorDynSize& jointVel, const VectorDynSize& jointAcc, const FrameIndex& floatingFrame, const Vector3& properClassicalLinearAcceleration, const Vector3& angularVel, const Vector3& angularAcc) -> bool
Set the kinematic information necessary for the force estimation using the acceleration and angular velocity information of a floating frame.
auto updateKinematicsFromFixedBase(const VectorDynSize& jointPos, const VectorDynSize& jointVel, const VectorDynSize& jointAcc, const FrameIndex& fixedFrame, const Vector3& gravity) -> bool
Set the kinematic information necessary for the force estimation assuming that a given frame is not accelerating with respect to the inertial frame.
auto computeExpectedFTSensorsMeasurements(const LinkUnknownWrenchContacts& unknowns, SensorsMeasurements& predictedMeasures, LinkContactWrenches& estimatedContactWrenches, JointDOFsDoubleArray& estimatedJointTorques) -> bool
Predict FT sensors using the knoledge of external wrenches location.
auto computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(const LinkUnknownWrenchContacts& unknowns, std::vector<iDynTree::MatrixDynSize>& A, std::vector<iDynTree::VectorDynSize>& b, std::vector<std::ptrdiff_t>& subModelID, std::vector<iDynTree::LinkIndex>& baseLinkIndeces) -> bool
For each submodel without any external wrench, computes the equation that relates the FT sensor measures with the kinematics-related known terms.
auto estimateExtWrenchesAndJointTorques(const LinkUnknownWrenchContacts& unknowns, const SensorsMeasurements& ftSensorsMeasures, LinkContactWrenches& estimatedContactWrenches, JointDOFsDoubleArray& estimatedJointTorques) -> bool
Estimate the external wrenches and the internal joint torques using the measurement of the internal F/T sensors.
auto checkThatTheModelIsStill(const double gravityNorm, const double properAccTol, const double verbose) -> bool
Check if the kinematics set in the model are the one of a fixed model.
auto estimateLinkNetWrenchesWithoutGravity(LinkNetTotalWrenchesWithoutGravity& netWrenches) -> bool
Compute the vector of the sum of all the wrenches (both internal and external, excluding gravity) acting on link i, expressed (both orientation and point) with respect to the reference frame of link i, using the articulated body model and the kinematics information provided by the updateKinematics* methods.

Function documentation

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::setModel(const Model& _model)

Set model and sensors used for the estimation.

Parameters
_model in the kinematic and dynamic model used for the estimation.
Returns true if all went well (model and sensors are well formed), false otherwise.

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::setModelAndSensors(const Model& _model, const SensorsList& _sensors)

Set model and sensors used for the estimation.

Parameters
_model in the kinematic and dynamic model used for the estimation.
_sensors in the sensor model used for the estimation.
Returns true if all went well (model and sensors are well formed), false otherwise.

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFile(const std::string filename, const std::string filetype = "")

Load model and sensors from file.

Parameters
filename in path to the file to load.
filetype in (optional) explicit definiton of the filetype to load. Only "urdf" is supported at the moment.
Returns true if all went well (files were correctly loaded and consistent), false otherwise.

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::loadModelAndSensorsFromFileWithSpecifiedDOFs(const std::string filename, const std::vector<std::string>& consideredDOFs, const std::string filetype = "")

Load model and sensors from file, specifieng the dof considered for the estimation.

Parameters
filename in path to the file to load.
consideredDOFs in list of dof to consider in the model.
filetype in (optional) explicit definiton of the filetype to load. Only "urdf" is supported at the moment.
Returns true if all went well (files were correctly loaded and consistent), false otherwise.

const Model& iDynTree::ExtWrenchesAndJointTorquesEstimator::model() const

Get used model.

Returns the kinematic and dynamic model used for estimation.

const SensorsList& iDynTree::ExtWrenchesAndJointTorquesEstimator::sensors() const

Get used sensors.

Returns the sensor model used for estimation.

const SubModelDecomposition& iDynTree::ExtWrenchesAndJointTorquesEstimator::submodels() const

Get the used submodel decomposition.

Returns the used submodel decomposition.

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFloatingBase(const VectorDynSize& jointPos, const VectorDynSize& jointVel, const VectorDynSize& jointAcc, const FrameIndex& floatingFrame, const Vector3& properClassicalLinearAcceleration, const Vector3& angularVel, const Vector3& angularAcc)

Set the kinematic information necessary for the force estimation using the acceleration and angular velocity information of a floating frame.

Parameters
jointPos in the position of the joints of the model.
jointVel in the velocities of the joints of the model.
jointAcc in the accelerations of the joints of the model.
floatingFrame in the index of the frame for which kinematic information is provided.
properClassicalLinearAcceleration in proper (actual acceleration-gravity) classical acceleration of the origin of the specified frame, expressed in the specified frame orientation.
angularVel in angular velocity (wrt to an inertial frame) of the specified floating frame, expressed in the specified frame orientation.
angularAcc in angular acceleration (wrt to an inertial frame) of the specified floating frame , expressed in the specified frame orientation.
Returns true if all went ok, false otherwise.

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::updateKinematicsFromFixedBase(const VectorDynSize& jointPos, const VectorDynSize& jointVel, const VectorDynSize& jointAcc, const FrameIndex& fixedFrame, const Vector3& gravity)

Set the kinematic information necessary for the force estimation assuming that a given frame is not accelerating with respect to the inertial frame.

Parameters
jointPos in the position of the joints of the model.
jointVel in the velocities of the joints of the model.
jointAcc in the accelerations of the joints of the model.
fixedFrame in the index of the frame that is not accelerating with respect to the inertial frame.
gravity in the gravity acceleration vector, expressed in the specified fixed frame.
Returns true if all went ok, false otherwise.

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::computeExpectedFTSensorsMeasurements(const LinkUnknownWrenchContacts& unknowns, SensorsMeasurements& predictedMeasures, LinkContactWrenches& estimatedContactWrenches, JointDOFsDoubleArray& estimatedJointTorques)

Predict FT sensors using the knoledge of external wrenches location.

Parameters
unknowns in the unknown external wrenches.
predictedMeasures out the estimate measures for the FT sensors.
estimatedContactWrenches out the estimated contact wrenches.
estimatedJointTorques out the estimated joint torques.
Returns true if all went well, false otherwise.

This function is used to estimate the expected measurement of the FT sensors. The typical use of this function is to specify only one external unknown wrench in the unknowns parameter and then compute the expected measurements of the FT sensors using the kinematic information specified with an updateKinematics*** method. The location of the single external unknown wrench is the one of the only link that is supporting the weight of the robot.

This function can also be used to estimate the FT sensor measurements in case two unknown external wrenches are applied on the robot, if some additional assumption about the simmetry of the robot configuration, the joint torques and the external wrenches can be done.

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::computeSubModelMatrixRelatingFTSensorsMeasuresAndKinematics(const LinkUnknownWrenchContacts& unknowns, std::vector<iDynTree::MatrixDynSize>& A, std::vector<iDynTree::VectorDynSize>& b, std::vector<std::ptrdiff_t>& subModelID, std::vector<iDynTree::LinkIndex>& baseLinkIndeces)

For each submodel without any external wrench, computes the equation that relates the FT sensor measures with the kinematics-related known terms.

Parameters
unknowns in the unknown external wrenches, that is used to understand the submodels in which no external wrench is present
A out vector of nrOfSubModelsWithoutExtWrenches matrices of size 6 x 6*nrOfFTSensors
b out vector of nrOfSubModelsWithoutExtWrenches vectors of size 6
subModelID
baseLinkIndeces out vector of size nrOfSubModelsWithoutExtWrenches of iDynTree::LinkIndex from 0 to nrOfLinks-1, baseLinkIndeces[i] specifies the link in which the equation i is expressed
Returns true if all went well, false otherwise.

For each submodel in which there are no external forces, we can write the following equation:

\[ A w = b \]

Where:

  • $w$ is the vector of dimension 6*nrOfFTSensors obtained by stacking the FT sensors measures:
  • $A$ is a matrix of size 6 x 6*nrOfFTSensors that depends on position in space of the FT sensors
  • $b$ is a vector of size 6 that depends on the position, velocity, acceleration and gravity of each link in the submodel

This function provides an easy way to compute A and B . Typically, these quantities are not used online during the estimation of external wrenches or internal torques, but rather as an helper method when calibrating FT sensors.

In the rest of the documentation, we will refer to this quantities:

  • nrOfSubModels ( $n_{sm}$ ): the number of submodels in which the model is divided, as induced by the FT sensors present in the model.
  • nrOfSubModelsWithoutExtWrenches: the number of submodels on which there is no external wrench
  • nrOfFTSensors ( $n_{ft}$ ): the number of FT sensors present in the model In particular, the value of A and b for a given submodel is the following. First of all, for any submodel $sm$ with no external force, we can write (from Equation 4.19 of https://traversaro.github.io/traversaro-phd-thesis/traversaro-phd-thesis.pdf, modified to account for all FT sensors in the submodel and to remove external wrenches):
\[ \sum_{s=1}^{n_ft}~\mu_{sm, ft}~\sigma_{sm, ft}~{}_{B_{sm}} X^{ft} \mathrm{f}^{meas}_{ft} = \sum_{L \in \mathbb{L}_{sm}} {}_{B_{sm}} X^{L} {}_L \phi_L \]

where:

  • $ \mu_{sm, ft} $ is equal to $1$ if the sensor $ft$ is attached to the submodel $sm$ , and $0$ otherwise
  • $ \sigma_{sm, ft} $ is equal to $1$ if the sensor $ft$ is measuring the force applied on submodel $sm$ or $-1$ if it is measuring the force that the submodel excerts on its neighbor submodel
  • $ {B_{sm}} $ is a frame in which this equation is expressed, that for this function it is the base link of the submodel.

With this definitions, we can see that A_sm and b_sm for a given submodel $sm$ can be simply be defined as:

$ A_{sm} = \begin{bmatrix} \mu_{sm, ft0}~\sigma_{sm, ft(0)}~{}_C X^{ft(0)} & \hdots & \mu_{sm, ft(n_ft-1)}~\sigma_{sm, ft(n_ft-1)}~X^{ft(n_ft-1)} \end{bmatrix} $

$ w_{sm} = \begin{bmatrix} \mathrm{f}^{meas}_{ft(0)} \\ \vdots \\ \mathrm{f}^{meas}_{ft(n_ft-1)} \end{bmatrix} $

$ b_{sm} = \sum_{L \in \mathbb{L}_{sm}} {}_C X^{L} {}^L \phi_L $

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::estimateExtWrenchesAndJointTorques(const LinkUnknownWrenchContacts& unknowns, const SensorsMeasurements& ftSensorsMeasures, LinkContactWrenches& estimatedContactWrenches, JointDOFsDoubleArray& estimatedJointTorques)

Estimate the external wrenches and the internal joint torques using the measurement of the internal F/T sensors.

Parameters
unknowns in the unknown external wrenches
ftSensorsMeasures in the measurements for the FT sensors.
estimatedContactWrenches out the estimated contact wrenches.
estimatedJointTorques out the estimated joint torques.
Returns true if all went ok, false otherwise.

This is the main method of the class. The technique implemented in this method is the one described in the paper:

"Contact force estimations using tactile sensors and force/torque sensors"

Del Prete, A., Natale, L., Nori, F., & Metta, G. (2012). Contact force estimations using tactile sensors and force/torque sensors. URL : http://www.researchgate.net/profile/Andrea_Del_Prete/publication/236152161_Contact_Force_Estimations_Using_Tactile_Sensors_and_Force__Torque_Sensors/links/00b495166e74a5369d000000.pdf

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::checkThatTheModelIsStill(const double gravityNorm, const double properAccTol, const double verbose)

Check if the kinematics set in the model are the one of a fixed model.

Parameters
gravityNorm in the norm of the gravity (tipically 9.81) against with all the proper accelerations are check (for a still model, the proper acceleration norm should be close to the gravity norm.
properAccTol in tolerance to use for the check on the proper acceleration norm.
verbose in true if you want to print debug information, false otherwise.
Returns true if the model is still, false if it is moving or if the kinematics was never setted.

While computing the expected F/T sensors measures, you tipically want the model to be still, to reduce the sources of noise .

bool iDynTree::ExtWrenchesAndJointTorquesEstimator::estimateLinkNetWrenchesWithoutGravity(LinkNetTotalWrenchesWithoutGravity& netWrenches)

Compute the vector of the sum of all the wrenches (both internal and external, excluding gravity) acting on link i, expressed (both orientation and point) with respect to the reference frame of link i, using the articulated body model and the kinematics information provided by the updateKinematics* methods.

Parameters
netWrenches out the vector of link net wrenches.
Returns true if all went ok, false otherwise.

This is tipically computed as I*a+v*(I*v) , where a is the proper acceleration.