Functions
void	RobotDynamics::calcBodySpatialJacobian (Model &model, const Math::VectorNd &Q, unsigned int body_id, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the spatial jacobian for a body. The result will be returned via the G argument and represents the body Jacobian expressed at the origin of the body. The corresponding spatial velocity of the body w.r.t the world frame expressed in body frame can be calculated, for the body, as $v^{i,0}_{i} = G(q) \dot{q}$ . For the spatial jacobian of body , each column j is computed as follows. More...

void	RobotDynamics::calcBodySpatialJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd QDot, unsigned int body_id, Math::MatrixNd &G, const bool update_kinematics=true)
	Computes the time derivative of the spatial jacobian for a body. The result will be returned via the G argument and represents the time derivative of the body Jacobian expressed at the origin of the body. The corresponding spatial acceleration of the body w.r.t the world frame expressed in body frame can be calculated, for the th body, as $a^{i,0}_{i} = G(q) \ddot{q} + \dot{G}(q)\dot{q}$ where is the body jacobian of body . More...

Math::FrameVector	RobotDynamics::calcPointAcceleration (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
	Computes the linear acceleration of a point on a body. More...

Math::FrameVectorPair	RobotDynamics::calcPointAcceleration6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
	Computes linear and angular acceleration of a point on a body. More...

Math::FrameVectorPair	RobotDynamics::calcPointAcceleration6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Computes linear and angular acceleration of a reference frame, relative to another reference frame and expressed in a third reference frame. More...

void	RobotDynamics::calcPointJacobian (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the 3D point jacobian for a point on a body. More...

void	RobotDynamics::calcPointJacobian (Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr frame, bool update_kinematics=true)
	Compute the 3D point jacobian of the origin of a reference frame If a position of a point is computed by a function for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix . More...

void	RobotDynamics::calcPointJacobian6D (Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr frame, bool update_kinematics=true)
	Compute the 6D point jacobian of the origin of a reference frame If a position of a point is computed by a function for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix . More...

void	RobotDynamics::calcPointJacobian6D (Model &model, const Math::VectorNd &Q, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
	Computes a 6-D Jacobian for a point on a body. More...

void	RobotDynamics::calcPointJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the time derivative of the linear components the a point jacobian on a body. More...

void	RobotDynamics::calcPointJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the time derivative of a point jacobian of a point on a body. Only computes the linear elemets. More...

void	RobotDynamics::calcPointJacobianDot6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the time derivative of a point jacobian of a point on a body. More...

void	RobotDynamics::calcPointJacobianDot6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, Math::MatrixNd &G, bool update_kinematics=true)
	Computes the 6D time derivative of a point jacobian on a body. More...

Math::FrameVector	RobotDynamics::calcPointVelocity (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
	Computes the velocity of a point on a body. More...

Math::FrameVectorPair	RobotDynamics::calcPointVelocity6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, unsigned int body_id, const Math::Vector3d &point_position, bool update_kinematics=true)
	Computes angular and linear velocity of a point that is fixed on a body. More...

Math::FrameVectorPair	RobotDynamics::calcPointVelocity6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr frame, bool update_kinematics=true)
	Computes angular and linear velocity of the origin of a reference frame relative to world and expressed in world frame. More...

Math::FrameVectorPair	RobotDynamics::calcPointVelocity6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, RobotDynamics::ReferenceFramePtr baseFrame, RobotDynamics::ReferenceFramePtr relativeFrame, RobotDynamics::ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
	Computes angular and linear velocity of the origin of a reference frame relative to another reference frame and expressed in a third reference frame. More...

void	RobotDynamics::calcRelativeBodySpatialJacobian (Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
	Computes the jacobian of a frame $J_{i}^{k,j}$ with being the "base" frame, being the "relative" frame, and being the "expressed in" frame. Multiplying this jacobian by a vector of joint velocities will result in the spatial motion of the baseFrame w.r.t relativeFrame expressed in expressedInFrame, a.k.a $v_{i}^{k,j} = J_{i}^{k,j}\dot{q}$ . More...

void	RobotDynamics::calcRelativeBodySpatialJacobianAndJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
	Computes both the body spatial jacobian and its time derivative. This function will be a bit more efficient at computing the jacobian and it's time derivative if you need both matrices rather than computing them with separate function calls. More...

void	RobotDynamics::calcRelativeBodySpatialJacobianDot (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=nullptr, bool update_kinematics=true)
	Computes the rate of change of the jacobian, $\dot{J}_{i}^{k,j}$ , with being the "base" frame, being the relative" frame, and \f$k\f$ being the "expressed in" frame. This jacobian is such that the following is true, $a_{i}^{k,j} = J_{i}^{k,j}\ddot{q} + \dot{J}_{i}^{k,j}\dot{q}$ where $a_{i}^{k,j}$ is the spatial acceleration of frame w.r.t frame , expressed in frame . Additionally the jacobian $J_{i}^{k,j}$ can be computed by RobotDynamics::calcRelativeBodySpatialJacobian. More...

void	RobotDynamics::calcRelativePointJacobian6D (Model &model, const Math::VectorNd &Q, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
	Compute the 6D jacobian of the origin of a reference frame relative to the origin of another reference frame and express the result in a third frame. More...

void	RobotDynamics::calcRelativePointJacobianAndJacobianDot6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, Math::MatrixNd &GDot, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
	Compute the point Jacobian of the origin of baseFrame, relative to the origin of relativeFrame, expressed in expressedInFrame. Also compute that point Jacobians time derivative. More...

void	RobotDynamics::calcRelativePointJacobianDot6D (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, Math::MatrixNd &G, ReferenceFramePtr baseFrame, ReferenceFramePtr relativeFrame, ReferenceFramePtr expressedInFrame=RobotDynamics::ReferenceFrame::getWorldFrame(), bool update_kinematics=true)
	Compute the time derivative of the 6D jacobian of the origin of a reference frame relative to the origin of another reference frame and express the result in a third frame. More...

Math::SpatialAcceleration	RobotDynamics::calcSpatialAcceleration (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, const unsigned int body_id, const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Compute the spatial acceleration of any body with respect to any other body and express the result in an arbitrary reference frame. The returned RobotDynamicS::Math::SpatialAcceleration can be expressed in the reference frame of choice by supplying the expressedInFrame argument. If left to the default value of nullptr, the result will be expressed in the reference frame corresponding to body_id. More...

Math::SpatialAcceleration	RobotDynamics::calcSpatialAcceleration (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Compute the spatial acceleration of any frame with respect to any other frame and express the result in an arbitrary third frame. The returned RobotDynamicS::Math::SpatialAcceleration can be expressed in the reference frame of choice by supplying the expressedInFrame argument. If left to the default value of nullptr, the result will be expressed in body_frame. More...

Math::SpatialMotion	RobotDynamics::calcSpatialVelocity (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, ReferenceFramePtr body_frame, ReferenceFramePtr relative_body_frame, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Compute the spatial velocity of any frame with respect to any other frame, expressed in an arbirtary third frame. The returned RobotDynamicS::Math::SpatialMotion is expressed in body_frame unless th expressedInFrame is provided. Each time RobotDynamics::updateKinematics is called, the spatial velocity of each body with respect to the world, and expressed in body frame is calculated. For body this is written as $v^{i,0}_{i}$ . Given another body, body , the velocity of body relative to body and expressed in body frame is computed by, $v^{i,j}_{i} = v^{i,0}_{i} - ^{i}X_{j} v^{j,0}_{j}$ . More...

Math::SpatialMotion	RobotDynamics::calcSpatialVelocity (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const unsigned int body_id, const unsigned int relative_body_id, ReferenceFramePtr expressedInFrame=nullptr, const bool update_kinematics=true)
	Compute the spatial velocity of any body with respect to any other body. The returned RobotDynamicS::Math::SpatialMotion is expressed in the body frame of body body_id. Each time RobotDynamics::updateKinematics is called, the spatial velocity of each body with respect to the world, and expressed in body frame is calculated. For body this is written as $v^{i,0}_{i}$ . Given another body, body , the velocity of body relative to body and expressed in body frame is computed by, $v^{i,j}_{i} = v^{i,0}_{i} - ^{i}X_{j} v^{j,0}_{j}$ . More...

bool	RobotDynamics::inverseKinematics (Model &model, const Math::VectorNd &Qinit, const std::vector< unsigned int > &body_id, const std::vector< Math::Vector3d > &body_point, const std::vector< Math::Vector3d > &target_pos, Math::VectorNd &Qres, double step_tol=1.0e-12, double lambda=0.01, unsigned int max_iter=50)
	Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method) More...

void	RobotDynamics::updateAccelerations (Model &model, const Math::VectorNd &QDDot)
	Computes only the accelerations of the bodies. More...

void	RobotDynamics::updateKinematics (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
	Updates and computes velocities and accelerations of the bodies. More...

void	RobotDynamics::updateKinematicsCustom (Model &model, const Math::VectorNd Q, const Math::VectorNd QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
	Selectively updates model internal states of body positions, velocities and/or accelerations. More...

void	RobotDynamics::updateKinematicsCustomParallel (Model &model, const Math::VectorNd Q, const Math::VectorNd QDot=nullptr, const Math::VectorNd *QDDot=nullptr)
	Selectively updates model internal states of body positions, velocities and/or accelerations and spawns threads far each branched chain. More...

void	RobotDynamics::updateKinematicsParallel (Model &model, const Math::VectorNd &Q, const Math::VectorNd &QDot, const Math::VectorNd &QDDot)
	Updates and computes velocities and accelerations of the bodies. More...

Detailed Description

Note: Please note that in the Robot Dynamics Library all angles are specified in radians.; Please note that before making any calls that involve reference frames you must first call either updateKinematics or updateKinematicsCustom. If this is not done then operations involving frames will return incorrect results.

Function Documentation

void RobotDynamics::calcBodySpatialJacobian	(	Model &	model,
		const Math::VectorNd &	Q,
		unsigned int	body_id,
		Math::MatrixNd &	G,
		bool	update_kinematics = `true`
	)

Computes the spatial jacobian for a body. The result will be returned via the G argument and represents the body Jacobian expressed at the origin of the body. The corresponding spatial velocity of the body w.r.t the world frame expressed in body frame can be calculated, for the $ ith $ body, as $v^{i,0}_{i} = G(q) \dot{q}$ . For the spatial jacobian of body $i$ , each column j is computed as follows.

$G_{j} = \begin{cases} \mathbf{0} & \text{if $j \notin \lambda_{i}$} \\ ^{i}X_{j}S_{j} & \text{otherwise} \end{cases}$

Parameters

model	rigid body model
Q	state vector of the internal joints
body_id	the id of the body
G	a matrix of size 6 x #qdot_size where the result will be stored in
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Note: This function only evaluates the entries of G 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 G.setZero().

Definition at line 1310 of file Kinematics.cc.

void RobotDynamics::calcBodySpatialJacobianDot	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd	QDot,
		unsigned int	body_id,
		Math::MatrixNd &	G,
		const bool	update_kinematics = `true`
	)

Computes the time derivative of the spatial jacobian for a body. The result will be returned via the G argument and represents the time derivative of the body Jacobian expressed at the origin of the body. The corresponding spatial acceleration of the body w.r.t the world frame expressed in body frame can be calculated, for the $i$ th body, as $a^{i,0}_{i} = G(q) \ddot{q} + \dot{G}(q)\dot{q}$ where $G(q)$ is the body jacobian of body $i$ .

Parameters

model	rigid body model
Q	joint positions
QDot	joint velocities
body_id	the id of the body
G	a matrix of size 6 x #qdot_size where the result will be stored in
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Note: This function only evaluates the entries of G 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 G.setZero().

Definition at line 1367 of file Kinematics.cc.

Math::FrameVector RobotDynamics::calcPointAcceleration	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot,
		unsigned int	body_id,
		const Math::Vector3d &	point_position,
		bool	update_kinematics = `true`
	)

Computes the linear acceleration of a point on a body.

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
QDDot	velocity vector of the internal joints
body_id	the id of the body
point_position	the position of the point in body-local data
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Returns: The cartesian acceleration of the point in world frame (output)

The kinematic state of the model has to be updated before valid values can be obtained. This can either be done by calling UpdateKinematics() or setting the last parameter update_kinematics to true (default).

Warning: If this function is called after ForwardDynamics() without an update of the kinematic state one has to add the gravity acceleration has to be added to the result.

Definition at line 1593 of file Kinematics.cc.

Math::FrameVectorPair RobotDynamics::calcPointAcceleration6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot,
		unsigned int	body_id,
		const Math::Vector3d &	point_position,
		bool	update_kinematics = `true`
	)

Computes linear and angular acceleration of a point on a body.

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
QDDot	velocity vector of the internal joints
body_id	the id of the body
point_position	the position of the point in body-local data
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Returns: A FrameVectorPair of the desired point with respect to world expressed in world frame

The kinematic state of the model has to be updated before valid values can be obtained. This can either be done by calling updateKinematics() or setting the last parameter update_kinematics to true (default).

Warning: If this function is called after ForwardDynamics() without an update of the kinematic state one has to add the gravity acceleration has to be added to the result.

Definition at line 1631 of file Kinematics.cc.

FrameVectorPair RobotDynamics::calcPointAcceleration6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot,
		ReferenceFramePtr	body_frame,
		ReferenceFramePtr	relative_body_frame,
		ReferenceFramePtr	expressedInFrame = `nullptr`,
		const bool	update_kinematics = `true`
	)

Computes linear and angular acceleration of a reference frame, relative to another reference frame and expressed in a third reference frame.

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
QDDot	velocity vector of the internal joints
body_frame	frame to calculate the acceleration of
relative_body_frame	calculated acceleration will be relative to this frame
expressedInFrame	resulting acceleration will be expressed in this frame
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Returns: A frame vector pair representing the acceleration of the origin of body_frame w.r.t relative_body_frame and expressed in expressedInFrame

The kinematic state of the model has to be updated before valid values can be obtained. This can either be done by calling updateKinematics() or setting the last parameter update_kinematics to true (default).

Warning: If this function is called after ForwardDynamics() without an update of the kinematic state one has to add the gravity acceleration has to be added to the result.

Definition at line 1543 of file Kinematics.cc.

void RobotDynamics::calcPointJacobian	(	Model &	model,
		const Math::VectorNd &	Q,
		unsigned int	body_id,
		const Math::Vector3d &	point_position,
		Math::MatrixNd &	G,
		bool	update_kinematics = `true`
	)

Computes the 3D point jacobian for a point on a body.

If a position of a point is computed by a function $g(q(t))$ for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix $G(q)$ .

Parameters

model	rigid body model
Q	state vector of the internal joints
body_id	the id of the body
point_position	the position of the point in body-local data
G	a matrix of dimensions 3 x #qdot_size where the result will be stored in
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

The result will be returned via the G argument.

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

Definition at line 510 of file Kinematics.cc.

void RobotDynamics::calcPointJacobian	(	Model &	model,
		const Math::VectorNd &	Q,
		Math::MatrixNd &	G,
		ReferenceFramePtr	frame,
		bool	update_kinematics = `true`
	)

Compute the 3D point jacobian of the origin of a reference frame If a position of a point is computed by a function $g(q(t))$ for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix $G(q)$ .

Parameters

model	Rigid body model
Q	Vector of joint positions
G	$3 \times qdot_size$ Matrix to store result
frame	The point jacobian will be at the origin of the frame
update_kinematics	If true, kinematics will be calculated

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

Definition at line 565 of file Kinematics.cc.

void RobotDynamics::calcPointJacobian6D	(	Model &	model,
		const Math::VectorNd &	Q,
		Math::MatrixNd &	G,
		ReferenceFramePtr	frame,
		bool	update_kinematics = `true`
	)

Compute the 6D point jacobian of the origin of a reference frame If a position of a point is computed by a function $g(q(t))$ for which its time derivative is $\frac{d}{dt} g(q(t)) = G(q)\dot{q}$ then this function computes the jacobian matrix $G(q)$ .

Parameters

model	Rigid body model
Q	Vector of joint positions
G	$6 \times qdot_size$ Matrix to store result
frame	The point jacobian will be at the origin of the frame
update_kinematics	If true, kinematics will be calculated

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

Definition at line 604 of file Kinematics.cc.

void RobotDynamics::calcPointJacobian6D	(	Model &	model,
		const Math::VectorNd &	Q,
		unsigned int	body_id,
		const Math::Vector3d &	point_position,
		Math::MatrixNd &	G,
		bool	update_kinematics = `true`
	)

Computes a 6-D Jacobian for a point on a body.

Computes the 6-D Jacobian $G(q)$ that when multiplied with $\dot{q}$ gives a 6-D vector that has the angular velocity as the first three entries and the linear velocity as the last three entries.

Parameters

model	rigid body model
Q	state vector of the internal joints
body_id	the id of the body
point_position	the position of the point in body-local data
G	a matrix of dimensions 6 x #qdot_size where the result will be stored in
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

The result will be returned via the G argument.

Note: This function only evaluates the entries of G 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 G.setZero().

Definition at line 762 of file Kinematics.cc.

void RobotDynamics::calcPointJacobianDot	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		unsigned int	body_id,
		const Math::Vector3d &	point_position,
		Math::MatrixNd &	G,
		bool	update_kinematics = `true`
	)

Computes the time derivative of the linear components the a point jacobian on a body.

For a point $p_{i}$ on body $i$ expressed in body $i$ 's frame, this function computes $\dot{J}$ such that $a_{i} = J(q)\ddot{q} + \dot{J}(q,\dot{q})\dot{q}$ , where $a_{i}$ is the 3D linear acceleration of point $p$ , and $a_{i}$ is expressed in world frame.

Parameters

model	Rigid body model
Q	Joint positions
QDot	Joint velocities
body_id	Id of the body
point_position	3D position on body
G	Matrix where the result is stored
update_kinematics	Defaults to true. If true, kinematics will be calculated. If kinematics have already been calculated, setting this to false will save time

Note: This function only evaluates the entries of G 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 G.setZero().

Definition at line 751 of file Kinematics.cc.

void RobotDynamics::calcPointJacobianDot	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		RobotDynamics::ReferenceFramePtr	frame,
		Math::MatrixNd &	G,
		bool	update_kinematics = `true`
	)

Computes the time derivative of a point jacobian of a point on a body. Only computes the linear elemets.

For a reference frame with origin $p_{i}$ , this function computes $\dot{J}$ such that $a_{i} = J(q)\ddot{q} + \dot{J}(q,\dot{q})\dot{q}$ , where $a_{i}$ is the 3D linear acceleration of point $p$ , and $a_{i}$ is expressed in world frame.

Parameters

model	Rigid body model
Q	Joint positions
QDot	Joint velocities
frame	Resulting point jacobian will be of the origin of this frame
G	Matrix where the result is stored, must be a $3 \times qdot_size$ matrix
update_kinematics	Defaults to true. If true, kinematics will be calculated. If kinematics have already been calculated, setting this to false will save time

Note: This function only evaluates the entries of G 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 G.setZero().

Definition at line 1221 of file Kinematics.cc.

void RobotDynamics::calcPointJacobianDot6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		RobotDynamics::ReferenceFramePtr	frame,
		Math::MatrixNd &	G,
		bool	update_kinematics = `true`
	)

Computes the time derivative of a point jacobian of a point on a body.

For a reference frame with origin $p_{i}$ , this function computes $\dot{J}$ such that $a_{i} = J(q)\ddot{q} + \dot{J}(q,\dot{q})\dot{q}$ , where $a_{i}$ is the 3D linear acceleration of point $p$ , and $a_{i}$ is expressed in world frame.

Parameters

model	Rigid body model
Q	Joint positions
QDot	Joint velocities
frame	Resulting point jacobian will be of the origin of this frame
G	Matrix where the result is stored, must be a $6 \times qdot_size$ matrix
update_kinematics	Defaults to true. If true, kinematics will be calculated. If kinematics have already been calculated, setting this to false will save time

Note: This function only evaluates the entries of G 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 G.setZero().

Definition at line 826 of file Kinematics.cc.

void RobotDynamics::calcPointJacobianDot6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		unsigned int	body_id,
		const Math::Vector3d &	point_position,
		Math::MatrixNd &	G,
		bool	update_kinematics = `true`
	)

Computes the 6D time derivative of a point jacobian on a body.

For a point $p_{i}$ on body $i$ expressed in body $ i $ 's frame, this function computes $\dot{J}$ such that $a_{i} = J(q)\ddot{q} + \dot{J}(q,\dot{q})\dot{q}$ , where $a_{i}$ is the 6D acceleration of point $p$ , and $a_{i}$ is expressed in world frame.

Essentially, this method computes the jacobian of a reference frame located at point $p_{i}$ , but aligned with the world frame.

Parameters

model	Rigid body model
Q	Joint positions
QDot	Joint velocities
body_id	Id of the body
point_position	3D position on body
G	Matrix where the result is stored
update_kinematics	Defaults to true. If true, kinematics will be calculated. If kinematics have already been calculated, setting this to false will save time

Note: This function only evaluates the entries of G 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 G.setZero().

Note that this takes advantage of the crossing of two vectors and being able to swap the order of them if you negate one of them, i.e. a x b = -b x a

Definition at line 1232 of file Kinematics.cc.

Math::FrameVector RobotDynamics::calcPointVelocity	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		unsigned int	body_id,
		const Math::Vector3d &	point_position,
		bool	update_kinematics = `true`
	)

Computes the velocity of a point on a body.

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
body_id	the id of the body
point_position	the position of the point in body-local data
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Returns: A FrameVector representing the points velocity in world frame.

Definition at line 1422 of file Kinematics.cc.

Math::FrameVectorPair RobotDynamics::calcPointVelocity6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		unsigned int	body_id,
		const Math::Vector3d &	point_position,
		bool	update_kinematics = `true`
	)

Computes angular and linear velocity of a point that is fixed on a body.

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
body_id	the id of the body
point_position	the position of the point in body-local data
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Returns: The frame vector pair in the global reference system.

