Contains all information about the rigid body model. More...

#include <Model.h>

Public Member Functions
unsigned int	addBody (const unsigned int parent_id, const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
	Connects a given body to the model. More...

unsigned int	addBodyCustomJoint (const unsigned int parent_id, const Math::SpatialTransform &joint_frame, CustomJoint *custom_joint, const Body &body, std::string body_name="")

unsigned int	addBodySphericalJoint (const unsigned int parent_id, const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")

unsigned int	appendBody (const Math::SpatialTransform &joint_frame, const Joint &joint, const Body &body, std::string body_name="")
	Adds a Body to the model such that the previously added Body is the Parent. More...

void	crawlChainKinematics (unsigned int b_id, std::atomic< unsigned int > branch_ends, const Math::VectorNd Q, const Math::VectorNd QDot, const Math::VectorNd QDDot)

unsigned int	GetBodyId (const char *body_name) const
	Returns the id of a body that was passed to addBody() More...

std::string	GetBodyName (unsigned int body_id) const
	Returns the name of a body for a given body id. More...

unsigned int	getCommonMovableParentId (unsigned int id_1, unsigned int id_2) const

Math::SpatialTransform	GetJointFrame (unsigned int id)
	Returns the joint frame transformtion, i.e. the second argument to Model::addBody(). More...

unsigned int	GetParentBodyId (unsigned int id)
	Determines id the actual parent body. More...

Math::Quaternion	GetQuaternion (unsigned int i, const Math::VectorNd &Q) const
	Gets the quaternion for body i (only valid if body i is connected by a JointTypeSpherical joint) More...

ReferenceFramePtr	getReferenceFrame (const std::string &frameName) const
	Get a fixed or moveable body's reference frame. More...

bool	IsBodyId (unsigned int id) const

bool	IsFixedBodyId (unsigned int body_id) const
	Checks whether the body is rigidly attached to another body. More...

	Model (unsigned int *n_threads=nullptr)
	Constructor. More...

	Model (const Model &)=delete

void	operator= (const Model &)=delete

void	SetJointFrame (unsigned int id, const Math::SpatialTransform &transform)
	Sets the joint frame transformtion, i.e. the second argument to Model::addBody(). More...

void	SetQuaternion (unsigned int i, const Math::Quaternion &quat, Math::VectorNd &Q) const
	Sets the quaternion for body i (only valid if body i is connected by a JointTypeSpherical joint) More...

bool	setThreadParameters (int policy, struct sched_param param)

	~Model ()

Public Attributes
Math::SpatialAccelerationV	a
	The spatial velocity of the bodies. More...

std::vector< ReferenceFramePtr >	bodyCenteredFrames

std::vector< ReferenceFramePtr >	bodyFrames

std::vector< Math::SpatialVector >	c
	The velocity dependent spatial acceleration. More...

std::vector< Math::SpatialVector >	c_J

ReferenceFramePtr	comFrame

Math::VectorNd	d
	Temporary variable D_i (RBDA p. 130) More...

unsigned int	dof_count
	number of degrees of freedoms of the model More...

Math::SpatialForceV	f
	Internal forces on the body (used only InverseDynamics()) More...

Math::SpatialForceV	f_b

unsigned int	fixed_body_discriminator
	Value that is used to discriminate between fixed and movable bodies. More...

std::vector< ReferenceFramePtr >	fixedBodyFrames

Math::MotionVector	gravity
	the cartesian vector of the gravity More...

std::vector< Math::Momentum >	hc

Math::SpatialInertiaV	I

std::vector< Math::SpatialMatrix >	IA
	The spatial inertia of the bodies. More...

Math::SpatialInertiaV	Ib_c

Math::RigidBodyInertiaV	Ic

boost::asio::io_service	io_service

std::vector< unsigned int >	lambda

std::vector< std::vector< unsigned int > >	lambda_chain

std::vector< unsigned int >	lambda_q

double	mass

std::vector< Body >	mBodies
	All bodies 0 ... N_B, including the base. More...

std::map< std::string, unsigned int >	mBodyNameMap
	Human readable names for the bodies. More...

std::vector< CustomJoint * >	mCustomJoints

std::vector< FixedBody >	mFixedBodies
	All bodies that are attached to a body via a fixed joint. More...

std::vector< unsigned int >	mFixedJointCount
	The number of fixed joints that have been declared before each joint. More...

std::vector< Joint >	mJoints
	All joints. More...

std::vector< unsigned int >	mJointUpdateOrder

std::vector< std::vector< unsigned int > >	mu

std::vector< Math::Matrix3d >	multdof3_Dinv

std::vector< Math::Matrix63 >	multdof3_S
	Motion subspace for joints with 3 degrees of freedom. More...

std::vector< Math::Matrix63 >	multdof3_S_o

std::vector< Math::Matrix63 >	multdof3_U

std::vector< Math::Vector3d >	multdof3_u

std::vector< unsigned int >	multdof3_w_index

Math::VectorNd	nbodies0_vec

Math::MatrixNd	ndof0_mat

Math::VectorNd	ndof0_vec

unsigned int	num_branch_ends

Math::SpatialForceV	pA
	The spatial bias force. More...

unsigned int	previously_added_body_id
	Id of the previously added body, required for Model::appendBody() More...

Math::VectorNd	q0_vec

unsigned int	q_size
	The size of the $\mathbf{q}$ -vector. For models without spherical joints the value is the same as Model::dof_count, otherwise additional values for the w-component of the Quaternion is stored at the end of $\mathbf{q}$ . More...

unsigned int	qdot_size
	The size of the. More...

std::map< std::string, ReferenceFramePtr >	referenceFrameMap

Math::MotionVectorV	S

Math::MotionVectorV	S_o

boost::thread_group	thread_pool

std::vector< boost::thread * >	threads

Math::MatrixNd	three_x_qd0

std::vector< Math::SpatialVector >	U
	Temporary variable U_i (RBDA p. 130) More...

Math::VectorNd	u
	Temporary variable u (RBDA p. 130) More...

Math::SpatialMotionV	v

Math::SpatialMotionV	v_J

std::unique_ptr< boost::asio::io_service::work >	work

ReferenceFramePtr	worldFrame

std::vector< Math::SpatialTransform >	X_J

std::vector< Math::SpatialTransform >	X_lambda
	Transformation from the parent body to the current body $X_{\lambda(i)} = {}^{i} X_{\lambda(i)}$ . More...

std::vector< Math::SpatialTransform >	X_T
	Transformations from the parent body to the frame of the joint. More...

Private Member Functions
bool	validateRigidBodyInertia (const Body &body)

Detailed Description

Contains all information about the rigid body model.

This class contains all information required to perform the forward dynamics calculation. The variables in this class are also used for storage of temporary values. It is designed for use of the Articulated Rigid Body Algorithm (which is implemented in ForwardDynamics()) and follows the numbering as described in Featherstones book.

Please note that body 0 is the root body and the moving bodies start at index 1. This numbering scheme is very beneficial in terms of readability of the code as the resulting code is very similar to the pseudo-code in the RBDA book. The generalized variables q, qdot, qddot and tau however start at 0 such that the first entry (e.g. q[0]) always specifies the value for the first moving body.

Todo:

Need to implement proper copy constructor, so I can use raw pointers and can clean them up properly

Note: To query the number of degrees of freedom use Model::dof_count.

Definition at line 121 of file Model.h.

Constructor & Destructor Documentation

Model::Model ( unsigned int * n_threads = nullptr )

explicit

Constructor.

Parameters

n_threads The number of threads that the model can create for performing parallel tasks such as parallel kinematics. This defaults to nullptr in which case the number of threads created will be equal to std::thread::hardware_concurrency()

Definition at line 22 of file Model.cc.

RobotDynamics::Model::~Model ( )

inline

Definition at line 130 of file Model.h.

RobotDynamics::Model::Model ( const Model & )

delete

Member Function Documentation

unsigned int Model::addBody	(	const unsigned int	parent_id,
		const Math::SpatialTransform &	joint_frame,
		const Joint &	joint,
		const Body &	body,
		std::string	body_name = `""`
	)

Connects a given body to the model.

When adding a body there are basically informations required:

what kind of body will be added?
where is the new body to be added?
by what kind of joint should the body be added?

The first information "what kind of body will be added" is contained in the Body class that is given as a parameter.

The question "where is the new body to be added?" is split up in two parts: first the parent (or successor) body to which it is added and second the transformation to the origin of the joint that connects the two bodies. With these two informations one specifies the relative positions of the bodies when the joint is in neutral position.gk

The last question "by what kind of joint should the body be added?" is again simply contained in the Joint class.

Parameters

parent_id	id of the parent body
joint_frame	the transformation from the parent frame to the origin of the joint frame (represents X_T in RBDA)
joint	specification for the joint that describes the connection
body	specification of the body itself
body_name	human readable name for the body (can be used to retrieve its id with GetBodyId())

Returns: id of the added body

Definition at line 271 of file Model.cc.

unsigned int Model::addBodyCustomJoint	(	const unsigned int	parent_id,
		const Math::SpatialTransform &	joint_frame,
		CustomJoint *	custom_joint,
		const Body &	body,
		std::string	body_name = `""`
	)

Definition at line 522 of file Model.cc.

unsigned int RobotDynamics::Model::addBodySphericalJoint	(	const unsigned int	parent_id,
		const Math::SpatialTransform &	joint_frame,
		const Joint &	joint,
		const Body &	body,
		std::string	body_name = `""`
	)

unsigned int Model::appendBody	(	const Math::SpatialTransform &	joint_frame,
		const Joint &	joint,
		const Body &	body,
		std::string	body_name = `""`
	)

Adds a Body to the model such that the previously added Body is the Parent.

This function is basically the same as Model::addBody() however the most recently added body (or body 0) is taken as parent.

Definition at line 517 of file Model.cc.

void Model::crawlChainKinematics	(	unsigned int	b_id,
		std::atomic< unsigned int > *	branch_ends,
		const Math::VectorNd *	Q,
		const Math::VectorNd *	QDot,
		const Math::VectorNd *	QDDot
	)

Definition at line 539 of file Model.cc.

unsigned int RobotDynamics::Model::GetBodyId ( const char * body_name ) const

inline

Returns the id of a body that was passed to addBody()

Bodies can be given a human readable name. This function allows to resolve its name to the numeric id.

Note: Instead of querying this function repeatedly, it might be advisable to query it once and reuse the returned id.

Returns: the id of the body or
std::numeric_limits<unsigned int>::max()
if the id was not found.

Definition at line 432 of file Model.h.

std::string RobotDynamics::Model::GetBodyName ( unsigned int body_id ) const

inline

Returns the name of a body for a given body id.

Definition at line 464 of file Model.h.

unsigned int RobotDynamics::Model::getCommonMovableParentId	(	unsigned int	id_1,
		unsigned int	id_2
	)		const

inline

Definition at line 620 of file Model.h.

Math::SpatialTransform RobotDynamics::Model::GetJointFrame ( unsigned int id )

inline

Returns the joint frame transformtion, i.e. the second argument to Model::addBody().

Definition at line 537 of file Model.h.

unsigned int RobotDynamics::Model::GetParentBodyId ( unsigned int id )

inline

Determines id the actual parent body.

Note: When adding bodies using joints with multiple degrees of freedom, additional virtual bodies are added for each degree of freedom. This function returns the id of the actual non-virtual parent body.

Definition at line 516 of file Model.h.

Math::Quaternion RobotDynamics::Model::GetQuaternion	(	unsigned int	i,
		const Math::VectorNd &	Q
	)		const

inline

Gets the quaternion for body i (only valid if body i is connected by a JointTypeSpherical joint)

See Joint Singularities for details.

Definition at line 596 of file Model.h.

ReferenceFramePtr RobotDynamics::Model::getReferenceFrame ( const std::string & frameName ) const

inline

Get a fixed or moveable body's reference frame.

Parameters

bodyName Name of the body frame you want

Returns: Pointer to the frame, or nullptr if it doesn't exist

Definition at line 448 of file Model.h.

bool RobotDynamics::Model::IsBodyId ( unsigned int id ) const

inline

Definition at line 492 of file Model.h.

bool RobotDynamics::Model::IsFixedBodyId ( unsigned int body_id ) const

inline

Checks whether the body is rigidly attached to another body.

Definition at line 483 of file Model.h.

void RobotDynamics::Model::operator= ( const Model & )

delete

void RobotDynamics::Model::SetJointFrame	(	unsigned int	id,
		const Math::SpatialTransform &	transform
	)

inline

Sets the joint frame transformtion, i.e. the second argument to Model::addBody().

Definition at line 565 of file Model.h.

void RobotDynamics::Model::SetQuaternion	(	unsigned int	i,
		const Math::Quaternion &	quat,
		Math::VectorNd &	Q
	)		const

inline

Sets the quaternion for body i (only valid if body i is connected by a JointTypeSpherical joint)

See Joint Singularities for details.

Definition at line 609 of file Model.h.

bool RobotDynamics::Model::setThreadParameters	(	int	policy,
		struct sched_param	param
	)

inline

Definition at line 338 of file Model.h.

bool RobotDynamics::Model::validateRigidBodyInertia ( const Body & body )

inlineprivate

Definition at line 677 of file Model.h.

Member Data Documentation

Math::SpatialAccelerationV RobotDynamics::Model::a

The spatial velocity of the bodies.

The spatial acceleration of the bodies expressed in body frame

Definition at line 200 of file Model.h.

std::vector<ReferenceFramePtr> RobotDynamics::Model::bodyCenteredFrames

< Body centered frames are aligned with the body frame, but located at a body's center of mass

Definition at line 152 of file Model.h.

std::vector<ReferenceFramePtr> RobotDynamics::Model::bodyFrames

Reference frames for each body. Frame names are the same as the body name

Definition at line 145 of file Model.h.

std::vector<Math::SpatialVector> RobotDynamics::Model::c

The velocity dependent spatial acceleration.

Definition at line 248 of file Model.h.

std::vector<Math::SpatialVector> RobotDynamics::Model::c_J

Apparent derivative of v_J in a coordinate moving with the successor body

Definition at line 217 of file Model.h.

ReferenceFramePtr RobotDynamics::Model::comFrame

Definition at line 331 of file Model.h.

Math::VectorNd RobotDynamics::Model::d

Temporary variable D_i (RBDA p. 130)

Definition at line 260 of file Model.h.

unsigned int RobotDynamics::Model::dof_count

number of degrees of freedoms of the model

This value contains the number of entries in the generalized state (q) velocity (qdot), acceleration (qddot), and force (tau) vector.

Definition at line 169 of file Model.h.

Math::SpatialForceV RobotDynamics::Model::f

Internal forces on the body (used only InverseDynamics())

Definition at line 266 of file Model.h.

Math::SpatialForceV RobotDynamics::Model::f_b

Definition at line 267 of file Model.h.

unsigned int RobotDynamics::Model::fixed_body_discriminator

Value that is used to discriminate between fixed and movable bodies.

Bodies with id 1 .. (fixed_body_discriminator - 1) are moving bodies while bodies with id fixed_body_discriminator .. max (unsigned int) are fixed to a moving body. The value of max(unsigned int) is determined via std::numeric_limits<unsigned int>::max() and the default value of fixed_body_discriminator is max (unsigned int) / 2.

On normal systems max (unsigned int) is 4294967294 which means there could be a total of 2147483646 movable and / or fixed bodies.

Definition at line 312 of file Model.h.

std::vector<ReferenceFramePtr> RobotDynamics::Model::fixedBodyFrames

Definition at line 154 of file Model.h.

Math::MotionVector RobotDynamics::Model::gravity

the cartesian vector of the gravity

Definition at line 196 of file Model.h.

std::vector<Math::Momentum> RobotDynamics::Model::hc

Definition at line 275 of file Model.h.

Math::SpatialInertiaV RobotDynamics::Model::I

Body inertia expressed in body frame

Definition at line 269 of file Model.h.

std::vector<Math::SpatialMatrix> RobotDynamics::Model::IA

The spatial inertia of the bodies.

Definition at line 251 of file Model.h.

Math::SpatialInertiaV RobotDynamics::Model::Ib_c

Ib_c is the inertia of (non-fixed) bodies expressed in the body's center of mass frame.

Definition at line 273 of file Model.h.

Math::RigidBodyInertiaV RobotDynamics::Model::Ic

A body inertia. See RobotDynamics::compositeRigidBodyAlgorithm for the uses of this

Definition at line 270 of file Model.h.

boost::asio::io_service RobotDynamics::Model::io_service

Definition at line 333 of file Model.h.

std::vector<unsigned int> RobotDynamics::Model::lambda

The id of the parents body

Definition at line 156 of file Model.h.

std::vector<std::vector<unsigned int> > RobotDynamics::Model::lambda_chain

Each body's chain of parents

Definition at line 157 of file Model.h.

std::vector<unsigned int> RobotDynamics::Model::lambda_q

The index of the parent degree of freedom that is directly influencing the current one

Definition at line 159 of file Model.h.

double RobotDynamics::Model::mass

Definition at line 190 of file Model.h.

std::vector<Body> RobotDynamics::Model::mBodies

All bodies 0 ... N_B, including the base.

mBodies[0] - base body
mBodies[1] - 1st moveable body
...
mBodies[N_B] - N_Bth moveable body

Definition at line 321 of file Model.h.

std::map<std::string, unsigned int> RobotDynamics::Model::mBodyNameMap

Human readable names for the bodies.

Map containaing name -> pointer to reference frame for each frame on the robot

Definition at line 324 of file Model.h.

std::vector<CustomJoint*> RobotDynamics::Model::mCustomJoints

Definition at line 242 of file Model.h.

std::vector<FixedBody> RobotDynamics::Model::mFixedBodies

All bodies that are attached to a body via a fixed joint.

Definition at line 298 of file Model.h.

std::vector<unsigned int> RobotDynamics::Model::mFixedJointCount

The number of fixed joints that have been declared before each joint.

Definition at line 229 of file Model.h.

std::vector<Joint> RobotDynamics::Model::mJoints

All joints.

Definition at line 209 of file Model.h.

std::vector<unsigned int> RobotDynamics::Model::mJointUpdateOrder

Definition at line 221 of file Model.h.

std::vector<std::vector<unsigned int> > RobotDynamics::Model::mu

Contains the ids of all the children of a given body

Definition at line 162 of file Model.h.

std::vector<Math::Matrix3d> RobotDynamics::Model::multdof3_Dinv

Definition at line 238 of file Model.h.

std::vector<Math::Matrix63> RobotDynamics::Model::multdof3_S

Motion subspace for joints with 3 degrees of freedom.

Definition at line 235 of file Model.h.

std::vector<Math::Matrix63> RobotDynamics::Model::multdof3_S_o

Definition at line 236 of file Model.h.

std::vector<Math::Matrix63> RobotDynamics::Model::multdof3_U

Definition at line 237 of file Model.h.

std::vector<Math::Vector3d> RobotDynamics::Model::multdof3_u

Definition at line 239 of file Model.h.

std::vector<unsigned int> RobotDynamics::Model::multdof3_w_index

Definition at line 240 of file Model.h.

Math::VectorNd RobotDynamics::Model::nbodies0_vec

mBodies.size() x 1 vector of zeros

Definition at line 285 of file Model.h.

Math::MatrixNd RobotDynamics::Model::ndof0_mat

Note: It's CRITICAL that the elements of the zero matrices never be modified! It will break absolutely everything.ndof x ndof matrix of zeros

Definition at line 281 of file Model.h.

Math::VectorNd RobotDynamics::Model::ndof0_vec

ndof x 1 vector of zeros

Definition at line 284 of file Model.h.

unsigned int RobotDynamics::Model::num_branch_ends

Reference frame who's origin is located at the robots center of mass, aligned with world frame

Definition at line 328 of file Model.h.

Math::SpatialForceV RobotDynamics::Model::pA

The spatial bias force.

Definition at line 254 of file Model.h.

unsigned int RobotDynamics::Model::previously_added_body_id

Id of the previously added body, required for Model::appendBody()

Definition at line 193 of file Model.h.

Math::VectorNd RobotDynamics::Model::q0_vec

q_size x 1 vector of zeros

Definition at line 283 of file Model.h.

unsigned int RobotDynamics::Model::q_size

The size of the $\mathbf{q}$ -vector. For models without spherical joints the value is the same as Model::dof_count, otherwise additional values for the w-component of the Quaternion is stored at the end of $\mathbf{q}$ .

See also: Joint Modeling for more details.

Definition at line 178 of file Model.h.

unsigned int RobotDynamics::Model::qdot_size

The size of the.

( $\mathbf{\dot{q}}, \mathbf{\ddot{q}}$ , and $\mathbf{\tau}$ -vector.

See also: Joint Modeling for more details.total robot mass(kg)

Definition at line 187 of file Model.h.

std::map<std::string, ReferenceFramePtr> RobotDynamics::Model::referenceFrameMap

Definition at line 327 of file Model.h.

Math::MotionVectorV RobotDynamics::Model::S

Motion subspace for 1 dof joints

Definition at line 211 of file Model.h.

Math::MotionVectorV RobotDynamics::Model::S_o

The ring derivative of S, a.k.a the rate of growth

Definition at line 212 of file Model.h.

boost::thread_group RobotDynamics::Model::thread_pool

Definition at line 336 of file Model.h.

std::vector<boost::thread*> RobotDynamics::Model::threads

Definition at line 335 of file Model.h.

Math::MatrixNd RobotDynamics::Model::three_x_qd0

3 x qdot_size matrix of zeros

Definition at line 282 of file Model.h.

std::vector<Math::SpatialVector> RobotDynamics::Model::U

Temporary variable U_i (RBDA p. 130)

Definition at line 257 of file Model.h.

Math::VectorNd RobotDynamics::Model::u

Temporary variable u (RBDA p. 130)

Definition at line 263 of file Model.h.

Math::SpatialMotionV RobotDynamics::Model::v

The spatial velocity of the bodies expressed in body frame

Definition at line 202 of file Model.h.

Math::SpatialMotionV RobotDynamics::Model::v_J

Joints velocity w.r.t its parent joint, expressed in body frame

Definition at line 219 of file Model.h.

std::unique_ptr<boost::asio::io_service::work> RobotDynamics::Model::work

Definition at line 334 of file Model.h.

ReferenceFramePtr RobotDynamics::Model::worldFrame

Pointer to world frame

Definition at line 141 of file Model.h.

std::vector<Math::SpatialTransform> RobotDynamics::Model::X_J

Definition at line 215 of file Model.h.

std::vector<Math::SpatialTransform> RobotDynamics::Model::X_lambda

Transformation from the parent body to the current body

$X_{\lambda(i)} = {}^{i} X_{\lambda(i)}$

.

Definition at line 295 of file Model.h.

std::vector<Math::SpatialTransform> RobotDynamics::Model::X_T

Transformations from the parent body to the frame of the joint.

Definition at line 225 of file Model.h.

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

Public Member Functions

Public Attributes

Private Member Functions

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation