Rigid Body Dynamics Library

Data Structures  
struct  ConstraintSet 
Structure that contains both constraint information and workspace memory. More...  
Functions  
ConstraintSet ()  
unsigned int  getGroupIndexByName (const char *userDefinedName) 
getGroupIndex returns the index to a constraints that have been grouped because they are of the same type, apply to the same bodies, and apply to the same local frames on each body. More...  
unsigned int  getGroupIndexById (unsigned int userDefinedId) 
getGroupIndex returns the index to a constraints that have been grouped because they are of the same type, apply to the same bodies, and apply to the same local frames on each body. More...  
unsigned int  getGroupIndexByAssignedId (unsigned int assignedId) 
getGroupIndex returns the index to a constraints that have been grouped because they are of the same type, apply to the same bodies, and apply to the same local frames on each body. More...  
unsigned int  getGroupIndexMax () 
getGroupIndexMax returns the maximum valid constraint group index (the min. is zero) so that constraint groups can be iterated over if desired. More...  
const char *  getGroupName (unsigned int groupIndex) 
Returns the name of the constraint group, which may differ from the names entered by the user if the constraint is one in which grouping is done automatically (e.g. contact and loop constraints) More...  
unsigned int  getGroupSize (unsigned int groupIndex) 
Returns the number of constraint equations in this group. More...  
unsigned int  getGroupType (unsigned int groupIndex) 
Returns integer corresponding to the ConstraintType. More...  
unsigned int  getGroupId (unsigned int groupIndex) 
Returns the userdefinedid of the constraint group, which may differ from the names entered by the user if the constraint is one in which grouping is done automatically (e.g. contact and loop constraints). More...  
unsigned int  getGroupAssignedId (unsigned int groupIndex) 
Returns assigned id of the constraint group which will be the first id assigned to an entry in a group (if the grouping was done automatically  as is done for contact and loop constraints). More...  
void  calcForces (unsigned int groupIndex, Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, std::vector< unsigned int > &updConstraintBodyIdsOutput, std::vector< Math::SpatialTransform > &updConstraintBodyFramesOutput, std::vector< Math::SpatialVector > &updConstraintForcesOutput, bool resolveAllInRootFrame=false, bool updateKinematics=false) 
calcForces resolves the generalized forces generated by this constraint into equivalent spatial forces (resolved in the local or the base frame) that are applied between the bodies and frames that this constraint applies to. More...  
void  calcImpulses (unsigned int groupIndex, Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, std::vector< unsigned int > &constraintBodyIdsOutput, std::vector< Math::SpatialTransform > &constraintBodyFramesOutput, std::vector< Math::SpatialVector > &constraintImpulsesOutput, bool resolveAllInRootFrame=false, bool updateKinematics=false) 
calcImpulses resolves the generalized impluses generated by this constraint into equivalent spatial impulses (resolved in the local or the base frame) that are applied between the bodies and frames that this constraint applies to. More...  
void  calcPositionError (unsigned int groupIndex, Model &model, const Math::VectorNd &Q, Math::VectorNd &positionErrorOutput, bool updateKinematics=false) 
calcPositionError calculates the vector of position errors associated with this constraint. Note that if the constraint group, or parts of it, are not defined at the position level then 0's will be returned. More...  
void  calcVelocityError (unsigned int groupIndex, Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &velocityErrorOutput, bool updateKinematics=false) 
calcVelocityError calculates the vector of position errors associated with this constraint. Note that if the constraint group, or parts of it, are not defined at the position level then 0's will be returned. More...  
void  calcBaumgarteStabilizationForces (unsigned int groupIndex, Model &model, const Math::VectorNd &positionError, const Math::VectorNd &velocityError, Math::VectorNd &baumgarteForcesOutput) 
bool  isBaumgarteStabilizationEnabled (unsigned int groupIndex) 
void  enableBaumgarteStabilization (unsigned int groupIndex) 
void  disableBaumgarteStabilization (unsigned int groupIndex) 
void  getBaumgarteStabilizationCoefficients (unsigned int groupIndex, Math::Vector2d &baumgartePositionVelocityCoefficientsOutput) 
unsigned int  AddContactConstraint (unsigned int bodyId, const Math::Vector3d &bodyPoint, const Math::Vector3d &worldNormal, const char *constraintName=NULL, unsigned int userDefinedId=std::numeric_limits< unsigned int >::max()) 
Adds a single contact constraint (pointground) to the constraint set. More...  
unsigned int  AddLoopConstraint (unsigned int bodyIdPredecessor, unsigned int bodyIdSuccessor, const Math::SpatialTransform &XPredecessor, const Math::SpatialTransform &XSuccessor, const Math::SpatialVector &constraintAxisInPredecessor, bool enableBaumgarteStabilization=false, double stabilizationTimeConstant=0.1, const char *constraintName=NULL, unsigned int userDefinedId=std::numeric_limits< unsigned int >::max()) 
Adds a loop constraint to the constraint set. More...  
unsigned int  AddCustomConstraint (std::shared_ptr< Constraint > customConstraint) 
Adds a custom constraint to the constraint set. More...  
ConstraintSet  Copy () 
Copies the constraints and resets its ConstraintSet::bound flag. More...  
void  SetSolver (Math::LinearSolver solver) 
Specifies which method should be used for solving undelying linear systems. More...  
bool  Bind (const Model &model) 
Initializes and allocates memory for the constraint set. More...  
void  SetActuationMap (const Model &model, const std::vector< bool > &actuatedDof) 
Initializes and allocates memory needed for InverseDynamicsConstraints and InverseDynamicsConstraintsRelaxed. More...  
size_t  size () const 
Returns the number of constraints. More...  
void  clear () 
Clears all variables in the constraint set. More...  
RBDL_DLLAPI void  CalcConstraintsPositionError (Model &model, const Math::VectorNd &Q, ConstraintSet &CS, Math::VectorNd &errOutput, 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 &GOutput, 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 &errOutput, 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 &CSOutput, bool update_kinematics=true, 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 &QOutput, 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 &QDotOutput, 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 &QDDotOutput, bool update_kinematics=true, 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 &QDDotOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL) 
RBDL_DLLAPI void  ForwardDynamicsConstraintsNullSpace (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDotOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL) 
RBDL_DLLAPI void  ForwardDynamicsContactsKokkevis (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDotOutput) 
Computes forward dynamics that accounts for active contacts in ConstraintSet. More...  
RBDL_DLLAPI void  InverseDynamicsConstraintsRelaxed (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDotControls, ConstraintSet &CS, Math::VectorNd &QDDotOutput, Math::VectorNd &TauOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL) 
A relaxed inversedynamics operator that can be applied to underactuated or fullyactuated constrained multibody systems. More...  
RBDL_DLLAPI void  InverseDynamicsConstraints (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDotDesired, ConstraintSet &CS, Math::VectorNd &QDDotOutput, Math::VectorNd &TauOutput, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL) 
An inversedynamics operator that can be applied to fullyactuated constrained systems. More...  
RBDL_DLLAPI bool  isConstrainedSystemFullyActuated (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ConstraintSet &CS, bool update_kinematics=true, std::vector< Math::SpatialVector > *f_ext=NULL) 
A method to evaluate if the constrained system is fully actuated. More...  
RBDL_DLLAPI void  ComputeConstraintImpulsesDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlusOutput) 
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 &QDotPlusOutput) 
Resolves contact gain using SolveContactSystemRangeSpaceSparse() More...  
RBDL_DLLAPI void  ComputeConstraintImpulsesNullSpace (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlusOutput) 
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 &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  
Math::LinearSolver  linear_solver 
Method that should be used to solve internal linear systems. More...  
bool  bound 
Whether the constraint set was bound to a model (mandatory!). More...  
std::vector< ConstraintType >  constraintType 
std::vector< std::string >  name 
std::map< std::string, unsigned int >  nameGroupMap 
std::map< unsigned int, unsigned int >  userDefinedIdGroupMap 
std::map< unsigned int, unsigned int >  idGroupMap 
std::vector< std::shared_ptr< Constraint > >  constraints 
std::vector< std::shared_ptr< ContactConstraint > >  contactConstraints 
std::vector< std::shared_ptr< LoopConstraint > >  loopConstraints 
Math::VectorNd  err 
Math::VectorNd  errd 
Math::VectorNd  force 
Math::VectorNd  impulse 
Constraint impulses along the constraint directions. More...  
Math::VectorNd  v_plus 
The velocities we want to have along the constraint directions. More...  
Math::MatrixNd  H 
Workspace for the joint space inertia matrix. More...  
Math::VectorNd  C 
Workspace for the coriolis forces. More...  
Math::VectorNd  gamma 
Workspace of the right hand side of the acceleration equation. More...  
Math::MatrixNd  G 
Workspace for the constraint Jacobian. More...  
Math::MatrixNd  A 
Workspace for the Lagrangian lefthandside matrix. More...  
Math::VectorNd  b 
Workspace for the Lagrangian righthandside. More...  
Math::VectorNd  x 
Workspace for the Lagrangian solution. More...  
Math::MatrixNd  S 
Math::MatrixNd  P 
Selection matrix for the nonactuated parts of the model. More...  
Math::MatrixNd  W 
Math::MatrixNd  Winv 
Math::VectorNd  WinvSC 
Math::VectorNd  u 
Math::VectorNd  v 
Math::MatrixNd  F 
Math::MatrixNd  Ful 
Math::MatrixNd  Fur 
Math::MatrixNd  Fll 
Math::MatrixNd  Flr 
Math::MatrixNd  GT 
Math::MatrixNd  GTu 
Math::MatrixNd  GTl 
Math::VectorNd  g 
Math::MatrixNd  Ru 
Math::VectorNd  py 
Math::VectorNd  pz 
Eigen::HouseholderQR< Math::MatrixNd >  GT_qr 
Workspace for the QR decomposition of the nullspace method. More...  
Eigen::FullPivHouseholderQR< Math::MatrixNd >  GPT_full_qr 
Math::MatrixNd  GT_qr_Q 
Math::MatrixNd  GPT 
Math::MatrixNd  Y 
Math::MatrixNd  Z 
Math::MatrixNd  R 
Math::VectorNd  qddot_y 
Math::VectorNd  qddot_z 
Math::MatrixNd  K 
Workspace for the Inverse ArticulatedBody Inertia. More...  
Math::VectorNd  a 
Workspace for the accelerations of due to the test forces. More...  
Math::VectorNd  QDDot_t 
Workspace for the test accelerations. More...  
Math::VectorNd  QDDot_0 
Workspace for the default accelerations. More...  
std::vector< Math::SpatialVector >  f_t 
Workspace for the test forces. More...  
std::vector< Math::SpatialVector >  f_ext_constraints 
Workspace for the actual spatial forces. More...  
std::vector< Math::Vector3d >  point_accel_0 
Workspace for the default point accelerations. More...  
std::vector< Math::SpatialVector >  d_pA 
Workspace for the bias force due to the test force. More...  
std::vector< Math::SpatialVector >  d_a 
Workspace for the acceleration due to the test force. More...  
Math::VectorNd  d_u 
std::vector< Math::SpatialMatrix >  d_IA 
Workspace for the inertia when applying constraint forces. More...  
std::vector< Math::SpatialVector >  d_U 
Workspace when applying constraint forces. More...  
Math::VectorNd  d_d 
Workspace when applying constraint forces. More...  
std::vector< Math::Vector3d >  d_multdof3_u 
ConstraintCache  cache 
Constraints are handled by specification of a ConstraintSet which contains all information 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 Lagrange multipliers or change in Lagrange multipliers 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 constraint gain one has to solve a system of the following form:
where is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), are the Jacobians of the constraints, 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.
FBaumgarte stabilization is disabled by default, and uses the default the stabilization parameter .

inline 
Definition at line 282 of file Constraints.h.
unsigned int AddContactConstraint  (  unsigned int  bodyId, 
const Math::Vector3d &  bodyPoint,  
const Math::Vector3d &  worldNormal,  
const char *  constraintName = NULL , 

unsigned int  userDefinedId = std::numeric_limits< unsigned int >::max() 

) 
Adds a single contact constraint (pointground) to the constraint set.
This type of constraints ensures that the velocity and acceleration of a specified body point along a specified axis are null.
bodyId  the body which is affected directly by the constraint 
bodyPoint  the point that is constrained relative to the contact body 
worldNormal  the normal direction in which to apply the constraint 
constraintName  a human readable name (optional, default: NULL). Set this field to a unique name (within this ConstraintSet) so that the function GetConstraintIndex can find it. 
userDefinedId  a user defined id (optional, defaults to max()). Set this field to a unique number (within this ConstraintSet) so that the function GetConstraintIndex can find it. 
unsigned int AddCustomConstraint  (  std::shared_ptr< Constraint >  customConstraint  ) 
Adds a custom constraint to the constraint set.
customConstraint  The CustomConstraint to be added to the ConstraintSet 
unsigned int AddLoopConstraint  (  unsigned int  bodyIdPredecessor, 
unsigned int  bodyIdSuccessor,  
const Math::SpatialTransform &  XPredecessor,  
const Math::SpatialTransform &  XSuccessor,  
const Math::SpatialVector &  constraintAxisInPredecessor,  
bool  enableBaumgarteStabilization = false , 

double  stabilizationTimeConstant = 0.1 , 

const char *  constraintName = NULL , 

unsigned int  userDefinedId = std::numeric_limits< unsigned int >::max() 

) 
Adds a loop constraint to the constraint set.
This type of constraints ensures that the relative orientation and position, spatial velocity, and spatial acceleration between two frames in two bodies are null along a specified spatial constraint axis.
bodyIdPredecessor  the identifier of the predecessor body 
bodyIdSuccessor  the identifier of the successor body 
XPredecessor  a spatial transform localizing the constrained frames on the predecessor body, expressed with respect to the predecessor body frame. Note the position vector should be the r_BPB (from the body's origin, to the predessor frame, in the coordinates of the body's frame) and E_BP (the rotation matrix that will rotate vectors from the coordinates of the P frame to the coordinates of the body's frame). 
XSuccessor  a spatial transform localizing the constrained frames on the successor body, expressed with respect to the successor body frame. Note the position vector should be the r_BSB (from the body's origin, to the successor frame, in the coordinates of the body's frame) and E_BS (the rotation matrix that will rotate vectors from the coordinates of the S frame to the coordinates of the body's frame). 
constraintAxisInPredecessor  a spatial vector, resolved in the frame of the predecessor frame, indicating the axis along which the constraint acts 
enableBaumgarteStabilization  (optional, default false) setting this flag to true will modify the right hand side of the acceleration equation with a penaltiy term that is proportional to the constraint error scaled by a constant. 
stabilizationTimeConstant  (optional, defaults to 0.1 sec) this value scales the strength of Baumgarte stabilization so that the settling time of the error is proportional the value given here. 
constraintName  a human readable name (optional, default: NULL). Set this field to a unique name (within this ConstraintSet) so that the function GetConstraintIndex can find it. 
userDefinedId  a user defined id (optional, defaults to max()). Set this field to a unique number (within this ConstraintSet) so that the function GetConstraintIndex can find it. 
bool Bind  (  const Model &  model  ) 
Initializes and allocates memory for the constraint set.
This function allocates memory for temporary values and matrices that are required for contact force computation. Both model and constraint set must not be changed after a binding as the required memory is dependent on the model size (i.e. the number of bodies and degrees of freedom) and the number of constraints in the Constraint set.
The values of ConstraintSet::acceleration may still be modified after the set is bound to the model.
RBDL_DLLAPI bool RigidBodyDynamics::CalcAssemblyQ  (  Model &  model, 
Math::VectorNd  QInit,  
ConstraintSet &  CS,  
Math::VectorNd &  QOutput,  
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 
QOutput  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. 
RBDL_DLLAPI void RigidBodyDynamics::CalcAssemblyQDot  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDotInit,  
ConstraintSet &  CS,  
Math::VectorNd &  QDotOutput,  
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 
QDotOutput  vector of the generalized joint velocities. 
weights  weighting coefficients for the different joint positions. 
void calcBaumgarteStabilizationForces  (  unsigned int  groupIndex, 
Model &  model,  
const Math::VectorNd &  positionError,  
const Math::VectorNd &  velocityError,  
Math::VectorNd &  baumgarteForcesOutput  
) 
groupIndex  the index number of this constraint (see getGroupIndex index functions) 
model  the multibody model 
positionError  the position errors associated with this constraint computed using the calcConstraintPositionError function 
velocityError  the velocity errors associated with this constraint computed using the calcConstraintVelocityError function 
baumgarteForcesOutput  (output) a reference to the vector of baumgarte stabilization forces applied to this this constraint. 
RBDL_DLLAPI void RigidBodyDynamics::CalcConstrainedSystemVariables  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
const Math::VectorNd &  Tau,  
ConstraintSet &  CSOutput,  
bool  update_kinematics = true , 

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 
CSOutput  the constraint set for which the error should be computed 
update_kinematics  whether the kinematics of the model should be updated from Q. 
f_ext  External forces acting on the body in base coordinates (optional, defaults to NULL) 
RBDL_DLLAPI void RigidBodyDynamics::CalcConstraintsJacobian  (  Model &  model, 
const Math::VectorNd &  Q,  
ConstraintSet &  CS,  
Math::MatrixNd &  GOutput,  
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 
GOutput  matrix where the output will be stored in 
update_kinematics  whether the kinematics of the model should be updated from Q 
RBDL_DLLAPI void RigidBodyDynamics::CalcConstraintsPositionError  (  Model &  model, 
const Math::VectorNd &  Q,  
ConstraintSet &  CS,  
Math::VectorNd &  errOutput,  
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 
errOutput  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. 
RBDL_DLLAPI void RigidBodyDynamics::CalcConstraintsVelocityError  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
ConstraintSet &  CS,  
Math::VectorNd &  errOutput,  
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 
errOutput  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. 
void calcForces  (  unsigned int  groupIndex, 
Model &  model,  
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
std::vector< unsigned int > &  updConstraintBodyIdsOutput,  
std::vector< Math::SpatialTransform > &  updConstraintBodyFramesOutput,  
std::vector< Math::SpatialVector > &  updConstraintForcesOutput,  
bool  resolveAllInRootFrame = false , 

bool  updateKinematics = false 

) 
calcForces resolves the generalized forces generated by this constraint into equivalent spatial forces (resolved in the local or the base frame) that are applied between the bodies and frames that this constraint applies to.
groupIndex  the index number of this constraint (see getGroupIndex index functions) 
model  the multibody model 
Q  the generalized positions of the model 
QDot  the generalized velocities of the model 
constraintBodyIdsOutputUpd  a reference to a vector of body ids in which the spatial forces are resolved into. The ordering for the standard constraints are as follows :

constraintBodyFramesOutputUpd  a reference to a vector of frames in which the spatial forces are resolved. The frame is located in the body id listed in constraintBodyIdsOutput. 
constraintForcesOutputUpd  a reference to a vector of spatial forces generated by this constraint. The spatial force is resolved into the frame listed in constraintBodyFramesOutput. 
resolveAllInRootFrame 

updateKinematics  setting this flag to true will trigger the kinematic transforms of the model to be updated. 
void calcImpulses  (  unsigned int  groupIndex, 
Model &  model,  
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
std::vector< unsigned int > &  constraintBodyIdsOutput,  
std::vector< Math::SpatialTransform > &  constraintBodyFramesOutput,  
std::vector< Math::SpatialVector > &  constraintImpulsesOutput,  
bool  resolveAllInRootFrame = false , 

bool  updateKinematics = false 

) 
calcImpulses resolves the generalized impluses generated by this constraint into equivalent spatial impulses (resolved in the local or the base frame) that are applied between the bodies and frames that this constraint applies to.
groupIndex  the index number of this constraint (see getGroupIndex index functions) 
model  the multibody model 
Q  the generalized positions of the model 
QDot  the generalized velocities of the model 
constraintBodyIdsOutput  a vector of body ids in which the spatial forces are resolved into. The ordering for the standard constraints are as follows :

constraintBodyFramesOutput  a vector of frames in which the spatial forces are resolved. The frame is located in the body id listed in constraintBodyIdsOutput. 
constraintImpulsesOutput  a vector of spatial impulses generated by this constraint. The spatial impulse is resolved into the frame listed in constraintBodyFramesOutput. 
resolveAllInRootFrame 

updateKinematics  setting this flag to true will trigger the kinematic transforms of the model to be updated. 
void calcPositionError  (  unsigned int  groupIndex, 
Model &  model,  
const Math::VectorNd &  Q,  
Math::VectorNd &  positionErrorOutput,  
bool  updateKinematics = false 

) 
calcPositionError calculates the vector of position errors associated with this constraint. Note that if the constraint group, or parts of it, are not defined at the position level then 0's will be returned.
groupIndex  the index number of this constraint (see getGroupIndex index functions) 
model  the multibody model 
Q  the generalized positions of the model 
positionErrorOutput  (output) a reference to the vector of constraint errors 
updateKinematics  setting this flag to true will trigger the kinematic transforms of the model to be updated. 
void calcVelocityError  (  unsigned int  groupIndex, 
Model &  model,  
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
Math::VectorNd &  velocityErrorOutput,  
bool  updateKinematics = false 

) 
calcVelocityError calculates the vector of position errors associated with this constraint. Note that if the constraint group, or parts of it, are not defined at the position level then 0's will be returned.
groupIndex  the index number of this constraint (see getGroupIndex index functions) 
model  the multibody model 
Q  the generalized positions of the model 
QDot  the generalized velocities of the model 
velocityErrorOutput  (output) the vector of constraint errors at the velocity level 
updateKinematics  setting this flag to true will trigger the kinematic transforms of the model to be updated. 
void clear  (  ) 
Clears all variables in the constraint set.
RBDL_DLLAPI void RigidBodyDynamics::ComputeConstraintImpulsesDirect  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDotMinus,  
ConstraintSet &  CS,  
Math::VectorNd &  QDotPlusOutput  
) 
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 
QDotPlusOutput  velocities of the internals joints after the impact 
RBDL_DLLAPI void RigidBodyDynamics::ComputeConstraintImpulsesNullSpace  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDotMinus,  
ConstraintSet &  CS,  
Math::VectorNd &  QDotPlusOutput  
) 
Resolves contact gain using SolveContactSystemNullSpace()
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 
QDotPlusOutput  velocities of the internals joints after the impact 
RBDL_DLLAPI void RigidBodyDynamics::ComputeConstraintImpulsesRangeSpaceSparse  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDotMinus,  
ConstraintSet &  CS,  
Math::VectorNd &  QDotPlusOutput  
) 
Resolves contact gain using SolveContactSystemRangeSpaceSparse()
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 
QDotPlusOutput  velocities of the internals joints after the impact 

inline 
Copies the constraints and resets its ConstraintSet::bound flag.
Definition at line 798 of file Constraints.h.
References ConstraintSet::bound.

inline 
groupIndex  the index number of this constraint (see getGroupIndex index functions) 
Definition at line 681 of file Constraints.h.

inline 
groupIndex  the index number of this constraint (see getGroupIndex index functions) 
Definition at line 672 of file Constraints.h.
RBDL_DLLAPI void RigidBodyDynamics::ForwardDynamicsConstraintsDirect  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
const Math::VectorNd &  Tau,  
ConstraintSet &  CS,  
Math::VectorNd &  QDDotOutput,  
bool  update_kinematics = true , 

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 
QDDotOutput  accelerations of the internals joints 
update_kinematics  whether the kinematics of the model should be updated from Q. 
f_ext  External forces acting on the body in base coordinates (optional, defaults to NULL) 
RBDL_DLLAPI void RigidBodyDynamics::ForwardDynamicsConstraintsNullSpace  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
const Math::VectorNd &  Tau,  
ConstraintSet &  CS,  
Math::VectorNd &  QDDotOutput,  
bool  update_kinematics = true , 

std::vector< Math::SpatialVector > *  f_ext = NULL 

) 
RBDL_DLLAPI void RigidBodyDynamics::ForwardDynamicsConstraintsRangeSpaceSparse  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
const Math::VectorNd &  Tau,  
ConstraintSet &  CS,  
Math::VectorNd &  QDDotOutput,  
bool  update_kinematics = true , 

std::vector< Math::SpatialVector > *  f_ext = NULL 

) 
RBDL_DLLAPI void RigidBodyDynamics::ForwardDynamicsContactsKokkevis  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
const Math::VectorNd &  Tau,  
ConstraintSet &  CS,  
Math::VectorNd &  QDDotOutput  
) 
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 
QDDotOutput  accelerations of the internals joints 

inline 
Definition at line 687 of file Constraints.h.

inline 
Returns assigned id of the constraint group which will be the first id assigned to an entry in a group (if the grouping was done automatically  as is done for contact and loop constraints).
groupIndex  the index number of a constraint group. 
Definition at line 432 of file Constraints.h.

inline 
Returns the userdefinedid of the constraint group, which may differ from the names entered by the user if the constraint is one in which grouping is done automatically (e.g. contact and loop constraints).
groupIndex  the index number of a constraint group. 
Definition at line 411 of file Constraints.h.

inline 
getGroupIndex returns the index to a constraints that have been grouped because they are of the same type, apply to the same bodies, and apply to the same local frames on each body.
assignedId  : the integer id that was returned when the constraint was added to the constraint set by the functions: AddContactConstraint, AddLoopConstraint, AddCustomConstraint, etc. 
Definition at line 337 of file Constraints.h.

inline 
getGroupIndex returns the index to a constraints that have been grouped because they are of the same type, apply to the same bodies, and apply to the same local frames on each body.
userDefinedId  : the optional integer id that was assigned to this constraint when it was created. 
Definition at line 320 of file Constraints.h.

inline 
getGroupIndex returns the index to a constraints that have been grouped because they are of the same type, apply to the same bodies, and apply to the same local frames on each body.
userDefinedName  : the optional name that the constraint was assigned when it was added to the constraint set. 
Definition at line 304 of file Constraints.h.