Definition at line 1459 of file Kinematics.cc.

Math::FrameVectorPair RobotDynamics::calcPointVelocity6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		RobotDynamics::ReferenceFramePtr	frame,
		bool	update_kinematics = `true`
	)

Computes angular and linear velocity of the origin of a reference frame relative to world and expressed in world frame.

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
frame	The frame whose origin will be used to calc the point velocity
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Returns: The a frame vector pair representing the two 3d vectors that describe the point velocity

Definition at line 1497 of file Kinematics.cc.

FrameVectorPair RobotDynamics::calcPointVelocity6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		RobotDynamics::ReferenceFramePtr	baseFrame,
		RobotDynamics::ReferenceFramePtr	relativeFrame,
		RobotDynamics::ReferenceFramePtr	expressedInFrame = `RobotDynamics::ReferenceFrame::getWorldFrame()`,
		bool	update_kinematics = `true`
	)

Computes angular and linear velocity of the origin of a reference frame relative to another reference frame and expressed in a third reference frame.

Parameters

model	rigid body model
Q	state vector of the internal joints
QDot	velocity vector of the internal joints
baseFrame	The frame whose origin the point jacobian will be about
relativeFrame	The resultant velocity will be relative to the origin of this frame
expressedInFrame	The resultant velocity will be expressed in this frame
update_kinematics	whether UpdateKinematics() should be called or not (default: true)

