Virtual base robot class. More...

#include <robot.h>

Inheritance diagram for Robot_basic:

Public Member Functions
ReturnMatrix	acceleration (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau)
	Joints acceleration without contact force.
ReturnMatrix	acceleration (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau, const ColumnVector &Fext, const ColumnVector &Next)
	Joints acceleration.
virtual ReturnMatrix	C (const ColumnVector &qp)=0
virtual void	delta_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector &torque, ColumnVector &dtorque)=0
virtual void	dq_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, ColumnVector &torque, ColumnVector &dtorque)=0
virtual void	dqp_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque)=0
ReturnMatrix	dtau_dq (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
	Sensitivity of the dynamics with respect to .
ReturnMatrix	dtau_dqp (const ColumnVector &q, const ColumnVector &qp)
	Sensitivity of the dynamics with respect to $\dot{q}$ .
virtual void	dTdqi (Matrix &dRot, ColumnVector &dp, const int i)=0
virtual ReturnMatrix	dTdqi (const int i)=0
void	error (const std::string &msg1) const
	Print the message msg1 on the console.
virtual ReturnMatrix	G ()=0
int	get_available_dof () const
	Counts number of currently non-immobile links.
int	get_available_dof (const int endlink) const
	Counts number of currently non-immobile links up to and including endlink.
ReturnMatrix	get_available_q (void) const
	Return the joint position vector of available (non-immobile) joints.
ReturnMatrix	get_available_q (const int endlink) const
	Return the joint position vector of available (non-immobile) joints up to and including endlink.
ReturnMatrix	get_available_qp (void) const
	Return the joint velocity vector of available (non-immobile) joints.
ReturnMatrix	get_available_qp (const int endlink) const
	Return the joint velocity vector of available (non-immobile) joints up to and including endlink.
ReturnMatrix	get_available_qpp (void) const
	Return the joint acceleration vector of available (non-immobile) joints.
ReturnMatrix	get_available_qpp (const int endlink) const
	Return the joint acceleration vector of available (non-immobile) joints up to and including endlink.
bool	get_DH () const
	Return true if in DH notation, false otherwise.
int	get_dof () const
	Return dof.
int	get_fix () const
	Return fix.
Real	get_q (const int i) const
ReturnMatrix	get_q (void) const
	Return the joint position vector.
ReturnMatrix	get_qp (void) const
	Return the joint velocity vector.
ReturnMatrix	get_qpp (void) const
	Return the joint acceleration vector.
ReturnMatrix	inertia (const ColumnVector &q)
	Inertia of the manipulator.
virtual ReturnMatrix	inv_kin (const Matrix &Tobj, const int mj=0)
	Numerical inverse kinematics. See inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)
ReturnMatrix	inv_kin (const Matrix &Tobj, const int mj, bool &converge)
virtual ReturnMatrix	inv_kin (const Matrix &Tobj, const int mj, const int endlink, bool &converge)
	Numerical inverse kinematics.
virtual ReturnMatrix	inv_kin_puma (const Matrix &Tobj, bool &converge)=0
virtual ReturnMatrix	inv_kin_rhino (const Matrix &Tobj, bool &converge)=0
virtual ReturnMatrix	inv_kin_schilling (const Matrix &Tobj, bool &converge)=0
virtual ReturnMatrix	jacobian (const int ref=0) const
	Jacobian of mobile links expressed at frame ref.
virtual ReturnMatrix	jacobian (const int endlink, const int ref) const =0
ReturnMatrix	jacobian_DLS_inv (const double eps, const double lambda_max, const int ref=0) const
	Inverse Jacobian based on damped least squares inverse.
virtual ReturnMatrix	jacobian_dot (const int ref=0) const =0
void	kine (Matrix &Rot, ColumnVector &pos) const
	Direct kinematics at end effector.
void	kine (Matrix &Rot, ColumnVector &pos, const int j) const
	Direct kinematics at end effector.
ReturnMatrix	kine (void) const
	Return the end effector direct kinematics transform matrix.
ReturnMatrix	kine (const int j) const
	Return the frame j direct kinematics transform matrix.
ReturnMatrix	kine_pd (const int ref=0) const
	Direct kinematics with velocity.
virtual void	kine_pd (Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const =0
Robot_basic &	operator= (const Robot_basic &x)
	Overload = operator.
	Robot_basic (const int ndof=1, const bool dh_parameter=false, const bool min_inertial_para=false)
	Constructor.
	Robot_basic (const Matrix &initrobot_motor, const bool dh_parameter=false, const bool min_inertial_para=false)
	Constructor.
	Robot_basic (const Matrix &initrobot, const Matrix &initmotor, const bool dh_parameter=false, const bool min_inertial_para=false)
	Constructor.
	Robot_basic (const std::string &filename, const std::string &robotName, const bool dh_parameter=false, const bool min_inertial_para=false)
	Robot_basic (const Robot_basic &x)
	Copy constructor.
virtual void	robotType_inv_kin ()=0
void	set_q (const ColumnVector &q)
	Set the joint position vector.
void	set_q (const Matrix &q)
	Set the joint position vector.
void	set_q (const Real q, const int i)
void	set_qp (const ColumnVector &qp)
	Set the joint velocity vector.
void	set_qpp (const ColumnVector &qpp)
	Set the joint acceleration vector.
virtual ReturnMatrix	torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)=0
virtual ReturnMatrix	torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &Fext_, const ColumnVector &Next_)=0
virtual ReturnMatrix	torque_novelocity (const ColumnVector &qpp)=0
virtual	~Robot_basic ()
	Destructor.
Public Attributes
ColumnVector *	a
ColumnVector *	da
ColumnVector *	df
ColumnVector *	dF
ColumnVector *	dn
ColumnVector *	dN
ColumnVector *	dp
ColumnVector *	dvp
ColumnVector *	dw
ColumnVector *	dwp
ColumnVector *	f
ColumnVector *	F
ColumnVector *	f_nv
ColumnVector	gravity
	Gravity vector.
Link *	links
	Pointer on Link cclass.
ColumnVector *	N
ColumnVector *	n
ColumnVector *	n_nv
ColumnVector *	p
ColumnVector *	pp
Matrix *	R
	Temprary rotation matrix.
ColumnVector *	vp
ColumnVector *	w
ColumnVector *	wp
ColumnVector	z0
	Axis vector at each joint.
Private Types
enum	EnumRobotType { DEFAULT = 0, RHINO = 1, PUMA = 2, SCHILLING = 3 }
	enum EnumRobotType More...
Private Member Functions
void	cleanUpPointers ()
	Free all vectors and matrix memory.
Private Attributes
int	dof
	Degree of freedom.
int	fix
	Virtual link, used with modified DH notation.
EnumRobotType	robotType
	Robot type.
Friends
class	mRobot
class	mRobot_min_para
class	mRobotgl
class	Robot
class	Robotgl

Detailed Description

Virtual base robot class.

Definition at line 216 of file robot.h.

Member Enumeration Documentation

enum Robot_basic::EnumRobotType [private]

enum EnumRobotType

Enumerator:

DEFAULT	Default robot familly.
RHINO	Rhino familly.
PUMA	Puma familly.
SCHILLING	Schilling familly.

Definition at line 324 of file robot.h.

Constructor & Destructor Documentation

Robot_basic::Robot_basic	(	const int	ndof = `1`,
		const bool	dh_parameter = `false`,
		const bool	min_inertial_para = `false`
	)

Constructor.

Parameters:

ndof,:	Robot degree of freedom.
dh_parameter,:	true if DH notation, false if modified DH notation.
min_inertial_para,:	true inertial parameter are in minimal form.

Allocate memory for vectors and matrix pointers. Initialize all the Links instance.

Definition at line 573 of file robot.cpp.

Robot_basic::Robot_basic	(	const Matrix &	dhinit,
		const bool	dh_parameter = `false`,
		const bool	min_inertial_para = `false`
	)

Constructor.

Parameters:

dhinit,:	Robot initialization matrix.
dh_parameter,:	true if DH notation, false if modified DH notation.
min_inertial_para,:	true inertial parameter are in minimal form.

Allocate memory for vectors and matrix pointers. Initialize all the Links instance.

Definition at line 343 of file robot.cpp.

Robot_basic::Robot_basic	(	const Matrix &	initrobot,
		const Matrix &	initmotor,
		const bool	dh_parameter = `false`,
		const bool	min_inertial_para = `false`
	)

Constructor.

Parameters:

initrobot,:	Robot initialization matrix.
initmotor,:	Motor initialization matrix.
dh_parameter,:	true if DH notation, false if modified DH notation.
min_inertial_para,:	true inertial parameter are in minimal form.

Allocate memory for vectors and matrix pointers. Initialize all the Links instance.

Definition at line 451 of file robot.cpp.

Robot_basic::Robot_basic	(	const std::string &	filename,
		const std::string &	robotName,
		const bool	dh_parameter = `false`,
		const bool	min_inertial_para = `false`
	)

Robot_basic::Robot_basic ( const Robot_basic & x )

Copy constructor.

Definition at line 651 of file robot.cpp.

Robot_basic::~Robot_basic ( ) [virtual]

Destructor.

Free all vectors and matrix memory.

Definition at line 879 of file robot.cpp.

Member Function Documentation

ReturnMatrix Robot_basic::acceleration	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	tau
	)

Joints acceleration without contact force.

Definition at line 100 of file dynamics.cpp.

ReturnMatrix Robot_basic::acceleration	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	tau_cmd,
		const ColumnVector &	Fext,
		const ColumnVector &	Next
	)

Joints acceleration.

The robot dynamics is

$B(q)\ddot{q} + C(q,\dot{q})\dot{q} + D\dot{q} + g(q) = \tau - J^T(q)f$

then the joint acceleration is

$\ddot{q} = B^{-1}(q)\big(\tau - J^T(q)f - C(q,\dot{q})\dot{q} - D\dot{q} - g(q)\big )$

Definition at line 112 of file dynamics.cpp.

virtual ReturnMatrix Robot_basic::C ( const ColumnVector & qp ) [pure virtual]

Implemented in mRobot_min_para, mRobot, and Robot.

void Robot_basic::cleanUpPointers ( ) [private]

Free all vectors and matrix memory.

Definition at line 1202 of file robot.cpp.

virtual void Robot_basic::delta_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	dq,
		const ColumnVector &	dqp,
		const ColumnVector &	dqpp,
		ColumnVector &	torque,
		ColumnVector &	dtorque
	)		`[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

virtual void Robot_basic::dq_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	dq,
		ColumnVector &	torque,
		ColumnVector &	dtorque
	)		`[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

virtual void Robot_basic::dqp_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	dqp,
		ColumnVector &	torque,
		ColumnVector &	dtorque
	)		`[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

ReturnMatrix Robot_basic::dtau_dq	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp
	)

Sensitivity of the dynamics with respect to $ q $ .

This function computes $S_2(q, \dot{q}, \ddot{q})$ .

Definition at line 58 of file sensitiv.cpp.

ReturnMatrix Robot_basic::dtau_dqp	(	const ColumnVector &	q,
		const ColumnVector &	qp
	)

Sensitivity of the dynamics with respect to $\dot{q}$ .

This function computes $S_1(q, \dot{q})$ .

Definition at line 84 of file sensitiv.cpp.

virtual void Robot_basic::dTdqi	(	Matrix &	dRot,
		ColumnVector &	dp,
		const int	i
	)		`[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

virtual ReturnMatrix Robot_basic::dTdqi ( const int i ) [pure virtual]

Implemented in mRobot_min_para, mRobot, and Robot.

void Robot_basic::error ( const std::string & msg1 ) const

Print the message msg1 on the console.

Definition at line 987 of file robot.cpp.

virtual ReturnMatrix Robot_basic::G ( ) [pure virtual]

Implemented in mRobot_min_para, mRobot, and Robot.

int Robot_basic::get_available_dof ( ) const [inline]

Counts number of currently non-immobile links.

Definition at line 241 of file robot.h.

int Robot_basic::get_available_dof ( const int endlink ) const

Counts number of currently non-immobile links up to and including endlink.

Definition at line 994 of file robot.cpp.

ReturnMatrix Robot_basic::get_available_q ( void ) const [inline]

Return the joint position vector of available (non-immobile) joints.

Definition at line 247 of file robot.h.

ReturnMatrix Robot_basic::get_available_q ( const int endlink ) const

Return the joint position vector of available (non-immobile) joints up to and including endlink.

Definition at line 1034 of file robot.cpp.

ReturnMatrix Robot_basic::get_available_qp ( void ) const [inline]

Return the joint velocity vector of available (non-immobile) joints.

Definition at line 248 of file robot.h.

ReturnMatrix Robot_basic::get_available_qp ( const int endlink ) const

Return the joint velocity vector of available (non-immobile) joints up to and including endlink.

Definition at line 1046 of file robot.cpp.

ReturnMatrix Robot_basic::get_available_qpp ( void ) const [inline]

Return the joint acceleration vector of available (non-immobile) joints.

Definition at line 249 of file robot.h.

ReturnMatrix Robot_basic::get_available_qpp ( const int endlink ) const

Return the joint acceleration vector of available (non-immobile) joints up to and including endlink.

Definition at line 1058 of file robot.cpp.

bool Robot_basic::get_DH ( ) const [inline]

Return true if in DH notation, false otherwise.

Definition at line 239 of file robot.h.

int Robot_basic::get_dof ( ) const [inline]

Return dof.

Definition at line 240 of file robot.h.

int Robot_basic::get_fix ( ) const [inline]

Return fix.

Definition at line 243 of file robot.h.

Real Robot_basic::get_q ( const int i ) const [inline]

< Return joint i position.

Definition at line 235 of file robot.h.

ReturnMatrix Robot_basic::get_q ( void ) const

Return the joint position vector.

Definition at line 1004 of file robot.cpp.

ReturnMatrix Robot_basic::get_qp ( void ) const

Return the joint velocity vector.

Definition at line 1014 of file robot.cpp.

ReturnMatrix Robot_basic::get_qpp ( void ) const

Return the joint acceleration vector.

Definition at line 1024 of file robot.cpp.

ReturnMatrix Robot_basic::inertia ( const ColumnVector & q )

Inertia of the manipulator.

Definition at line 83 of file dynamics.cpp.

ReturnMatrix Robot_basic::inv_kin	(	const Matrix &	Tobj,
		const int	mj = `0`
	)		`[virtual]`

Numerical inverse kinematics. See inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge)

Overload inv_kin function.

Reimplemented in mRobot_min_para, mRobot, and Robot.

Definition at line 83 of file invkine.cpp.

ReturnMatrix Robot_basic::inv_kin	(	const Matrix &	Tobj,
		const int	mj,
		bool &	converge
	)		`[inline]`

Definition at line 271 of file robot.h.

ReturnMatrix Robot_basic::inv_kin	(	const Matrix &	Tobj,
		const int	mj,
		const int	endlink,
		bool &	converge
	)		`[virtual]`

Numerical inverse kinematics.

Parameters:

Tobj,:	Transformation matrix expressing the desired end effector pose.
mj,:	Select algorithm type, 0: based on Jacobian, 1: based on derivative of T.
converge,:	Indicate if the algorithm converge.
endlink,:	the link to pretend is the end effector

The joint position vector before the inverse kinematics is returned if the algorithm does not converge.

Reimplemented in mRobot_min_para, mRobot, and Robot.

Definition at line 91 of file invkine.cpp.

virtual ReturnMatrix Robot_basic::inv_kin_puma	(	const Matrix &	Tobj,
		bool &	converge
	)		`[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

virtual ReturnMatrix Robot_basic::inv_kin_rhino	(	const Matrix &	Tobj,
		bool &	converge
	)		`[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

virtual ReturnMatrix Robot_basic::inv_kin_schilling	(	const Matrix &	Tobj,
		bool &	converge
	)		`[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

virtual ReturnMatrix Robot_basic::jacobian ( const int ref = 0 ) const [inline, virtual]

Jacobian of mobile links expressed at frame ref.

Reimplemented in mRobot_min_para, mRobot, and Robot.

Definition at line 277 of file robot.h.

virtual ReturnMatrix Robot_basic::jacobian	(	const int	endlink,
		const int	ref
	)		const `[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

ReturnMatrix Robot_basic::jacobian_DLS_inv	(	const double	eps,
		const double	lambda_max,
		const int	ref = `0`
	)		const

Inverse Jacobian based on damped least squares inverse.

Parameters:

eps,:	Range of singular region.
lambda_max,:	Value to obtain a good solution in singular region.
ref,:	Selected frame (ex: joint 4).

The Jacobian inverse, based on damped least squares, is

$J^{-1}(q) = \big(J^T(q)J(q) + \lambda^2I \big)^{-1}J^T(q)$

where $\lambda$ and $I$ is a damping factor and the identity matrix respectively. Based on SVD (Singular Value Decomposition) the Jacobian is $J = \sum_{i=1}^{m}\sigma_i u_i v_i^T$ , where $u_i$ , $v_i$ and $\sigma_i$ are respectively the input vector, the ouput vector and the singular values ( $\sigma_1 \geq \sigma_2 \cdots \geq \sigma_r \geq 0$ , $r$ is the rank of J). Using the previous equations we obtain

$J^{-1}(q) = \sum_{i=1}^{m} \frac{\sigma_i}{\sigma_i^2 + \lambda_i^2}v_iu_i$

A singular region, based on the smallest singular value, can be defined by

$\lambda^2 = \Bigg\{ \begin{array}{cc} 0 & \textrm{si $\sigma_6 \geq \epsilon$} \\ \Big(1 - (\frac{\sigma_6}{\epsilon})^2 \Big)\lambda^2_{max} & \textrm{sinon} \end{array}$

Definition at line 169 of file kinemat.cpp.

virtual ReturnMatrix Robot_basic::jacobian_dot ( const int ref = 0 ) const [pure virtual]

Implemented in mRobot_min_para, mRobot, and Robot.

void Robot_basic::kine	(	Matrix &	Rot,
		ColumnVector &	pos
	)		const

Direct kinematics at end effector.

Parameters:

Rot,:	End effector orientation.
pos,:	Enf effector position.

Definition at line 92 of file kinemat.cpp.

void Robot_basic::kine	(	Matrix &	Rot,
		ColumnVector &	pos,
		const int	j
	)		const

Direct kinematics at end effector.

Parameters:

Rot,:	Frame j orientation.
pos,:	Frame j position.
j,:	Selected frame.

Definition at line 102 of file kinemat.cpp.

ReturnMatrix Robot_basic::kine ( void ) const

Return the end effector direct kinematics transform matrix.

Definition at line 120 of file kinemat.cpp.

ReturnMatrix Robot_basic::kine ( const int j ) const

Return the frame j direct kinematics transform matrix.

Definition at line 129 of file kinemat.cpp.

ReturnMatrix Robot_basic::kine_pd ( const int j = 0 ) const

Direct kinematics with velocity.

Return a $3\times 5$ matrix. The first three columns are the frame j to the base rotation, the fourth column is the frame j w.r.t to the base postion vector and the last column is the frame j w.r.t to the base translational velocity vector. Print an error on the console if j is out of range.

Definition at line 142 of file kinemat.cpp.

virtual void Robot_basic::kine_pd	(	Matrix &	Rot,
		ColumnVector &	pos,
		ColumnVector &	pos_dot,
		const int	ref
	)		const `[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

Robot_basic & Robot_basic::operator= ( const Robot_basic & x )

Overload = operator.

Definition at line 889 of file robot.cpp.

virtual void Robot_basic::robotType_inv_kin ( ) [pure virtual]

Implemented in mRobot_min_para, mRobot, and Robot.

void Robot_basic::set_q ( const ColumnVector & q )

Set the joint position vector.

Set the joint position vector and recalculate the orientation matrix R and the position vector p (see Link class). Print an error if the size of q is incorrect.

Definition at line 1137 of file robot.cpp.

void Robot_basic::set_q ( const Matrix & q )

Set the joint position vector.

Set the joint position vector and recalculate the orientation matrix R and the position vector p (see Link class). Print an error if the size of q is incorrect.

Definition at line 1070 of file robot.cpp.

void Robot_basic::set_q	(	const Real	q,
		const int	i
	)		`[inline]`

< Set joint i position.

Definition at line 255 of file robot.h.

void Robot_basic::set_qp ( const ColumnVector & qp )

Set the joint velocity vector.

Definition at line 1176 of file robot.cpp.

void Robot_basic::set_qpp ( const ColumnVector & qpp )

Set the joint acceleration vector.

Definition at line 1191 of file robot.cpp.

virtual ReturnMatrix Robot_basic::torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp
	)		`[pure virtual]`

Implemented in mRobot_min_para, mRobot, and Robot.

virtual ReturnMatrix Robot_basic::torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	Fext_,
		const ColumnVector &	Next_
	)		`[pure virtual]`