cisstRobot API

cisstRobot provides kinematics, dynamics, and trajectory generation for robot manipulators using DH and modified DH conventions.

Key classes

class robManipulator

Subclassed by robComputedTorque

Public Types

enum Errno

Values:

enumerator ESUCCESS
enumerator EFAILURE
enum LinkID

Values:

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

Public Functions

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

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.

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.

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

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.

Evaluate the geometric spatial Jacobian.

Warning

To evaluate the spatial Jacobian you must first evaluate the body 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.

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

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.

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

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.

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

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

Parameters:

  • [input] – A A pointer to a 6x6 matrix

  • [output] – 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.

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

Set joint limits

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

Get joint limits

virtual void GetFTMaximums(vctDynamicVectorRef<double> ftMaximums) const

Get force/torque max

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

Get joint names

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

Get joint types

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

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

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.

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

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> InverseDynamics(const vctDynamicVector<double> &q, const vctDynamicVector<double> &qd, const vctFixedSizeVector<double, 6> &vdwd) const

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 vctDynamicMatrix<double> JacobianKinematicsIdentification(const vctDynamicVector<double> &q, double epsilon = 1e-6) const

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

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.

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

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

Get last error message

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

Public Members

std::string mLastError
vctFrame4x4<double> 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

double **Jn

Body Jacobian.

The (geometric) body Jacobian in column major order

double **Js

Spatial Jacobian.

The (geometric) spatial Jacobian in column major order

std::vector<robLink> links

A vector of links.

class robLink

A robot link.

robLink implements the methods necessary to model a robot link. The class is derived from robBody to store the dynamics parameters of the body and the link’s position and orientation. The link is also derived from robDH to determine the link’s position and orientation from joint values

Public Types

enum Errno

Values:

enumerator ESUCCESS
enumerator EFAILURE

Public Functions

robLink()

Default constructor.

robLink(const robLink &link)

Copy constructor.

robLink(robKinematics *kinematics, const robMass &mass)

Overloaded constructor.

~robLink()

Default destructor.

robLink::Errno Read(std::istream &is)

Read the DH and body parameters.

First read the DH parameters and then the body’s parameters. At the end of the day, the parameters must be in the following order DH convention (string): modified or standard \( \alpha \) (1 double): DH parameter \( a \) (1 double): DH parameter \( \theta \) (1 double): DH parameter \( d \) (1 double): DH parameter joint type (string): revolute/hinge or prismatic/slider joint mode (string): active or passive joint offset position (1 double): value added to each joint value joint minimum position (1 double): lower joint limit joint maximum position (1 double): upper joint limit joint maximum force/torque (1 double): absolute force/torque limit mass (1 double): The mass of the body center of mass (3 double): \( \matrix{x & y & z} \) principal moment of inertia (3 double): \( \matrix{I_{xx} & I_{yy} & I_{zz}} \) body principal axis (9 double):

robLink::Errno Write(std::ostream &os) const

Write the DH and body parameters.

vctFrame4x4<double> ForwardKinematics(double q) const
vctFixedSizeVector<double, 3> PStar() const
vctMatrixRotation3<double> Orientation(double q) const
robKinematics *GetKinematics() const
const robMass &MassData(void) const
robMass &MassData(void)
robKinematics::Convention GetConvention() const
cmnJointType GetType() const
inline double Mass() const
inline vctFixedSizeVector<double, 3> CenterOfMass() const
inline vctFixedSizeMatrix<double, 3, 3> MomentOfInertiaAtCOM() const
inline vctFixedSizeMatrix<double, 3, 3> MomentOfInertia() const
inline double PositionMin() const
inline double PositionMax() const
inline double ForceTorqueMax() const
class robJoint

Joint types.

These are used to identify the type of joints.

Subclassed by robKinematics

Public Types

enum Mode

Joint modes.

Each joint can be active (powered by a motor) or passive

Values:

enumerator ACTIVE
enumerator PASSIVE
enum Errno

Values:

enumerator ESUCCESS
enumerator EFAILURE
typedef cmnJointType Type

Public Functions

