Namespaces
	RobotDynamics::Utils
	Namespace that contains optional helper functions.

Functions
void	RobotDynamics::Utils::calcCenterOfMass (Model &model, const Math::VectorNd &q, Math::Vector3d &com, bool update_kinematics=true)
	Computes the Center of Mass (COM) position. More...

void	RobotDynamics::Utils::calcCenterOfMass (Model &model, const Math::VectorNd &q, Math::FramePoint &com, bool update_kinematics=true)
	Computes the Center of Mass (COM) position. More...

void	RobotDynamics::Utils::calcCenterOfMass (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, double &mass, Math::Vector3d &com, Math::Vector3d com_velocity=NULL, Math::Vector3d angular_momentum=NULL, bool update_kinematics=true)
	Computes the Center of Mass (COM) and optionally its linear velocity. More...

void	RobotDynamics::Utils::calcCenterOfMassJacobian (Model &model, const Math::VectorNd &q, Math::MatrixNd &jCom, bool update_kinematics=true)
	Computes the matrix $J_{com}$ such that $v_{com} = J_{com} \dot{q}$ . More...

void	RobotDynamics::Utils::calcCenterOfMassVelocity (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::Vector3d &com_vel, bool update_kinematics=true)
	Computes the Center of Mass (COM) velocity in world frame. More...

void	RobotDynamics::Utils::calcCenterOfMassVelocity (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::FrameVector &com_vel, bool update_kinematics=true)
	Computes the Center of Mass (COM) velocity in world frame. More...

void	RobotDynamics::Utils::calcCenterOfMassVelocity (Model &model, Math::Vector3d &com_vel)
	Computes the Center of Mass (COM) velocity in world frame. More...

void	RobotDynamics::Utils::calcCenterOfMassVelocity (Model &model, Math::FrameVector &com_vel)
	Computes the Center of Mass (COM) velocity in world frame. More...

void	RobotDynamics::Utils::calcCentroidalMomentumMatrix (Model &model, const Math::VectorNd &q, Math::MatrixNd &A, bool update_kinematics=true)
	Calculates the centroidal momentum matrix, for a model. The centroidal momentum matrix is a $6 \times N$ matrix such that the 6dof centroidal momentum vector is computed by, $h = A(q) \dot{q}$ . More...

void	RobotDynamics::Utils::calcCentroidalMomentumMatrixDot (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, Math::MatrixNd &Adot, bool update_kinematics=true)
	Calculates the time derivative of the centroidal momentum matrix, i.e. the matrix computed by RobotDynamics::Utils::calcCentroidalMomentumMatrix and stores it in the Adot argument. More...

Math::SpatialForce	RobotDynamics::Utils::calcGravityWrenchOnCenterOfMass (Model &model, const Math::VectorNd &q, bool update_kinematics=true)
	Computes the gravitational wrench experienced on the robots center of mass. More...

double	RobotDynamics::Utils::calcKineticEnergy (Model &model, const Math::VectorNd &q, const Math::VectorNd &qdot, bool update_kinematics=true)
	Computes the kinetic energy of the full model. More...

double	RobotDynamics::Utils::calcPotentialEnergy (Model &model, const Math::VectorNd &q, bool update_kinematics=true)
	Computes the potential energy of the full model. More...

Math::Vector3d	RobotDynamics::Utils::calcSubtreeCenterOfMassScaledByMass (Model &model, const unsigned int bodyId, const Math::VectorNd &q, bool updateKinematics=true)
	Calculates the center of mass of a subtree starting with the body with ID bodyId and scales it by the total mass of the subtree. More...

double	RobotDynamics::Utils::calcSubtreeMass (Model &model, const unsigned int bodyId)
	Calculates the total mass of the subtree beginning with body bodyId and traversing outwards from there. More...

std::string	RobotDynamics::Utils::getBodyName (const RobotDynamics::Model &model, unsigned int body_id)
	get body name, returns empty string if bodyid is virtual and has multiple child bodies More...

std::string	RobotDynamics::Utils::getDofName (const Math::SpatialVector &joint_dof)
	get string abbreviation for dof name from spatial vector. More...

std::string	RobotDynamics::Utils::getModelDOFOverview (const Model &model)
	Creates a human readable overview of the Degrees of Freedom. More...

std::string	RobotDynamics::Utils::getModelHierarchy (const Model &model)
	Creates a human readable overview of the model. More...

std::string	RobotDynamics::Utils::getNamedBodyOriginsOverview (Model &model)
	Creates a human readable overview of the locations of all bodies that have names. More...

Detailed Description

Utility functions are those not necessarily required for kinematics/dynamics, but provide utility by giving model information, calculating useful quantities such as the position/velocity of the center of mass, etc.

Function Documentation

void RobotDynamics::Utils::calcCenterOfMass	(	Model &	model,
		const Math::VectorNd &	q,
		Math::Vector3d &	com,
		bool	update_kinematics = `true`
	)

Computes the Center of Mass (COM) position.

Parameters

model	The model for which we want to compute the COM
q	The current joint positions
com	(output) location of the Center of Mass of the model in world frame
update_kinematics	(optional input) whether the kinematics should be updated (defaults to true)

Note: Updates the transforms of Model::comFrame

Definition at line 202 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCenterOfMass	(	Model &	model,
		const Math::VectorNd &	q,
		Math::FramePoint &	com,
		bool	update_kinematics = `true`
	)

Computes the Center of Mass (COM) position.

Parameters

model	The model for which we want to compute the COM
q	The current joint positions
com	(output) location of the Center of Mass of the model in world frame
update_kinematics	(optional input) whether the kinematics should be updated (defaults to true)

Definition at line 233 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCenterOfMass	(	Model &	model,
		const Math::VectorNd &	q,
		const Math::VectorNd &	qdot,
		double &	mass,
		Math::Vector3d &	com,
		Math::Vector3d *	com_velocity = `NULL`,
		Math::Vector3d *	angular_momentum = `NULL`,
		bool	update_kinematics = `true`
	)

Computes the Center of Mass (COM) and optionally its linear velocity.

When only interested in computing the location of the COM you can use nullptr as value for com_velocity.

Parameters

model	The model for which we want to compute the COM
q	The current joint positions
qdot	The current joint velocities
mass	(output) total mass of the model
com	(output) location of the Center of Mass of the model in world frame
com_velocity	(optional output) linear velocity of the COM in world frame
angular_momentum	(optional output) angular momentum of the model at the COM in world frame
update_kinematics	(optional input) whether the kinematics should be updated (defaults to true)

Definition at line 262 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCenterOfMassJacobian	(	Model &	model,
		const Math::VectorNd &	q,
		Math::MatrixNd &	jCom,
		bool	update_kinematics = `true`
	)

Computes the matrix $J_{com}$ such that $v_{com} = J_{com} \dot{q}$ .

Parameters

model	The model for which the COM jacobian will be computed for
q	The current joint positions
jCom	A 3 x model.qdot_size matrix where the jacobian will be stored.
update_kinematics	If true, kinematic variables will be computed. Default = true.

Definition at line 464 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCenterOfMassVelocity	(	Model &	model,
		const Math::VectorNd &	q,
		const Math::VectorNd &	qdot,
		Math::Vector3d &	com_vel,
		bool	update_kinematics = `true`
	)

Computes the Center of Mass (COM) velocity in world frame.

Parameters

model	The model for which we want to compute the COM velocity
q	The current joint positions
qdot	The current joint velocities
com_vel	(modified) translational velocity of the Center of Mass of the model in world frame
update_kinematics	(optional input) whether the kinematics should be updated (defaults to true)

Definition at line 420 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCenterOfMassVelocity	(	Model &	model,
		const Math::VectorNd &	q,
		const Math::VectorNd &	qdot,
		Math::FrameVector &	com_vel,
		bool	update_kinematics = `true`
	)

Computes the Center of Mass (COM) velocity in world frame.

Parameters

model	The model for which we want to compute the COM velocity
q	The current joint positions
qdot	The current joint velocities
com_vel	(modified) Frame vector where the result vector and frame will be stored
update_kinematics	(optional input) whether the kinematics should be updated (defaults to true)

Definition at line 430 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCenterOfMassVelocity	(	Model &	model,
		Math::Vector3d &	com_vel
	)

Computes the Center of Mass (COM) velocity in world frame.

Parameters

model	The model for which we want to compute the COM velocity
com_vel	(modified) Vector3d where the result vector and frame will be stored

Definition at line 387 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCenterOfMassVelocity	(	Model &	model,
		Math::FrameVector &	com_vel
	)

Computes the Center of Mass (COM) velocity in world frame.

Parameters

model	The model for which we want to compute the COM velocity
com_vel	(modified) Frame vector where the result vector and frame will be stored

Definition at line 413 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCentroidalMomentumMatrix	(	Model &	model,
		const Math::VectorNd &	q,
		Math::MatrixNd &	A,
		bool	update_kinematics = `true`
	)

