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

Helper class for computing Berdy matrices.

Berdy refers to a class for algorithms to compute the Maximum A Posteriori (MAP) estimation of the dynamic variables of a multibody model (accelerations, external forces, joint torques) assuming the knowledge the measurements of an arbitrary set of sensors and of the kinematics and inertial characteristics of the model.

The MAP estimation is computed using a sparse matrix representation of the multibody dynamics Newton-Euler equations, originally described in:

Latella, C.; Kuppuswamy, N.; Romano, F.; Traversaro, S.; Nori, F. Whole-Body Human Inverse Dynamics with Distributed Micro-Accelerometers, Gyros and Force Sensing. Sensors 2016, 16, 727. http://www.mdpi.com/1424-8220/16/5/727

Nori F, Kuppuswamy N, Traversaro S. Simultaneous state and dynamics estimation in articulated structures. In Intelligent Robots and Systems (IROS), 2015 IEEE/RSJ International Conference on 2015 Sep 28 (pp. 3380-3386). IEEE. http://ieeexplore.ieee.org/xpl/articleDetails.jsp?arnumber=7353848

This helper class implements two different variants of the Berdy dynamics representation, identified with the BerdyVariants enum:

The ORIGINAL_BERDY_FIXED_BASE is the original BERDY representation described in the papers, that was assuming a fixed base model with all the joints with 1-DOF .
The BERDY_FLOATING_BASE is a variant in which the model is assumed to be floating, does not need the position or linear velocity of the floating base and supports joints with an arbitrary number of DOF .

The sensors supported by this class are the one contained in the SensorsList representation, which can be loaded directly from an URDF representation, see https://github.com/robotology/idyntree/blob/master/doc/model_loading.md#sensor-extensions . Using this representation the following sensors can be loaded:

Internal Six-Axis Force Torque sensors
Gyroscopes
Accelerometers Support for joint torques/acceleration or external wrenches measurements still needs to be added.

Constructors, destructors, conversion operators

BerdyHelper(): Constructor.

Public functions

