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

A simple legged odometry for a legged robot.

Under the assumption that at least a link of the robot at the time is not moving (no slippage), it computes the estimate of the transform between a inertial/world frame and the robot floating base.

The algorithm implemented is the following :

  1. During initialization the user of the class specifies (through init()):
    • a frame that is rigidly attached to a link that is not moving (initialFixedFrame)
    • an optional transform between the desired location of the world and the fixed frame (world_H_initialFixedFrame) At the beginning, the world_H_fixed is the one specified (default is the identity)
  2. At this point, the getWorldFrameTransform(iDynTree::FrameIndex frame_id) will return the world_H_frame ( ${}^{world} H_{frame}$ ) transform simply by computing the forward kinematics from the fixed frame to the frame specified by frame_id : world_H_frame = world_H_fixed * fixed_H_frame(qj), i.e. ${}^{world} H_{frame} = {}^{world} H_{fixed} \cdot {}^{fixed} H_{frame}(q_j)$
  3. If the fixed frame changes, we can simply change the frame used as "fixed" (changeFixedLink()), and consistently update the world_H_fixed transform to be equal to world_H_new_fixed = world_H_old_fixed * old_fixed_H_new_fixed(qj), i.e. ${}^{world} H_{new\_fixed} = {}^{world} H_{old\_fixed} \cdot {}^{old\_fixed} H_{new\_fixed}(q_j)$
  4. After the update, the getWorldFrameTransform(iDynTree::FrameIndex frame_id) can be obtained as in point 1b .

To reset the location of the world, init can simply be called again.

Constructors, destructors, conversion operators

SimpleLeggedOdometry()
Constructor.
~SimpleLeggedOdometry()
Destructor.

Public functions

auto setModel(const Model& _model) -> bool
Set model used for the odometry.
auto model() const -> const Model&
Get used model.
auto updateKinematics(JointPosDoubleArray& jointPos) -> bool
Set the measured joint positions.
auto init(const std::string& initialFixedFrame, const Transform initialFixedFrame_H_world = Transform::Identity()) -> bool
Initialize the odometry.
auto init(const FrameIndex initialFixedFrameIndex, const Transform initialFixedFrame_H_world = Transform::Identity()) -> bool
Initialize the odometry.
auto init(const std::string& initialFixedFrame, const std::string& initialReferenceFrameForWorld, const Transform initialReferenceFrame_H_world = Transform::Identity()) -> bool
Initialize the odometry, specifying separately initial fixed frame and world.
auto init(const FrameIndex initialFixedFrameIndex, const FrameIndex initialReferenceFrameIndexForWorld, const Transform initialReferenceFrame_H_world = Transform::Identity()) -> bool
Initialize the odometry, specifying separately initial fixed frame and world.
auto changeFixedFrame(const std::string& newFixedFrame) -> bool
Change the link that the odometry assumes to be fixed with respect to the inertial/world frame.
auto changeFixedFrame(const FrameIndex newFixedFrame) -> bool
Change the link that the odometry assumes to be fixed with respect to the inertial/world frame.
auto changeFixedFrame(const std::string& newFixedFrame, const Transform& world_H_newFixedFrame) -> bool
Change the link that the odometry assumes to be fixed with respect to the inertial/world frame.
auto changeFixedFrame(const FrameIndex newFixedFrame, const Transform& world_H_newFixedFrame) -> bool
Change the link that the odometry assumes to be fixed with respect to the inertial/world frame.
auto getCurrentFixedLink() -> std::string
Get the link currently considered fixed with respect to the inertial frame.
auto getWorldLinkTransform(const LinkIndex frame_index) -> iDynTree::Transform
Get the world_H_link transform for an arbitrary link.
auto getWorldFrameTransform(const FrameIndex frame_index) -> iDynTree::Transform
Get the world_H_frame transform for an arbitrary frame.

Function documentation

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

Set model used for the odometry.

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

The model is copied inside the class, to be used for the odometry estimation.

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

Get used model.

Returns the kinematic and dynamic model used for estimation.

