Arm Model class for serial kinematic chains (manipulators). More...

#include <ArmModel.h>

Inheritance diagram for rml::ArmModel:

Public Member Functions
	ArmModel (const std::string id) noexcept(false)
	Default constructor.

virtual	~ArmModel ()
	Default destructor.

void	AddJointLink (JointType type, const Eigen::Vector3d &axis, const Eigen::TransformationMatrix &baseTransf, double jointLimMin, double jointLimMax)
	Adds a link to the kinematic chain of the model.

void	AddFixedLink (const Eigen::TransformationMatrix &baseTransf)
	Adds a fixed link to the kinematic chain of the model.

virtual void	JointsPosition (const Eigen::VectorXd q) noexcept(false)
	Set the joint position.

virtual auto	JointsPosition () const -> const Eigen::VectorXd &
	Get the moving joint position.

virtual void	JointsVelocity (const Eigen::VectorXd qdot) noexcept(false)
	Set the moving joints velocity.

virtual auto	JointsVelocity () const -> const Eigen::VectorXd &
	Get the moving joints velocity.

void	JointsAcceleration (const Eigen::VectorXd qddot) noexcept(false)
	Set the moving joints acceleration.

auto	JointsAcceleration () const -> const Eigen::VectorXd &
	Get moving joints acceleration.

void	AttachRigidBodyFrame (std::string frameID, std::string attachedFrameID, Eigen::TransformationMatrix attachedFrameID_T_frameID) noexcept(false)
	Method adding a rigid body frame to a joint.

Eigen::TransformationMatrix	TransformationMatrix (const std::string &frameID) noexcept(false)
	Method returning the transformation matrix related to the input frameID wrt to the arm base.

Eigen::TransformationMatrix	TransformationMatrix (const std::string &frameID_j, const std::string &frameID_k)
	Method returing a transformation matrix from frameID_j to frameID_k, i.e. jTk.

virtual Eigen::MatrixXd	Jacobian (const std::string &frameID) noexcept(false)
	Method returning the jacobian related to the input frameID wrt to the arm base.

Eigen::MatrixXd	ManipulabilityJacobian (const std::string &frameID)
	Method returning the manipulability jacobian related to the input frameID wrt to the arm base.

virtual auto	NumJoints () const -> unsigned int
	Method returning the arm number of moving joints.

auto	dJdq () const -> const std::vector< Eigen::MatrixXd > &
	Method returning dJdq evaluated numerically.

RobotLink &	Link (int jointIndex) noexcept(false)
	Method returning the arm link.

auto	IsModelInitialized () const -> bool
	Method returning true if the model is initialized false otherwise.

auto	ControlVector () const -> const Eigen::VectorXd &
	Method returning the arm control vector.

void	ControlVector (const Eigen::VectorXd controlRef) noexcept(false)
	Method setting the arm control vector.

double	Manipulability (const std::string &frameID)
	Method returning the arm manipulability.

Eigen::MatrixXd	DexterityJacobian (const std::string &frameID)
	Method returning the dexterity jacobian related to the input frameID wrt to the arm base.

double	Dexterity (const std::string &frameID)
	Method returning the arm dexterity.

auto	ID () const -> const std::string &
	Method returning the arm id.

auto	ID () -> std::string &
	Method setting the arm id.

std::vector< std::string >	GetRigidBodyFrameIDs () const noexcept
	Method returning the list of rigid body frame IDs.

std::vector< std::string >	GetJointFrameIDs () const noexcept
	Method returning the list of joint frame IDs.

std::vector< std::string >	GetJacobianFrameIDs () const noexcept
	Method returning the list of Jacobian frame IDs.

std::string	GetEndEffectorFrameID () const noexcept
	Method returning the end-effector frame ID.

Protected Member Functions
void	EvaluateTotalForwardGeometry ()
	Method evaluating the total forward geometry for the arm.

virtual void	EvaluatedJdqNumeric ()
	Evaluates numerically the Jacobian derivative w.r.t. joint variations.

virtual Eigen::MatrixXd	EvaluateManipulability (const std::string &frameID)
	Evaluates the manipulability measure and its Jacobian This method returns the manipulability measure and its Jacobian.

virtual Eigen::MatrixXd	EvaluateDexterity (const std::string &frameID)
	Evaluates the dexterity measure and its Jacobian This method returns the dexterity measure and its Jacobian.

Eigen::TransformationMatrix	EvaluateRigidBodyTransf (const std::string &frameID)
	Method returning the attached body frame wrt to the arm base.

Eigen::MatrixXd	EvaluateRigidBodyJacobian (const std::string &frameID)
	Method returning the attached body frame jacobian wrt to the arm base.

Eigen::MatrixXd	EvaluateBase2JointJacobian (unsigned int jointIndex)
	Evaluates the jacobian matrix (w.r.t. robot base) of the specified joint.

void	ForwardDirectGeometry (unsigned int jointNumber)
	Method performing the forward direct geometry untill the input joint number.

void	BackwardDirectGeometry (unsigned int jointNumber, unsigned int endEffectorIndex)
	Backward Direct Geometry from the input joint number to the input end effector index.

Protected Attributes
bool	modelInitialized_
	boolean stating whether the model is initialized

bool	isMapInitialized_
	boolean stating whether the transformation and jacobian maps are initialized

unsigned int	totalNumJoints_

unsigned int	movingNumJoints_
	moving joints number

std::vector< unsigned int >	movingJoints_
	vector containing the indexes of the moving joints

std::vector< RobotLink >	links_
	vector of the arm links

std::unordered_map< std::string, IndexedTMat >	rigidBodyFrames_

std::unordered_map< std::string, Eigen::MatrixXd >	jacobians_

std::unordered_map< std::string, Eigen::TransformationMatrix >	transformation_

std::unordered_map< std::string, Eigen::MatrixXd >	manipulabilityJacobians_
	map of the manipulability jacobians

std::unordered_map< std::string, double >	manipulability_
	map of the manipulability values

Eigen::VectorXd	q_total_

Eigen::VectorXd	q_moving_

Eigen::VectorXd	q_dot_moving_

Eigen::VectorXd	q_ddot_moving_

Eigen::VectorXd	controlRef_

std::vector< Eigen::TransformationMatrix >	baseTei_

std::vector< Eigen::TransformationMatrix >	biTei_

Eigen::TransformationMatrix	baseTbi_

Eigen::TransformationMatrix	Tz_
	transformation matrix for rotation or prismatic joint

Eigen::Vector3d	base_ki_

std::vector< Eigen::Vector6d >	h_

std::vector< Eigen::MatrixXd >	dJdq_
	dJdq evaluated numerically

Eigen::MatrixXd	djdqJpinv_
	djdq * jacobian pseudoinverse

Eigen::RotationMatrix	I3_
	identity matrix

bool	modelReadFromFile_
	boolean stating whether the model is read from file

std::string	id_
	arm id

std::unordered_map< std::string, Eigen::MatrixXd >	dexterityJacobians_
	map of the manipulability jacobians

std::unordered_map< std::string, double >	dexterity_
	map of the manipulability values

Eigen::MatrixXd	Jdjdq_
	jacobian * djdq

Eigen::MatrixXd	Jpinvdjpinvdq_
	jacobian pseudoinverse * djpinvdq

Detailed Description

Arm Model class for serial kinematic chains (manipulators).

This class implements a model for serial kinematic chains, the whole structure is identified by an ID given by the user in the constructor. It contains a vector of RobotLink which can be added up using the AddFixedLink() function for fixed links and the AddJointLink() one for moving links, the two methods attach each new link to the previous one. Optionally we can also add rigid bodies to each link with the function SetRigidBodyFrame() which takes as input the frameName to identify the frame to which attach the body and a IDstring to identify it.

Once the model has been constructed, the class provides all the necessary functions to evaluate the transformation and jacobian matrices for every link and rigid body added. Each frame is identified by a label. The following policy is used:

Joint Frame: armID + FrameID::Joint + joint°;
Rigid Body: armID + "_" + frameID

In order to get the transformation matrix the GetTransformation() method is provided. The method takes as input a string which is the id of the frame asked.

All the transformation matrices are expressed wrt the arm base. In order to get the jacobian the GetJacobian() method is provided. The method takes as input a string which is the id of the frame asked. All the jacobian matrix are expressed wrt the arm base.

This class has been designed with two use cases in mind:

Used by itself in can be exploited to control fixed base manipulators.
Loaded in a RobotModel, using RobotModel::LoadArm(), it can be used to control either mobile manipulators or multiple arms.

Constructor & Destructor Documentation

◆ ArmModel()

rml::ArmModel::ArmModel ( const std::string id )

Default constructor.

Parameters

[in] id Arm id.

◆ ~ArmModel()

virtual rml::ArmModel::~ArmModel ( )

virtual

Default destructor.

Member Function Documentation

◆ AddFixedLink()

void rml::ArmModel::AddFixedLink ( const Eigen::TransformationMatrix & baseTransf )

Adds a fixed link to the kinematic chain of the model.

Parameters

baseTransf Transformation matrix from previous to current

◆ AddJointLink()

void rml::ArmModel::AddJointLink	(	JointType	type,
		const Eigen::Vector3d &	axis,
		const Eigen::TransformationMatrix &	baseTransf,
		double	jointLimMin,
		double	jointLimMax
	)

Adds a link to the kinematic chain of the model.

Parameters

type	The JointType, whether: JointType::Revolute, JointType::Prismatic
axis	The axis along which the joint rotates or translates
baseTransf	Transformation matrix from previous link to current link
jointLimMin	Minimum excursion for the joint
jointLimMax	Maximum excursion for the joint

◆ AttachRigidBodyFrame()

void rml::ArmModel::AttachRigidBodyFrame	(	std::string	frameID,
		std::string	attachedFrameID,
		Eigen::TransformationMatrix	attachedFrameID_T_frameID
	)

Method adding a rigid body frame to a joint.

Parameters

frameID	Id of the frame to add
attachedFrameID	ID of the frame to attach
TMat	Transformation matrix of the frame.

◆ BackwardDirectGeometry()

void rml::ArmModel::BackwardDirectGeometry	(	unsigned int	jointNumber,
		unsigned int	endEffectorIndex
	)

protected

Backward Direct Geometry from the input joint number to the input end effector index.

Parameters

jointNumber	which joint is intended as last of the chain
endEffectorIndex

◆ ControlVector() [1/2]

auto rml::ArmModel::ControlVector ( ) const -> const Eigen::VectorXd&

inline

Method returning the arm control vector.

Returns: Control vector

◆ ControlVector() [2/2]

void rml::ArmModel::ControlVector ( const Eigen::VectorXd controlRef )

Method setting the arm control vector.

Parameters

controlRef control vector

◆ Dexterity()

double rml::ArmModel::Dexterity ( const std::string & frameID )

Method returning the arm dexterity.

Returns: dexterity value.

◆ DexterityJacobian()

Eigen::MatrixXd rml::ArmModel::DexterityJacobian ( const std::string & frameID )

Method returning the dexterity jacobian related to the input frameID wrt to the arm base.

Parameters

frameId frame id

Returns: jacobian matrix

◆ dJdq()

auto rml::ArmModel::dJdq ( ) const -> const std::vector<Eigen::MatrixXd>&

inline

Method returning dJdq evaluated numerically.

Returns: dJdq

◆ EvaluateBase2JointJacobian()

Eigen::MatrixXd rml::ArmModel::EvaluateBase2JointJacobian ( unsigned int jointIndex )

protected

Evaluates the jacobian matrix (w.r.t. robot base) of the specified joint.

Parameters

[in] jointIndex Joint index

Returns: Joint jacobian matrix

◆ EvaluateDexterity()

virtual Eigen::MatrixXd rml::ArmModel::EvaluateDexterity ( const std::string & frameID )

protectedvirtual

Evaluates the dexterity measure and its Jacobian This method returns the dexterity measure and its Jacobian.

◆ EvaluatedJdqNumeric()

virtual void rml::ArmModel::EvaluatedJdqNumeric ( )

protectedvirtual

Evaluates numerically the Jacobian derivative w.r.t. joint variations.

◆ EvaluateManipulability()

virtual Eigen::MatrixXd rml::ArmModel::EvaluateManipulability ( const std::string & frameID )

protectedvirtual

Evaluates the manipulability measure and its Jacobian This method returns the manipulability measure and its Jacobian.

◆ EvaluateRigidBodyJacobian()

Eigen::MatrixXd rml::ArmModel::EvaluateRigidBodyJacobian ( const std::string & frameID )

protected

Method returning the attached body frame jacobian wrt to the arm base.

Parameters

ID frame ID.

Returns: jacobian matrix.

◆ EvaluateRigidBodyTransf()

Eigen::TransformationMatrix rml::ArmModel::EvaluateRigidBodyTransf ( const std::string & frameID )

protected

Method returning the attached body frame wrt to the arm base.

Parameters

ID frame ID.

Returns: transformation matrix.

◆ EvaluateTotalForwardGeometry()

void rml::ArmModel::EvaluateTotalForwardGeometry ( )

protected

Method evaluating the total forward geometry for the arm.

◆ ForwardDirectGeometry()

void rml::ArmModel::ForwardDirectGeometry ( unsigned int jointNumber )

protected

Method performing the forward direct geometry untill the input joint number.

Parameters

jointNumber which joint is intended as last of the chain

◆ GetEndEffectorFrameID()

std::string rml::ArmModel::GetEndEffectorFrameID ( ) const

noexcept

Method returning the end-effector frame ID.

Returns: frame ID

◆ GetJacobianFrameIDs()

std::vector< std::string > rml::ArmModel::GetJacobianFrameIDs ( ) const

noexcept

Method returning the list of Jacobian frame IDs.

Returns: vector of frame IDs

◆ GetJointFrameIDs()

std::vector< std::string > rml::ArmModel::GetJointFrameIDs ( ) const

noexcept

Method returning the list of joint frame IDs.

Returns: vector of frame IDs

◆ GetRigidBodyFrameIDs()

std::vector< std::string > rml::ArmModel::GetRigidBodyFrameIDs ( ) const

noexcept

Method returning the list of rigid body frame IDs.

Returns: vector of frame IDs

◆ ID() [1/2]

auto rml::ArmModel::ID ( ) -> std::string&

inline

Method setting the arm id.

Parameters

id arm id.

◆ ID() [2/2]

auto rml::ArmModel::ID ( ) const -> const std::string&

inline

Method returning the arm id.

Returns: arm id

◆ IsModelInitialized()

auto rml::ArmModel::IsModelInitialized ( ) const -> bool

inline

Method returning true if the model is initialized false otherwise.

◆ Jacobian()

virtual Eigen::MatrixXd rml::ArmModel::Jacobian ( const std::string & frameID )

virtual

Method returning the jacobian related to the input frameID wrt to the arm base.

Parameters

frameId frame id

Returns: jacobian matrix

◆ JointsAcceleration() [1/2]

auto rml::ArmModel::JointsAcceleration ( ) const -> const Eigen::VectorXd&

inline

Get moving joints acceleration.

Returns: the joints acceleration vector (must be a numMovingJoints x 1 vector)

◆ JointsAcceleration() [2/2]

void rml::ArmModel::JointsAcceleration ( const Eigen::VectorXd qddot )

Set the moving joints acceleration.

Parameters

qddot the joints acceleration vector (must be a numMovingJoints x 1 vector)

◆ JointsPosition() [1/2]

virtual auto rml::ArmModel::JointsPosition ( ) const -> const Eigen::VectorXd&

inlinevirtual

Get the moving joint position.

Returns: q the joint position vector (a numMovingJoints x 1 vector)

◆ JointsPosition() [2/2]

virtual void rml::ArmModel::JointsPosition ( const Eigen::VectorXd q )

virtual

Set the joint position.

The method updates the internal joint position state [only the moving joints]. This method updates also the internal transformation matrices and jacobians.

Parameters

[in] q the joint position vector (must be a numMovingJoints x 1 vector)

◆ JointsVelocity() [1/2]

