Rigid Body Dynamics Library

Data Structures  
struct  ConstraintSet 
Structure that contains both constraint information and workspace memory. More...  
struct  CustomConstraint 
Interface to define generalpurpose 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 , , and 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=1e12, 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 
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.
In the presence of constraints, to compute the acceleration one has to solve a linear system of the form:
where is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), is the constraint jacobian, the bias force (sometimes called "nonlinear effects"), and the generalized acceleration independent part of the constraints.
Similarly to compute the response of the model to a contact gain one has to solve a system of the following form:
where is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), are the point jacobians of the contact points, the generalized velocity after the impact, the impulses at each constraint, the generalized velocity before the impact, and the desired velocity of each constraint after the impact (known beforehand, usually 0). The value of can is specified via the variable ConstraintSet::v_plus and defaults to 0.
There are essentially three different approaches to solve these systems:
RBDL provides methods for all approaches. The implementation for the rangespace method also exploits sparsities in the joint space inertia matrix using a sparse structure preserving 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 tradeoffs as factors such as model topology, number of constraints, constrained bodies, numerical stability, and performance vary and evaluation has to be made on a casebycase basis.
Dynamics
RBDL provides the following methods to compute the acceleration of a constrained system:
RBDL provides the following methods to compute the collision response on new contact gains:
satisfying the constraint equations.
When considering a model subject position level constraints expressed by the equation , it is often necessary to compute generalized joint position and velocities which satisfy the constraints. Even velocitylevel constraints may have positionlevel assembly constraints: a rollingwithoutslipping constraint is a velocitylevel 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:
In order to compute a vector of generalized joint velocities that satisfy the constraints it is necessary to solve the following optimization problem:
and are initial guesses, is the constraints jacobian (partial derivative of with respect to ), and is the partial derivative of with respect to time. 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 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:
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:
A term is added to the right hand side of the equation, defined in the following way:
where are the position level constraint errors and and 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:
with 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 .
RBDL_DLLAPI bool CalcAssemblyQ  (  Model &  model, 
Math::VectorNd  QInit,  
ConstraintSet &  CS,  
Math::VectorNd &  Q,  
const Math::VectorNd &  weights,  
double  tolerance = 1e12 , 

unsigned int  max_iter = 100 

) 
Computes a feasible initial value of the generalized joint positions.
model  the model 
QInit  initial guess for the generalized positions of the joints 
CS  the constraint set for which the error should be computed 
Q  (output) vector of the generalized joint positions. 
weights  weighting coefficients for the different joint positions. 
tolerance  the function will return successfully if the constraint position error norm is lower than this value. 
max_iter  the funciton will return unsuccessfully after performing this number of iterations. 
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().
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.
model  the model 
Q  the generalized joint position of the joints. It is assumed that this vector satisfies the position level assemblt constraints. 
QDotInit  initial guess for the generalized velocities of the joints 
CS  the constraint set for which the error should be computed 
QDot  (output) vector of the generalized joint velocities. 
weights  weighting 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().
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 , , and of the constrained dynamic problem and stores them in the ConstraintSet.
model  the model 
Q  the generalized positions of the joints 
QDot  the generalized velocities of the joints 
Tau  the generalized forces of the joints 
CS  the constraint set for which the error should be computed 
f_ext  External forces acting on the body in base coordinates (optional, defaults to NULL) 
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.
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.
model  the model 
Q  the generalized positions of the joints 
CS  the constraint set for which the Jacobian should be computed 
G  (output) matrix where the output will be stored in 
update_kinematics  whether the kinematics of the model should be updated from Q 
Definition at line 570 of file Constraints.cc.
References SpatialTransform::apply(), ConstraintSet::body, ConstraintSet::body_p, ConstraintSet::body_s, RigidBodyDynamics::CalcBodyToBaseCoordinates(), RigidBodyDynamics::CalcBodyWorldOrientation(), RigidBodyDynamics::CalcPointJacobian(), RigidBodyDynamics::CalcPointJacobian6D(), ConstraintSet::constraintAxis, Model::dof_count, SpatialTransform::E, ConstraintSet::G, ConstraintSet::Gi, ConstraintSet::GSJ, ConstraintSet::GSpi, ConstraintSet::GSsi, ConstraintSet::mContactConstraintIndices, ConstraintSet::mCustomConstraintIndices, ConstraintSet::mCustomConstraints, ConstraintSet::mLoopConstraintIndices, ConstraintSet::normal, ConstraintSet::point, SpatialTransform::r, RigidBodyDynamics::UpdateKinematicsCustom(), ConstraintSet::X_p, ConstraintSet::X_s, and RigidBodyDynamics::Math::Xtrans().
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.
model  the model 
Q  the generalized positions of the joints 
CS  the 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_kinematics  whether the kinematics of the model should be updated from Q. 
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.
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.
model  the model 
Q  the generalized positions of the joints 
QDot  the generalized velocities of the joints 
CS  the 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_kinematics  whether the kinematics of the model should be updated from Q. 
Definition at line 666 of file Constraints.cc.
References RigidBodyDynamics::CalcConstraintsJacobian(), ConstraintSet::G, ConstraintSet::mCustomConstraintIndices, and ConstraintSet::mCustomConstraints.
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
where is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), are the point jacobians of the contact points, the generalized velocity after the impact, the impulses at each constraint, the generalized velocity before the impact, and the desired velocity of each constraint after the impact (known beforehand, usually 0). The value of can is specified via the variable ConstraintSet::v_plus and defaults to 0.
model  rigid body model 
Q  state vector of the internal joints 
QDotMinus  velocity vector of the internal joints before the impact 
CS  the set of active constraints 
QDotPlus  velocities 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.
RBDL_DLLAPI void ComputeConstraintImpulsesNullSpace  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDotMinus,  
ConstraintSet &  CS,  
Math::VectorNd &  QDotPlus  
) 
Resolves contact gain using SolveContactSystemNullSpace()
Definition at line 1104 of file Constraints.cc.
References RigidBodyDynamics::CalcConstraintsJacobian(), RigidBodyDynamics::CompositeRigidBodyAlgorithm(), ConstraintSet::G, ConstraintSet::GT_qr, ConstraintSet::GT_qr_Q, ConstraintSet::H, ConstraintSet::impulse, ConstraintSet::linear_solver, ConstraintSet::qddot_y, ConstraintSet::qddot_z, RigidBodyDynamics::SolveConstrainedSystemNullSpace(), RigidBodyDynamics::UpdateKinematicsCustom(), ConstraintSet::v_plus, ConstraintSet::Y, and ConstraintSet::Z.
RBDL_DLLAPI void ComputeConstraintImpulsesRangeSpaceSparse  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDotMinus,  
ConstraintSet &  CS,  
Math::VectorNd &  QDotPlus  
) 
Resolves contact gain using SolveContactSystemRangeSpaceSparse()
Definition at line 1083 of file Constraints.cc.
References ConstraintSet::a, RigidBodyDynamics::CalcConstraintsJacobian(), RigidBodyDynamics::CompositeRigidBodyAlgorithm(), ConstraintSet::G, ConstraintSet::H, ConstraintSet::impulse, ConstraintSet::K, ConstraintSet::linear_solver, RigidBodyDynamics::SolveConstrainedSystemRangeSpaceSparse(), RigidBodyDynamics::UpdateKinematicsCustom(), and ConstraintSet::v_plus.
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
where is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), are the point jacobians of the contact points, the bias force (sometimes called "nonlinear effects"), and the generalized acceleration independent part of the contact point accelerations.
model  rigid body model 
Q  state vector of the internal joints 
QDot  velocity vector of the internal joints 
Tau  actuations of the internal joints 
CS  the description of all acting constraints 
QDDot  accelerations of the internals joints (output) 
f_ext  External forces acting on the body in base coordinates (optional, defaults to NULL) 
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.
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  
) 
Definition at line 1023 of file Constraints.cc.
References ConstraintSet::C, RigidBodyDynamics::CalcConstrainedSystemVariables(), ConstraintSet::force, ConstraintSet::G, ConstraintSet::gamma, ConstraintSet::GT_qr, ConstraintSet::GT_qr_Q, ConstraintSet::H, ConstraintSet::linear_solver, LOG, ConstraintSet::qddot_y, ConstraintSet::qddot_z, RigidBodyDynamics::SolveConstrainedSystemNullSpace(), ConstraintSet::Y, and ConstraintSet::Z.
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  
) 
Definition at line 1007 of file Constraints.cc.
References ConstraintSet::a, ConstraintSet::C, RigidBodyDynamics::CalcConstrainedSystemVariables(), ConstraintSet::force, ConstraintSet::G, ConstraintSet::gamma, ConstraintSet::H, ConstraintSet::K, ConstraintSet::linear_solver, and RigidBodyDynamics::SolveConstrainedSystemRangeSpaceSparse().
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 articulatedbody inertia (IABI) which is then used to build and solve a system of the form:
Here 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 are the constraint accelerations, the constraint forces, and are the constraint bias forces.
model  rigid body model 
Q  state vector of the internal joints 
QDot  velocity vector of the internal joints 
Tau  actuations of the internal joints 
CS  a list of all contact points 
QDDot  accelerations of the internals joints (output) 
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().
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 linear system.
H  the joint space inertia matrix 
G  the constraint jacobian 
c  the vector of the upper part of the right hand side of the system 
gamma  the vector of the lower part of the right hand side of the system 
qddot  result: joint accelerations 
lambda  result: constraint forces 
A  workspace for the matrix of the linear system 
b  workspace for the righthandside of the linear system 
x  workspace for the solution of the linear system 
type  of solver that should be used to solve the system 
Definition at line 349 of file Constraints.cc.
References LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, LinearSolverPartialPivLU, and LOG.
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 matrix of the form with the property that can be computed using a QR decomposition (e.g. see code for ForwardDynamicsContactsNullSpace()).
H  the joint space inertia matrix 
G  the constraint jacobian 
c  the vector of the upper part of the right hand side of the system 
gamma  the vector of the lower part of the right hand side of the system 
qddot  result: joint accelerations 
lambda  result: constraint forces 
Y  basis for the rangespace of the constraints 
Z  basis for the nullspace of the constraints 
qddot_y  workspace of size 
qddot_z  workspace of size 
linear_solver  type of solver that should be used to solve the system 
Definition at line 437 of file Constraints.cc.
References LinearSolverColPivHouseholderQR, LinearSolverHouseholderQR, LinearSolverPartialPivLU, and LOG.
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 branchinduced sparsity by the structure preserving decomposition described in RBDL, Section 6.5.
H  the joint space inertia matrix 
G  the constraint jacobian 
c  the vector of the upper part of the right hand side of the system 
gamma  the vector of the lower part of the right hand side of the system 
qddot  result: joint accelerations 
lambda  result: constraint forces 
K  workspace for the matrix of the constraint force linear system 
a  workspace for the righthandside of the constraint force linear system 
linear_solver  type 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().
struct RBDL_DLLAPI CustomConstraint 
Definition at line 255 of file Constraints.h.