Rigid Body Dynamics Library
Kinematics

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=50)
 Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method) More...
 

Detailed Description

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

Function Documentation

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 201 of file Kinematics.cc.

References Model::fixed_body_discriminator, Model::mFixedBodies, RigidBodyDynamics::UpdateKinematicsCustom(), and Model::X_base.

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 the base coordinate system can be expressed as ${}^0 \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 381 of file Kinematics.cc.

References SpatialTransform::apply(), Model::fixed_body_discriminator, Model::IsFixedBodyId(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mCustomJoints, Model::mFixedBodies, Model::mJoints, Model::multdof3_S, Model::qdot_size, Model::S, RigidBodyDynamics::UpdateKinematicsCustom(), and Model::X_base.

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 168 of file Kinematics.cc.

References Model::fixed_body_discriminator, Model::mFixedBodies, RigidBodyDynamics::UpdateKinematicsCustom(), and Model::X_base.

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 233 of file Kinematics.cc.

References Model::fixed_body_discriminator, Model::mFixedBodies, RigidBodyDynamics::UpdateKinematicsCustom(), and Model::X_base.

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 526 of file Kinematics.cc.

References Model::a, SpatialTransform::apply(), RigidBodyDynamics::CalcBaseToBodyCoordinates(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), Model::fixed_body_discriminator, Model::IsFixedBodyId(), LOG, Model::mFixedBodies, RigidBodyDynamics::UpdateKinematics(), and Model::v.

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 573 of file Kinematics.cc.

References Model::a, SpatialTransform::apply(), RigidBodyDynamics::CalcBaseToBodyCoordinates(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), Model::fixed_body_discriminator, Model::IsFixedBodyId(), LOG, Model::mFixedBodies, RigidBodyDynamics::UpdateKinematics(), and Model::v.

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 255 of file Kinematics.cc.

References SpatialTransform::apply(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), Model::fixed_body_discriminator, Model::IsFixedBodyId(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mCustomJoints, Model::mFixedBodies, Model::mJoints, Model::multdof3_S, Model::qdot_size, Model::S, RigidBodyDynamics::UpdateKinematicsCustom(), and Model::X_base.

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 319 of file Kinematics.cc.

References SpatialTransform::apply(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), Model::fixed_body_discriminator, Model::IsFixedBodyId(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mCustomJoints, Model::mFixedBodies, Model::mJoints, Model::multdof3_S, Model::qdot_size, Model::S, RigidBodyDynamics::UpdateKinematicsCustom(), and Model::X_base.

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 445 of file Kinematics.cc.

References SpatialTransform::apply(), RigidBodyDynamics::CalcBaseToBodyCoordinates(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), Model::fixed_body_discriminator, Model::IsBodyId(), Model::IsFixedBodyId(), LOG, Model::mFixedBodies, Model::q_size, Model::qdot_size, RigidBodyDynamics::UpdateKinematicsCustom(), and Model::v.

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 489 of file Kinematics.cc.

References SpatialTransform::apply(), RigidBodyDynamics::CalcBaseToBodyCoordinates(), RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), Model::fixed_body_discriminator, Model::IsBodyId(), Model::IsFixedBodyId(), LOG, Model::mFixedBodies, Model::q_size, Model::qdot_size, RigidBodyDynamics::UpdateKinematicsCustom(), and Model::v.

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 = 50 
)

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 615 of file Kinematics.cc.

References RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcPointJacobian(), RigidBodyDynamics::Math::LinSolveGaussElimPivot(), LOG, Model::q_size, Model::qdot_size, and RigidBodyDynamics::UpdateKinematicsCustom().

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 23 of file Kinematics.cc.

References Model::a, Model::c, Model::c_J, RigidBodyDynamics::Math::crossm(), Model::gravity, RigidBodyDynamics::jcalc(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, CustomJoint::mDoFCount, Model::mJoints, Model::multdof3_S, Model::S, Model::v, Model::v_J, Model::X_base, Model::X_J, Model::X_lambda, and Model::X_T.

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 88 of file Kinematics.cc.

References Model::a, Model::c, Model::c_J, RigidBodyDynamics::Math::crossm(), RigidBodyDynamics::jcalc(), RigidBodyDynamics::JointTypeCustom, Model::lambda, LOG, Model::mBodies, Model::mCustomJoints, CustomJoint::mDoFCount, Model::mJoints, Model::multdof3_S, Model::q_size, Model::S, Model::v, Model::v_J, Model::X_base, Model::X_J, Model::X_lambda, and Model::X_T.