#include <robManipulator.h>

Inheritance diagram for robManipulator:

Public Types
enum	Errno { ESUCCESS , EFAILURE }
enum	LinkID { L0 , L1 , L2 , L3 , L4 , L5 , L6 , L7 , L8 , L9 , LN }

Public Member Functions
virtual robManipulator::Errno	LoadRobot (const std::string &linkfile)
	Load the kinematics and the dynamics of the robot.
virtual robManipulator::Errno	LoadRobot (std::istream &ifs)
robManipulator::Errno	LoadRobot (std::vector< robKinematics * > KinParms)
void	JacobianBody (const vctDynamicVector< double > &q) const
	Evaluate the body Jacobian.
bool	JacobianBody (const vctDynamicVector< double > &q, vctDynamicMatrix< double > &J) const
	Evaluate the body Jacobian and return it in the dynamic matrix J.
void	JacobianSpatial (const vctDynamicVector< double > &q) const
	Evaluate the spatial Jacobian.
bool	JacobianSpatial (const vctDynamicVector< double > &q, vctDynamicMatrix< double > &J) const
	Evaluate the spatial Jacobian and return it in the dynamic matrix J.
vctDynamicVector< double >	RNE (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctDynamicVector< double > &qdd, const vctFixedSizeVector< double, 6 > &f, double g=9.81) const
	Recursive Newton-Euler altorithm.
vctDynamicVector< double >	RNE_MDH (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctDynamicVector< double > &qdd, const vctFixedSizeVector< double, 6 > &f, double g=9.81) const
vctDynamicVector< double >	RNE_MDH (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctDynamicVector< double > &qdd, const vctFixedSizeVector< double, 6 > &f, const vct3 &g) const
vctDynamicVector< double >	CCG (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, double g=9.81) const
	Coriolis/centrifugal and gravity.
vctDynamicVector< double >	CCG_MDH (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, double g=9.81) const
vctDynamicVector< double >	CCG_MDH (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vct3 &g) const
vctFixedSizeVector< double, 6 >	BiasAcceleration (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd) const
	End-effector accelerations.
void	JSinertia (double **A, const vctDynamicVector< double > &q) const
	Compute the NxN manipulator inertia matrix.
vctDynamicMatrix< double >	JSinertia (const vctDynamicVector< double > &q) const
void	OSinertia (double Ac[6][6], const vctDynamicVector< double > &q) const
	Compute the 6x6 manipulator inertia matrix in operation space.
vctFixedSizeMatrix< double, 4, 4 >	SE3Difference (const vctFrame4x4< double > &Rt1, const vctFrame4x4< double > &Rt2) const
void	AddIdentificationColumn (vctDynamicMatrix< double > &J, vctFixedSizeMatrix< double, 4, 4 > &delRt) const
	robManipulator (const vctFrame4x4< double > &Rtw0=vctFrame4x4< double >())
	robManipulator (const std::string &robotfilename, const vctFrame4x4< double > &Rtw0=vctFrame4x4< double >())
	Manipulator generic constructor.
	robManipulator (const std::vector< robKinematics * > linkParms, const vctFrame4x4< double > &Rtw0=vctFrame4x4< double >())
virtual	~robManipulator ()
	Manipulator destructor.
virtual void	SetJointLimits (const vctDynamicVector< double > &lowerLimits, const vctDynamicVector< double > &upperLimits)
virtual void	GetJointLimits (vctDynamicVectorRef< double > lowerLimits, vctDynamicVectorRef< double > upperLimits) const
virtual void	GetFTMaximums (vctDynamicVectorRef< double > ftMaximums) const
virtual void	GetJointNames (std::vector< std::string > &names) const
virtual void	GetJointTypes (std::vector< cmnJointType > &types) const
virtual vctFrame4x4< double >	ForwardKinematics (const vctDynamicVector< double > &q, int N=-1) const
	Evaluate the forward kinematics.
virtual robManipulator::Errno	InverseKinematics (vctDynamicVector< double > &q, const vctFrame4x4< double > &Rts, double tolerance=1e-12, size_t Niteration=1000, double LAMBDA=0.001)
	Evaluate the inverse kinematics.
virtual robManipulator::Errno	InverseKinematics (vctDynamicVector< double > &q, const vctFrm3 &Rts, double tolerance=1e-12, size_t Niteration=1000)
virtual void	NormalizeAngles (vctDynamicVector< double > &q)
	Normalize angles to -pi to pi.
virtual vctDynamicVector< double >	InverseDynamics (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctDynamicVector< double > &qdd) const
	Inverse dynamics in joint space.
virtual vctDynamicVector< double >	InverseDynamics (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctFixedSizeVector< double, 6 > &vdwd) const
	Inverse dynamics in operation space.
virtual vctDynamicMatrix< double >	JacobianKinematicsIdentification (const vctDynamicVector< double > &q, double epsilon=1e-6) const
	Compute Jacobian for kinematics identification.
virtual void	PrintKinematics (std::ostream &os) const
	Print the kinematics parameters to the specified output stream.
virtual void	Attach (robManipulator *tool)
	Attach a tool.
void	DeleteTools (void)
virtual robManipulator::Errno	Truncate (const size_t linksToKeep)
	Remove all links expect n first ones.
const std::string &	LastError (void) const
bool	ClampJointValueAndUpdateError (const size_t jointIndex, double &value, const double &tolerance=1e-6)

Public Attributes
std::string	mLastError
vctFrame4x4< double >	Rtw0
	Position and orientation of the first link.
double **	Jn
	Body Jacobian.
double **	Js
	Spatial Jacobian.
std::vector< robLink >	links
	A vector of links.

Protected Types
typedef std::vector< robManipulator * >	ToolsType
	A vector of tools.

Protected Attributes
ToolsType	tools

Member Typedef Documentation

◆ ToolsType

typedef std::vector<robManipulator*> robManipulator::ToolsType

protected

A vector of tools.

Member Enumeration Documentation

◆ Errno

enum robManipulator::Errno

Enumerator
ESUCCESS
EFAILURE

◆ LinkID

enum robManipulator::LinkID

Enumerator
L0
L1
L2
L3
L4
L5
L6
L7
L8
L9
LN

Constructor & Destructor Documentation

◆ robManipulator() [1/3]

robManipulator::robManipulator ( const vctFrame4x4< double > & Rtw0 = vctFrame4x4< double >() )

◆ robManipulator() [2/3]

robManipulator::robManipulator	(	const std::string &	robotfilename,
		const vctFrame4x4< double > &	Rtw0 = vctFrame4x4< double >() )

Manipulator generic constructor.

This constructor initializes a manipulator with the kinematics and dynamics contained in a file.

Parameters

robotfilename	The file with the kinematics and dynamics parameters
Rtw0	The offset transformation of the robot base

◆ robManipulator() [3/3]

robManipulator::robManipulator	(	const std::vector< robKinematics * >	linkParms,
		const vctFrame4x4< double > &	Rtw0 = vctFrame4x4< double >() )

◆ ~robManipulator()

virtual robManipulator::~robManipulator ( )

virtual

Manipulator destructor.

Member Function Documentation

◆ AddIdentificationColumn()

void robManipulator::AddIdentificationColumn	(	vctDynamicMatrix< double > &	J,
		vctFixedSizeMatrix< double, 4, 4 > &	delRt ) const

◆ Attach()

virtual void robManipulator::Attach ( robManipulator * tool )

virtual

Attach a tool.

◆ BiasAcceleration()

vctFixedSizeVector< double, 6 > robManipulator::BiasAcceleration	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd ) const

End-effector accelerations.

Compute the linear and angular accelerations of the last link. This is akin to computing the forward recursion of the RNE. Compute the bias acceleration The bias acceleration is the 6D vector Jdqd that is used to evaluate the inverse dynamics in operation space. This vector is derived from d (J qd) / dt = Jdqd + J qdd

◆ CCG()

vctDynamicVector< double > robManipulator::CCG	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd,
		double	g = 9.81 ) const

Coriolis/centrifugal and gravity.

Evaluate the coriolis/centrifugal and gravitational forces acting on the manipulator. The joint positions, velocities and accelerations must be set before calling this method. It returns a vector of forces/torques that realize the given positions and accelerations. This method is akin to calling RNE without the joint accelerations

◆ CCG_MDH() [1/2]

vctDynamicVector< double > robManipulator::CCG_MDH	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd,
		const vct3 &	g ) const

◆ CCG_MDH() [2/2]

vctDynamicVector< double > robManipulator::CCG_MDH	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd,
		double	g = 9.81 ) const

◆ ClampJointValueAndUpdateError()

bool robManipulator::ClampJointValueAndUpdateError	(	const size_t	jointIndex,
		double &	value,
		const double &	tolerance = 1e-6 )

Clamp joint value between joint limits and update the last error message if the value provided is outside joint limits. Return true if clamping was necessary.

◆ DeleteTools()

void robManipulator::DeleteTools ( void )

◆ ForwardKinematics()

virtual vctFrame4x4< double > robManipulator::ForwardKinematics	(	const vctDynamicVector< double > &	q,
		int	N = -1 ) const

virtual

Evaluate the forward kinematics.

Compute the position and orientation of each link wrt to the world frame

Parameters

[input]	q The vector of joint positions
[input]	N The link number (0 => base, negative => end-effector)

Returns: The position and orientation, as a 4x4 frame

◆ GetFTMaximums()

virtual void robManipulator::GetFTMaximums ( vctDynamicVectorRef< double > ftMaximums ) const

virtual

Get force/torque max

◆ GetJointLimits()

virtual void robManipulator::GetJointLimits	(	vctDynamicVectorRef< double >	lowerLimits,
		vctDynamicVectorRef< double >	upperLimits ) const

virtual

Get joint limits

◆ GetJointNames()

virtual void robManipulator::GetJointNames ( std::vector< std::string > & names ) const

virtual

Get joint names

◆ GetJointTypes()

virtual void robManipulator::GetJointTypes ( std::vector< cmnJointType > & types ) const

virtual

Get joint types

◆ InverseDynamics() [1/2]

virtual vctDynamicVector< double > robManipulator::InverseDynamics	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd,
		const vctDynamicVector< double > &	qdd ) const

virtual

Inverse dynamics in joint space.

Compute and return the inverse dynamics of the manipulator in joint space. InverseDynamics returns the joint torques that correspond to a manipulator with the given joint positions, velocities and accelerations.

Parameters

q	A vector of joint positions
qd	A vector of joint velocities
qdd	A vector of joint accelerations

Returns: A vector of joint torques

◆ InverseDynamics() [2/2]

virtual vctDynamicVector< double > robManipulator::InverseDynamics	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd,
		const vctFixedSizeVector< double, 6 > &	vdwd ) const

virtual

Inverse dynamics in operation space.

Compute and return the inverse dynamics of the manipulator in operation space. InverseDynamics returns the joint torques that correspond to a manipulator with the given joint positions, velocities and the tool control point (TCP) accelerations. The reason why joint positions and velocities are given instead of the position and velocity of the TCP is that the coriolis, centrifugal and gravitational forces are uniquely determined by the joint positions and velocities.

Parameters

q	A vector of joint positions
qd	A vector of joint velocities
vdwd	A 6D vector of the TCP linear and angular accelerations

Returns: A vector of joint torques

◆ InverseKinematics() [1/2]

virtual robManipulator::Errno robManipulator::InverseKinematics	(	vctDynamicVector< double > &	q,
		const vctFrame4x4< double > &	Rts,
		double	tolerance = 1e-12,
		size_t	Niteration = 1000,
		double	LAMBDA = 0.001 )

virtual

Evaluate the inverse kinematics.

Compute the inverse kinematics. The solution is computed numerically using Newton's algorithm.

Parameters

[input]	q An initial guess of the solution
[output]	q The inverse kinematics solution
Rts	The desired position and orientation of the tool control point
tolerance	The error tolerance of the solution
Niteration	The maximum number of iterations allowed to find a solution

Returns: SUCCESS if a solution was found within the given tolerance and number of iterations. ERROR otherwise.

◆ InverseKinematics() [2/2]

virtual robManipulator::Errno robManipulator::InverseKinematics	(	vctDynamicVector< double > &	q,
		const vctFrm3 &	Rts,
		double	tolerance = 1e-12,
		size_t	Niteration = 1000 )

virtual

◆ JacobianBody() [1/2]

void robManipulator::JacobianBody ( const vctDynamicVector< double > & q ) const

Evaluate the body Jacobian.

Evaluates the geometric body Jacobian. This implements the algorithm of Paul, Shimano, Mayer (SMC81)

◆ JacobianBody() [2/2]

bool robManipulator::JacobianBody	(	const vctDynamicVector< double > &	q,
		vctDynamicMatrix< double > &	J ) const

Evaluate the body Jacobian and return it in the dynamic matrix J.

◆ JacobianKinematicsIdentification()

virtual vctDynamicMatrix< double > robManipulator::JacobianKinematicsIdentification	(	const vctDynamicVector< double > &	q,
		double	epsilon = 1e-6 ) const

virtual

Compute Jacobian for kinematics identification.

Computes the Jacobian for kinematics identification by numerically differentiating with respect to the kinematics parameters (DH parameters).

Parameters

q	The vector of joint positions
epsilon	The DH parameter difference to use for numerical differentiation

Returns: The Jacobian matrix to use for kinematics identification

◆ JacobianSpatial() [1/2]

void robManipulator::JacobianSpatial ( const vctDynamicVector< double > & q ) const

Evaluate the spatial Jacobian.

Evaluate the geometric spatial Jacobian.

Warning: To evaluate the spatial Jacobian you must first evaluate the body Jacobian

◆ JacobianSpatial() [2/2]

bool robManipulator::JacobianSpatial	(	const vctDynamicVector< double > &	q,
		vctDynamicMatrix< double > &	J ) const

Evaluate the spatial Jacobian and return it in the dynamic matrix J.

◆ JSinertia() [1/2]

vctDynamicMatrix< double > robManipulator::JSinertia ( const vctDynamicVector< double > & q ) const

◆ JSinertia() [2/2]

void robManipulator::JSinertia	(	double **	A,
		const vctDynamicVector< double > &	q ) const

Compute the NxN manipulator inertia matrix.

Parameters

[input]	A A pointer to an NxN matrix
[output]	A The NxN manipulator inertia matrix

◆ LastError()

const std::string & robManipulator::LastError ( void ) const

inline

Get last error message

◆ LoadRobot() [1/3]

virtual robManipulator::Errno robManipulator::LoadRobot ( const std::string & linkfile )

virtual

Load the kinematics and the dynamics of the robot.

If the file name ends with .json it will open the file and then call the overloaded method LoadRobot for Json::Value. Otherwise, assumes it's the .rob format and calls the overloaded method LoadRobot for istream.

◆ LoadRobot() [2/3]

virtual robManipulator::Errno robManipulator::LoadRobot ( std::istream & ifs )

virtual

◆ LoadRobot() [3/3]

robManipulator::Errno robManipulator::LoadRobot ( std::vector< robKinematics * > KinParms )

◆ NormalizeAngles()

virtual void robManipulator::NormalizeAngles ( vctDynamicVector< double > & q )

virtual

Normalize angles to -pi to pi.

◆ OSinertia()

void robManipulator::OSinertia	(	double	Ac[6][6],
		const vctDynamicVector< double > &	q ) const

Compute the 6x6 manipulator inertia matrix in operation space.

Parameters

[input]	A A pointer to a 6x6 matrix
[output]	The 6x6 manipulator inertia matrix in operation space

◆ PrintKinematics()

virtual void robManipulator::PrintKinematics ( std::ostream & os ) const

virtual

Print the kinematics parameters to the specified output stream.

◆ RNE()

vctDynamicVector< double > robManipulator::RNE	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd,
		const vctDynamicVector< double > &	qdd,
		const vctFixedSizeVector< double, 6 > &	f,
		double	g = 9.81 ) const

Recursive Newton-Euler altorithm.

Evaluate the inverse dynamics through RNE. The joint positions, velocities and accelerations must be set before calling this method. It returns a vector of forces/torques that realize the desired state.

Parameters

q	The joint positions
qd	The joint velocities
qdd	The joint accelerations
fext	An external force/moment acting on the tool control point
g	The gravity acceleration

◆ RNE_MDH() [1/2]

vctDynamicVector< double > robManipulator::RNE_MDH	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd,
		const vctDynamicVector< double > &	qdd,
		const vctFixedSizeVector< double, 6 > &	f,
		const vct3 &	g ) const

◆ RNE_MDH() [2/2]

vctDynamicVector< double > robManipulator::RNE_MDH	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd,
		const vctDynamicVector< double > &	qdd,
		const vctFixedSizeVector< double, 6 > &	f,
		double	g = 9.81 ) const

◆ SE3Difference()

vctFixedSizeMatrix< double, 4, 4 > robManipulator::SE3Difference	(	const vctFrame4x4< double > &	Rt1,
		const vctFrame4x4< double > &	Rt2 ) const

◆ SetJointLimits()

virtual void robManipulator::SetJointLimits	(	const vctDynamicVector< double > &	lowerLimits,
		const vctDynamicVector< double > &	upperLimits )

virtual

Set joint limits

◆ Truncate()

virtual robManipulator::Errno robManipulator::Truncate ( const size_t linksToKeep )

virtual

Remove all links expect n first ones.

This method also resizes internal data members as needed (jacobian matrices). Returns EFAILURE if the current manipulator doesn't have at least n links.

Member Data Documentation

◆ Jn

double** robManipulator::Jn

Body Jacobian.

The (geometric) body Jacobian in column major order

◆ Js

double** robManipulator::Js

Spatial Jacobian.

The (geometric) spatial Jacobian in column major order

◆ links

std::vector<robLink> robManipulator::links

A vector of links.

◆ mLastError

std::string robManipulator::mLastError

◆ Rtw0

vctFrame4x4<double> robManipulator::Rtw0

Position and orientation of the first link.

Simply put, this is the position and orientation of the base of the first link with respect to a known world frame

◆ tools

ToolsType robManipulator::tools

protected

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

tmp/cisst-saw/cisst/cisstRobot/robManipulator.h

Public Types

Public Member Functions

Public Attributes

Protected Types

Protected Attributes

Member Typedef Documentation

◆ ToolsType

Member Enumeration Documentation

◆ Errno

◆ LinkID

Constructor & Destructor Documentation

◆ robManipulator() [1/3]

◆ robManipulator() [2/3]

◆ robManipulator() [3/3]

◆ ~robManipulator()

Member Function Documentation

◆ AddIdentificationColumn()

◆ Attach()

◆ BiasAcceleration()

◆ CCG()

◆ CCG_MDH() [1/2]

◆ CCG_MDH() [2/2]

◆ ClampJointValueAndUpdateError()

◆ DeleteTools()

◆ ForwardKinematics()

◆ GetFTMaximums()

◆ GetJointLimits()

◆ GetJointNames()

◆ GetJointTypes()

◆ InverseDynamics() [1/2]

◆ InverseDynamics() [2/2]

◆ InverseKinematics() [1/2]

◆ InverseKinematics() [2/2]

◆ JacobianBody() [1/2]

◆ JacobianBody() [2/2]

◆ JacobianKinematicsIdentification()

◆ JacobianSpatial() [1/2]

◆ JacobianSpatial() [2/2]

◆ JSinertia() [1/2]

◆ JSinertia() [2/2]

◆ LastError()

◆ LoadRobot() [1/3]

◆ LoadRobot() [2/3]

◆ LoadRobot() [3/3]

◆ NormalizeAngles()

◆ OSinertia()

◆ PrintKinematics()

◆ RNE()

◆ RNE_MDH() [1/2]

◆ RNE_MDH() [2/2]

◆ SE3Difference()

◆ SetJointLimits()

◆ Truncate()

Member Data Documentation

◆ Jn

◆ Js

◆ links

◆ mLastError

◆ Rtw0

◆ tools