virtual auto rml::ArmModel::JointsVelocity ( ) const -> const Eigen::VectorXd&

inlinevirtual

Get the moving joints velocity.

Returns: the joints velocity vector (must be a numMovingJoints x 1 vector)

◆ JointsVelocity() [2/2]

virtual void rml::ArmModel::JointsVelocity ( const Eigen::VectorXd qdot )

virtual

Set the moving joints velocity.

Parameters

qdot	the joint velocity vector (must be a numMovingJoints x 1 vector)

◆ Link()

RobotLink & rml::ArmModel::Link ( int jointIndex )

Method returning the arm link.

Parameters

jointIndex link index

Returns: robot link

◆ Manipulability()

double rml::ArmModel::Manipulability ( const std::string & frameID )

Method returning the arm manipulability.

Returns: manipulability value.

◆ ManipulabilityJacobian()

Eigen::MatrixXd rml::ArmModel::ManipulabilityJacobian ( const std::string & frameID )

Method returning the manipulability jacobian related to the input frameID wrt to the arm base.

Parameters

frameId frame id

Returns: jacobian matrix

◆ NumJoints()

virtual auto rml::ArmModel::NumJoints ( ) const -> unsigned int

inlinevirtual

Method returning the arm number of moving joints.

Returns: arm number of moving joints

◆ TransformationMatrix() [1/2]

Eigen::TransformationMatrix rml::ArmModel::TransformationMatrix ( const std::string & frameID )

Method returning the transformation matrix related to the input frameID wrt to the arm base.

Parameters

frameId frame id

Returns: transformation matrix

◆ TransformationMatrix() [2/2]

Eigen::TransformationMatrix rml::ArmModel::TransformationMatrix	(	const std::string &	frameID_j,
		const std::string &	frameID_k
	)

Method returing a transformation matrix from frameID_j to frameID_k, i.e. jTk.

Parameters

[in]	frameID_j	first frame;
[in]	framID_k	second frame;

Returns: Transformation Matrix jTk.

Member Data Documentation

◆ base_ki_

Eigen::Vector3d rml::ArmModel::base_ki_

protected

◆ baseTbi_

Eigen::TransformationMatrix rml::ArmModel::baseTbi_

protected

◆ baseTei_

std::vector<Eigen::TransformationMatrix> rml::ArmModel::baseTei_

protected

◆ biTei_

std::vector<Eigen::TransformationMatrix> rml::ArmModel::biTei_

protected

◆ controlRef_

Eigen::VectorXd rml::ArmModel::controlRef_

protected

◆ dexterity_

std::unordered_map<std::string, double> rml::ArmModel::dexterity_

protected

map of the manipulability values

◆ dexterityJacobians_

std::unordered_map<std::string, Eigen::MatrixXd> rml::ArmModel::dexterityJacobians_

protected

map of the manipulability jacobians

◆ dJdq_

std::vector<Eigen::MatrixXd> rml::ArmModel::dJdq_

protected

dJdq evaluated numerically

◆ djdqJpinv_

Eigen::MatrixXd rml::ArmModel::djdqJpinv_

protected

djdq * jacobian pseudoinverse

◆ h_

std::vector<Eigen::Vector6d> rml::ArmModel::h_

protected

◆ I3_

Eigen::RotationMatrix rml::ArmModel::I3_

protected

identity matrix

◆ id_

std::string rml::ArmModel::id_

protected

arm id

◆ isMapInitialized_

bool rml::ArmModel::isMapInitialized_

protected

boolean stating whether the transformation and jacobian maps are initialized

◆ jacobians_

std::unordered_map<std::string, Eigen::MatrixXd> rml::ArmModel::jacobians_

protected

◆ Jdjdq_

Eigen::MatrixXd rml::ArmModel::Jdjdq_

protected

jacobian * djdq

◆ Jpinvdjpinvdq_

Eigen::MatrixXd rml::ArmModel::Jpinvdjpinvdq_

protected

jacobian pseudoinverse * djpinvdq

◆ links_

std::vector<RobotLink> rml::ArmModel::links_

protected

vector of the arm links

◆ manipulability_

std::unordered_map<std::string, double> rml::ArmModel::manipulability_

protected

map of the manipulability values

◆ manipulabilityJacobians_

std::unordered_map<std::string, Eigen::MatrixXd> rml::ArmModel::manipulabilityJacobians_

protected

map of the manipulability jacobians

◆ modelInitialized_

bool rml::ArmModel::modelInitialized_

protected

boolean stating whether the model is initialized

◆ modelReadFromFile_

bool rml::ArmModel::modelReadFromFile_

protected

boolean stating whether the model is read from file

◆ movingJoints_

std::vector<unsigned int> rml::ArmModel::movingJoints_

protected

vector containing the indexes of the moving joints

◆ movingNumJoints_

unsigned int rml::ArmModel::movingNumJoints_

protected

moving joints number

◆ q_ddot_moving_

Eigen::VectorXd rml::ArmModel::q_ddot_moving_

protected

◆ q_dot_moving_

Eigen::VectorXd rml::ArmModel::q_dot_moving_

protected

◆ q_moving_

Eigen::VectorXd rml::ArmModel::q_moving_

protected

◆ q_total_

Eigen::VectorXd rml::ArmModel::q_total_

protected

◆ rigidBodyFrames_

std::unordered_map<std::string, IndexedTMat> rml::ArmModel::rigidBodyFrames_

protected

◆ totalNumJoints_

unsigned int rml::ArmModel::totalNumJoints_

protected

◆ transformation_

std::unordered_map<std::string, Eigen::TransformationMatrix> rml::ArmModel::transformation_

protected

◆ Tz_

Eigen::TransformationMatrix rml::ArmModel::Tz_

protected

transformation matrix for rotation or prismatic joint

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

ArmModel.h

Public Member Functions

Protected Member Functions

Protected Attributes

Detailed Description

Constructor & Destructor Documentation

◆ ArmModel()

◆ ~ArmModel()

Member Function Documentation

◆ AddFixedLink()

◆ AddJointLink()

◆ AttachRigidBodyFrame()

◆ BackwardDirectGeometry()

◆ ControlVector() [1/2]

◆ ControlVector() [2/2]

◆ Dexterity()

◆ DexterityJacobian()

◆ dJdq()

◆ EvaluateBase2JointJacobian()

◆ EvaluateDexterity()

◆ EvaluatedJdqNumeric()

◆ EvaluateManipulability()

◆ EvaluateRigidBodyJacobian()

◆ EvaluateRigidBodyTransf()

◆ EvaluateTotalForwardGeometry()

◆ ForwardDirectGeometry()

◆ GetEndEffectorFrameID()

◆ GetJacobianFrameIDs()

◆ GetJointFrameIDs()

◆ GetRigidBodyFrameIDs()

◆ ID() [1/2]

◆ ID() [2/2]

◆ IsModelInitialized()

◆ Jacobian()

◆ JointsAcceleration() [1/2]

◆ JointsAcceleration() [2/2]

◆ JointsPosition() [1/2]

◆ JointsPosition() [2/2]

◆ JointsVelocity() [1/2]

◆ JointsVelocity() [2/2]

◆ Link()

◆ Manipulability()

◆ ManipulabilityJacobian()

◆ NumJoints()

◆ TransformationMatrix() [1/2]

◆ TransformationMatrix() [2/2]

Member Data Documentation

◆ base_ki_

◆ baseTbi_

◆ baseTei_

◆ biTei_

◆ controlRef_

◆ dexterity_

◆ dexterityJacobians_

◆ dJdq_

◆ djdqJpinv_

◆ h_

◆ I3_

◆ id_

◆ isMapInitialized_

◆ jacobians_

◆ Jdjdq_

◆ Jpinvdjpinvdq_

◆ links_

◆ manipulability_

◆ manipulabilityJacobians_

◆ modelInitialized_

◆ modelReadFromFile_

◆ movingJoints_

◆ movingNumJoints_

◆ q_ddot_moving_

◆ q_dot_moving_

◆ q_moving_

◆ q_total_

◆ rigidBodyFrames_

◆ totalNumJoints_

◆ transformation_

◆ Tz_