Calculates the centroidal momentum matrix, $ A(q) $ for a model. The centroidal momentum matrix is a $6 \times N$ matrix such that the 6dof centroidal momentum vector is computed by,

$h = A(q) \dot{q}$

.

Note: It is crucial that the matrix be all zeros when this function is called, as the elements will be added to. To be sure, call A.setZero() before calling this function.

Parameters

model	RDL model
q	Vector of joint positions
A	6 by N matrix where the result will be stored
update_kinematics	If true, calculates kinematic parameters

Definition at line 560 of file rdl_utils.cc.

void RobotDynamics::Utils::calcCentroidalMomentumMatrixDot	(	Model &	model,
		const Math::VectorNd &	q,
		const Math::VectorNd &	qdot,
		Math::MatrixNd &	Adot,
		bool	update_kinematics = `true`
	)

Calculates the time derivative of the centroidal momentum matrix, i.e. the matrix computed by RobotDynamics::Utils::calcCentroidalMomentumMatrix and stores it in the Adot argument.

Parameters

model
q
qdot
Adot
update_kinematics

Definition at line 613 of file rdl_utils.cc.

Math::SpatialForce RobotDynamics::Utils::calcGravityWrenchOnCenterOfMass	(	Model &	model,
		const Math::VectorNd &	q,
		bool	update_kinematics = `true`
	)

Computes the gravitational wrench experienced on the robots center of mass.

Parameters

model
q
update_kinematics	(optional) Defaults to true

Note: The reference frame the resulting force is expressed in is the center of mass reference frame owned by the RobotDynamics::Model object. This means that you must first have the position/orientation of that reference frame updated prior to calling this function otherwise the frame it's expressed in is likely to not be what you want.

Definition at line 440 of file rdl_utils.cc.

double RobotDynamics::Utils::calcKineticEnergy	(	Model &	model,
		const Math::VectorNd &	q,
		const Math::VectorNd &	qdot,
		bool	update_kinematics = `true`
	)

Computes the kinetic energy of the full model.

Parameters

model
q
qdot
update_kinematics	(optional) Defaults to true

Returns: Kinetic energy

Definition at line 544 of file rdl_utils.cc.

double RobotDynamics::Utils::calcPotentialEnergy	(	Model &	model,
		const Math::VectorNd &	q,
		bool	update_kinematics = `true`
	)

Computes the potential energy of the full model.

Parameters

model
q
update_kinematics	(optional) Defaults to true

Returns: Potential energy

Definition at line 533 of file rdl_utils.cc.

Math::Vector3d RobotDynamics::Utils::calcSubtreeCenterOfMassScaledByMass	(	Model &	model,
		const unsigned int	bodyId,
		const Math::VectorNd &	q,
		bool	updateKinematics = `true`
	)

Calculates the center of mass of a subtree starting with the body with ID bodyId and scales it by the total mass of the subtree.

Parameters

model	The model used for the calculation
bodyId	The ID of the first body in the desired subtree
q	The current joint positions
updateKinematics	If true, kinematic variables will be computed. Default = true

Definition at line 500 of file rdl_utils.cc.

double RobotDynamics::Utils::calcSubtreeMass	(	Model &	model,
		const unsigned int	bodyId
	)

Calculates the total mass of the subtree beginning with body bodyId and traversing outwards from there.

Parameters

model	The model used for the calculation
bodyId	The ID of the first body in the desired subtree

Returns: Mass of subtree

Definition at line 519 of file rdl_utils.cc.

string RobotDynamics::Utils::getBodyName	(	const RobotDynamics::Model &	model,
		unsigned int	body_id
	)

get body name, returns empty string if bodyid is virtual and has multiple child bodies

Definition at line 59 of file rdl_utils.cc.

string RobotDynamics::Utils::getDofName ( const Math::SpatialVector & joint_dof )

get string abbreviation for dof name from spatial vector.

Definition at line 27 of file rdl_utils.cc.

std::string RobotDynamics::Utils::getModelDOFOverview ( const Model & model )

Creates a human readable overview of the Degrees of Freedom.

Definition at line 75 of file rdl_utils.cc.

std::string RobotDynamics::Utils::getModelHierarchy ( const Model & model )

Creates a human readable overview of the model.

Definition at line 169 of file rdl_utils.cc.

std::string RobotDynamics::Utils::getNamedBodyOriginsOverview ( Model & model )

Creates a human readable overview of the locations of all bodies that have names.

Definition at line 178 of file rdl_utils.cc.

Namespaces

Functions

Detailed Description

Function Documentation