#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
robManipulator::Errno	LoadRobot (const std::string &linkfile)
	Load the kinematics and the dynamics of the robot. More...

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

void	JacobianBody (const vctDynamicVector< double > &q) const
	Evaluate the body Jacobian. More...

bool	JacobianBody (const vctDynamicVector< double > &q, vctDynamicMatrix< double > &J) const
	Evaluate the body Jacobian and return it in the dynamic matrix J. More...

void	JacobianSpatial (const vctDynamicVector< double > &q) const
	Evaluate the spatial Jacobian. More...

bool	JacobianSpatial (const vctDynamicVector< double > &q, vctDynamicMatrix< double > &J) const
	Evaluate the spatial Jacobian and return it in the dynamic matrix J. More...

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

vctDynamicVector< double >	CCG (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd) const
	Coriolis/centrifugal and gravity. More...

vctFixedSizeVector< double, 6 >	BiasAcceleration (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd) const
	End-effector accelerations. More...

void	JSinertia (double **A, const vctDynamicVector< double > &q) const
	Compute the NxN manipulator inertia matrix. More...

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

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

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

virtual	~robManipulator ()
	Manipulator destructor. More...

virtual vctFrame4x4< double >	ForwardKinematics (const vctDynamicVector< double > &q, int N=-1) const
	Evaluate the forward kinematics. More...

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

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

virtual vctDynamicVector< double >	InverseDynamics (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctDynamicVector< double > &qdd) const
	Inverse dynamics in joint space. More...

virtual vctDynamicVector< double >	InverseDynamics (const vctDynamicVector< double > &q, const vctDynamicVector< double > &qd, const vctFixedSizeVector< double, 6 > &vdwd) const
	Inverse dynamics in operation space. More...

virtual vctDynamicMatrix< double >	JacobianKinematicsIdentification (const vctDynamicVector< double > &q, double epsilon=1e-6) const
	Compute Jacobian for kinematics identification. More...

void	PrintKinematics (std::ostream &os) const
	Print the kinematics parameters to the specified output stream. More...

virtual void	Attach (robManipulator *tool)
	Attach a tool. More...

Public Attributes
vctFrame4x4< double >	Rtw0
	Position and orientation of the first link. More...

double **	Jn
	Body Jacobian. More...

double **	Js
	Spatial Jacobian. More...

std::vector< robLink >	links
	A vector of links. More...

Protected Attributes
std::vector< robManipulator * >	tools
	A vector of tools. More...

Member Enumeration Documentation

enum robManipulator::Errno

Enumerator
ESUCCESS
EFAILURE

enum robManipulator::LinkID

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

Constructor & Destructor Documentation

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

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::robManipulator	(	const std::vector< robKinematics * >	linkParms,
		const vctFrame4x4< double > &	Rtw0 = `vctFrame4x4< double >()`
	)

virtual robManipulator::~robManipulator ( )

virtual

Manipulator destructor.

Member Function Documentation

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

virtual void robManipulator::Attach ( robManipulator * tool )

virtual

Attach a tool.

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

vctDynamicVector<double> robManipulator::CCG	(	const vctDynamicVector< double > &	q,
		const vctDynamicVector< double > &	qd
	)		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

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

Reimplemented in osaODEManipulator.

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

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

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.

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

virtual

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)

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

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

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

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

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

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

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

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

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

Load the kinematics and the dynamics of the robot.

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

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

virtual

Normalize angles to -pi to pi.

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

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

Print the kinematics parameters to the specified output stream.

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

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

Member Data Documentation

double** robManipulator::Jn

Body Jacobian.

The (geometric) body Jacobian in column major order

double** robManipulator::Js

Spatial Jacobian.

The (geometric) spatial Jacobian in column major order

std::vector<robLink> robManipulator::links

A vector of links.

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

std::vector<robManipulator*> robManipulator::tools

protected

A vector of tools.

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

/home/adeguet1/catkin_ws/src/cisst-saw/cisst/cisstRobot/robManipulator.h

Public Types

Public Member Functions

Public Attributes

Protected Attributes

Member Enumeration Documentation

Constructor & Destructor Documentation

Member Function Documentation

Member Data Documentation