auto model() -> Model&: Access the model.
auto sensors() -> SensorsList&: Access the sensors.
auto dynamicTraversal() const -> const Traversal&: Acces the traveral used for the dynamics computations (const version)
auto model() const -> const Model&: Access the model (const version).
auto sensors() const -> const SensorsList&: Access the model (const version).
auto isValid() const -> bool: Returns if the helper is valid.
auto init(const Model& model, const BerdyOptions options = BerdyOptions()) -> bool: Init the class.
auto init(const Model& model, const SensorsList& sensors, const BerdyOptions options = BerdyOptions()) -> bool: Init the class.
auto getOptions() const -> BerdyOptions: Get currently used options.
auto getNrOfDynamicVariables() const -> size_t: Get the number of columns of the D matrix.
auto getNrOfDynamicEquations() const -> size_t: Get the number of dynamics equations used in the Berdy equations.
auto getNrOfSensorsMeasurements() const -> size_t: Get the number of sensors measurements.
auto resizeAndZeroBerdyMatrices(SparseMatrix<iDynTree::ColumnMajor>& D, VectorDynSize& bD, SparseMatrix<iDynTree::ColumnMajor>& Y, VectorDynSize& bY) -> bool: Resize and set to zero Berdy matrices.
auto resizeAndZeroBerdyMatrices(MatrixDynSize& D, VectorDynSize& bD, MatrixDynSize& Y, VectorDynSize& bY) -> bool: Resize and set to zero Berdy matrices.
auto getBerdyMatrices(SparseMatrix<iDynTree::ColumnMajor>& D, VectorDynSize& bD, SparseMatrix<iDynTree::ColumnMajor>& Y, VectorDynSize& bY) -> bool: Get Berdy matrices.
auto getBerdyMatrices(MatrixDynSize& D, VectorDynSize& bD, MatrixDynSize& Y, VectorDynSize& bY) -> bool: Get Berdy matrices.
auto getSensorsOrdering() const -> const std::vector<BerdySensor>&: Return the internal ordering of the sensors.
auto getRangeSensorVariable(const SensorType type, const unsigned int sensorIdx) const -> IndexRange: Get the range of the specified sensor in.
auto getRangeDOFSensorVariable(const BerdySensorTypes sensorType, const DOFIndex idx) const -> IndexRange
auto getRangeJointSensorVariable(const BerdySensorTypes sensorType, const JointIndex idx) const -> IndexRange
auto getRangeLinkSensorVariable(const BerdySensorTypes sensorType, const LinkIndex idx) const -> IndexRange
auto getRangeRCMSensorVariable(const BerdySensorTypes sensorType) const -> IndexRange
auto getRangeLinkVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const LinkIndex idx) const -> IndexRange: Ranges of dynamic variables.
auto getRangeJointVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const JointIndex idx) const -> IndexRange
auto getRangeDOFVariable(const BerdyDynamicVariablesTypes dynamicVariableType, const DOFIndex idx) const -> IndexRange
auto getDynamicVariablesOrdering() const -> const std::vector<BerdyDynamicVariable>&
auto serializeDynamicVariables(LinkProperAccArray& properAccs, LinkNetTotalWrenchesWithoutGravity& netTotalWrenchesWithoutGrav, LinkNetExternalWrenches& netExtWrenches, LinkInternalWrenches& linkJointWrenches, JointDOFsDoubleArray& jointTorques, JointDOFsDoubleArray& jointAccs, VectorDynSize& d) -> bool: Serialized dynamic variables from the separate buffers.
auto serializeSensorVariables(SensorsMeasurements& sensMeas, LinkNetExternalWrenches& netExtWrenches, JointDOFsDoubleArray& jointTorques, JointDOFsDoubleArray& jointAccs, LinkInternalWrenches& linkJointWrenches, SpatialForceVector& rcm, VectorDynSize& y) -> bool: Serialize sensor variable from separate buffers.
auto serializeDynamicVariablesComputedFromFixedBaseRNEA(JointDOFsDoubleArray& jointAccs, LinkNetExternalWrenches& netExtWrenches, VectorDynSize& d) -> bool: Debug function:
auto extractJointTorquesFromDynamicVariables(const VectorDynSize& d, const VectorDynSize& jointPos, VectorDynSize& jointTorques) const -> bool: Extract the joint torques from the dynamic variables.
auto extractLinkNetExternalWrenchesFromDynamicVariables(const VectorDynSize& d, LinkNetExternalWrenches& netExtWrenches) const -> bool: Extract the net external force-torques from the dynamic variables.

Public variables

size_t dofAccelerationOffset
size_t dofTorquesOffset
size_t netExtWrenchOffset
size_t jointWrenchOffset
size_t rcmOffset
std::vector<JointIndex> wrenchSensors: List of joint wrench sensors.
std::vector<size_t> jntIdxToOffset: Mapping between jndIx in wrenchSensor and.

Methods to submit the input data for dynamics computations.

auto updateKinematicsFromFloatingBase(const JointPosDoubleArray& jointPos, const JointDOFsDoubleArray& jointVel, const FrameIndex& floatingFrame, const Vector3& angularVel) -> bool: Set the kinematic information necessary for the dynamics estimation using the angular velocity information of a floating frame.
auto updateKinematicsFromFixedBase(const JointPosDoubleArray& jointPos, const JointDOFsDoubleArray& jointVel, const FrameIndex& fixedFrame, const Vector3& gravity) -> bool: Set the kinematic information necessary for the dynamics estimation assuming that a given frame is not accelerating with respect to the inertial frame.
auto updateKinematicsFromTraversalFixedBase(const JointPosDoubleArray& jointPos, const JointDOFsDoubleArray& jointVel, const Vector3& gravity) -> bool: Set the kinematic information necessary for the dynamics estimation assuming that a given baseframe (specified by the m_dynamicsTraversal) is not accelerating with respect to the inertial frame.
auto setNetExternalWrenchMeasurementFrame(const LinkIndex lnkIndex, const Transform& link_H_contact) -> bool: Set/get the transformation link_H_contact between the link frame and the frame in which the measured net ext wrench is expressed.
auto getNetExternalWrenchMeasurementFrame(const LinkIndex lnkIndex, Transform& link_H_contact) const -> bool

Function documentation

bool iDynTree::BerdyHelper::isValid() const

Returns if the helper is valid.

Returns	true if the helper is valid. False otherwise

The helper is valid if the model and the sensors have been loaded

bool iDynTree::BerdyHelper::init(const Model& model, const BerdyOptions options = BerdyOptions())

Init the class.

Parameters
model in	The used model.
options in	The used options, check BerdyOptions docs.
Returns	true if all went well, false otherwise.

bool iDynTree::BerdyHelper::init(const Model& model, const SensorsList& sensors, const BerdyOptions options = BerdyOptions())

Init the class.

Parameters
model in	The used model.
sensors in	The used sensors.
options in	The used options, check BerdyOptions docs.
Returns	true if all went well, false otherwise.

size_t iDynTree::BerdyHelper::getNrOfDynamicVariables() const

Get the number of columns of the D matrix.

This depends on the Berdy variant selected.

bool iDynTree::BerdyHelper::getBerdyMatrices(MatrixDynSize& D, VectorDynSize& bD, MatrixDynSize& Y, VectorDynSize& bY)

Get Berdy matrices.

const std::vector<BerdySensor>& iDynTree::BerdyHelper::getSensorsOrdering() const

Return the internal ordering of the sensors.

Returns	the sensors ordering

Measurements are expected to respect the internal sensors ordering Use this function to obtain the sensors ordering.

bool iDynTree::BerdyHelper::serializeDynamicVariablesComputedFromFixedBaseRNEA(JointDOFsDoubleArray& jointAccs, LinkNetExternalWrenches& netExtWrenches, VectorDynSize& d)

Debug function:

bool iDynTree::BerdyHelper::updateKinematicsFromFloatingBase(const JointPosDoubleArray& jointPos, const JointDOFsDoubleArray& jointVel, const FrameIndex& floatingFrame, const Vector3& angularVel)

Set the kinematic information necessary for the dynamics estimation using the 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.
floatingFrame in	the index of the frame for which kinematic information is provided.
angularVel in	angular velocity (wrt to any inertial frame) of the specified floating frame, expressed in the specified floating frame orientation.
Returns	true if all went ok, false otherwise.

bool iDynTree::BerdyHelper::updateKinematicsFromFixedBase(const JointPosDoubleArray& jointPos, const JointDOFsDoubleArray& jointVel, const FrameIndex& fixedFrame, const Vector3& gravity)

Set the kinematic information necessary for the dynamics 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.
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::BerdyHelper::updateKinematicsFromTraversalFixedBase(const JointPosDoubleArray& jointPos, const JointDOFsDoubleArray& jointVel, const Vector3& gravity)

Set the kinematic information necessary for the dynamics estimation assuming that a given baseframe (specified by the m_dynamicsTraversal) 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.
gravity in	the gravity acceleration vector, expressed in the specified fixed frame.
Returns	true if all went ok, false otherwise.

bool iDynTree::BerdyHelper::setNetExternalWrenchMeasurementFrame(const LinkIndex lnkIndex, const Transform& link_H_contact)

Set/get the transformation link_H_contact between the link frame and the frame in which the measured net ext wrench is expressed.

The default value is the identity. This is extremly useful to correctly tune the variances when only a subset of the external net wrench is known (for example when it is known that the external net wrench is a pure force on a point different from the link frame.