Rigid Body Dynamics Library
Constraints

Data Structures

struct  ConstraintSet
 Structure that contains both constraint information and workspace memory. More...
 
struct  CustomConstraint
 Interface to define general-purpose constraints. More...
 

Functions

RBDL_DLLAPI void CalcConstraintsPositionError (Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::VectorNd &err, bool update_kinematics=true)
 Computes the position errors for the given ConstraintSet. More...
 
RBDL_DLLAPI void CalcConstraintsJacobian (Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::MatrixNd &G, bool update_kinematics=true)
 Computes the Jacobian for the given ConstraintSet. More...
 
RBDL_DLLAPI void CalcConstraintsVelocityError (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ConstraintSet &CS, Math::VectorNd &err, bool update_kinematics=true)
 Computes the velocity errors for the given ConstraintSet. More...
 
RBDL_DLLAPI void CalcConstrainedSystemVariables (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, std::vector< Math::SpatialVector > *f_ext=NULL)
 Computes the terms $H$, $G$, and $\gamma$ of the constrained dynamic problem and stores them in the ConstraintSet. More...
 
RBDL_DLLAPI bool CalcAssemblyQ (Model &model, Math::VectorNd QInit, ConstraintSet &CS, Math::VectorNd &Q, const Math::VectorNd &weights, double tolerance=1e-12, unsigned int max_iter=100)
 Computes a feasible initial value of the generalized joint positions. More...
 
RBDL_DLLAPI void CalcAssemblyQDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotInit, ConstraintSet &CS, Math::VectorNd &QDot, const Math::VectorNd &weights)
 Computes a feasible initial value of the generalized joint velocities. More...
 
RBDL_DLLAPI void ForwardDynamicsConstraintsDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext=NULL)
 Computes forward dynamics with contact by constructing and solving the full lagrangian equation. More...
 
RBDL_DLLAPI void ForwardDynamicsConstraintsRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext)
 
RBDL_DLLAPI void ForwardDynamicsConstraintsNullSpace (Model &model, const VectorNd &Q, const VectorNd &QDot, const VectorNd &Tau, ConstraintSet &CS, VectorNd &QDDot, std::vector< Math::SpatialVector > *f_ext)
 
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 ComputeConstraintImpulsesDirect (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 ComputeConstraintImpulsesRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
 Resolves contact gain using SolveContactSystemRangeSpaceSparse() More...
 
RBDL_DLLAPI void ComputeConstraintImpulsesNullSpace (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
 Resolves contact gain using SolveContactSystemNullSpace() More...
 
RBDL_DLLAPI void SolveConstrainedSystemDirect (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 SolveConstrainedSystemRangeSpaceSparse (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 SolveConstrainedSystemNullSpace (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...
 

Variables

struct RBDL_DLLAPI CustomConstraint
 

Detailed Description

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

Separate constraints can be specified by calling ConstraintSet::AddContactConstraint(), ConstraintSet::AddLoopConstraint(), and ConstraintSet::AddCustomConstraint(). 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 ForwardDynamicsContactsDirects().

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

In the presence of constraints, 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$ is the constraint jacobian, $C$ the bias force (sometimes called "non-linear effects"), and $\gamma$ the generalized acceleration independent part of the constraints.

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 gains 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:

Computing generalized joint positions and velocities

satisfying the constraint equations.

When considering a model subject position level constraints expressed by the equation $\phi (q) = 0$, it is often necessary to compute generalized joint position and velocities which satisfy the constraints. Even velocity-level constraints may have position-level assembly constraints: a rolling-without-slipping constraint is a velocity-level constraint, but during assembly it might be desireable to put the rolling surfaces in contact with eachother.

In order to compute a vector of generalized joint positions that satisfy the constraints it is necessary to solve the following optimization problem:

\begin{eqnarray*} \text{minimize} && \sum_{i = 0}^{n} (q - q_{0})^T W (q - q_{0}) \\ \text{over} && q \\ \text{subject to} && \phi (q) = 0 \end{eqnarray*}

In order to compute a vector of generalized joint velocities that satisfy the constraints it is necessary to solve the following optimization problem:

\begin{eqnarray*} \text{minimize} && \sum_{i = 0}^{n} (\dot{q} - \dot{q}_{0})^T W (\dot{q} - \dot{q}_{0}) \\ \text{over} && \dot{q} \\ \text{subject to} && \dot{\phi} (q) = \phi _{q}(q) \dot{q} + \phi _{t}(q) = 0 \end{eqnarray*}

$q_{0}$ and $\dot{q}_{0}$ are initial guesses, $\phi _{q}$ is the constraints jacobian (partial derivative of $\phi$ with respect to $q$), and $\phi _{t}(q)$ is the partial derivative of $\phi$ with respect to time. $W$ is a diagonal weighting matrix, which can be exploited to prioritize changes in the position/ velocity of specific joints. With a solver capable of handling singular matrices, it is possible to set to 1 the weight of the joint positions/ velocities that should not be changed from the initial guesses, and to 0 those corresponding to the values that can be changed.

These problems are solved using the Lagrange multipliers method. For the velocity problem the solution is exact. For the position problem the constraints are linearized in the form $ \phi (q_{0}) + \phi _{t}(q0) + \phi _{q_0}(q) (q - q_{0}) $ and the linearized problem is solved iteratively until the constraint position errors are smaller than a given threshold.

RBDL provides two functions to compute feasible joint position and velocities:

Baumgarte Stabilization

The constrained dynamic equations are correct at the acceleration level but will drift at the velocity and position level during numerical integration. RBDL implements Baumgarte stabilization to avoid the accumulation of position and velocity errors for loop constraints and custom constraints. Contact constraints do not have Baumgarte stabilization because they are a special case which does not typically suffer from drift. The stabilization term can be enabled/disabled using the appropriate ConstraintSet::AddLoopConstraint and ConstraintSet::AddCustomConstraint functions.

The dynamic equations are changed to the following 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 + \gamma _{stab} \end{array} \right) \]

A term $\gamma _{stab}$ is added to the right hand side of the equation, defined in the following way:

\[ \gamma _{stab} = - 2 \alpha \dot{\phi} (q) - \beta^2 \phi (q) \]

where $\phi (q)$ are the position level constraint errors and $\alpha$ and $\beta$ are tuning coefficients. There is no clear and simple rule on how to choose them as good values also depend on the used integration method and time step. If the values are too small the constrained dynamics equation becomes stiff, too big values result in errors not being reduced.

A good starting point is to parameterize the coefficients as:

\[ \alpha = \beta = 1 / T_\textit{stab} \]

with $T_\textit{stab}$ specifies a time constant for errors in position and velocity errors to reduce. Featherstone suggests in his book "Rigid Body Dynamics Algorithms" that for a big industrial robot a value of 0.1 is reasonable. When testing different values best is to try different orders of magnitude as e.g. doubling a value only has little effect.

For Loop- and CustomConstraints Baumgarte stabilization is enabled by default and uses the stabilization parameter $T_\textit{stab} = 0.1$.

Function Documentation

◆ CalcAssemblyQ()

RBDL_DLLAPI bool CalcAssemblyQ ( Model model,
Math::VectorNd  QInit,
ConstraintSet CS,
Math::VectorNd Q,
const Math::VectorNd weights,
double  tolerance = 1e-12,
unsigned int  max_iter = 100 
)

Computes a feasible initial value of the generalized joint positions.

Parameters
modelthe model
QInitinitial guess for the generalized positions of the joints
CSthe constraint set for which the error should be computed
Q(output) vector of the generalized joint positions.
weightsweighting coefficients for the different joint positions.
tolerancethe function will return successfully if the constraint position error norm is lower than this value.
max_iterthe funciton will return unsuccessfully after performing this number of iterations.
Returns
true if the generalized joint positions were computed successfully, false otherwise.

Definition at line 819 of file Constraints.cc.

References RigidBodyDynamics::CalcConstraintsJacobian(), RigidBodyDynamics::CalcConstraintsPositionError(), Model::dof_count, Model::GetQuaternion(), RigidBodyDynamics::JointTypeSpherical, ConstraintSet::linear_solver, Model::mJoints, Quaternion::omegaToQDot(), Model::q_size, Model::SetQuaternion(), ConstraintSet::size(), and RigidBodyDynamics::SolveLinearSystem().

◆ CalcAssemblyQDot()

RBDL_DLLAPI void CalcAssemblyQDot ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDotInit,
ConstraintSet CS,
Math::VectorNd QDot,
const Math::VectorNd weights 
)

Computes a feasible initial value of the generalized joint velocities.

Parameters
modelthe model
Qthe generalized joint position of the joints. It is assumed that this vector satisfies the position level assemblt constraints.
QDotInitinitial guess for the generalized velocities of the joints
CSthe constraint set for which the error should be computed
QDot(output) vector of the generalized joint velocities.
weightsweighting coefficients for the different joint positions.

Definition at line 926 of file Constraints.cc.

References RigidBodyDynamics::CalcConstraintsJacobian(), Model::dof_count, ConstraintSet::linear_solver, Model::q_size, ConstraintSet::size(), and RigidBodyDynamics::SolveLinearSystem().

◆ CalcConstrainedSystemVariables()

RBDL_DLLAPI void CalcConstrainedSystemVariables ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
const Math::VectorNd Tau,
ConstraintSet CS,
std::vector< Math::SpatialVector > *  f_ext = NULL 
)

Computes the terms $H$, $G$, and $\gamma$ of the constrained dynamic problem and stores them in the ConstraintSet.

Parameters
modelthe model
Qthe generalized positions of the joints
QDotthe generalized velocities of the joints
Tauthe generalized forces of the joints
CSthe constraint set for which the error should be computed
f_extExternal forces acting on the body in base coordinates (optional, defaults to NULL)
Note
This function is normally called automatically in the various constrained dynamics functions, the user normally does not have to call it.

Definition at line 696 of file Constraints.cc.

References ConstraintSet::acceleration, SpatialTransform::apply(), ConstraintSet::baumgarteParameters, ConstraintSet::body, ConstraintSet::body_p, ConstraintSet::body_s, ConstraintSet::C, RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), RigidBodyDynamics::CalcConstraintsJacobian(), RigidBodyDynamics::CalcConstraintsPositionError(), RigidBodyDynamics::CalcConstraintsVelocityError(), RigidBodyDynamics::CalcPointAcceleration(), RigidBodyDynamics::CalcPointAcceleration6D(), RigidBodyDynamics::CalcPointVelocity6D(), RigidBodyDynamics::CompositeRigidBodyAlgorithm(), ConstraintSet::constraintAxis, RigidBodyDynamics::Math::crossm(), Model::dof_count, ConstraintSet::err, ConstraintSet::errd, ConstraintSet::G, ConstraintSet::gamma, ConstraintSet::H, Model::lambda, Model::mBodies, ConstraintSet::mContactConstraintIndices, ConstraintSet::mCustomConstraintIndices, ConstraintSet::mCustomConstraints, ConstraintSet::mLoopConstraintIndices, RigidBodyDynamics::NonlinearEffects(), ConstraintSet::normal, ConstraintSet::point, ConstraintSet::QDDot_0, RigidBodyDynamics::UpdateKinematicsCustom(), Model::X_base, Model::X_lambda, ConstraintSet::X_p, and ConstraintSet::X_s.

◆ CalcConstraintsJacobian()

◆ CalcConstraintsPositionError()

RBDL_DLLAPI void CalcConstraintsPositionError ( Model model,
const Math::VectorNd Q,
ConstraintSet CS,
Math::VectorNd err,
bool  update_kinematics = true 
)

Computes the position errors for the given ConstraintSet.

Parameters
modelthe model
Qthe generalized positions of the joints
CSthe constraint set for which the error should be computed
err(output) vector where the error will be stored in (should have the size of CS).
update_kinematicswhether the kinematics of the model should be updated from Q.
Note
the position error is always 0 for contact constraints.

Definition at line 498 of file Constraints.cc.

References ConstraintSet::body_p, ConstraintSet::body_s, RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), ConstraintSet::constraintAxis, ConstraintSet::mContactConstraintIndices, ConstraintSet::mCustomConstraintIndices, ConstraintSet::mCustomConstraints, ConstraintSet::mLoopConstraintIndices, ConstraintSet::size(), RigidBodyDynamics::UpdateKinematicsCustom(), ConstraintSet::X_p, and ConstraintSet::X_s.

◆ CalcConstraintsVelocityError()

RBDL_DLLAPI void CalcConstraintsVelocityError ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
ConstraintSet CS,
Math::VectorNd err,
bool  update_kinematics = true 
)