bool iDynTree::SimpleLeggedOdometry::updateKinematics(JointPosDoubleArray& jointPos)

Set the measured joint positions.

This is used to update the joints positions used by the odometry.

bool iDynTree::SimpleLeggedOdometry::init(const std::string& initialFixedFrame, const Transform initialFixedFrame_H_world = Transform::Identity())

Initialize the odometry.

Parameters
initialFixedFrame in Frame initially assumed to be fixed.
initialFixedFrame_H_world in Pose of the world w.r.t. the initial fixed frame (default: identity, i.e. the initialFixedFrame is the world).
Returns true if all went well, false otherwise.

This method initializes the world location w.r.t. to a frame that is not the frame that is initially assumed fixed. For this reason it is necessary to call updateKinematics at least once before calling this method.

bool iDynTree::SimpleLeggedOdometry::init(const FrameIndex initialFixedFrameIndex, const Transform initialFixedFrame_H_world = Transform::Identity())

Initialize the odometry.

Parameters
initialFixedFrameIndex in Frame initially assumed to be fixed.
initialFixedFrame_H_world in Pose of the world w.r.t. the initial fixed frame (default: identity, i.e. the initialFixedFrame is the world).
Returns true if all went well, false otherwise.

This method initializes the world location w.r.t. to a frame that is not the frame that is initially assumed fixed. For this reason it is necessary to call updateKinematics at least once before calling this method.

bool iDynTree::SimpleLeggedOdometry::init(const std::string& initialFixedFrame, const std::string& initialReferenceFrameForWorld, const Transform initialReferenceFrame_H_world = Transform::Identity())

Initialize the odometry, specifying separately initial fixed frame and world.

Parameters
initialFixedFrame in Frame initially assumed to be fixed.
initialReferenceFrameForWorld in Frame in which the initial world is expressed.
initialReferenceFrame_H_world in Pose of the world w.r.t. the initial reference frame (default: identity, i.e. the initialReferenceFrameForWorld is the world).
Returns true if all went well, false otherwise.

This method initializes the world location w.r.t. to a frame that is not the frame that is initially assumed fixed, for this reason it is necessary to call updateKinematics at least once before calling this method.

bool iDynTree::SimpleLeggedOdometry::init(const FrameIndex initialFixedFrameIndex, const FrameIndex initialReferenceFrameIndexForWorld, const Transform initialReferenceFrame_H_world = Transform::Identity())

Initialize the odometry, specifying separately initial fixed frame and world.

Parameters
initialFixedFrameIndex in Frame initially assumed to be fixed.
initialReferenceFrameIndexForWorld in Frame in which the initial world is expressed.
initialReferenceFrame_H_world in Pose of the world w.r.t. the initial reference frame (default: identity, i.e. the initialReferenceFrameForWorld is the world).
Returns true if all went well, false otherwise.

This method initializes the world location w.r.t. to a frame that is not the frame that is initially assumed fixed, for this reason it is necessary to call updateKinematics at least once before calling this method.

bool iDynTree::SimpleLeggedOdometry::changeFixedFrame(const std::string& newFixedFrame, const Transform& world_H_newFixedFrame)

Change the link that the odometry assumes to be fixed with respect to the inertial/world frame.

bool iDynTree::SimpleLeggedOdometry::changeFixedFrame(const FrameIndex newFixedFrame, const Transform& world_H_newFixedFrame)

Change the link that the odometry assumes to be fixed with respect to the inertial/world frame.

std::string iDynTree::SimpleLeggedOdometry::getCurrentFixedLink()

Get the link currently considered fixed with respect to the inertial frame.

Returns the name of the link currently considered fixed.

iDynTree::Transform iDynTree::SimpleLeggedOdometry::getWorldLinkTransform(const LinkIndex frame_index)

Get the world_H_link transform for an arbitrary link.

iDynTree::Transform iDynTree::SimpleLeggedOdometry::getWorldFrameTransform(const FrameIndex frame_index)

Get the world_H_frame transform for an arbitrary frame.