iDynTree/ForwardKinematics.h file

Namespaces

namespace iDynTree

Functions

auto ForwardPositionKinematics(const Model& model, const Traversal& traversal, const Transform& worldHbase, const VectorDynSize& jointPositions, LinkPositions& linkPositions) -> bool
Given a robot floating base configuration (i.e.
auto ForwardPositionKinematics(const Model& model, const Traversal& traversal, const FreeFloatingPos& jointPos, LinkPositions& linkPos) -> bool
Variant of ForwardPositionKinematics that takes in input a FreeFloatingPos object instead of a separate couple of (worldHbase,jointPositions)
auto ForwardVelAccKinematics(const iDynTree::Model& model, const iDynTree::Traversal& traversal, const iDynTree::FreeFloatingPos& robotPos, const iDynTree::FreeFloatingVel& robotVel, const iDynTree::FreeFloatingAcc& robotAcc, iDynTree::LinkVelArray& linkVel, iDynTree::LinkAccArray& linkAcc) -> bool
Function that compute the links velocities and accelerations given the free floating robot velocities and accelerations.
auto ForwardPosVelAccKinematics(const iDynTree::Model& model, const iDynTree::Traversal& traversal, const iDynTree::FreeFloatingPos& robotPos, const iDynTree::FreeFloatingVel& robotVel, const iDynTree::FreeFloatingAcc& robotAcc, iDynTree::LinkPositions& linkPos, iDynTree::LinkVelArray& linkVel, iDynTree::LinkAccArray& linkAcc) -> bool
Function that compute the links position, velocities and accelerations given the free floating robot position, velocities and accelerations.
auto ForwardPosVelKinematics(const iDynTree::Model& model, const iDynTree::Traversal& traversal, const iDynTree::FreeFloatingPos& robotPos, const iDynTree::FreeFloatingVel& robotVel, iDynTree::LinkPositions& linkPos, iDynTree::LinkVelArray& linkVel) -> bool
Function that compute the links position and velocities and accelerations given the free floating robot position and velocities.
auto ForwardAccKinematics(const Model& model, const Traversal& traversal, const FreeFloatingPos& robotPos, const FreeFloatingVel& robotVel, const FreeFloatingAcc& robotAcc, const LinkVelArray& linkVel, LinkAccArray& linkAcc) -> bool
Function that computes the links accelerations given the free floating robot velocities and accelerations.
auto ForwardBiasAccKinematics(const Model& model, const Traversal& traversal, const FreeFloatingPos& robotPos, const FreeFloatingVel& robotVel, const SpatialAcc& baseBiasAcc, const LinkVelArray& linkVel, LinkAccArray& linkBiasAcc) -> bool
Function that computes the links bias accelerations given the free floating robot velocities.
auto ForwardBiasAccKinematics(const Model& model, const Traversal& traversal, const FreeFloatingPos& robotPos, const FreeFloatingVel& robotVel, const LinkVelArray& linkVel, LinkAccArray& linkBiasAcc) -> bool
Legacy function, will be deprecated, use the variant with an explicit baseBiasAcc value.

Function documentation

bool ForwardVelAccKinematics(const iDynTree::Model& model, const iDynTree::Traversal& traversal, const iDynTree::FreeFloatingPos& robotPos, const iDynTree::FreeFloatingVel& robotVel, const iDynTree::FreeFloatingAcc& robotAcc, iDynTree::LinkVelArray& linkVel, iDynTree::LinkAccArray& linkAcc)

Function that compute the links velocities and accelerations given the free floating robot velocities and accelerations.

This function implements what is usually known as the "forward pass" of the Recursive Newton Euler algorithm.

bool ForwardAccKinematics(const Model& model, const Traversal& traversal, const FreeFloatingPos& robotPos, const FreeFloatingVel& robotVel, const FreeFloatingAcc& robotAcc, const LinkVelArray& linkVel, LinkAccArray& linkAcc)

Function that computes the links accelerations given the free floating robot velocities and accelerations.

This function implements what is usually known as the acceleration part "forward pass" of the Recursive Newton Euler algorithm.

bool ForwardBiasAccKinematics(const Model& model, const Traversal& traversal, const FreeFloatingPos& robotPos, const FreeFloatingVel& robotVel, const SpatialAcc& baseBiasAcc, const LinkVelArray& linkVel, LinkAccArray& linkBiasAcc)

Function that computes the links bias accelerations given the free floating robot velocities.

Parameters
model in the used model,
traversal in the used traversal,
robotPos in the position of the robot, i.e. $ ({}^A H_B, s)$ ,
robotVel in the velocity of the robot, with the base velocity expressed in BODY_FIXED representation i.e. $ \nu = \begin{bmatrix} {}^B \mathrm{v}_{A,B} \newline \dot{s} \end{bmatrix} $ ,
baseBiasAcc in base bias acceleration with BODY_FIXED rapresentation, useful when the bias acceleration is considering the MIXED base acceleration to be zero,
linkVel in the velocity of each link of the robot, with velocity expressed with BODY_FIXED representation i.e. for each link $ $ we have $ {}^L \mathrm{v}_{A,L} $ ,
linkBiasAcc out the bias acceleration of each link of the robot, with the acceleration expressed with BODY_FIXED representation.