Returns: A frame vector pair representing the point velocity of baseFrame relative to relativeFrame and expressed in expressedInFrame

Definition at line 1525 of file Kinematics.cc.

void RobotDynamics::calcRelativeBodySpatialJacobian	(	Model &	model,
		const Math::VectorNd &	Q,
		Math::MatrixNd &	G,
		ReferenceFramePtr	baseFrame,
		ReferenceFramePtr	relativeFrame,
		ReferenceFramePtr	expressedInFrame = `nullptr`,
		bool	update_kinematics = `true`
	)

Computes the jacobian of a frame $J_{i}^{k,j}$ with $i$ being the "base" frame, $j$ being the "relative" frame, and $k$ being the "expressed in" frame. Multiplying this jacobian by a vector of joint velocities will result in the spatial motion of the baseFrame w.r.t relativeFrame expressed in expressedInFrame, a.k.a $v_{i}^{k,j} = J_{i}^{k,j}\dot{q}$ .

Parameters

model
Q
G
baseFrame
relativeFrame
expressedInFrame
update_kinematics

Note: This function only modifies the elements of the jacobian that are nonzero, so be sure the other elements are not nonzero because those elements will not be zero'd. Best practice would be to call G.setZero() before calling this function; If expressedInFrame=nullptr then the expressedInFrame will be set to baseFrame

Definition at line 234 of file Kinematics.cc.

void RobotDynamics::calcRelativeBodySpatialJacobianAndJacobianDot	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::MatrixNd &	G,
		Math::MatrixNd &	GDot,
		ReferenceFramePtr	baseFrame,
		ReferenceFramePtr	relativeFrame,
		ReferenceFramePtr	expressedInFrame = `nullptr`,
		bool	update_kinematics = `true`
	)

Computes both the body spatial jacobian and its time derivative. This function will be a bit more efficient at computing the jacobian and it's time derivative if you need both matrices rather than computing them with separate function calls.

Parameters

model
Q
QDot
G
GDot
baseFrame
relativeFrame
expressedInFrame
update_kinematics

Note: Only non-zero elements of G and GDot will be evaluated. Be sure to call G.setZero() and GDot.setZero() before calling this function

Definition at line 402 of file Kinematics.cc.

void RobotDynamics::calcRelativeBodySpatialJacobianDot	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::MatrixNd &	G,
		ReferenceFramePtr	baseFrame,
		ReferenceFramePtr	relativeFrame,
		ReferenceFramePtr	expressedInFrame = `nullptr`,
		bool	update_kinematics = `true`
	)

Computes the rate of change of the jacobian, $\dot{J}_{i}^{k,j}$ , with $i$ being the "base" frame, $j$ being the relative" frame, and \f$k\f$ being the "expressed in" frame. This jacobian is such that the following is true, $a_{i}^{k,j} = J_{i}^{k,j}\ddot{q} + \dot{J}_{i}^{k,j}\dot{q}$ where $a_{i}^{k,j}$ is the spatial acceleration of frame $i$ w.r.t frame $j$ , expressed in frame $k$ . Additionally the jacobian $J_{i}^{k,j}$ can be computed by RobotDynamics::calcRelativeBodySpatialJacobian.

Parameters

model
Q
QDot
G
baseFrame
relativeFrame
expressedInFrame
update_kinematics

Note: If expressedInFrame=nullptr then the expressedInFrame will be set to baseFrame

Definition at line 301 of file Kinematics.cc.

void RobotDynamics::calcRelativePointJacobian6D	(	Model &	model,
		const Math::VectorNd &	Q,
		Math::MatrixNd &	G,
		ReferenceFramePtr	baseFrame,
		ReferenceFramePtr	relativeFrame,
		ReferenceFramePtr	expressedInFrame = `RobotDynamics::ReferenceFrame::getWorldFrame()`,
		bool	update_kinematics = `true`
	)

Compute the 6D jacobian of the origin of a reference frame relative to the origin of another reference frame and express the result in a third frame.

Parameters

model	Rigid body model
Q	Vector of joint positions
G	$6 \times qdot_size$ Matrix to store result
baseFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion of this frame relative to the argument relativeFrame
relativeFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion vector of baseFrame relative to this reference frame
expressedInFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion vector expressed in this frame
update_kinematics	If true, kinematics will be calculated, defaults to true.

This next bit takes some explaining. I'm not using a SpatialTransform object bc there's subtraction involved and there's no support for subtracting two spatial transforms, mainly because calling transform() does a -Exr operation, and if E is zeros from subtraction and you're kinda screwed. So for the below stuff, we'll be using a spatial matrix directly.

Definition at line 643 of file Kinematics.cc.

void RobotDynamics::calcRelativePointJacobianAndJacobianDot6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::MatrixNd &	G,
		Math::MatrixNd &	GDot,
		ReferenceFramePtr	baseFrame,
		ReferenceFramePtr	relativeFrame,
		ReferenceFramePtr	expressedInFrame = `RobotDynamics::ReferenceFrame::getWorldFrame()`,
		bool	update_kinematics = `true`
	)

Compute the point Jacobian of the origin of baseFrame, relative to the origin of relativeFrame, expressed in expressedInFrame. Also compute that point Jacobians time derivative.

Parameters

model	Rigid body model
Q	Vector of joint positions
QDot	Vector of joint positions
G	$6 \times qdot_size$ Matrix to store jacobian
GDot	$6 \times qdot_size$ Matrix to store jacobian dot
baseFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion of this frame relative to the argument relativeFrame
relativeFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion vector of baseFrame relative to this reference frame
expressedInFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion vector expressed in this frame
update_kinematics	If true, kinematics will be calculated, defaults to true.

Note: If you need both the jacobian and it's time derivative, this function should be more efficient than calculating them both with separate function calls.; Make sure to call G.setZero() and GDot.setZero() before calling this function as it will only evaluate the elements of the jacobian and it's derivative that are nonzero

This while loop is calculating the relative body stuff not in common with base body, so it's subtracted

This chunk here is some pre calculated expressions to try to save some computations

This next bit takes some explaining. I'm not using a SpatialTransform object bc there's subtraction involved and there's no support for subtracting two spatial transforms, mainly because calling transform() does a -Exr operation, and if E is zeros from subtraction and you're kinda screwed. So for the below stuff, we'll be using a spatial matrix directly.

This needs to be a spatial matrix instead of a spatial transform because of the subtraction operator which isn't defined for spatial transform.

This while loop is calculating the relative body stuff not in common with base body, so it's subtracted

This chunk here is some pre calculated expressions to try to save some computations

This next bit takes some explaining. I'm not using a SpatialTransform object bc there's subtraction involved and there's no support for subtracting two spatial transforms, mainly because calling transform() does a -Exr operation, and if E is zeros from subtraction and you're kinda screwed. So for the below stuff, we'll be using a spatial matrix directly.

This needs to be a spatial matrix instead of a spatial transform because of the subtraction operator which isn't defined for spatial transform.

Definition at line 1033 of file Kinematics.cc.

void RobotDynamics::calcRelativePointJacobianDot6D	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		Math::MatrixNd &	G,
		ReferenceFramePtr	baseFrame,
		ReferenceFramePtr	relativeFrame,
		ReferenceFramePtr	expressedInFrame = `RobotDynamics::ReferenceFrame::getWorldFrame()`,
		bool	update_kinematics = `true`
	)

Compute the time derivative of the 6D jacobian of the origin of a reference frame relative to the origin of another reference frame and express the result in a third frame.

Parameters

model	Rigid body model
Q	Vector of joint positions
QDot	Vector of joint positions
G	$6 \times qdot_size$ Matrix to store result
baseFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion of this frame relative to the argument relativeFrame
relativeFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion vector of baseFrame relative to this reference frame
expressedInFrame	Multiplying the resulting jacobian by the vector of joint velocities will result in a motion vector expressed in this frame
update_kinematics	If true, kinematics will be calculated, defaults to true.

This while loop is calculating the relative body stuff not in common with base body, so it's subtracted

This chunk here is some pre calculated expressions to try to save some computations

This needs to be a spatial matrix instead of a spatial transform because of the subtraction operator which isn't defined for spatial transform.

This while loop is calculating the relative body stuff not in common with base body, so it's subtracted

This chunk here is some pre calculated expressions to try to save some computations

This needs to be a spatial matrix instead of a spatial transform because of the subtraction operator which isn't defined for spatial transform.

Definition at line 877 of file Kinematics.cc.

Math::SpatialAcceleration RobotDynamics::calcSpatialAcceleration	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot,
		const unsigned int	body_id,
		const unsigned int	relative_body_id,
		ReferenceFramePtr	expressedInFrame = `nullptr`,
		const bool	update_kinematics = `true`
	)

Compute the spatial acceleration of any body with respect to any other body and express the result in an arbitrary reference frame. The returned RobotDynamicS::Math::SpatialAcceleration can be expressed in the reference frame of choice by supplying the expressedInFrame argument. If left to the default value of nullptr, the result will be expressed in the reference frame corresponding to body_id.

Note: If the expressedInFrame arg is nullptr, the resulting acceleration will be expressed in the referenceFrame corresponding to body_id; that in this notation $\widetilde{v}$ is the same as the cross operator, $v\times$ , commonly used in RBD by Featherstone.

Parameters

model	A RDL robot model
Q	Vector of joint positions
QDot	Vector of joint velocities
QDDot	Vector of joint accelerations
body_id	The
relative_body_id
update_kinematics
expressedInFrame	The reference frame to express the result

Returns

Definition at line 1852 of file Kinematics.cc.

Math::SpatialAcceleration RobotDynamics::calcSpatialAcceleration	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot,
		ReferenceFramePtr	body_frame,
		ReferenceFramePtr	relative_body_frame,
		ReferenceFramePtr	expressedInFrame = `nullptr`,
		const bool	update_kinematics = `true`
	)

Compute the spatial acceleration of any frame with respect to any other frame and express the result in an arbitrary third frame. The returned RobotDynamicS::Math::SpatialAcceleration can be expressed in the reference frame of choice by supplying the expressedInFrame argument. If left to the default value of nullptr, the result will be expressed in body_frame.

Note: If the expressedInFrame arg is nullptr, the resulting acceleration will be expressed in body_frame; that in this notation $\widetilde{v}$ is the same as the cross operator, $v\times$ , commonly used in RBD by Featherstone.

Parameters

model	A RDL robot model
Q	Vector of joint positions
QDot	Vector of joint velocities
QDDot	Vector of joint accelerations
body_frame	The frame of the body
relative_body_frame	The body frame of the relative body
expressedInFrame	The frame the result will be expressed in. Defaults to nullptr which will be world frame
update_kinematics	If true, kinematics will be calculated

Returns: Spatial acceleration of body_frame relative to relative_frame expressed in expressedInFrame

If the desired expressedInFrame of the result is body_frame, then only a_relative_body needs to have its frame change.

User wants the result in a different frame. So change the frame of a_body to the desired frame then return it

If the desired expressedInFrame of the result is body_frame, then only a_relative_body needs to have its frame change.

User wants the result in a different frame. So change the frame of a_body to the desired frame then return it

Definition at line 1784 of file Kinematics.cc.

Math::SpatialMotion RobotDynamics::calcSpatialVelocity	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		ReferenceFramePtr	body_frame,
		ReferenceFramePtr	relative_body_frame,
		ReferenceFramePtr	expressedInFrame = `nullptr`,
		const bool	update_kinematics = `true`
	)

Compute the spatial velocity of any frame with respect to any other frame, expressed in an arbirtary third frame. The returned RobotDynamicS::Math::SpatialMotion is expressed in body_frame unless th expressedInFrame is provided. Each time RobotDynamics::updateKinematics is called, the spatial velocity of each body with respect to the world, and expressed in body frame is calculated. For body $ i $ this is written as $v^{i,0}_{i}$ . Given another body, body $ j $ , the velocity of body $ i $ relative to body $ j $ and expressed in body frame $ i $ is computed by,

$v^{i,j}_{i} = v^{i,0}_{i} - ^{i}X_{j} v^{j,0}_{j}$

.

Parameters

model	A RDL robot model
Q	Vector of joint positions
QDot	Vector of joint velocities
body_frame	The primary frame
relative_body_frame	The frame the result will be w.r.t
expressedInFrame	Frame the result will be expressed in. Default value=nullptr in which case the result will be expressed in the frame corresponding to body_id
update_kinematics

Returns

Note: If no expressedInFrame is given then the result will be expressed in the frame corresponding to the arg body_id.

Definition at line 1670 of file Kinematics.cc.

Math::SpatialMotion RobotDynamics::calcSpatialVelocity	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const unsigned int	body_id,
		const unsigned int	relative_body_id,
		ReferenceFramePtr	expressedInFrame = `nullptr`,
		const bool	update_kinematics = `true`
	)

Compute the spatial velocity of any body with respect to any other body. The returned RobotDynamicS::Math::SpatialMotion is expressed in the body frame of body body_id. Each time RobotDynamics::updateKinematics is called, the spatial velocity of each body with respect to the world, and expressed in body frame is calculated. For body $ i $ this is written as $v^{i,0}_{i}$ . Given another body, body $ j $ , the velocity of body $ i $ relative to body $ j $ and expressed in body frame $ i $ is computed by,

$v^{i,j}_{i} = v^{i,0}_{i} - ^{i}X_{j} v^{j,0}_{j}$

.

Parameters

model	A RDL robot model
Q	Vector of joint positions
QDot	Vector of joint velocities
body_id	The
relative_body_id
expressedInFrame	Frame the result will be expressed in. Default value=nullptr in which case the result will be expressed in the frame corresponding to body_id
update_kinematics

Returns

Note: If no expressedInFrame is given then the result will be expressed in the frame corresponding to the arg body_id.

Definition at line 1716 of file Kinematics.cc.

bool RobotDynamics::inverseKinematics	(	Model &	model,
		const Math::VectorNd &	Qinit,
		const std::vector< unsigned int > &	body_id,
		const std::vector< Math::Vector3d > &	body_point,
		const std::vector< Math::Vector3d > &	target_pos,
		Math::VectorNd &	Qres,
		double	step_tol = `1.0e-12`,
		double	lambda = `0.01`,
		unsigned int	max_iter = `50`
	)

Computes the inverse kinematics iteratively using a damped Levenberg-Marquardt method (also known as Damped Least Squares method)

Parameters