robJoint()

Default constructor.

robJoint(cmnJointType type, robJoint::Mode mode, double offset, double min, double max, double ftmax)
inline virtual ~robJoint()
inline const std::string &Name(void) const
inline std::string &Name(void)
cmnJointType GetType() const

Return the type of the joint.

Returns:

The type of the joint (hinge, slider, universal, ball and socket)

robJoint::Mode GetMode() const

Return the mode of the joint.

Returns:

The mode of the joint (active or passive)

double GetPosition() const

Return the joint position.

If supported, this returns the joint position.

Returns:

The joint angular or linear position (no unit)

double GetVelocity() const

Return the joint velocity.

If supported, this returns the joint velocity.

Returns:

The joint angular or linear velocity (no unit)

double GetForceTorque() const

Return the joint force or torque.

If supported, this returns the joint force or torque.

Returns:

The joint torque or force (no unit)

void SetPosition(double q)

Set the joint position.

If supported, this sets the joint position. The position is NOT clipped to the position limits.

Parameters:

q – The new joint angular or linear position

void SetVelocity(double qd)

Set the joint velocity.

If supported, this sets the joint velocity. The velocity is NOT clipped to the velocity limit.

Parameters:

qd – The new joint angular or linear velocity.

void SetForceTorque(double ft)

Set the force/torque.

If supported, this sets the force/torque. The new value is NOT clipped to the force/torque limit.

Parameters:

ft – The new force/torque

double PositionOffset() const

Return the offset position.

Returns:

The offset position of the joint. This value has no unit.

void SetPositionOffset(const double offset)
const double &PositionMin(void) const

Return the minimum position.

Returns:

The minimum position of the joint. This value has no unit.

double &PositionMin(void)
const double &PositionMax(void) const

Return the maximum position.

Returns:

The maximum position of the joint. This value has no unit.

double &PositionMax(void)
const double &ForceTorqueMax(void) const

Return the maximum force/torque.

Returns:

The absolute value for the maximum force or torque that can be applied by the joint.

double &ForceTorqueMax(void)
virtual robJoint::Errno Read(std::istream &is)

Read from an input stream.

Use this method to configure the parameters of the joints from an input stream. The parameters are in the following order: type, mode, position offset, min position, max position, max force/torque.

Parameters:

is[in] – The input stream

virtual robJoint::Errno Write(std::ostream &os) const

Read from an input stream.

Use this method to write the parameters of the joints to an output stream. This method can be overloaded for more specific joints.

Parameters:

os[in] – The output stream

Public Static Attributes

static const cmnJointType UNDEFINED = CMN_JOINT_UNDEFINED
static const cmnJointType HINGE = CMN_JOINT_REVOLUTE
static const cmnJointType SLIDER = CMN_JOINT_PRISMATIC
static const cmnJointType UNIVERSAL = CMN_JOINT_UNIVERSAL
static const cmnJointType BALLSOCKET = CMN_JOINT_BALL_SOCKET
class robKinematics : public robJoint

Subclassed by robDH, robHayati, robModifiedDH, robModifiedHayati

Public Types

enum Convention

Kinematics convention.

Values:

enumerator UNDEFINED
enumerator STANDARD_DH
enumerator MODIFIED_DH
enumerator HAYATI
enumerator MODIFIED_HAYATI

Public Functions

robKinematics(robKinematics::Convention convention)

Default constructor.

robKinematics(const robJoint &joint, robKinematics::Convention convention)

Overloaded constructor.

inline virtual ~robKinematics()
inline robKinematics::Convention GetConvention() const

Return the kinematics convention.

Returns:

The DH convention: robDHStandard or robDHModified

robKinematics::Errno Read(std::istream &is)

Read the kinematics parameters and joints parameters.

robKinematics::Errno Write(std::ostream &os) const

Write the kinematics parameters and joints parameters.

virtual vctFrame4x4<double> ForwardKinematics(double q) const = 0

Get the position and orientation of the link.

Returns the position and orientation of the link with respect to the proximal link for a given joint vale.

Parameters:

joint – The joint associated with the link

