Functions
void	RobotDynamics::calcBodyGravityWrench (Model &model, unsigned int body_id, RobotDynamics::Math::SpatialForce &gravity_wrench)
	Calculate the wrench due to gravity on a body. More...

void	RobotDynamics::calcMInvTimesTau (Model &model, const Math::VectorNd &Q, const Math::VectorNd &Tau, Math::VectorNd &QDDot, bool update_kinematics=true)
	Computes the effect of multiplying the inverse of the joint space inertia matrix with a vector in linear time. More...

void	RobotDynamics::compositeRigidBodyAlgorithm (Model &model, const Math::VectorNd &Q, Math::MatrixNd &H, bool update_kinematics=true)
	Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm. More...

void	RobotDynamics::coriolisEffects (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
	Computes the coriolis forces. More...

void	RobotDynamics::forwardDynamics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
	Computes forward dynamics with the Articulated Body Algorithm. More...

void	RobotDynamics::forwardDynamicsLagrangian (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &Tau, Math::VectorNd &QDDot, Math::MatrixNd &H, Math::VectorNd &C, Math::LinearSolver linear_solver=Math::LinearSolverColPivHouseholderQR, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)
	Computes forward dynamics by building and solving the full Lagrangian equation. More...

void	RobotDynamics::gravityEffects (Model &model, Math::VectorNd &Tau)
	Computes the gravity vector. More...

void	RobotDynamics::inverseDynamics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, Math::VectorNd &Tau, Math::SpatialForceV *f_ext=nullptr, bool update_kinematics=true)

void	RobotDynamics::nonlinearEffects (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::VectorNd &Tau, bool update_kinematics=true)
	Computes the coriolis forces. More...

Detailed Description

Function Documentation

void RobotDynamics::calcBodyGravityWrench	(	Model &	model,
		unsigned int	body_id,
		RobotDynamics::Math::SpatialForce &	gravity_wrench
	)

Calculate the wrench due to gravity on a body.

Parameters

model	RDL model
body_id	Wrench will be of this body
gravity_wrench	Wrench due to gravity on body body_id. Modified

Note: This function WILL NOT compute any of the kinematics, so you MUST call either updateKinematics or updateKinematicsCustom prior to calling this function or the result will be wrong.

Definition at line 207 of file Dynamics.cc.

void RobotDynamics::calcMInvTimesTau	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	Tau,
		Math::VectorNd &	QDDot,
		bool	update_kinematics = `true`
	)

Computes the effect of multiplying the inverse of the joint space inertia matrix with a vector in linear time.

Parameters

model	rigid body model
Q	state vector of the generalized positions
Tau	the vector that should be multiplied with the inverse of the joint space inertia matrix
QDDot	vector where the result will be stored
update_kinematics	whether the kinematics should be computed.

Note: If you have already updated kinematics by calling either RobotDynamics::updateKinematics or RobotDynamics::updateKinematicsCustom then you may safely pass update_kinematics=false here which will save you a little bit of computation

This function uses a reduced version of the Articulated Body Algorithm to compute

$\ddot{q} = M(q)^{-1}*\tau$

in $O(n_{\textit{dof}}$ ) time.

Definition at line 596 of file Dynamics.cc.

void RobotDynamics::compositeRigidBodyAlgorithm	(	Model &	model,
		const Math::VectorNd &	Q,
		Math::MatrixNd &	H,
		bool	update_kinematics = `true`
	)

Computes the joint space inertia matrix by using the Composite Rigid Body Algorithm.

This function computes the joint space inertia matrix from a given model and the generalized state vector: $ M(q) $

Parameters

model	rigid body model
Q	state vector of the model
H	a matrix where the result will be stored in
update_kinematics	whether the kinematics should be updated (safer, but at a higher computational cost!)

Note: This function only evaluates the entries of H that are non-zero. One Before calling this function one has to ensure that all other values have been set to zero, e.g. by calling H.setZero().

Definition at line 302 of file Dynamics.cc.

void RobotDynamics::coriolisEffects	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::VectorNd &	Tau,
		bool	update_kinematics = `true`
	)

Computes the coriolis forces.

This function computes the generalized forces from given generalized states, and velocities from coriolis effects $\tau = C(q, \dot{q})$

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 (output)
update_kinematics	whether the kinematics should be updated (safer, but at a higher computational cost!)

