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

Functions
void	RobotDynamics::calcContactJacobian (Model &model, const Math::VectorNd &Q, const ConstraintSet &CS, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the Jacobian for the given ConstraintSet. More...

void	RobotDynamics::calcContactSystemVariables (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS)

void	RobotDynamics::computeContactImpulsesDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
	Computes contact gain by constructing and solving the full lagrangian equation. More...

void	RobotDynamics::computeContactImpulsesNullSpace (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
	Resolves contact gain using solveContactSystemNullSpace() More...

void	RobotDynamics::computeContactImpulsesRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDotMinus, ConstraintSet &CS, Math::VectorNd &QDotPlus)
	Resolves contact gain using solveContactSystemRangeSpaceSparse() More...

void	RobotDynamics::forwardDynamicsContactsDirect (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)
	Computes forward dynamics with contact by constructing and solving the full lagrangian equation. More...

void	RobotDynamics::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...

void	RobotDynamics::forwardDynamicsContactsNullSpace (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)

void	RobotDynamics::forwardDynamicsContactsRangeSpaceSparse (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, ConstraintSet &CS, Math::VectorNd &QDDot)

void	RobotDynamics::solveContactSystemDirect (Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &A, Math::VectorNd &b, Math::VectorNd &x, Math::LinearSolver &linear_solver)
	Solves the full contact system directly, i.e. simultaneously for contact forces and joint accelerations. More...

void	RobotDynamics::solveContactSystemNullSpace (Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &Y, Math::MatrixNd &Z, Math::VectorNd &qddot_y, Math::VectorNd &qddot_z, Math::LinearSolver &linear_solver)
	Solves the contact system by first solving for the joint accelerations and then for the constraint forces. More...

void	RobotDynamics::solveContactSystemRangeSpaceSparse (Model &model, Math::MatrixNd &H, const Math::MatrixNd &G, const Math::VectorNd &c, const Math::VectorNd &gamma, Math::VectorNd &qddot, Math::VectorNd &lambda, Math::MatrixNd &K, Math::VectorNd &a, Math::LinearSolver linear_solver)
	Solves the contact system by first solving for the the joint accelerations and then the contact forces using a sparse matrix decomposition of the joint space inertia matrix. More...

Detailed Description

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

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

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

Solution of the Constraint System

Linear System of the Constrained Dynamics

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

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

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

Linear System of the Contact Collision

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

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

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

Solution Methods

There are essentially three different approaches to solve these systems:

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

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

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

Methods for Solving Constrained Dynamics

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

ForwardDynamicsContactsDirect()
ForwardDynamicsContactsRangeSpaceSparse()
ForwardDynamicsContactsNullSpace()

Methods for Computing Collisions

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

ComputeContactImpulsesDirect()
ComputeContactImpulsesRangeSpaceSparse()
ComputeContactImpulsesNullSpace()

Function Documentation

void RobotDynamics::calcContactJacobian	(	Model &	model,
		const Math::VectorNd &	Q,
		const ConstraintSet &	CS,
		Math::MatrixNd &	G,
		bool	update_kinematics = `true`
	)

Computes the Jacobian for the given ConstraintSet.

Parameters

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 275 of file Contacts.cc.

void RobotDynamics::calcContactSystemVariables	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	Tau,
		ConstraintSet &	CS
	)

Definition at line 308 of file Contacts.cc.

void RobotDynamics::computeContactImpulsesDirect	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDotMinus,
		ConstraintSet &	CS,
		Math::VectorNd &	QDotPlus
	)

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

This method builds and solves the linear system

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

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

Note: So far, only constraints acting along cartesian coordinate axes are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must not specify redundant constraints!

Note: To increase performance group constraints body and pointwise such that constraints acting on the same body point are sequentially in ConstraintSet. This can save computation of point Jacobians .

Parameters

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 383 of file Contacts.cc.

void RobotDynamics::computeContactImpulsesNullSpace	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDotMinus,
		ConstraintSet &	CS,
		Math::VectorNd &	QDotPlus
	)

Resolves contact gain using solveContactSystemNullSpace()

Definition at line 419 of file Contacts.cc.

void RobotDynamics::computeContactImpulsesRangeSpaceSparse	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDotMinus,
		ConstraintSet &	CS,
		Math::VectorNd &	QDotPlus
	)

Resolves contact gain using solveContactSystemRangeSpaceSparse()

Definition at line 407 of file Contacts.cc.

void RobotDynamics::forwardDynamicsContactsDirect	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	Tau,
		ConstraintSet &	CS,
		Math::VectorNd &	QDDot
	)

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

This method builds and solves the linear system

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

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

Note: So far, only constraints acting along cartesian coordinate axes are allowed (i.e. (1, 0, 0), (0, 1, 0), and (0, 0, 1)). Also, one must not specify redundant constraints!

Note: To increase performance group constraints body and pointwise such that constraints acting on the same body point are sequentially in ConstraintSet. This can save computation of point jacobians .

Parameters

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)

Note: During execution of this function values such as ConstraintSet::force get modified and will contain the value of the force acting along the normal.

Definition at line 343 of file Contacts.cc.

void RobotDynamics::forwardDynamicsContactsKokkevis	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	Tau,
		ConstraintSet &	CS,
		Math::VectorNd &	QDDot
	)

Computes forward dynamics that accounts for active contacts in ConstraintSet.

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

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

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

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

Parameters

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)

Note: During execution of this function values such as ConstraintSet::force get modified and will contain the value of the force acting along the normal.

Todo:: Allow for external forces

Definition at line 682 of file Contacts.cc.

void RobotDynamics::forwardDynamicsContactsNullSpace	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	Tau,
		ConstraintSet &	CS,
		Math::VectorNd &	QDDot
	)

Definition at line 370 of file Contacts.cc.

void RobotDynamics::forwardDynamicsContactsRangeSpaceSparse	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	Tau,
		ConstraintSet &	CS,
		Math::VectorNd &	QDDot
	)

Definition at line 362 of file Contacts.cc.

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

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

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

Parameters

H	the joint space inertia matrix
G	the constraint jacobian
c	the $\mathbb{R}^{n_\textit{dof}}$ vector of the upper part of the right hand side of the system
gamma	the $\mathbb{R}^{n_c}$ vector of the lower part of the right hand side of the system
qddot	result: joint accelerations
lambda	result: constraint forces
A	work-space for the matrix of the linear system
b	work-space for the right-hand-side of the linear system
x	work-space for the solution of the linear system
linear_solver	of solver that should be used to solve the system

Definition at line 175 of file Contacts.cc.

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

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

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

Parameters

H	the joint space inertia matrix
G	the constraint jacobian
c	the $\mathbb{R}^{n_\textit{dof}}$ vector of the upper part of the right hand side of the system
gamma	the $\mathbb{R}^{n_c}$ 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 range-space of the constraints
Z	basis for the null-space of the constraints
qddot_y	work-space of size $\mathbb{R}^{n_\textit{dof}}$
qddot_z	work-space of size $\mathbb{R}^{n_\textit{dof}}$
linear_solver	type of solver that should be used to solve the system

Definition at line 234 of file Contacts.cc.

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

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

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

Parameters

model
H	the joint space inertia matrix
G	the constraint jacobian
c	the $\mathbb{R}^{n_\textit{dof}}$ vector of the upper part of the right hand side of the system
gamma	the $\mathbb{R}^{n_c}$ vector of the lower part of the right hand side of the system
qddot	result: joint accelerations
lambda	result: constraint forces
K	work-space for the matrix of the constraint force linear system
a	work-space for the right-hand-side of the constraint force linear system
linear_solver	type of solver that should be used to solve the constraint force system

Definition at line 206 of file Contacts.cc.

Classes

Functions

Detailed Description

Solution of the Constraint System

Linear System of the Constrained Dynamics

Linear System of the Contact Collision

Solution Methods

Methods for Solving Constrained Dynamics

Methods for Computing Collisions

Function Documentation