model	rigid body model
Qinit	initial guess for the state
body_id	a vector of all bodies for which we we have kinematic target positions
body_point	a vector of points in body local coordinates that are to be matched to target positions
target_pos	a vector of target positions
Qres	output of the computed inverse kinematics
step_tol	tolerance used for convergence detection
lambda	damping factor for the least squares function
max_iter	maximum number of steps that should be performed

Returns: true on success, false otherwise

This function repeatedly computes

$Qres = Qres + \Delta \theta$

$\Delta \theta = G^T (G^T G + \lambda^2 I)^{-1} e$

where $G = G(q) = \frac{d}{dt} g(q(t))$ and $e$ is the correction of the body points so that they coincide with the target positions. The function returns true when $||\Delta \theta||_2 \le$ step_tol or if the error between body points and target gets smaller than step_tol. Otherwise it returns false.

The parameter $\lambda$ is the damping factor that has to be chosen carefully. In case of unreachable positions higher values (e.g 0.9) can be helpful. Otherwise values of 0.0001, 0.001, 0.01, 0.1 might yield good results. See the literature for best practices.

Warning: The actual accuracy might be rather low (~1.0e-2)! Use this function with a grain of suspicion.

void RobotDynamics::updateAccelerations	(	Model &	model,
		const Math::VectorNd &	QDDot
	)

Computes only the accelerations of the bodies.

Parameters

model	the model
QDDot	the generalized accelerations of the joints

Definition at line 152 of file Kinematics.cc.

void RobotDynamics::updateKinematics	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot
	)

Updates and computes velocities and accelerations of the bodies.

This function updates the kinematic variables such as body velocities, accelerations, and reference frames in the model to reflect the variables passed to this function.

Parameters

model	the model
Q	the positional variables of the model
QDot	the generalized velocities of the joints
QDDot	the generalized accelerations of the joints

Definition at line 25 of file Kinematics.cc.

void RobotDynamics::updateKinematicsCustom	(	Model &	model,
		const Math::VectorNd *	Q,
		const Math::VectorNd *	QDot = `nullptr`,
		const Math::VectorNd *	QDDot = `nullptr`
	)

Selectively updates model internal states of body positions, velocities and/or accelerations.

This function updates the kinematic variables such as body velocities, accelerations, and reference frames in the model to reflect the variables passed to this function.

In contrast to RobotDynamics::updateKinematics() this function allows to update the model state with values one is interested and thus reduce computations (e.g. only positions, only positions + velocities, positions + velocities + accelerations).

Parameters

model	the model
Q	the positional variables of the model
QDot	the generalized velocities of the joints
QDDot	the generalized accelerations of the joints

Note: for each non-null input, the previous input must also be non-null. For example, this call is invalid, updateKinematicsCustom(model, &q, nullptr, &qddot). Because QDot is null, the input qddot will be ignored.

Definition at line 72 of file Kinematics.cc.

void RobotDynamics::updateKinematicsCustomParallel	(	Model &	model,
		const Math::VectorNd *	Q,
		const Math::VectorNd *	QDot = `nullptr`,
		const Math::VectorNd *	QDDot = `nullptr`
	)

Selectively updates model internal states of body positions, velocities and/or accelerations and spawns threads far each branched chain.

This function updates the kinematic variables such as body velocities, accelerations, and reference frames in the model to reflect the variables passed to this function.

In contrast to RobotDynamics::updateKinematics() this function allows to update the model state with values one is interested and thus reduce computations (e.g. only positions, positions + velocities, or positions + velocities + accelerations).

Parameters

model	the model
Q	the positional variables of the model
QDot	the generalized velocities of the joints, defaults nullptr
QDDot	the generalized accelerations of the joints, defaults nullptr

Note: This function utilizes threading that may result in faster computation of the kinematics for certain models depending on the structure of the kinematic tree as well as the computational capabailities of the computer.; If the number of additional threads available to the model(See the RobotDynamics::Model constructor and it's behavior) this will default to calling updateKinematicsCustom; for each non-null input, the previous input must also be non-null. For example, this call is invalid, updateKinematicsCustom(model, &q, nullptr, &qddot). Because QDot is null, the input QDDot will be ignored.; The main thread will busy wait until all threads have finished their computation. This may result in high CPU usage for the main thread while it waits; however, in comparing the performance of parallel computation of kinematics it was found that implementing the wait functionality with standard synchronization tools(conditionals etc) actually resulted in longer computation times probably due to the time it takes for the main thread to sleep and then wake up again.

Definition at line 194 of file Kinematics.cc.

void RobotDynamics::updateKinematicsParallel	(	Model &	model,
		const Math::VectorNd &	Q,
		const Math::VectorNd &	QDot,
		const Math::VectorNd &	QDDot
	)

Updates and computes velocities and accelerations of the bodies.

This function updates the kinematic variables such as body velocities, accelerations, and reference frames in the model to reflect the variables passed to this function.

Parameters

model	the model
Q	the positional variables of the model
QDot	the generalized velocities of the joints
QDDot	the generalized accelerations of the joints

Note: This function utilizes threading that may result in faster computation of the kinematics for certain models depending on the structure of the kinematic tree as well as the computational capabailities of the computer.; If the number of additional threads available to the model(See the RobotDynamics::Model constructor and it's behavior) this will default to calling updateKinematics; The main thread will busy wait until all threads have finished their computation. This may result in high CPU usage for the main thread while it waits; however, in comparing the performance of parallel computation of kinematics it was found that implementing the wait functionality with standard synchronization tools(conditionals etc) actually resulted in longer computation times probably due to the time it takes for the main thread to sleep and then wake up again.

Definition at line 214 of file Kinematics.cc.

Functions

Detailed Description

Function Documentation