Computes the velocity errors for the given ConstraintSet.

Parameters
modelthe model
Qthe generalized positions of the joints
QDotthe generalized velocities of the joints
CSthe constraint set for which the error should be computed
err(output) vector where the error will be stored in (should have the size of CS).
update_kinematicswhether the kinematics of the model should be updated from Q.
Note
this is equivalent to multiplying the constraint jacobian by the generalized velocities of the joints.

Definition at line 666 of file Constraints.cc.

References RigidBodyDynamics::CalcConstraintsJacobian(), ConstraintSet::G, ConstraintSet::mCustomConstraintIndices, and ConstraintSet::mCustomConstraints.

◆ ComputeConstraintImpulsesDirect()

RBDL_DLLAPI void ComputeConstraintImpulsesDirect ( 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 1053 of file Constraints.cc.

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

◆ ComputeConstraintImpulsesNullSpace()

◆ ComputeConstraintImpulsesRangeSpaceSparse()

◆ ForwardDynamicsConstraintsDirect()

RBDL_DLLAPI void ForwardDynamicsConstraintsDirect ( Model model,
const Math::VectorNd Q,
const Math::VectorNd QDot,
const Math::VectorNd Tau,
ConstraintSet CS,
Math::VectorNd QDDot,
std::vector< Math::SpatialVector > *  f_ext = NULL 
)

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
This function works with ContactConstraints, LoopConstraints and Custom Constraints. Nonetheless, this method will not tolerate 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)
f_extExternal forces acting on the body in base coordinates (optional, defaults to NULL)
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 980 of file Constraints.cc.

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

◆ ForwardDynamicsConstraintsNullSpace()

◆ ForwardDynamicsConstraintsRangeSpaceSparse()

◆ ForwardDynamicsContactsKokkevis()

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) $\Phi_{i,j}$ which is then used to build and solve a system of the form:

\[ \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). \]

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.
This function supports only contact constraints.
Todo:
Allow for external forces

Definition at line 1445 of file Constraints.cc.

References ConstraintSet::a, ConstraintSet::acceleration, SpatialTransform::applyAdjoint(), ConstraintSet::body, RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcPointAcceleration(), ConstraintSet::constraintType, ConstraintSet::ContactConstraint, Model::dof_count, ConstraintSet::f_ext_constraints, ConstraintSet::f_t, Model::fixed_body_discriminator, ConstraintSet::force, RigidBodyDynamics::ForwardDynamics(), RigidBodyDynamics::ForwardDynamicsAccelerationDeltas(), RigidBodyDynamics::ForwardDynamicsApplyConstraintForces(), RigidBodyDynamics::GetMovableBodyId(), 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().

◆ SolveConstrainedSystemDirect()

RBDL_DLLAPI void SolveConstrainedSystemDirect ( 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 349 of file Constraints.cc.

References LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, LinearSolverPartialPivLU, and LOG.

◆ SolveConstrainedSystemNullSpace()

RBDL_DLLAPI void SolveConstrainedSystemNullSpace ( 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 437 of file Constraints.cc.

References LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, LinearSolverPartialPivLU, and LOG.

◆ SolveConstrainedSystemRangeSpaceSparse()

RBDL_DLLAPI void SolveConstrainedSystemRangeSpaceSparse ( 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 400 of file Constraints.cc.

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

Variable Documentation

◆ CustomConstraint

struct RBDL_DLLAPI CustomConstraint

Definition at line 255 of file Constraints.h.