#include <CDynamicsSclSpatial.hpp>

Inheritance diagram for scl_ext::CDynamicsSclSpatial:

Collaboration diagram for scl_ext::CDynamicsSclSpatial:

Public Member Functions
bool	forwardDynamicsCRBA (const scl::SRobotIO arg_io_data, scl::SGcModel arg_gc_model, Eigen::VectorXd &ret_ddq)

bool	forwardDynamicsABA (const scl::SRobotIO arg_io_data, scl::SGcModel arg_gc_model, Eigen::VectorXd &ret_ddq)

bool	inverseDynamicsNER (const scl::SRobotIO arg_io_data, scl::SGcModel arg_gc_model, Eigen::VectorXd &ret_fgc)

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

bool	integrator (scl::SRobotIO &arg_io_data, scl::SGcModel *arg_gc_model, const scl::sFloat arg_time_interval)

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

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

bool	calculateKineticEnergy (const scl::SRobotIO arg_io_data, scl::SGcModel arg_gc_model, scl::sFloat &ret_kinetic_energy)

bool	calculatePotentialEnergy (const scl::SRobotIO arg_io_data, scl::SGcModel arg_gc_model, scl::sFloat &ret_potential_energy)

virtual bool	computeGCModel (const scl::SRobotSensors arg_sensor_data, scl::SGcModel arg_gc_model)

virtual bool	init (const scl::SRobotParsed &arg_robot_data)

	CDynamicsSclSpatial ()

virtual	~CDynamicsSclSpatial ()

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, 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	hasBeenInit ()

Protected Attributes
sBool	has_been_init_

const SRobotParsed *	robot_parsed_data_

Detailed Description

This class calculates forward and inverse dynamics using algorithms that rely on spatial vectors. It provides a generic way to integrate three algorithms with scl:

CRBA
ABA
RNEA

Constructor & Destructor Documentation

scl_ext::CDynamicsSclSpatial::CDynamicsSclSpatial ( )

Constructor

scl_ext::CDynamicsSclSpatial::~CDynamicsSclSpatial ( )

virtual

The Destructor

Member Function Documentation

bool scl_ext::CDynamicsSclSpatial::calculateKineticEnergy	(	const scl::SRobotIO *	arg_io_data,
		scl::SGcModel *	arg_gc_model,
		scl::sFloat &	ret_kinetic_energy
	)

Gets the robot's kinetic energy using spatial algorithm

Parameters

arg_io_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.
ret_kinetic_energy	the returned kinetic energy

bool scl_ext::CDynamicsSclSpatial::calculatePotentialEnergy	(	const scl::SRobotIO *	arg_io_data,
		scl::SGcModel *	arg_gc_model,
		scl::sFloat &	ret_potential_energy
	)

Gets the robot's potential energy using spatial algorithm

Parameters

arg_io_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.
ret_potential_energy	the returned potential energy

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

inlinevirtual

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.

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

inlinevirtual

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.

virtual bool scl_ext::CDynamicsSclSpatial::computeGCModel	(	const scl::SRobotSensors *	arg_sensor_data,
		scl::SGcModel *	arg_gc_model
	)

inlinevirtual

Updates the generalized coordinate model matrices (Everything in SGcModel).

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

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 intertial, centrifugal/coriolis gravity estimates.

Implements scl::CDynamicsBase.

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

inlinevirtualinherited

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 : NOTE : The regular version should call this. **

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 in scl::CDynamicsScl.

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).

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).

virtual sBool scl::CDynamicsBase::computeJacobianWithTransforms	(	Eigen::MatrixXd &	arg_J,
		SRigidBodyDyn &	arg_link,
		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_global_origin = 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 in scl::CDynamicsScl.

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

inlinevirtualinherited

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 in scl::CDynamicsScl.

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

inlinevirtualinherited

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 in scl::CDynamicsScl.

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

inlinevirtualinherited

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.

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 in scl::CDynamicsScl.

bool scl_ext::CDynamicsSclSpatial::forwardDynamicsABA	(	const scl::SRobotIO *	arg_io_data,
		scl::SGcModel *	arg_gc_model,
		Eigen::VectorXd &	ret_ddq
	)

Calculate Joint Acceleration using Articulated Body Algorithm

Parameters

arg_io_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.
ret_ddq	The returned generalized accelerations (Eg. joint accelerations)

bool scl_ext::CDynamicsSclSpatial::forwardDynamicsCRBA	(	const scl::SRobotIO *	arg_io_data,
		scl::SGcModel *	arg_gc_model,
		Eigen::VectorXd &	ret_ddq
	)

Calculate Joint Acceleration using Composite Rigid Body Algorithm

Parameters

arg_io_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.
ret_ddq	The returned generalized accelerations (Eg. joint accelerations)

virtual sBool scl::CDynamicsBase::hasBeenInit ( )

inlinevirtualinherited

Initialization state

virtual bool scl_ext::CDynamicsSclSpatial::init ( const scl::SRobotParsed & arg_robot_data )

inlinevirtual

Initializes the dynamics to be computed for a specific robot. This implementation is stateless. Ideally all implementations should be stateless...

Implements scl::CDynamicsBase.

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

inlinevirtual

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 from scl::CDynamicsBase.

bool scl_ext::CDynamicsSclSpatial::integrator	(	scl::SRobotIO &	arg_io_data,
		scl::SGcModel *	arg_gc_model,
		const scl::sFloat	arg_time_interval
	)

Calculate joint position and velocity using Newton numerical integrator

Parameters

arg_io_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_time_interval	step dt time

bool scl_ext::CDynamicsSclSpatial::inverseDynamicsNER	(	const scl::SRobotIO *	arg_io_data,
		scl::SGcModel *	arg_gc_model,
		Eigen::VectorXd &	ret_fgc
	)

Calculate joint Torque using Newton Euler Recursive Algorithm

Parameters

arg_io_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.
ret_fgc	The returned generalized forces (Eg. joint torques)

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_ext/dynamics/scl_spatial/CDynamicsSclSpatial.hpp
/home/samir/Code/control/scl.git/src/scl_ext/dynamics/scl_spatial/CDynamicsSclSpatial.cpp

Public Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation