Rigid Body Dynamics Library
Contacts

Data Structures

struct  ConstraintSet
 Structure that contains both constraint information and workspace memory. More...
 

Functions

RBDL_DLLAPI void CalcContactJacobian (Model &model, const Math::VectorNd &Q, const ConstraintSet &CS, Math::MatrixNd &G, bool update_kinematics=true)
 Computes the Jacobian for the given ConstraintSet. More...
 
RBDL_DLLAPI void CalcContactSystemVariables (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS)
 
RBDL_DLLAPI void ForwardDynamicsContactsDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
 Computes forward dynamics with contact by constructing and solving the full lagrangian equation. More...
 
RBDL_DLLAPI void ForwardDynamicsContactsRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
 
RBDL_DLLAPI void ForwardDynamicsContactsNullSpace (Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot)
 
RBDL_DLLAPI void ForwardDynamicsContactsKokkevis (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
 Computes forward dynamics that accounts for active contacts in ConstraintSet. More...
 
RBDL_DLLAPI void ComputeContactImpulsesDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
 Computes contact gain by constructing and solving the full lagrangian equation. More...
 
RBDL_DLLAPI void ComputeContactImpulsesRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
 Resolves contact gain using SolveContactSystemRangeSpaceSparse() More...
 
RBDL_DLLAPI void ComputeContactImpulsesNullSpace (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
 Resolves contact gain using SolveContactSystemNullSpace() More...
 
RBDL_DLLAPI void SolveContactSystemDirect (Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &A, Math::VectorNd &b, Math::VectorNd &x, Math::LinearSolver &linear_solver)
 Solves the full contact system directly, i.e. simultaneously for contact forces and joint accelerations. More...
 
RBDL_DLLAPI void SolveContactSystemRangeSpaceSparse (Model &model, Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &K, Math::VectorNd &a, Math::LinearSolver linear_solver)
 Solves the contact system by first solving for the the joint accelerations and then the contact forces using a sparse matrix decomposition of the joint space inertia matrix. More...
 
RBDL_DLLAPI void SolveContactSystemNullSpace (Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &Y, Math::MatrixNd &Z, Math::VectorNd &qddot_y, Math::VectorNd &qddot_z, Math::LinearSolver &linear_solver)
 Solves the contact system by first solving for the joint accelerations and then for the constraint forces. More...
 

Detailed Description

External contacts are handled by specification of a ConstraintSet which contains all informations about the current contacts and workspace memory.

Separate contacts can be specified by calling ConstraintSet::AddConstraint(). After all constraints have been specified, this ConstraintSet has to be bound to the model via ConstraintSet::Bind(). This initializes workspace memory that is later used when calling one of the contact functions, such as ForwardDynamicsContacts() or ForwardDynamicsContactsLagrangian().

The values in the vectors ConstraintSet::force and ConstraintSet::impulse contain the computed force or impulse values for each constraint when returning from one of the contact functions.

Solution of the Constraint System

Linear System of the Constrained Dynamics

External contacts are constraints that act on the model. To compute the acceleration one has to solve a linear system of the form:

\[ \left( \begin{array}{cc} H & G^T \\ G & 0 \end{array} \right) \left( \begin{array}{c} \ddot{q} \\ - \lambda \end{array} \right) = \left( \begin{array}{c} -C + \tau \\ \gamma \end{array} \right) \]

where $H$ is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), $G$ are the point jacobians of the contact points, $C$ the bias force (sometimes called "non-linear effects"), and $\gamma$ the generalized acceleration independent part of the contact point accelerations.

Linear System of the Contact Collision

Similarly to compute the response of the model to a contact gain one has to solve a system of the following form:

\[ \left( \begin{array}{cc} H & G^T \\ G & 0 \end{array} \right) \left( \begin{array}{c} \dot{q}^{+} \\ \Lambda \end{array} \right) = \left( \begin{array}{c} H \dot{q}^{-} \\ v^{+} \end{array} \right) \]

where $H$ is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), $G$ are the point jacobians of the contact points, $\dot{q}^{+}$ the generalized velocity after the impact, $\Lambda$ the impulses at each constraint, $\dot{q}^{-}$ the generalized velocity before the impact, and $v^{+}$ the desired velocity of each constraint after the impact (known beforehand, usually 0). The value of $v^{+}$ can is specified via the variable ConstraintSet::v_plus and defaults to 0.

Solution Methods

There are essentially three different approaches to solve these systems:

  1. Direct: solve the full system to simultaneously compute $\ddot{q}$ and $\lambda$. This may be slow for large systems and many constraints.
  2. Range-Space: solve first for $\lambda$ and then for $\ddot{q}$.
  3. Null-Space: solve furst for $\ddot{q}$ and then for $\lambda$ The methods are the same for the contact gaints just with different variables on the right-hand-side.

RBDL provides methods for all approaches. The implementation for the range-space method also exploits sparsities in the joint space inertia matrix using a sparse structure preserving $L^TL$ decomposition as described in Chapter 8.5 of "Rigid Body Dynamics Algorithms".

None of the methods is generally superior to the others and each has different trade-offs as factors such as model topology, number of constraints, constrained bodies, numerical stability, and performance vary and evaluation has to be made on a case-by-case basis.

Methods for Solving Constrained Dynamics

RBDL provides the following methods to compute the acceleration of a constrained system:

Methods for Computing Collisions

RBDL provides the following methods to compute the collision response on new contact gains:

Function Documentation

RBDL_DLLAPI void CalcContactJacobian ( Model model,
const Math::VectorNd Q,
const ConstraintSet CS,
Math::MatrixNd G,
bool  update_kinematics = true 
)

Computes the Jacobian for the given ConstraintSet.

Parameters
modelthe model
Qthe generalized positions of the joints
CSthe constraint set for which the Jacobian should be computed
G(output) matrix where the output will be stored in
update_kinematicswhether the kinematics of the model should be * updated from Q

Definition at line 309 of file Contacts.cc.

References ConstraintSet::body, RigidBodyDynamics::CalcPointJacobian(), Model::dof_count, ConstraintSet::normal, ConstraintSet::point, ConstraintSet::size(), and RigidBodyDynamics::UpdateKinematicsCustom().

RBDL_DLLAPI void ComputeContactImpulsesDirect ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDotMinus,
ConstraintSet CS,
Math::VectorNd QDotPlus 
)

Computes contact gain by constructing and solving the full lagrangian equation.

This method builds and solves the linear system

\[ \left( \begin{array}{cc} H & G^T \\ G & 0 \end{array} \right) \left( \begin{array}{c} \dot{q}^{+} \\ \Lambda \end{array} \right) = \left( \begin{array}{c} H \dot{q}^{-} \\ v^{+} \end{array} \right) \]

where $H$ is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), $G$ are the point jacobians of the contact points, $\dot{q}^{+}$ the generalized velocity after the impact, $\Lambda$ the impulses at each constraint, $\dot{q}^{-}$ the generalized velocity before the impact, and $v^{+}$ the desired velocity of each constraint after the impact (known beforehand, usually 0). The value of $v^{+}$ can is specified via the variable ConstraintSet::v_plus and defaults to 0.

Note
So far, only constraints acting along cartesian coordinate axes are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must not specify redundant constraints!
Note
To increase performance group constraints body and pointwise such that constraints acting on the same body point are sequentially in ConstraintSet. This can save computation of point Jacobians $G$.
Parameters
modelrigid body model
Qstate vector of the internal joints
QDotMinusvelocity vector of the internal joints before the impact
CSthe set of active constraints
QDotPlusvelocities of the internals joints after the impact (output)

Definition at line 454 of file Contacts.cc.

References ConstraintSet::A, ConstraintSet::b, RigidBodyDynamics::CalcContactJacobian(), RigidBodyDynamics::CompositeRigidBodyAlgorithm(), Model::dof_count, ConstraintSet::G, ConstraintSet::H, ConstraintSet::impulse, ConstraintSet::linear_solver, ConstraintSet::size(), RigidBodyDynamics::SolveContactSystemDirect(), RigidBodyDynamics::UpdateKinematicsCustom(), ConstraintSet::v_plus, and ConstraintSet::x.

RBDL_DLLAPI void ForwardDynamicsContactsDirect ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
const Math::VectorNd Tau,
ConstraintSet CS,
Math::VectorNd QDDot 
)

Computes forward dynamics with contact by constructing and solving the full lagrangian equation.

This method builds and solves the linear system

\[ \left( \begin{array}{cc} H & G^T \\ G & 0 \end{array} \right) \left( \begin{array}{c} \ddot{q} \\ -\lambda \end{array} \right) = \left( \begin{array}{c} -C + \tau \\ \gamma \end{array} \right) \]

where $H$ is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), $G$ are the point jacobians of the contact points, $C$ the bias force (sometimes called "non-linear effects"), and $\gamma$ the generalized acceleration independent part of the contact point accelerations.

Note
So far, only constraints acting along cartesian coordinate axes are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must not specify redundant constraints!
Note
To increase performance group constraints body and pointwise such that constraints acting on the same body point are sequentially in ConstraintSet. This can save computation of point jacobians $G$.
Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
Tauactuations of the internal joints
CSthe description of all acting constraints
QDDotaccelerations of the internals joints (output)
Note
During execution of this function values such as ConstraintSet::force get modified and will contain the value of the force acting along the normal.

Definition at line 388 of file Contacts.cc.

References ConstraintSet::A, ConstraintSet::b, ConstraintSet::C, RigidBodyDynamics::CalcContactSystemVariables(), Model::dof_count, ConstraintSet::force, ConstraintSet::G, ConstraintSet::gamma, ConstraintSet::H, ConstraintSet::linear_solver, LOG, ConstraintSet::size(), RigidBodyDynamics::SolveContactSystemDirect(), and ConstraintSet::x.

RBDL_DLLAPI void ForwardDynamicsContactsKokkevis ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
const Math::VectorNd Tau,
ConstraintSet CS,
Math::VectorNd QDDot 
)

Computes forward dynamics that accounts for active contacts in ConstraintSet.

The method used here is the one described by Kokkevis and Metaxas in the Paper "Practical Physics for Articulated Characters", Game Developers Conference, 2004.

It does this by recursively computing the inverse articulated-body inertia (IABI)

\[ \left( \begin{array}{c} \dot{v}_1 \\ \dot{v}_2 \\ \vdots \\ \dot{v}_n \end{array} \right) = \left( \begin{array}{cccc} \Phi_{1,1} & \Phi_{1,2} & \cdots & \Phi{1,n} \\ \Phi_{2,1} & \Phi_{2,2} & \cdots & \Phi{2,n} \\ \cdots & \cdots & \cdots & \vdots \\ \Phi_{n,1} & \Phi_{n,2} & \cdots & \Phi{n,n} \end{array} \right) \left( \begin{array}{c} f_1 \\ f_2 \\ \vdots \\ f_n \end{array} \right) + \left( \begin{array}{c} \phi_1 \\ \phi_2 \\ \vdots \\ \phi_n \end{array} \right). \]

$\Phi_{i,j}$ which is then used to build and solve a system of the form: Here $n$ is the number of constraints and the method for building the system uses the Articulated Body Algorithm to efficiently compute entries of the system. The values $\dot{v}_i$ are the constraint accelerations, $f_i$ the constraint forces, and $\phi_i$ are the constraint bias forces.

Parameters
modelrigid body model
Qstate vector of the internal joints
QDotvelocity vector of the internal joints
Tauactuations of the internal joints
CSa list of all contact points
QDDotaccelerations of the internals joints (output)
Note
During execution of this function values such as ConstraintSet::force get modified and will contain the value of the force acting along the normal.
Todo:
Allow for external forces

Definition at line 836 of file Contacts.cc.

References ConstraintSet::a, ConstraintSet::acceleration, SpatialTransform::applyAdjoint(), ConstraintSet::body, RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcPointAcceleration(), Model::dof_count, ConstraintSet::f_ext_constraints, ConstraintSet::f_t, Model::fixed_body_discriminator, ConstraintSet::force, RigidBodyDynamics::ForwardDynamics(), RigidBodyDynamics::ForwardDynamicsAccelerationDeltas(), RigidBodyDynamics::ForwardDynamicsApplyConstraintForces(), Model::IsFixedBodyId(), ConstraintSet::K, ConstraintSet::linear_solver, LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, LinearSolverPartialPivLU, RigidBodyDynamics::Math::LinSolveGaussElimPivot(), LOG, Model::mBodies, Model::mFixedBodies, ConstraintSet::normal, ConstraintSet::point, ConstraintSet::point_accel_0, ConstraintSet::QDDot_0, ConstraintSet::QDDot_t, ConstraintSet::size(), SUPPRESS_LOGGING, and RigidBodyDynamics::UpdateKinematicsCustom().

