#include <CDynamicsScl.hpp>

Inheritance diagram for scl::CDynamicsScl:

Collaboration diagram for scl::CDynamicsScl:

Public Member Functions
virtual sBool	computeGCModel (const SRobotSensors arg_sensor_data, SGcModel arg_gc_model)

virtual sBool	computeTransformsForAllLinks (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q)

virtual sBool	computeTransform (SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q)

virtual sBool	computeTransformToAncestor (Eigen::Affine3d &arg_T, SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q)

virtual sBool	computeJacobianWithTransforms (Eigen::MatrixXd &arg_J, SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local)

virtual sBool	computeJacobian (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local) const

virtual sBool	computeJacobian (Eigen::MatrixXd &arg_J, SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local, const bool arg_recompute_transforms=true)

sBool	computeJacobianComForAllLinks (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q) const

sBool	computeInertiaGC (Eigen::MatrixXd &ret_Mgc, sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q)

sBool	computeForceGravityGC (Eigen::VectorXd &ret_FgravGC, sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q)

virtual sFloat	computeEnergyKinetic (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q, const Eigen::VectorXd &arg_dq)

virtual sFloat	computeEnergyPotential (sutil::CMappedTree< std::string, SRigidBodyDyn > &arg_tree, const Eigen::VectorXd &arg_q)

virtual sBool	computeImpulseForce (const SRobotSensors &arg_sensor_data, const SGcModel &arg_gc_model, sFloat arg_coeff_rest, sFloat arg_collision_time, scl::SForceContact &ret_force_)

	CDynamicsScl ()

virtual	~CDynamicsScl ()

virtual sBool	init (const SRobotParsed &arg_robot_data)

virtual sBool	computeJacobian (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local)

virtual sBool	computeJacobianTrans (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local)

virtual sBool	computeJacobianRot (Eigen::MatrixXd &arg_J, const SRigidBodyDyn &arg_link, const SRigidBodyDyn *arg_ancestor, const Eigen::VectorXd &arg_q, const Eigen::Vector3d &arg_pos_local)

virtual sBool	computeExternalContacts (sutil::CMappedList< std::string, SForce > &arg_contacts)

virtual sBool	integrate (SRobotIO &arg_inputs, const sFloat arg_time_interval)

virtual sBool	hasBeenInit ()

Protected Attributes
sBool	has_been_init_

const SRobotParsed *	robot_parsed_data_

Detailed Description

This class implements dynamics algorithms for control and simulation using the scl library.

The primary goal of this implementation—given that there are alternatives—is to enable quick Jacobian and transformation matrix computations.

The main feature of this dynamics implementation is that it is truly stateless.

It does not (presently) implement an integrator so plan on using an alternative Physics engine (like Tao).

Constructor & Destructor Documentation

scl::CDynamicsScl::CDynamicsScl ( )

inline

Default constructor sets the initialization state to false

virtual scl::CDynamicsScl::~CDynamicsScl ( )

inlinevirtual

Default destructor does nothing

Member Function Documentation

sFloat scl::CDynamicsScl::computeEnergyKinetic	(	sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q,
		const Eigen::VectorXd &	arg_dq
	)

virtual

Gets the robot's kinetic energy

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.
arg_dq	The current generalized velocities.

Reimplemented from scl::CDynamicsBase.

sFloat scl::CDynamicsScl::computeEnergyPotential	(	sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q
	)

virtual

Gets the robot's potential energy

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.

Reimplemented from scl::CDynamicsBase.

virtual sBool scl::CDynamicsBase::computeExternalContacts ( sutil::CMappedList< std::string, SForce > & arg_contacts )

inlinevirtualinherited

Calculates the collision forces for the robot to which this dynamics object is assigned.

Uses std::string based force lookup. The dynamics implementation should support this (maintain a map or something).

NOTE : The dynamics engine may delete forces from the passed mapped list at will. Do not store pointers to them at random places in your code.

As of now, this is optional for dynamics engines

Parameters

arg_contacts This is where the simulator will store the contact forces. It may use the std::string to identify when to remove or re-add forces.

Reimplemented in scl_ext::CDynamics3d.

sBool scl::CDynamicsScl::computeForceGravityGC	(	Eigen::VectorXd &	ret_FgravGC,
		sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q
	)

inline

Updates the center of mass Jacobians for the robot to which this dynamics object is assigned. dx_com_origin_coords = tree_link.J_com_ * dq

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.

sBool scl::CDynamicsScl::computeGCModel	(	const SRobotSensors *	arg_sensor_data,
		SGcModel *	arg_gc_model
	)

virtual

Updates the generalized coordinate model (Everything in SGcModel).

This is the most efficient method to access the standard matrices. Computing transformations and Jacobians individually is typically wasteful.

Updates the joint space model matrices (Everything in SGcModel)

Parameters

arg_sensor_data	Current robot state. q, dq, ddq, sensed generalized forces and perceived external forces.
arg_gc_model	Individual link Jacobians, and composite inertial, centrifugal/coriolis gravity estimates.

Implements scl::CDynamicsBase.

sBool scl::CDynamicsScl::computeImpulseForce	(	const SRobotSensors &	arg_sensor_data,
		const SGcModel &	arg_gc_model,
		sFloat	arg_coeff_rest,
		sFloat	arg_collision_time,
		scl::SForceContact &	ret_force_
	)

virtual

Computes an instantaneous impulse force and the duration for which it should be applied

Parameters

arg_sensor_data	Current robot state. q, dq, ddq, sensed generalized forces and perceived external forces.
arg_gc_model	Individual link Jacobians, and composite inertial, centrifugal/coriolis gravity estimates.
arg_coeff_rest	Coefficient of restitution
arg_collision_time	Time of collision
ret_force_	A contact points at which the contact force will be applied.

sBool scl::CDynamicsScl::computeInertiaGC	(	Eigen::MatrixXd &	ret_Mgc,
		sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q
	)

inline

Updates the generalized inertia for the robot to which this dynamics object is assigned. Mgc = sum_link_i [ J_com_' * M_com_i * J_com_]

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.

sBool scl::CDynamicsScl::computeJacobian	(	Eigen::MatrixXd &	arg_J,
		const SRigidBodyDyn &	arg_link,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)		const

virtual

Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_global_origin = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link. ** CONST VERSION **

For linear joints: Jv => dq = The linear velocity vector simply needs to be rotated to the global frame Jω => Zero. There is no contribution to the angular velocity.

For rotational joints: Jv => cross product of operational point and angular velocity at the given dof. Jω => The angular velocity vector simply needs to be rotated to the global frame.

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

Reimplemented from scl::CDynamicsBase.

virtual sBool scl::CDynamicsScl::computeJacobian	(	Eigen::MatrixXd &	arg_J,
		SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local,
		const bool	arg_recompute_transforms = `true`
	)

inlinevirtual

Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_ancestor_coords = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link.

Parameters

arg_J	The Jacobian will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_ancestor	The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).
arg_recompute_transforms	Whether to recompute the transformations up to the ancestor Default = true. Set to false to speed things up.

virtual sBool scl::CDynamicsBase::computeJacobian	(	Eigen::MatrixXd &	arg_J,
		const SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)

inlinevirtualinherited

Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_ancestor_coords = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link.

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_ancestor	The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

sBool scl::CDynamicsScl::computeJacobianComForAllLinks	(	sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q
	)		const

inline

Updates the center of mass Jacobians for the robot to which this dynamics object is assigned. dx_com_origin_coords = tree_link.J_com_ * dq

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.

virtual sBool scl::CDynamicsBase::computeJacobianRot	(	Eigen::MatrixXd &	arg_J,
		const SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)

inlinevirtualinherited

Calculates the Jacobian for the robot to which this dynamics object is assigned. ONLY for rotation. Ie. size(Jx) = (3,ndof) dx_ancestor_coords = Jx . dq

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_ancestor	The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

virtual sBool scl::CDynamicsBase::computeJacobianTrans	(	Eigen::MatrixXd &	arg_J,
		const SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)

inlinevirtualinherited

Calculates the Jacobian for the robot to which this dynamics object is assigned. ONLY for translation. Ie. size(Jx) = (3,ndof) dx_ancestor_coords = Jx . dq

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated
arg_ancestor	The link up to which the Jacobian is to be calculated Pass NULL to compute the Jacobian up to the global root.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

sBool scl::CDynamicsScl::computeJacobianWithTransforms	(	Eigen::MatrixXd &	arg_J,
		SRigidBodyDyn &	arg_link,
		const Eigen::VectorXd &	arg_q,
		const Eigen::Vector3d &	arg_pos_local
	)

virtual

Calculates the Jacobian for the robot to which this dynamics object is assigned. dx_global_origin = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link.

Calculates the Jacobian for the robot to which this dynamics object is assigned. dx = Jx . dq The Jacobian is specified by a link and an offset (in task space dimensions)from that link.

Parameters