Note: If you pass update_kinematics=false, then you must have previously calculated the kinematics from one of the other functions, e.g. updateKinematics or updateKinematicsCustom. If you have not computed kinematics, then passing update_kinematics will give an incorrect result

Definition at line 141 of file Dynamics.cc.

void RobotDynamics::forwardDynamics	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	Tau,
		Math::VectorNd &	QDDot,
		Math::SpatialForceV *	f_ext = `nullptr`,
		bool	update_kinematics = `true`
	)

Computes forward dynamics with the Articulated Body Algorithm.

This function computes the generalized accelerations from given generalized states, velocities and forces: $\ddot{q} = M(q)^{-1} ( -N(q, \dot{q}) + \tau)$ It does this by using the recursive Articulated Body Algorithm that runs in $O(n_{dof})$ with $n_{dof}$ being the number of joints.

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
QDDot	accelerations of the internal joints (output)
f_ext	External forces acting on the body (optional, defaults to NULL)

Definition at line 455 of file Dynamics.cc.

void RobotDynamics::forwardDynamicsLagrangian	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	Tau,
		Math::VectorNd &	QDDot,
		Math::MatrixNd &	H,
		Math::VectorNd &	C,
		Math::LinearSolver	linear_solver = `Math::LinearSolverColPivHouseholderQR`,
		Math::SpatialForceV *	f_ext = `nullptr`,
		bool	update_kinematics = `true`
	)

Computes forward dynamics by building and solving the full Lagrangian equation.

This method builds and solves the linear system

$H \ddot{q} = -C + \tau$

for $\ddot{q}$ where $H$ is the joint space inertia matrix computed with the CompositeRigidBodyAlgorithm(), $C$ the bias force (sometimes called "non-linear effects").

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
QDDot	accelerations of the internal joints (output)
linear_solver	specification which method should be used for solving the linear system
f_ext	External forces acting on the body (optional, defaults to NULL)
H	joint space inertia matrix of size dof_count x dof_count. Modified
C	right hand side vector of size dof_count x 1. Modified
update_kinematics	whether or not to calculate kinematics. Defaults to true which will compute kinematic quatities

Note: If you have already updated kinematics by calling either RobotDynamics::updateKinematics or RobotDynamics::updateKinematicsCustom then you may safely pass update_kinematics=false here which will save you a little bit of computation; It is up to the caller to ensure the H matrix and C vector are appropriately sized before calling this function.

Definition at line 579 of file Dynamics.cc.

void RobotDynamics::gravityEffects	(	Model &	model,
		Math::VectorNd &	Tau
	)

Computes the gravity vector.

This function computes the gravity vector for an RDL model

Parameters

model	rigid body model
Tau	Gravity vector. Modified

Note: This function WILL NOT compute any of the kinematics, so you MUST call either updateKinematics or updateKinematicsCustom prior to calling this function or the result will be wrong.

Definition at line 100 of file Dynamics.cc.

void RobotDynamics::inverseDynamics	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot,
		Math::VectorNd &	Tau,
		Math::SpatialForceV *	f_ext = `nullptr`,
		bool	update_kinematics = `true`
	)

@brief Computes inverse dynamics with the Newton-Euler Algorithm

This function computes the generalized forces from given generalized states, velocities, and accelerations: $\tau = M(q) \ddot{q} + N(q, \dot{q})$

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
QDDot	accelerations of the internals joints
Tau	actuations of the internal joints (output)
f_ext	External forces acting on the body (optional, defaults to nullptr)
update_kinematics	whether or not to calculate kinematics. Defaults to true which will compute kinematic quatities

Note: If you have already updated kinematics by calling either RobotDynamics::updateKinematics or RobotDynamics::updateKinematicsCustom then you may safely pass update_kinematics=false here which will save you a little bit of computation

Definition at line 23 of file Dynamics.cc.

void RobotDynamics::nonlinearEffects	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::VectorNd &	Tau,
		bool	update_kinematics = `true`
	)

Computes the coriolis forces.

This function computes the generalized forces from given generalized states, velocities, and accelerations: $\tau = M(q) \ddot{q} + N(q, \dot{q})$

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 (output)
update_kinematics	whether or not to calculate kinematics. Defaults to true which will compute kinematic quatities

Note: If you have already updated kinematics by calling either RobotDynamics::updateKinematics or RobotDynamics::updateKinematicsCustom then you may safely pass update_kinematics=false here which will save you a little bit of computation

Definition at line 236 of file Dynamics.cc.

Functions

Detailed Description

Function Documentation