RBDL_DLLAPI void SolveContactSystemDirect ( Math::MatrixNd H,
const Math::MatrixNd G,
const Math::VectorNd c,
const Math::VectorNd gamma,
Math::VectorNd qddot,
Math::VectorNd lambda,
Math::MatrixNd A,
Math::VectorNd b,
Math::VectorNd x,
Math::LinearSolver &  linear_solver 
)

Solves the full contact system directly, i.e. simultaneously for contact forces and joint accelerations.

This solves a $ (n_\textit{dof} + n_c) \times (n_\textit{dof} + n_c$ linear system.

Parameters
Hthe joint space inertia matrix
Gthe constraint jacobian
cthe $ \mathbb{R}^{n_\textit{dof}}$ vector of the upper part of the right hand side of the system
gammathe $ \mathbb{R}^{n_c}$ vector of the lower part of the right hand side of the system
qddotresult: joint accelerations
lambdaresult: constraint forces
Awork-space for the matrix of the linear system
bwork-space for the right-hand-side of the linear system
xwork-space for the solution of the linear system
typeof solver that should be used to solve the system

Definition at line 161 of file Contacts.cc.

References LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, LinearSolverPartialPivLU, and LOG.

RBDL_DLLAPI void SolveContactSystemNullSpace ( Math::MatrixNd H,
const Math::MatrixNd G,
const Math::VectorNd c,
const Math::VectorNd gamma,
Math::VectorNd qddot,
Math::VectorNd lambda,
Math::MatrixNd Y,
Math::MatrixNd Z,
Math::VectorNd qddot_y,
Math::VectorNd qddot_z,
Math::LinearSolver &  linear_solver 
)

Solves the contact system by first solving for the joint accelerations and then for the constraint forces.

This methods requires a $n_\textit{dof} \times n_\textit{dof}$ matrix of the form $\left[ \ Y \ | Z \ \right]$ with the property $GZ = 0$ that can be computed using a QR decomposition (e.g. see code for ForwardDynamicsContactsNullSpace()).

Parameters
Hthe joint space inertia matrix
Gthe constraint jacobian
cthe $ \mathbb{R}^{n_\textit{dof}}$ vector of the upper part of the right hand side of the system
gammathe $ \mathbb{R}^{n_c}$ vector of the lower part of the right hand side of the system
qddotresult: joint accelerations
lambdaresult: constraint forces
Ybasis for the range-space of the constraints
Zbasis for the null-space of the constraints
qddot_ywork-space of size $\mathbb{R}^{n_\textit{dof}}$
qddot_zwork-space of size $\mathbb{R}^{n_\textit{dof}}$
linear_solvertype of solver that should be used to solve the system

Definition at line 248 of file Contacts.cc.

References LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, LinearSolverPartialPivLU, and LOG.

RBDL_DLLAPI void SolveContactSystemRangeSpaceSparse ( Model model,
Math::MatrixNd H,
const Math::MatrixNd G,
const Math::VectorNd c,
const Math::VectorNd gamma,
Math::VectorNd qddot,
Math::VectorNd lambda,
Math::MatrixNd K,
Math::VectorNd a,
Math::LinearSolver  linear_solver 
)

Solves the contact system by first solving for the the joint accelerations and then the contact forces using a sparse matrix decomposition of the joint space inertia matrix.

This method exploits the branch-induced sparsity by the structure preserving $L^TL $ decomposition described in RBDL, Section 6.5.

Parameters
Hthe joint space inertia matrix
Gthe constraint jacobian
cthe $ \mathbb{R}^{n_\textit{dof}}$ vector of the upper part of the right hand side of the system
gammathe $ \mathbb{R}^{n_c}$ vector of the lower part of the right hand side of the system
qddotresult: joint accelerations
lambdaresult: constraint forces
Kwork-space for the matrix of the constraint force linear system
awork-space for the right-hand-side of the constraint force linear system
linear_solvertype of solver that should be used to solve the constraint force system

Definition at line 211 of file Contacts.cc.

References RigidBodyDynamics::Math::SparseFactorizeLTL(), RigidBodyDynamics::Math::SparseSolveLTx(), and RigidBodyDynamics::Math::SparseSolveLx().