inline 
getGroupIndexMax returns the maximum valid constraint group index (the min. is zero) so that constraint groups can be iterated over if desired.
Definition at line 348 of file Constraints.h.

inline 
Returns the name of the constraint group, which may differ from the names entered by the user if the constraint is one in which grouping is done automatically (e.g. contact and loop constraints)
groupIndex  the index number of a constraint group. 
Definition at line 368 of file Constraints.h.

inline 
Returns the number of constraint equations in this group.
groupIndex  the index number of a constraint group. 
Definition at line 380 of file Constraints.h.

inline 
Returns integer corresponding to the ConstraintType.
groupIndex  the index number of a constraint group. 
Definition at line 390 of file Constraints.h.
RBDL_DLLAPI void RigidBodyDynamics::InverseDynamicsConstraints  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
const Math::VectorNd &  QDDotDesired,  
ConstraintSet &  CS,  
Math::VectorNd &  QDDotOutput,  
Math::VectorNd &  TauOutput,  
bool  update_kinematics = true , 

std::vector< Math::SpatialVector > *  f_ext = NULL 

) 
An inversedynamics operator that can be applied to fullyactuated constrained systems.
where is an (number of actuated degreesoffreedom) by (number of degrees of freedom of the unconstrained system) selection matrix that picks out the actuated indices in and
where is an (number of unactuated degreesoffreedom) by selection matrix that picks out the unactuated indices in . By construction
and thus
We begin by projecting the constrained equations of motion
and adding the constraint that
where is a vector of desired accelerations. By considering only forces that are applied to the actuated parts (that is ) we can rearrange the system of equations
By projecting this onto the onto the and spaces
it becomes clear that this system of equations will be singular if loses rank. Thus this method is appropriate to use provided that
The implementation exploits the triangular structure of the matrix which means that only two linear systems of size and are performed which is much faster than solving the KKT matrix directly.
References
model  rigid body model 
Q  Nelement vector of generalized positions 
QDot  Nelement vector of generalized velocities 
QDDotDesired  Nelement vector of desired generalized accelerations ( in the above equation) 
CS  Structure that contains information about the set of kinematic constraints. Note that the 'force' vector is appropriately updated after this function is called so that it contains the Lagrange multipliers. 
QDDotOutput  Nelement vector of generalized accelerations which satisfy the kinematic constraints ( in the above equation) 
TauOutput  Nelement vector of generalized forces which satisfy the the equations of motion for this constrained system. 
update_kinematics  whether the kinematics of the model should be updated from Q. 
f_ext  External forces acting on the body in base coordinates (optional, defaults to NULL) 
RBDL_DLLAPI void RigidBodyDynamics::InverseDynamicsConstraintsRelaxed  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
const Math::VectorNd &  QDDotControls,  
ConstraintSet &  CS,  
Math::VectorNd &  QDDotOutput,  
Math::VectorNd &  TauOutput,  
bool  update_kinematics = true , 

std::vector< Math::SpatialVector > *  f_ext = NULL 

) 
A relaxed inversedynamics operator that can be applied to underactuated or fullyactuated constrained multibody systems.
s.t.
In contrast to the RigidBodyDynamics::InverseDynamicsConstraints method, this method can work with underactuated systems. Mathematically this method does not depend on
where is the number of degrees of freedom and is the number of actuated degrees of freedom.
To solve the above constrained minimization problem we take the derivative of the above system of equations w.r.t. and set the result to zero and solve. This results in the KKT system
This system of linear equations is not solved directly, but instead the nullspace formulation presented in Sec. 2.5 of Koch as it is much faster. As with the RigidBodyDynamics::InverseDynamicsConstraints method the matrices and select the actuated and unactuated parts of
, and
, where is a selection matrix that returns the actuated subspace of ( ) and returns the unactuated subspace of ( ).
This system has an upper block triangular structure which can be seen by noting that
by grouping the upper block into
and by grouping the right hand side into
resulting in
This system can be triangularized by projecting the system into the null space of . First we begin with a QR decomposition of into
and projecting into the space
This allows us to express the previous KKT system as
Though this system is still it can be solved in parts for
and
which is enough to yield a solution for
and finally
This method is less computationally expensive than the KKT system directly since
is of size and is of size which is far smaller than the matrix used in the direct method. As it is relatively inexpensive, the dual variables are also evaluated
and
References
model  rigid body model 
Q  Nelement vector of generalized positions 
QDot  Nelement vector of generalized velocities 
QDDotControls  Nelement vector of generalized acceleration controls ( in the above equation). If the idea of a control vector is unclear please read the above text for additional details. 
CS  Structure that contains information about the set of kinematic constraints. Note that the 'force' vector is appropriately updated after this function is called so that it contains the Lagrange multipliers. 
QDDotOutput  Nelement vector of generalized accelerations which satisfy the kinematic constraints ( in the above equation) 
TauOutput  Nelement vector of generalized forces which satisfy the the equations of motion for this constrained system. 
update_kinematics  whether the kinematics of the model should be updated from Q. 
f_ext  External forces acting on the body in base coordinates (optional, defaults to NULL) 

inline 
groupIndex  the index number of this constraint (see getGroupIndex index functions) 
Definition at line 663 of file Constraints.h.
RBDL_DLLAPI bool RigidBodyDynamics::isConstrainedSystemFullyActuated  (  Model &  model, 
const Math::VectorNd &  Q,  
const Math::VectorNd &  QDot,  
ConstraintSet &  CS,  
bool  update_kinematics = true , 

std::vector< Math::SpatialVector > *  f_ext = NULL 

) 
A method to evaluate if the constrained system is fully actuated.
model  rigid body model 
Q  Nelement vector of generalized positions 
QDot  Nelement vector of generalized velocities 
CS  Structure that contains information about the set of kinematic constraints. 
update_kinematics  whether the kinematics of the model should be updated from Q. 
f_ext  External forces acting on the body in base coordinates (optional, defaults to NULL) 
void SetActuationMap  (  const Model &  model, 
const std::vector< bool > &  actuatedDof  
) 
Initializes and allocates memory needed for InverseDynamicsConstraints and InverseDynamicsConstraintsRelaxed.
This function allocates the temporary vectors and matrices needed for the RigidBodyDynamics::InverseDynamicsConstraints and RigidBodyDynamics::InverseDynamicsConstraintsRelaxed methods. In addition, the constant matrices S and P are set here. This function needs to be called once before calling either RigidBodyDynamics::InverseDynamicsConstraints or RigidBodyDynamics::InverseDynamicsConstraintsRelaxed. It does not ever need be called again, unless the actuated degrees of freedom change, or the constraint set changes.
model  rigid body model 
actuatedDof  : a vector that is q_size in length (or dof_count in length) which has a 'true' entry for every generalized degreeof freedom that is driven by an actuator and 'false' for every degreeoffreedom that is not. 

inline 
Specifies which method should be used for solving undelying linear systems.
Definition at line 807 of file Constraints.h.

inline 
Returns the number of constraints.
Definition at line 849 of file Constraints.h.
RBDL_DLLAPI void RigidBodyDynamics::SolveConstrainedSystemDirect  (  Math::MatrixNd &  H, 
const Math::MatrixNd &  G,  
const Math::VectorNd &  c,  
const Math::VectorNd &  gamma,  
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 
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 
linear_solver  type of solver that should be used to solve the system 
RBDL_DLLAPI void RigidBodyDynamics::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 
RBDL_DLLAPI void RigidBodyDynamics::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.
model  rigid body model 
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 
Workspace for the Lagrangian lefthandside matrix.
Definition at line 915 of file Constraints.h.
Workspace for the accelerations of due to the test forces.
Definition at line 963 of file Constraints.h.
Workspace for the Lagrangian righthandside.
Definition at line 917 of file Constraints.h.
bool bound 
Whether the constraint set was bound to a model (mandatory!).
Definition at line 859 of file Constraints.h.
Workspace for the coriolis forces.
Definition at line 908 of file Constraints.h.
ConstraintCache cache 
Definition at line 990 of file Constraints.h.
std::vector< std::shared_ptr<Constraint> > constraints 
Definition at line 883 of file Constraints.h.
std::vector<ConstraintType> constraintType 
Definition at line 862 of file Constraints.h.
std::vector< std::shared_ptr<ContactConstraint> > contactConstraints 
Definition at line 885 of file Constraints.h.
std::vector<Math::SpatialVector> d_a 
Workspace for the acceleration due to the test force.
Definition at line 978 of file Constraints.h.
Math::VectorNd d_d 
Workspace when applying constraint forces.
Definition at line 986 of file Constraints.h.
std::vector<Math::SpatialMatrix> d_IA 
Workspace for the inertia when applying constraint forces.
Definition at line 982 of file Constraints.h.
std::vector<Math::Vector3d> d_multdof3_u 
Definition at line 988 of file Constraints.h.
std::vector<Math::SpatialVector> d_pA 
Workspace for the bias force due to the test force.
Definition at line 976 of file Constraints.h.
Math::VectorNd d_u 
Definition at line 979 of file Constraints.h.
std::vector<Math::SpatialVector> d_U 
Workspace when applying constraint forces.
Definition at line 984 of file Constraints.h.
Math::VectorNd err 
Position error for the Baumgarte stabilization
Definition at line 891 of file Constraints.h.
Math::VectorNd errd 
Velocity error for the Baumgarte stabilization
Definition at line 893 of file Constraints.h.
Definition at line 935 of file Constraints.h.
std::vector<Math::SpatialVector> f_ext_constraints 
Workspace for the actual spatial forces.
Definition at line 971 of file Constraints.h.
std::vector<Math::SpatialVector> f_t 
Workspace for the test forces.
Definition at line 969 of file Constraints.h.
Math::MatrixNd Fll 
Definition at line 936 of file Constraints.h.
Math::MatrixNd Flr 
Definition at line 936 of file Constraints.h.
Math::VectorNd force 
The Lagrange multipliers, which due to some careful normalization in the formulation of constraints are the constraint forces.
Definition at line 897 of file Constraints.h.
Math::MatrixNd Ful 
Definition at line 936 of file Constraints.h.
Math::MatrixNd Fur 
Definition at line 936 of file Constraints.h.
Workspace for the constraint Jacobian.
Definition at line 912 of file Constraints.h.
Definition at line 940 of file Constraints.h.
Math::VectorNd gamma 
Workspace of the right hand side of the acceleration equation.
Definition at line 910 of file Constraints.h.
Math::MatrixNd GPT 
Definition at line 952 of file Constraints.h.
Eigen::FullPivHouseholderQR<Math::MatrixNd> GPT_full_qr 
Definition at line 948 of file Constraints.h.
Definition at line 938 of file Constraints.h.
Eigen::HouseholderQR<Math::MatrixNd> GT_qr 
Workspace for the QR decomposition of the nullspace method.
Definition at line 947 of file Constraints.h.
Math::MatrixNd GT_qr_Q 
Definition at line 951 of file Constraints.h.
Math::MatrixNd GTl 
Definition at line 939 of file Constraints.h.
Math::MatrixNd GTu 
Definition at line 939 of file Constraints.h.
Workspace for the joint space inertia matrix.
Definition at line 906 of file Constraints.h.
std::map< unsigned int, unsigned int> idGroupMap 
Definition at line 868 of file Constraints.h.
Math::VectorNd impulse 
Constraint impulses along the constraint directions.
Definition at line 899 of file Constraints.h.
Workspace for the Inverse ArticulatedBody Inertia.
Definition at line 961 of file Constraints.h.
Math::LinearSolver linear_solver 
Method that should be used to solve internal linear systems.
Definition at line 857 of file Constraints.h.
std::vector< std::shared_ptr<LoopConstraint> > loopConstraints 
Definition at line 887 of file Constraints.h.
std::vector<std::string> name 
Definition at line 864 of file Constraints.h.
std::map< std::string, unsigned int > nameGroupMap 
Definition at line 866 of file Constraints.h.
Selection matrix for the nonactuated parts of the model.
Definition at line 925 of file Constraints.h.
std::vector<Math::Vector3d> point_accel_0 
Workspace for the default point accelerations.
Definition at line 973 of file Constraints.h.
Definition at line 942 of file Constraints.h.
Definition at line 943 of file Constraints.h.
Math::VectorNd QDDot_0 
Workspace for the default accelerations.
Definition at line 967 of file Constraints.h.
Math::VectorNd QDDot_t 
Workspace for the test accelerations.
Definition at line 965 of file Constraints.h.
Math::VectorNd qddot_y 
Definition at line 956 of file Constraints.h.
Math::VectorNd qddot_z 
Definition at line 957 of file Constraints.h.
Definition at line 955 of file Constraints.h.
Definition at line 941 of file Constraints.h.
Selection matrix for the actuated parts of the model needed for the inversedynamicswithconstraints operator
Definition at line 923 of file Constraints.h.
Definition at line 932 of file Constraints.h.
std::map< unsigned int, unsigned int> userDefinedIdGroupMap 
Definition at line 867 of file Constraints.h.
Definition at line 933 of file Constraints.h.
Math::VectorNd v_plus 
The velocities we want to have along the constraint directions.
Definition at line 901 of file Constraints.h.
Matrix that holds the relative cost of deviating from the desired accelerations
Definition at line 928 of file Constraints.h.
Math::MatrixNd Winv 
Definition at line 929 of file Constraints.h.
Math::VectorNd WinvSC 
Definition at line 930 of file Constraints.h.
Workspace for the Lagrangian solution.
Definition at line 919 of file Constraints.h.
Definition at line 953 of file Constraints.h.
Definition at line 954 of file Constraints.h.