arg_J	The Jacobain will be saved here.
arg_link	The link at which the Jacobian is to be calculated. Transforms are updated in this link and ancestors in the tree.
arg_q	The current generalized coordinates.
arg_pos_local	The offset from the link's frame (in link coordinates).

Reimplemented from scl::CDynamicsBase.

sBool scl::CDynamicsScl::computeTransform	(	SRigidBodyDyn &	arg_link,
		const Eigen::VectorXd &	arg_q
	)

virtual

Calculates the Transformation Matrix for the robot to which this dynamics object is assigned. x_parent_link_coords = arg_link.T_lnk_ * x_link_coords Note that the matrix is stored in the passed link data struct.

Parameters

arg_link	The link at which the transformation matrix is to be calculated
arg_q	The current generalized coordinates.

Reimplemented from scl::CDynamicsBase.

sBool scl::CDynamicsScl::computeTransformsForAllLinks	(	sutil::CMappedTree< std::string, SRigidBodyDyn > &	arg_tree,
		const Eigen::VectorXd &	arg_q
	)

inlinevirtual

Updates the Transformation Matrices for the robot to which this dynamics object is assigned. x_ancestor_link_coords = arg_link.T_lnk_ * x_link_coords

Parameters

arg_tree	The tree for which the transformation matrices are to be updated
arg_q	The current generalized coordinates.

Reimplemented from scl::CDynamicsBase.

sBool scl::CDynamicsScl::computeTransformToAncestor	(	Eigen::Affine3d &	arg_T,
		SRigidBodyDyn &	arg_link,
		const SRigidBodyDyn *	arg_ancestor,
		const Eigen::VectorXd &	arg_q
	)

virtual

Calculates the Transformation Matrix for the robot to which this dynamics object is assigned. x_ancestor_link_coords = arg_link.T_lnk_ * x_link_coords

Note that the local transformation matrices are updated in all the link data structs from arg_link to arg_ancestor.

Also note that there are two origins, robot origin, which actually has a node allocated for it. And the global scl origin, for which you should pass in a NULL as ancestor.

Calculates the Transformation Matrix for the robot to which this dynamics object is assigned. x_ancestor_link_coords = arg_link.T_lnk_ * x_link_coords Note that the local transformation matrices are updated in all the link data structs from arg_link to arg_ancestor.

Parameters

arg_T	The transformation matrix in which to store the answer
arg_link	The link at which the transformation matrix is to be calculated
arg_ancestor	The link up to which the transformation matrix is to be calculated Pass NULL to compute the transform up to the global root.
arg_q	The current generalized coordinates.

Reimplemented from scl::CDynamicsBase.

virtual sBool scl::CDynamicsBase::hasBeenInit ( )

inlinevirtualinherited

Initialization state

sBool scl::CDynamicsScl::init ( const SRobotParsed & arg_robot_data )

virtual

Initializes the dynamics to be computed for a specific robot. Returns: true : Succeeds in creating a dynamics object

false : If it can't find the robot or if the information is inconsistent with what the implementation requires, it returns false

Initializes the dynamics to be computed for a specific robot. Returns: true : Always. false : If something is seriously wrong

Implements scl::CDynamicsBase.

virtual sBool scl::CDynamicsBase::integrate	(	SRobotIO &	arg_inputs,
		const sFloat	arg_time_interval
	)

inlinevirtualinherited

Integrates the robot's state. Uses the given applied forces, torques, positions and velocities and its internal dynamic model to compute new positions and velocities. Operates on the SRobotIO data structure.

Reads from, and updates: arg_inputs_.sensors_.q_, dq_, ddq_

Reads from: arg_inputs_.sensors_.forces_external_ arg_inputs_.actuators_.force_gc_commanded_

Parameters

arg_inputs	The existing generalized coordinates, velocities and accelerations + The generalized forces + task (euclidean) forces and the list of contact points and links.
arg_time_interval	The time across which the system should integrate the dynamics. Could take fixed steps or dynamic ones in between. Up to the integrator implementation.

Reimplemented in scl::CSimbodyDynamics, scl::CDynamicsTao, scl_ext::CDynamics3d, and scl_ext::CDynamicsSclSpatial.

Member Data Documentation

sBool scl::CDynamicsBase::has_been_init_

protectedinherited

True if the dynamics object has been initialized for a given robot

const SRobotParsed* scl::CDynamicsBase::robot_parsed_data_

protectedinherited

A read-only pointer to access parsed data

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

/home/samir/Code/control/scl.git/src/scl/dynamics/scl/CDynamicsScl.hpp
/home/samir/Code/control/scl.git/src/scl/dynamics/scl/CDynamicsScl.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation