Rigid Body Dynamics Library
ConstraintSet Struct Reference

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

Public Types

enum  ConstraintType {
  ContactConstraint, LoopConstraint, ConstraintTypeCustom,
  ConstraintTypeLast
}
 

Public Member Functions

 ConstraintSet ()
 
unsigned int AddContactConstraint (unsigned int body_id, const Math::Vector3d &body_point, const Math::Vector3d &world_normal, const char *constraint_name=NULL, double normal_acceleration=0.)
 Adds a contact constraint to the constraint set. More...
 
unsigned int AddLoopConstraint (unsigned int id_predecessor, unsigned int id_successor, const Math::SpatialTransform &X_predecessor, const Math::SpatialTransform &X_successor, const Math::SpatialVector &axis, bool enable_stabilization=false, const double stabilization_param=0.1, const char *constraint_name=NULL)
 Adds a loop constraint to the constraint set. More...
 
unsigned int AddCustomConstraint (CustomConstraint *custom_constraint, unsigned int id_predecessor, unsigned int id_successor, const Math::SpatialTransform &X_predecessor, const Math::SpatialTransform &X_successor, bool enable_stabilization=false, const double stabilization_param=0.1, const char *constraint_name=NULL)
 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...
 
size_t size () const
 Returns the number of constraints. More...
 
void clear ()
 Clears all variables in the constraint set. More...
 

Data Fields

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< ConstraintTypeconstraintType
 
std::vector< std::string > name
 
std::vector< unsigned int > mContactConstraintIndices
 
std::vector< unsigned int > mLoopConstraintIndices
 
std::vector< unsigned int > mCustomConstraintIndices
 
std::vector< CustomConstraint *> mCustomConstraints
 
std::vector< unsigned int > body
 
std::vector< Math::Vector3dpoint
 
std::vector< Math::Vector3dnormal
 
std::vector< unsigned int > body_p
 
std::vector< unsigned int > body_s
 
std::vector< Math::SpatialTransformX_p
 
std::vector< Math::SpatialTransformX_s
 
std::vector< Math::SpatialVectorconstraintAxis
 
std::vector< Math::Vector2dbaumgarteParameters
 
Math::VectorNd err
 
Math::VectorNd errd
 
Math::VectorNd acceleration
 
Math::VectorNd force
 
Math::VectorNd impulse
 
Math::VectorNd v_plus
 
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 lower part of b. More...
 
Math::MatrixNd G
 
Math::MatrixNd A
 Workspace for the Lagrangian left-hand-side matrix. More...
 
Math::VectorNd b
 Workspace for the Lagrangian right-hand-side. More...
 
Math::VectorNd x
 Workspace for the Lagrangian solution. More...
 
Math::MatrixNd Gi
 Workspace when evaluating contact Jacobians. More...
 
Math::MatrixNd GSpi
 Workspace when evaluating loop/CustomConstraint Jacobians. More...
 
Math::MatrixNd GSsi
 Workspace when evaluating loop/CustomConstraint Jacobians. More...
 
Math::MatrixNd GSJ
 Workspace when evaluating loop/CustomConstraint Jacobians. More...
 
Eigen::HouseholderQR< Math::MatrixNdGT_qr
 Workspace for the QR decomposition of the null-space method. More...
 
Math::MatrixNd GT_qr_Q
 
Math::MatrixNd Y
 
Math::MatrixNd Z
 
Math::VectorNd qddot_y
 
Math::VectorNd qddot_z
 
Math::MatrixNd K
 Workspace for the Inverse Articulated-Body 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::SpatialVectorf_t
 Workspace for the test forces. More...
 
std::vector< Math::SpatialVectorf_ext_constraints
 Workspace for the actual spatial forces. More...
 
std::vector< Math::Vector3dpoint_accel_0
 Workspace for the default point accelerations. More...
 
std::vector< Math::SpatialVectord_pA
 Workspace for the bias force due to the test force. More...
 
std::vector< Math::SpatialVectord_a
 Workspace for the acceleration due to the test force. More...
 
Math::VectorNd d_u
 
std::vector< Math::SpatialMatrixd_IA
 Workspace for the inertia when applying constraint forces. More...
 
std::vector< Math::SpatialVectord_U
 Workspace when applying constraint forces. More...
 
Math::VectorNd d_d
 Workspace when applying constraint forces. More...
 
std::vector< Math::Vector3dd_multdof3_u
 

Detailed Description

Structure that contains both constraint information and workspace memory.

This structure is used to reduce the amount of memory allocations that are needed when computing constraint forces.

The ConstraintSet has to be bound to a model using ConstraintSet::Bind() before it can be used in ForwardDynamicsContacts .

Definition at line 266 of file Constraints.h.

Member Enumeration Documentation

◆ ConstraintType

Enumerator
ContactConstraint 
LoopConstraint 
ConstraintTypeCustom 
ConstraintTypeLast 

Definition at line 272 of file Constraints.h.

Constructor & Destructor Documentation

◆ ConstraintSet()

ConstraintSet ( )
inline

Definition at line 267 of file Constraints.h.

Member Function Documentation

◆ AddContactConstraint()

unsigned int AddContactConstraint ( unsigned int  body_id,
const Math::Vector3d body_point,
const Math::Vector3d world_normal,
const char *  constraint_name = NULL,
double  normal_acceleration = 0. 
)

Adds a contact constraint 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. This constraint does not act at the position level.

Parameters
body_idthe body which is affected directly by the constraint
body_pointthe point that is constrained relative to the contact body
world_normalthe normal along the constraint acts (in base coordinates)
constraint_namea human readable name (optional, default: NULL)
normal_accelerationthe acceleration of the contact along the normal (optional, default: 0.)

Definition at line 36 of file Constraints.cc.

◆ AddCustomConstraint()

unsigned int AddCustomConstraint ( CustomConstraint custom_constraint,
unsigned int  id_predecessor,
unsigned int  id_successor,
const Math::SpatialTransform X_predecessor,
const Math::SpatialTransform X_successor,
bool  enable_stabilization = false,
const double  stabilization_param = 0.1,
const char *  constraint_name = NULL 
)

Adds a custom constraint to the constraint set.

Parameters
custom_constraintThe CustomConstraint to be added to the ConstraintSet.
id_predecessorthe identifier of the predecessor body
id_successorthe identifier of the successor body
X_predecessora spatial transform localizing the constrained frames on the predecessor body, expressed with respect to the predecessor body frame
X_successora spatial transform localizing the constrained frames on the successor body, expressed with respect to the successor body frame
axisa spatial vector indicating the axis along which the constraint acts
enable_stabilizationWhether Baumgarte Stabilization should be enabled or not.
stabilization_paramThe value for $T_\textit{stab}$ used for the Baumgarte Stabilization (defaults to 0.1).
constraint_namea human readable name (optional, default: NULL)

Definition at line 160 of file Constraints.cc.

References CustomConstraint::mConstraintCount.

◆ AddLoopConstraint()

unsigned int AddLoopConstraint ( unsigned int  id_predecessor,
unsigned int  id_successor,
const Math::SpatialTransform X_predecessor,
const Math::SpatialTransform X_successor,
const Math::SpatialVector axis,
bool  enable_stabilization = false,
const double  stabilization_param = 0.1,
const char *  constraint_name = NULL 
)

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.

Parameters
id_predecessorthe identifier of the predecessor body
id_successorthe identifier of the successor body
X_predecessora spatial transform localizing the constrained frames on the predecessor body, expressed with respect to the predecessor body frame
X_successora spatial transform localizing the constrained frames on the successor body, expressed with respect to the successor body frame
axisa spatial vector indicating the axis along which the constraint acts
enable_stabilizationWhether Baumgarte Stabilization should be enabled or not.
stabilization_paramThe value for $T_\textit{stab}$ used for the Baumgarte Stabilization (defaults to 0.1).
constraint_namea human readable name (optional, default: NULL)

Definition at line 90 of file Constraints.cc.

◆ Bind()

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.

Definition at line 241 of file Constraints.cc.

References Model::dof_count, Model::mBodies, and Model::qdot_size.

◆ clear()

void clear ( )

Clears all variables in the constraint set.

Definition at line 311 of file Constraints.cc.

◆ Copy()

ConstraintSet Copy ( )
inline

Copies the constraints and resets its ConstraintSet::bound flag.

Definition at line 371 of file Constraints.h.

References ConstraintSet::bound.

◆ SetSolver()

void SetSolver ( Math::LinearSolver  solver)
inline

Specifies which method should be used for solving undelying linear systems.

Definition at line 380 of file Constraints.h.

◆ size()

size_t size ( ) const
inline

Returns the number of constraints.

Definition at line 399 of file Constraints.h.

Field Documentation

◆ A

Workspace for the Lagrangian left-hand-side matrix.

Definition at line 457 of file Constraints.h.

◆ a

Workspace for the accelerations of due to the test forces.

Definition at line 490 of file Constraints.h.

◆ acceleration

Math::VectorNd acceleration

Enforced accelerations of the contact points along the contact normal.

Definition at line 439 of file Constraints.h.

◆ b

Workspace for the Lagrangian right-hand-side.

Definition at line 459 of file Constraints.h.

◆ baumgarteParameters

std::vector<Math::Vector2d> baumgarteParameters

Baumgarte stabilization parameter

Definition at line 431 of file Constraints.h.

◆ body

std::vector<unsigned int> body

Definition at line 420 of file Constraints.h.

◆ body_p

std::vector<unsigned int> body_p

Definition at line 425 of file Constraints.h.

◆ body_s

std::vector<unsigned int> body_s

Definition at line 426 of file Constraints.h.

◆ bound

bool bound

Whether the constraint set was bound to a model (mandatory!).

Definition at line 409 of file Constraints.h.

◆ C

Workspace for the coriolis forces.

Definition at line 452 of file Constraints.h.

◆ constraintAxis

std::vector<Math::SpatialVector> constraintAxis

Definition at line 429 of file Constraints.h.

◆ constraintType

std::vector<ConstraintType> constraintType

Definition at line 412 of file Constraints.h.

◆ d_a

std::vector<Math::SpatialVector> d_a

Workspace for the acceleration due to the test force.

Definition at line 505 of file Constraints.h.

◆ d_d

Workspace when applying constraint forces.

Definition at line 513 of file Constraints.h.

◆ d_IA

std::vector<Math::SpatialMatrix> d_IA

Workspace for the inertia when applying constraint forces.

Definition at line 509 of file Constraints.h.

◆ d_multdof3_u

std::vector<Math::Vector3d> d_multdof3_u

Definition at line 515 of file Constraints.h.

◆ d_pA

std::vector<Math::SpatialVector> d_pA

Workspace for the bias force due to the test force.

Definition at line 503 of file Constraints.h.

◆ d_u

Definition at line 506 of file Constraints.h.

◆ d_U

std::vector<Math::SpatialVector> d_U

Workspace when applying constraint forces.

Definition at line 511 of file Constraints.h.

◆ err

Position error for the Baumgarte stabilization

Definition at line 433 of file Constraints.h.

◆ errd

Velocity error for the Baumgarte stabilization

Definition at line 435 of file Constraints.h.

◆ f_ext_constraints

std::vector<Math::SpatialVector> f_ext_constraints

Workspace for the actual spatial forces.

Definition at line 498 of file Constraints.h.

◆ f_t

std::vector<Math::SpatialVector> f_t

Workspace for the test forces.

Definition at line 496 of file Constraints.h.

◆ force

Actual constraint forces along the contact normals.

Definition at line 441 of file Constraints.h.

◆ G

Definition at line 455 of file Constraints.h.

◆ gamma

Workspace of the lower part of b.

Definition at line 454 of file Constraints.h.

◆ Gi

Workspace when evaluating contact Jacobians.

Definition at line 464 of file Constraints.h.

◆ GSJ

Workspace when evaluating loop/CustomConstraint Jacobians.

Definition at line 470 of file Constraints.h.

◆ GSpi

Workspace when evaluating loop/CustomConstraint Jacobians.

Definition at line 466 of file Constraints.h.

◆ GSsi

Workspace when evaluating loop/CustomConstraint Jacobians.

Definition at line 468 of file Constraints.h.

◆ GT_qr

Eigen::HouseholderQR<Math::MatrixNd> GT_qr

Workspace for the QR decomposition of the null-space method.

Definition at line 476 of file Constraints.h.

◆ GT_qr_Q

Math::MatrixNd GT_qr_Q

Definition at line 479 of file Constraints.h.

◆ H

Workspace for the joint space inertia matrix.

Definition at line 450 of file Constraints.h.

◆ impulse

Math::VectorNd impulse

Actual constraint impulses along the contact normals.

Definition at line 443 of file Constraints.h.

◆ K

Workspace for the Inverse Articulated-Body Inertia.

Definition at line 488 of file Constraints.h.

◆ linear_solver

Math::LinearSolver linear_solver

Method that should be used to solve internal linear systems.

Definition at line 407 of file Constraints.h.

◆ mContactConstraintIndices

std::vector<unsigned int> mContactConstraintIndices

Definition at line 414 of file Constraints.h.

◆ mCustomConstraintIndices

std::vector<unsigned int> mCustomConstraintIndices

Definition at line 416 of file Constraints.h.

◆ mCustomConstraints

std::vector< CustomConstraint* > mCustomConstraints

Definition at line 417 of file Constraints.h.

◆ mLoopConstraintIndices

std::vector<unsigned int> mLoopConstraintIndices

Definition at line 415 of file Constraints.h.

◆ name

std::vector<std::string> name

Definition at line 413 of file Constraints.h.

◆ normal

std::vector<Math::Vector3d> normal

Definition at line 422 of file Constraints.h.

◆ point

std::vector<Math::Vector3d> point

Definition at line 421 of file Constraints.h.

◆ point_accel_0

std::vector<Math::Vector3d> point_accel_0

Workspace for the default point accelerations.

Definition at line 500 of file Constraints.h.

◆ QDDot_0

Math::VectorNd QDDot_0

Workspace for the default accelerations.

Definition at line 494 of file Constraints.h.

◆ QDDot_t

Math::VectorNd QDDot_t

Workspace for the test accelerations.

Definition at line 492 of file Constraints.h.

◆ qddot_y

Math::VectorNd qddot_y

Definition at line 482 of file Constraints.h.

◆ qddot_z

Math::VectorNd qddot_z

Definition at line 483 of file Constraints.h.

◆ v_plus

The velocities we want to have along the contact normals

Definition at line 445 of file Constraints.h.

◆ x

Workspace for the Lagrangian solution.

Definition at line 461 of file Constraints.h.

◆ X_p

std::vector<Math::SpatialTransform> X_p

Definition at line 427 of file Constraints.h.

◆ X_s

std::vector<Math::SpatialTransform> X_s

Definition at line 428 of file Constraints.h.

◆ Y

Definition at line 480 of file Constraints.h.

◆ Z

Definition at line 481 of file Constraints.h.


The documentation for this struct was generated from the following files: