Rigid Body Dynamics Library
Data Structures | Functions
Kinematics

Data Structures

struct  InverseKinematicsConstraintSet
 

Functions

RBDL_DLLAPI void UpdateKinematics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
 Updates and computes velocities and accelerations of the bodies. More...
 
RBDL_DLLAPI void UpdateKinematicsCustom (Model &model, const Math::VectorNd *Q, const Math::VectorNd *QDot, const Math::VectorNd *QDDot)
 Selectively updates model internal states of body positions, velocities and/or accelerations. More...
 
RBDL_DLLAPI Vector3d CalcBodyToBaseCoordinates (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &body_point_position, bool update_kinematics=true)
 Returns the base coordinates of a point given in body coordinates. More...
 
RBDL_DLLAPI Vector3d CalcBaseToBodyCoordinates (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &base_point_position, bool update_kinematics=true)
 Returns the body coordinates of a point given in base coordinates. More...
 
RBDL_DLLAPI Matrix3d CalcBodyWorldOrientation (Model &model, const Math::VectorNd &Q, const unsigned int body_id, bool update_kinematics=true)
 Returns the orientation of a given body as 3x3 matrix. More...
 
RBDL_DLLAPI void CalcPointJacobian (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
 Computes the point jacobian for a point on a body. More...
 
RBDL_DLLAPI void CalcPointJacobian6D (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
 Computes a 6-D Jacobian for a point on a body. More...
 
RBDL_DLLAPI void CalcBodySpatialJacobian (Model &model, const Math::VectorNd &Q, unsigned int body_id, Math::MatrixNd &G, bool update_kinematics=true)
 Computes the spatial jacobian for a body. More...
 
RBDL_DLLAPI Vector3d CalcPointVelocity (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
 Computes the velocity of a point on a body. More...
 
RBDL_DLLAPI Math::SpatialVector CalcPointVelocity6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
 Computes angular and linear velocity of a point that is fixed on a body. More...
 
RBDL_DLLAPI Vector3d CalcPointAcceleration (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
 Computes the linear acceleration of a point on a body. More...
 
RBDL_DLLAPI SpatialVector CalcPointAcceleration6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
 Computes linear and angular acceleration of a point on a body. More...
 
RBDL_DLLAPI bool InverseKinematics (Model &model, const Math::VectorNd &Qinit, const std::vector< unsigned int > &body_id, const std::vector< Math::Vector3d > &body_point, const std::vector< Math::Vector3d > &target_pos, Math::VectorNd &Qres, double step_tol=1.0e-12, double lambda=0.01, unsigned int max_iter=55)
 Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method) More...
 
RBDL_DLLAPI Vector3d CalcAngularVelocityfromMatrix (const Matrix3d &RotMat)
 
RBDL_DLLAPI bool InverseKinematics (Model &model, const Math::VectorNd &Qinit, InverseKinematicsConstraintSet &CS, Math::VectorNd &Qres)
 

Detailed Description

Note
Please note that in the Rigid Body Dynamics Library all angles are specified in radians.

Function Documentation

◆ CalcAngularVelocityfromMatrix()

RBDL_DLLAPI Math::Vector3d CalcAngularVelocityfromMatrix ( const Matrix3d &  RotMat)

Definition at line 699 of file Kinematics.cc.

References std::atan2(), and RigidBodyDynamics::Math::Vector3dZero.

◆ CalcBaseToBodyCoordinates()

RBDL_DLLAPI Math::Vector3d CalcBaseToBodyCoordinates ( Model &  model,
const Math::VectorNd &  Q,
unsigned int  body_id,
const Math::Vector3d &  base_point_position,
bool  update_kinematics = true 
)

Returns the body coordinates of a point given in base coordinates.

Parameters
modelthe rigid body model
Qthe curent genereralized positions
body_idid of the body for which the point coordinates are expressed
base_point_positioncoordinates of the point in base coordinates
update_kinematicswhether UpdateKinematics() should be called or not (default: true).
Returns
a 3-D vector with coordinates of the point in body coordinates

Definition at line 190 of file Kinematics.cc.

References RigidBodyDynamics::UpdateKinematicsCustom().

◆ CalcBodySpatialJacobian()

RBDL_DLLAPI void CalcBodySpatialJacobian ( Model &  model,
const Math::VectorNd &  Q,
unsigned int  body_id,
Math::MatrixNd &  G,
bool  update_kinematics = true 
)

Computes the spatial jacobian for a body.

The spatial velocity of a body at the origin of coordinate system of body $i$ can be expressed as ${}^i \hat{v}_i = G(q) * \dot{q}$. The matrix $G(q)$ is called the spatial body jacobian of the body and can be computed using this function.

Parameters
modelrigid body model
Qstate vector of the internal joints
body_idthe id of the body
Ga matrix of size 6 x #qdot_size where the result will be stored in
update_kinematicswhether UpdateKinematics() should be called or not (default: true)

The result will be returned via the G argument and represents the body Jacobian expressed at the origin of the body.

Note
This function only evaluates the entries of G that are non-zero. One Before calling this function one has to ensure that all other values have been set to zero, e.g. by calling G.setZero().

Definition at line 370 of file Kinematics.cc.

References SpatialTransform::apply(), MX_Xd_dynamic::block(), MX_Xd_dynamic::cols(), SpatialTransform::inverse(), MX_Xd_dynamic::rows(), and RigidBodyDynamics::UpdateKinematicsCustom().

◆ CalcBodyToBaseCoordinates()

RBDL_DLLAPI Math::Vector3d CalcBodyToBaseCoordinates ( Model &  model,
const Math::VectorNd &  Q,
unsigned int  body_id,
const Math::Vector3d &  body_point_position,
bool  update_kinematics = true 
)

Returns the base coordinates of a point given in body coordinates.

Parameters
modelthe rigid body model
Qthe curent genereralized positions
body_idid of the body for which the point coordinates are expressed
body_point_positioncoordinates of the point in body coordinates
update_kinematicswhether UpdateKinematics() should be called or not (default: true)
Returns
a 3-D vector with coordinates of the point in base coordinates

Definition at line 157 of file Kinematics.cc.

References RigidBodyDynamics::UpdateKinematicsCustom().

◆ CalcBodyWorldOrientation()

RBDL_DLLAPI Math::Matrix3d CalcBodyWorldOrientation ( Model &  model,
const Math::VectorNd &  Q,
const unsigned int  body_id,
bool  update_kinematics = true 
)

Returns the orientation of a given body as 3x3 matrix.

Parameters
modelthe rigid body model
Qthe curent genereralized positions
body_idid of the body for which the point coordinates are expressed
update_kinematicswhether UpdateKinematics() should be called or not (default: true).
Returns
An orthonormal 3x3 matrix that rotates vectors from base coordinates to body coordinates.

Definition at line 222 of file Kinematics.cc.

References RigidBodyDynamics::UpdateKinematicsCustom().

◆ CalcPointAcceleration()

RBDL_DLLAPI Math::Vector3d CalcPointAcceleration ( Model &  model,
const Math::VectorNd &  Q,
const Math::VectorNd &  QDot,
const Math::VectorNd &  QDDot,
unsigned int  body_id,
const Math::Vector3d &  point_position,
bool  update_kinematics = true 
)

Computes the linear acceleration of a point on a body.

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
QDDotvelocity vector of the internal joints
body_idthe id of the body
point_positionthe position of the point in body-local data
update_kinematicswhether UpdateKinematics() should be called or not (default: true)
Returns
The cartesian acceleration of the point in global frame (output)

The kinematic state of the model has to be updated before valid values can be obtained. This can either be done by calling UpdateKinematics() or setting the last parameter update_kinematics to true (default).

Warning
If this function is called after ForwardDynamics() without an update of the kinematic state one has to add the gravity acceleration has to be added to the result.

Definition at line 515 of file Kinematics.cc.

References SpatialTransform::apply(), RigidBodyDynamics::CalcBaseToBodyCoordinates(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), and RigidBodyDynamics::UpdateKinematics().

◆ CalcPointAcceleration6D()

RBDL_DLLAPI Math::SpatialVector CalcPointAcceleration6D ( Model &  model,
const Math::VectorNd &  Q,
const Math::VectorNd &  QDot,
const Math::VectorNd &  QDDot,
unsigned int  body_id,
const Math::Vector3d &  point_position,
bool  update_kinematics = true 
)

Computes linear and angular acceleration of a point on a body.

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
QDDotvelocity vector of the internal joints
body_idthe id of the body
point_positionthe position of the point in body-local data
update_kinematicswhether UpdateKinematics() should be called or not (default: true)
Returns
A 6-D vector where the first three elements are the angular acceleration and the last three elements the linear accelerations of the point.

The kinematic state of the model has to be updated before valid values can be obtained. This can either be done by calling UpdateKinematics() or setting the last parameter update_kinematics to true (default).

Warning
If this function is called after ForwardDynamics() without an update of the kinematic state one has to add the gravity acceleration has to be added to the result.

Definition at line 562 of file Kinematics.cc.

References SpatialTransform::apply(), RigidBodyDynamics::CalcBaseToBodyCoordinates(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), and RigidBodyDynamics::UpdateKinematics().

◆ CalcPointJacobian()

RBDL_DLLAPI void CalcPointJacobian ( Model &  model,
const Math::VectorNd &  Q,
unsigned int  body_id,
const Math::Vector3d &  point_position,
Math::MatrixNd &  G,
bool  update_kinematics = true 
)

Computes the point jacobian for a point on a body.

If a position of a point is computed by a function $g(q(t))$ for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix $G(q)$.

Parameters
modelrigid body model
Qstate vector of the internal joints
body_idthe id of the body
point_positionthe position of the point in body-local data
Ga matrix of dimensions 3 x #qdot_size where the result will be stored in
update_kinematicswhether UpdateKinematics() should be called or not (default: true)

The result will be returned via the G argument.

Note
This function only evaluates the entries of G that are non-zero. One Before calling this function one has to ensure that all other values have been set to zero, e.g. by calling G.setZero().

Definition at line 244 of file Kinematics.cc.

References SpatialTransform::apply(), MX_Xd_dynamic::block(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), MX_Xd_dynamic::cols(), SpatialTransform::inverse(), MX_Xd_dynamic::rows(), and RigidBodyDynamics::UpdateKinematicsCustom().

◆ CalcPointJacobian6D()

RBDL_DLLAPI void CalcPointJacobian6D ( Model &  model,
const Math::VectorNd &  Q,
unsigned int  body_id,
const Math::Vector3d &  point_position,
Math::MatrixNd &  G,
bool  update_kinematics = true 
)

Computes a 6-D Jacobian for a point on a body.

Computes the 6-D Jacobian $G(q)$ that when multiplied with $\dot{q}$ gives a 6-D vector that has the angular velocity as the first three entries and the linear velocity as the last three entries.

Parameters
modelrigid body model
Qstate vector of the internal joints
body_idthe id of the body
point_positionthe position of the point in body-local data
Ga matrix of dimensions 6 x #qdot_size where the result will be stored in
update_kinematicswhether UpdateKinematics() should be called or not (default: true)

The result will be returned via the G argument.

Note
This function only evaluates the entries of G that are non-zero. One Before calling this function one has to ensure that all other values have been set to zero, e.g. by calling G.setZero().

Definition at line 308 of file Kinematics.cc.

References SpatialTransform::apply(), MX_Xd_dynamic::block(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), MX_Xd_dynamic::cols(), SpatialTransform::inverse(), MX_Xd_dynamic::rows(), and RigidBodyDynamics::UpdateKinematicsCustom().

◆ CalcPointVelocity()

RBDL_DLLAPI Math::Vector3d CalcPointVelocity ( Model &  model,
const Math::VectorNd &  Q,
const Math::VectorNd &  QDot,
unsigned int  body_id,
const Math::Vector3d &  point_position,
bool  update_kinematics = true 
)

Computes the velocity of a point on a body.

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
body_idthe id of the body
point_positionthe position of the point in body-local data
update_kinematicswhether UpdateKinematics() should be called or not (default: true)
Returns
The cartesian velocity of the point in global frame (output)

Definition at line 434 of file Kinematics.cc.

References SpatialTransform::apply(), RigidBodyDynamics::CalcBaseToBodyCoordinates(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), MX_Xd_dynamic::size(), and RigidBodyDynamics::UpdateKinematicsCustom().

◆ CalcPointVelocity6D()

RBDL_DLLAPI Math::SpatialVector CalcPointVelocity6D ( Model &  model,
const Math::VectorNd &  Q,
const Math::VectorNd &  QDot,
unsigned int  body_id,
const Math::Vector3d &  point_position,
bool  update_kinematics = true 
)

Computes angular and linear velocity of a point that is fixed on a body.

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
body_idthe id of the body
point_positionthe position of the point in body-local data
update_kinematicswhether UpdateKinematics() should be called or not (default: true)
Returns
The a 6-D vector for which the first three elements are the angular velocity and the last three elements the linear velocity in the global reference system.

Definition at line 478 of file Kinematics.cc.

References SpatialTransform::apply(), RigidBodyDynamics::CalcBaseToBodyCoordinates(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), MX_Xd_dynamic::size(), and RigidBodyDynamics::UpdateKinematicsCustom().

◆ InverseKinematics() [1/2]

RBDL_DLLAPI bool InverseKinematics ( Model &  model,
const Math::VectorNd &  Qinit,
const std::vector< unsigned int > &  body_id,
const std::vector< Math::Vector3d > &  body_point,
const std::vector< Math::Vector3d > &  target_pos,
Math::VectorNd &  Qres,
double  step_tol = 1.0e-12,
double  lambda = 0.01,
unsigned int  max_iter = 55 
)

Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method)

Parameters
modelrigid body model
Qinitinitial guess for the state
body_ida vector of all bodies for which we we have kinematic target positions
body_pointa vector of points in body local coordinates that are to be matched to target positions
target_posa vector of target positions
Qresoutput of the computed inverse kinematics
step_toltolerance used for convergence detection
lambdadamping factor for the least squares function
max_itermaximum number of steps that should be performed
Returns
true on success, false otherwise

This function repeatedly computes

\[ Qres = Qres + \Delta \theta\]

\[ \Delta \theta = G^T (G^T G + \lambda^2 I)^{-1} e \]

where $G = G(q) = \frac{d}{dt} g(q(t))$ and $e$ is the correction of the body points so that they coincide with the target positions. The function returns true when $||\Delta \theta||_2 \le$ step_tol or if the error between body points and target gets smaller than step_tol. Otherwise it returns false.

The parameter $\lambda$ is the damping factor that has to be chosen carefully. In case of unreachable positions higher values (e.g 0.9) can be helpful. Otherwise values of 0.0001, 0.001, 0.01, 0.1 might yield good results. See the literature for best practices.

Warning
The actual accuracy might be rather low (~1.0e-2)! Use this function with a grain of suspicion.

Definition at line 605 of file Kinematics.cc.

References RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcPointJacobian(), MX_Xd_dynamic::Identity(), RigidBodyDynamics::Math::LinSolveGaussElimPivot(), MX_Xd_dynamic::norm(), MX_Xd_dynamic::setZero(), MX_Xd_dynamic::size(), MX_Xd_dynamic::squaredNorm(), MX_Xd_dynamic::transpose(), RigidBodyDynamics::UpdateKinematicsCustom(), and MX_Xd_dynamic::Zero().

◆ InverseKinematics() [2/2]

RBDL_DLLAPI bool InverseKinematics ( Model &  model,
const Math::VectorNd &  Qinit,
InverseKinematicsConstraintSet &  CS,
Math::VectorNd &  Qres 
)

Definition at line 855 of file Kinematics.cc.

References InverseKinematicsConstraintSet::body_ids, InverseKinematicsConstraintSet::body_points, RigidBodyDynamics::CalcAngularVelocityfromMatrix(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), RigidBodyDynamics::Utils::CalcCenterOfMass(), RigidBodyDynamics::CalcPointJacobian6D(), InverseKinematicsConstraintSet::constraint_row_index, InverseKinematicsConstraintSet::constraint_type, InverseKinematicsConstraintSet::constraint_weight, InverseKinematicsConstraintSet::ConstraintTypeFull, InverseKinematicsConstraintSet::ConstraintTypeOrientation, InverseKinematicsConstraintSet::ConstraintTypePosition, InverseKinematicsConstraintSet::ConstraintTypePositionCoMXY, InverseKinematicsConstraintSet::ConstraintTypePositionXY, InverseKinematicsConstraintSet::ConstraintTypePositionZ, InverseKinematicsConstraintSet::delta_q_norm, InverseKinematicsConstraintSet::e, InverseKinematicsConstraintSet::error_norm, InverseKinematicsConstraintSet::G, InverseKinematicsConstraintSet::J, InverseKinematicsConstraintSet::lambda, InverseKinematicsConstraintSet::max_steps, MX_Xd_dynamic::norm(), InverseKinematicsConstraintSet::num_constraints, InverseKinematicsConstraintSet::num_steps, MX_Xd_dynamic::size(), InverseKinematicsConstraintSet::step_tol, InverseKinematicsConstraintSet::target_orientations, InverseKinematicsConstraintSet::target_positions, MX_Xd_dynamic::transpose(), RigidBodyDynamics::UpdateKinematicsCustom(), and MX_Xd_dynamic::Zero().

◆ UpdateKinematics()

RBDL_DLLAPI void UpdateKinematics ( Model &  model,
const Math::VectorNd &  Q,
const Math::VectorNd &  QDot,
const Math::VectorNd &  QDDot 
)

Updates and computes velocities and accelerations of the bodies.

This function updates the kinematic variables such as body velocities and accelerations in the model to reflect the variables passed to this function.

Parameters
modelthe model
Qthe positional variables of the model
QDotthe generalized velocities of the joints
QDDotthe generalized accelerations of the joints

Definition at line 25 of file Kinematics.cc.

References MX_Xd_dynamic::block(), RigidBodyDynamics::Math::crossm(), and RigidBodyDynamics::jcalc().

◆ UpdateKinematicsCustom()

RBDL_DLLAPI void UpdateKinematicsCustom ( Model &  model,
const Math::VectorNd *  Q,
const Math::VectorNd *  QDot,
const Math::VectorNd *  QDDot 
)

Selectively updates model internal states of body positions, velocities and/or accelerations.

This function updates the kinematic variables such as body velocities and accelerations in the model to reflect the variables passed to this function.

In contrast to UpdateKinematics() this function allows to update the model state with values one is interested and thus reduce computations (e.g. only positions, only positions + accelerations, only velocities, etc.).

Parameters
modelthe model
Qthe positional variables of the model
QDotthe generalized velocities of the joints
QDDotthe generalized accelerations of the joints

Definition at line 78 of file Kinematics.cc.

References MX_Xd_dynamic::block(), RigidBodyDynamics::Math::crossm(), RigidBodyDynamics::jcalc(), and MX_Xd_dynamic::Zero().