Returns:

The position and orientation associated with the DH parameters

virtual vctMatrixRotation3<double> Orientation(double q) const = 0

Get the orientation of the link.

Returns the orientation of the link with respect to the proximal link for a given joint vale.

Parameters:

joint – The joint associated with the link

Returns:

The orientation associated with the DH parameters

virtual vctFixedSizeVector<double, 3> PStar() const = 0

Return the position of the next (distal) link coordinate frame.

This method returns the $XYZ$ coordinates of the origin of the distal link in the coordinate frame of the proximal link. “PStar” is not a good name for this but the literature uses $ \mathbf{p}^* $ to denote this value.

Returns:

The position of the next coordinate frame wrt to the current frame

virtual robKinematics *Clone() const = 0

Kinematics objects must be able to clone themselves.

Public Static Functions

static robKinematics *Instantiate(const std::string &type)

Instantiate a kinematic convention by name.

class robDH : public robKinematics

Standard DH parameters of a coordinate frame.

The DH class is used for kinematics parameters.

Public Functions

robDH()

Default constructor.

robDH(double alpha, double a, double theta, double d, const robJoint &joint)

Overloaded constructor.

Parameters:

  • alpha –

  • a –

  • theta –

  • d –

  • joint –

~robDH()

Default destructor.

vctFrame4x4<double> ForwardKinematics(double q) const

Get the position and orientation of the coordinate frame.

Returns the position and orientation of the coordinate frame.

Parameters:

joint – The joint value

Returns:

The position and orientation of the coordinate frame

vctMatrixRotation3<double> Orientation(double q) const

Get the orientation of the link.

Returns the orientation of the coordinate frame.

Parameters:

joint – The joint associated with the link

Returns:

The orientation of the frame

vctFixedSizeVector<double, 3> PStar() const

Return the position of the next (distal) link coordinate frame.

This method returns the $XYZ$ coordinates of the origin of the distal link in the coordinate frame of the proximal link. “PStar” is not a good name for this but the literature uses $ \mathbf{p}^* $ to denote this value.

Returns:

The position of the next coordinate frame wrt to the current frame

robKinematics *Clone() const

Clone.

inline double GetRotationX() const
inline double GetRotationZ() const
inline double GetTranslationX() const
inline double GetTranslationZ() const
inline void SetRotationX(double x)
inline void SetRotationZ(double x)
inline void SetTranslationX(double x)
inline void SetTranslationZ(double x)
class robFunction

Base class for robot function.

Subclassed by robFunctionRn, robFunctionSE3, robFunctionSO3

Public Functions

robFunction(void)
robFunction(double startTime, double stopTime)
inline virtual ~robFunction()
void Set(double startTime, double stopTime)

Set start and stop time.

Parameters:

  • startTime – start time

  • stopTime – stop time

virtual double &StartTime(void)

Return start time.

virtual double &StopTime(void)

Return stop time.

virtual double Duration(void) const

Return duration.

class robFunctionRn : public robFunction

Subclassed by robLinearRn, robQLQRn, robQuintic

Public Functions

robFunctionRn(void)
robFunctionRn(double t1, const vctFixedSizeVector<double, 3> &p1, const vctFixedSizeVector<double, 3> &p1d, const vctFixedSizeVector<double, 3> &p1dd, double t2, const vctFixedSizeVector<double, 3> &p2, const vctFixedSizeVector<double, 3> &p2d, const vctFixedSizeVector<double, 3> &p2dd)
robFunctionRn(double t1, const vctDynamicVector<double> &p1, const vctDynamicVector<double> &p1d, const vctDynamicVector<double> &p1dd, double t2, const vctDynamicVector<double> &p2, const vctDynamicVector<double> &p2d, const vctDynamicVector<double> &p2dd)
inline virtual ~robFunctionRn()
void Set(double t1, const vctDynamicVector<double> &p1, const vctDynamicVector<double> &p1d, const vctDynamicVector<double> &p1dd, double t2, const vctDynamicVector<double> &p2, const vctDynamicVector<double> &p2d, const vctDynamicVector<double> &p2dd)
void InitialState(vctDynamicVector<double> &p, vctDynamicVector<double> &pd, vctDynamicVector<double> &pdd)
void FinalState(vctDynamicVector<double> &p, vctDynamicVector<double> &pd, vctDynamicVector<double> &pdd)
void InitialState(vctFixedSizeVector<double, 3> &p, double &v, double &vd)
void FinalState(vctFixedSizeVector<double, 3> &p, double &v, double &vd)
inline virtual void Evaluate(double, vctFixedSizeVector<double, 3>&, vctFixedSizeVector<double, 3>&, vctFixedSizeVector<double, 3>&)
inline virtual void Evaluate(double, vctDynamicVector<double>&, vctDynamicVector<double>&, vctDynamicVector<double>&)
virtual void Blend(robFunction *function, const vctDynamicVector<double> &qd, const vctDynamicVector<double> &qdd) = 0
virtual void Blend(robFunction *function, double vmax, double vdmax) = 0
class robLinearRn : public robFunctionRn

A linear function (as in Ax=b)

Public Functions

robLinearRn(const vctFixedSizeVector<double, 3> &y1, const vctFixedSizeVector<double, 3> &y2, double vmax, double t1 = 0.0)

Define a linear function \(L: R^1\rightarrow R^3 \).

Define a line passing throuh \((t_1,\mathbf{y}_1)\) and \((t_2,\mathbf{y}_2)\). The final time \(t_2\) is determined by the maximum velocities. The function is bounded by \([t_1, t_2]\).

Parameters:

  • t1 – The initial time

  • y1 – The value \(\mathbf{y}_1 = L(t_1)\)

  • y2 – The value \(\mathbf{y}_2 = L(t_2)\)

  • vmax – The magnitude of the maximum linear velocity

robLinearRn(const vctDynamicVector<double> &y1, const vctDynamicVector<double> &y2, const vctDynamicVector<double> &ydmax, double t1 = 0.0)

Define a linear function \(L: R^1\rightarrow R^n \).

Define a line passing throuh \((t_1,\mathbf{y}_1)\) and \((t_2,\mathbf{y}_2)\). The final time \(t_2\) is determined by the maximum velocities. The function is bounded by \([t_1, t_2]\).

Parameters:

  • t1 – The initial time

  • y1 – The value \(\mathbf{y}_1 = L(t_1)\)

  • y2 – The value \(\mathbf{y}_2 = L(t_2)\)

  • ydmax – The maximum velocity for each dimension

void Evaluate(double t, double &y, double &yd, double &ydd)

Evaluate the function.

Evaluate the linear function for the given input. The domain of the input

Parameters:

  • input – [in] The input to the function. The input domain must be robSpace::TIME

  • t – The time

  • y – [out] The linear function at time t

  • yd – [out] The 1st order time derivative at time t

  • ydd – [out] The 2nd order time derivative at time t (always zero)

void Evaluate(double t, vctFixedSizeVector<double, 3> &p, vctFixedSizeVector<double, 3> &v, vctFixedSizeVector<double, 3> &vd)

Evaluate the function.

Evaluate the linear function for the given input. The domain of the input

Parameters:

  • input – [in] The input to the function. The input domain must be robSpace::TIME

  • t – The time

  • p – [out] The linear function at time t

  • v – [out] The linear velocity

  • vd – [out] The linear acceleration

void Evaluate(double t, vctDynamicVector<double> &y, vctDynamicVector<double> &yd, vctDynamicVector<double> &ydd)

Evaluate the function.

Evaluate the linear function for the given input. The domain of the input

Parameters:

  • input – [in] The input to the function. The input domain must be robSpace::TIME

  • t – The time

  • y – [out] The linear function at time t

  • yd – [out] The 1st order time derivative at time t

  • ydd – [out] The 2nd order time derivative at time t (always zero)

void Blend(robFunction *function, const vctDynamicVector<double> &qd, const vctDynamicVector<double> &qdd)
void Blend(robFunction *function, double vmax, double vdmax)
vctDynamicVector<double> Slope() const