DH notation robot class. More...

#include <robot.h>

Inheritance diagram for Robot:

Public Member Functions
virtual ReturnMatrix	C (const ColumnVector &qp)
	Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation. More...

virtual void	delta_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, const ColumnVector &dqp, const ColumnVector &dqpp, ColumnVector &ltorque, ColumnVector &dtorque)
	Delta torque dynamics. More...

virtual void	dq_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, ColumnVector &torque, ColumnVector &dtorque)
	Delta torque due to delta joint position. More...

virtual void	dqp_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque)
	Delta torque due to delta joint velocity. More...

virtual void	dTdqi (Matrix &dRot, ColumnVector &dp, const int i)
	Partial derivative of the robot position (homogeneous transf.) More...

virtual ReturnMatrix	dTdqi (const int i)
	Partial derivative of the robot position (homogeneous transf.) More...

virtual ReturnMatrix	G ()
	Joint torque due to gravity based on Recursive Newton-Euler formulation. More...

ReturnMatrix	inv_kin (const Matrix &Tobj, const int mj=0)
	Overload inv_kin function. More...

virtual ReturnMatrix	inv_kin (const Matrix &Tobj, const int mj, const int endlink, bool &converge)
	Inverse kinematics solutions. More...

virtual ReturnMatrix	inv_kin_puma (const Matrix &Tobj, bool &converge)
	Analytic Puma inverse kinematics. More...

virtual ReturnMatrix	inv_kin_rhino (const Matrix &Tobj, bool &converge)
	Analytic Rhino inverse kinematics. More...

virtual ReturnMatrix	inv_kin_schilling (const Matrix &Tobj, bool &converge)
	Analytic Schilling inverse kinematics. More...

virtual ReturnMatrix	jacobian (const int ref=0) const
	Jacobian of mobile links expressed at frame ref. More...

virtual ReturnMatrix	jacobian (const int endlink, const int ref) const
	Jacobian of mobile links up to endlink expressed at frame ref. More...

virtual ReturnMatrix	jacobian_dot (const int ref=0) const
	Jacobian derivative of mobile joints expressed at frame ref. More...

virtual void	kine_pd (Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const
	Direct kinematics with velocity. More...

	Robot (const int ndof=1)
	Constructor. More...

	Robot (const Matrix &initrobot)
	Constructor. More...

	Robot (const Matrix &initrobot, const Matrix &initmotor)
	Constructor. More...

	Robot (const Robot &x)
	Copy constructor. More...

	Robot (const std::string &filename, const std::string &robotName)

virtual void	robotType_inv_kin ()
	Identify inverse kinematics familly. More...

virtual ReturnMatrix	torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
	Joint torque, without contact force, based on Recursive Newton-Euler formulation. More...

virtual ReturnMatrix	torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &Fext_, const ColumnVector &Next_)
	Joint torque based on Recursive Newton-Euler formulation. More...

virtual ReturnMatrix	torque_novelocity (const ColumnVector &qpp)
	Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation. More...

virtual	~Robot ()
	Destructor. More...

Public Member Functions inherited from Robot_basic
ReturnMatrix	acceleration (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau)
	Joints acceleration without contact force. More...

ReturnMatrix	acceleration (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau, const ColumnVector &Fext, const ColumnVector &Next)
	Joints acceleration. More...

ReturnMatrix	dtau_dq (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
	Sensitivity of the dynamics with respect to . More...

ReturnMatrix	dtau_dqp (const ColumnVector &q, const ColumnVector &qp)
	Sensitivity of the dynamics with respect to $\dot{q}$ . More...

void	error (const std::string &msg1) const
	Print the message msg1 on the console. More...

int	get_available_dof () const
	Counts number of currently non-immobile links. More...

int	get_available_dof (const int endlink) const
	Counts number of currently non-immobile links up to and including endlink. More...

ReturnMatrix	get_available_q (void) const
	Return the joint position vector of available (non-immobile) joints. More...

ReturnMatrix	get_available_q (const int endlink) const
	Return the joint position vector of available (non-immobile) joints up to and including endlink. More...

ReturnMatrix	get_available_qp (void) const
	Return the joint velocity vector of available (non-immobile) joints. More...

ReturnMatrix	get_available_qp (const int endlink) const
	Return the joint velocity vector of available (non-immobile) joints up to and including endlink. More...

ReturnMatrix	get_available_qpp (void) const
	Return the joint acceleration vector of available (non-immobile) joints. More...

ReturnMatrix	get_available_qpp (const int endlink) const
	Return the joint acceleration vector of available (non-immobile) joints up to and including endlink. More...

bool	get_DH () const
	Return true if in DH notation, false otherwise. More...

int	get_dof () const
	Return dof. More...

int	get_fix () const
	Return fix. More...

Real	get_q (const int i) const

ReturnMatrix	get_q (void) const
	Return the joint position vector. More...

ReturnMatrix	get_qp (void) const
	Return the joint velocity vector. More...

ReturnMatrix	get_qpp (void) const
	Return the joint acceleration vector. More...

ReturnMatrix	inertia (const ColumnVector &q)
	Inertia of the manipulator. More...

ReturnMatrix	inv_kin (const Matrix &Tobj, const int mj, bool &converge)

ReturnMatrix	jacobian_DLS_inv (const double eps, const double lambda_max, const int ref=0) const
	Inverse Jacobian based on damped least squares inverse. More...

void	kine (Matrix &Rot, ColumnVector &pos) const
	Direct kinematics at end effector. More...

void	kine (Matrix &Rot, ColumnVector &pos, const int j) const
	Direct kinematics at end effector. More...

ReturnMatrix	kine (void) const
	Return the end effector direct kinematics transform matrix. More...

ReturnMatrix	kine (const int j) const
	Return the frame j direct kinematics transform matrix. More...

ReturnMatrix	kine_pd (const int ref=0) const
	Direct kinematics with velocity. More...

Robot_basic &	operator= (const Robot_basic &x)
	Overload = operator. More...

	Robot_basic (const int ndof=1, const bool dh_parameter=false, const bool min_inertial_para=false)
	Constructor. More...

	Robot_basic (const Matrix &initrobot_motor, const bool dh_parameter=false, const bool min_inertial_para=false)
	Constructor. More...

	Robot_basic (const Matrix &initrobot, const Matrix &initmotor, const bool dh_parameter=false, const bool min_inertial_para=false)
	Constructor. More...

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

void	set_q (const ColumnVector &q)
	Set the joint position vector. More...

void	set_q (const Matrix &q)
	Set the joint position vector. More...

void	set_q (const Real q, const int i)

void	set_qp (const ColumnVector &qp)
	Set the joint velocity vector. More...

void	set_qpp (const ColumnVector &qpp)
	Set the joint acceleration vector. More...

virtual	~Robot_basic ()
	Destructor. More...

Additional Inherited Members
Public Attributes inherited from Robot_basic
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. More...

Link *	links
	Pointer on Link cclass. More...

ColumnVector *	N

ColumnVector *	n

ColumnVector *	n_nv

ColumnVector *	p

ColumnVector *	pp

Matrix *	R
	Temprary rotation matrix. More...

ColumnVector *	vp

ColumnVector *	w

ColumnVector *	wp

ColumnVector	z0
	Axis vector at each joint. More...

Detailed Description

DH notation robot class.

Definition at line 340 of file robot.h.

Constructor & Destructor Documentation

Robot::Robot ( const int ndof = 1 )

Constructor.

Definition at line 1242 of file robot.cpp.

Robot::Robot ( const Matrix & initrobot )

Constructor.

Definition at line 1251 of file robot.cpp.

Robot::Robot	(	const Matrix &	initrobot,
		const Matrix &	initmotor
	)

Constructor.

Definition at line 1260 of file robot.cpp.

Robot::Robot ( const Robot & x )

Copy constructor.

Definition at line 1280 of file robot.cpp.

Robot::Robot	(	const std::string &	filename,
		const std::string &	robotName
	)

virtual Robot::~Robot ( )

inlinevirtual

Destructor.

Definition at line 348 of file robot.h.

Member Function Documentation

ReturnMatrix Robot::C ( const ColumnVector & qp )

virtual

Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.

Implements Robot_basic.

Definition at line 360 of file dynamics.cpp.

void Robot::delta_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	dq,
		const ColumnVector &	dqp,
		const ColumnVector &	dqpp,
		ColumnVector &	ltorque,
		ColumnVector &	dtorque
	)

virtual

Delta torque dynamics.

This function computes

$\delta \tau = D(q) \delta \ddot{q} + S_1(q,\dot{q}) \delta \dot{q} + S_2(q,\dot{q},\ddot{q}) \delta q$

Murray and Neuman Cite_: Murray86 have developed an efficient recursive linearized Newton-Euler formulation. In order to apply the RNE as presented in let us define the following variables

$p_{di} = \frac{\partial p_i}{\partial d_i} = \left [ \begin{array}{ccc} 0 & \sin \alpha_i & \cos \alpha_i \end{array} \right ]^T$

$Q = \left [ \begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array} \right ]$

Forward Iterations for $i=1, 2, \ldots, n$ . Initialize: $\delta \omega_0 = \delta \dot{\omega}_0 = \delta \dot{v}_0 = 0$ .

$\delta \omega_i = R_i^T \{\delta \omega_{i-1} + \sigma_i [ z_0 \delta \dot{\theta}_i - Q(\omega_{i-1} + \dot{\theta}_i ) \delta \theta_i ] \}$

$\delta \dot{\omega}_i = R_i^T \{ \delta \dot{\omega}_{i-1} + \sigma_i [z_0 \delta \ddot{\theta}_i + \delta \omega_{i-1} \times (z_0 \dot{\theta}_i ) + \omega_{i-1} \times (z_0 \delta \dot{\theta}_i )] - \sigma_i Q [ \omega_{i-1} + z_0 \ddot{\theta}_i + \omega_{i-1} \times (z_0 \dot{\theta}_i )] \delta \theta_i \}$

$\delta \dot{v}_i = R_i^T \{ \delta \dot{v}_{i-1} - \sigma_i Q \dot{v}_{i-1} \delta \theta_i + (1 -\sigma_i) [z_0 \delta \ddot{d}_i + 2 \delta \omega_{i-1} \times (z_0 \dot{d}_i ) + 2 \omega_{i-1} \times (z_0 \delta \dot{d}_i )] \} + \delta \dot{\omega}_i \times p_i + \delta \omega_i \times ( \omega_i \times p_i) + \omega_i \times ( \delta \omega_i \times p_i) + (1 - \sigma_i) (\dot{\omega}_i \times p_{di} + \omega_i \times ( \omega_i \times p_{di}) ) \delta d_i$

Backward Iterations for $i=n, n-1, \ldots, 1$ . Initialize: $\delta f_{n+1} = \delta n_{n+1} = 0$ .

$\delta \dot{v}_{ci} = \delta v_i + \delta \omega_i \times r_i + \delta \omega_i \times (\omega_i \times r_i) + \omega_i \times (\delta \omega_i \times r_i)$

$\delta F_i = m_i \delta \dot{v}_{ci}$

$\delta N_i = I_{ci} \delta \dot{\omega}_i + \delta \omega_i \times (I_{ci} \omega_i) + \omega_i \times (I_{ci} \delta \omega_i)$

$\delta f_i = R_{i+1} [ \delta f_{i+1} ] + \delta F_{i} + \sigma_{i+1} Q R_{i+1} [ f_{i+1} ] \delta \theta_{i+1}$

$\delta n_i = R_{i+1} [ \delta n_{i+1} ] + \delta N_{i} + p_{i} \times \delta f_{i} + r_{i} \times \delta F_{i} + (1 - \sigma_i) (p_{di} \times f_{i}) \delta d_i + \sigma_{i+1} Q R_{i+1} [ n_{i+1} ] \delta \theta_{i+1}$

$\delta \tau_i = \sigma_i [ \delta n_i^T (R_i^T z_0) - n_i^T (R_i^T Q z_0) \delta \theta_i] + (1 -\sigma_i) [ \delta f_i^T (R_i^T z_0) ]$

Implements Robot_basic.

Definition at line 65 of file delta_t.cpp.

void Robot::dq_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	dq,
		ColumnVector &	ltorque,
		ColumnVector &	dtorque
	)

virtual

Delta torque due to delta joint position.

This function computes $S_2(q, \dot{q}, \ddot{q})\delta q$ . See Robot::delta_torque for equations.

Implements Robot_basic.

Definition at line 65 of file comp_dq.cpp.

void Robot::dqp_torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	dqp,
		ColumnVector &	ltorque,
		ColumnVector &	dtorque
	)

virtual

Delta torque due to delta joint velocity.

This function computes $S_1(q, \dot{q}, \ddot{q})\delta \dot{q}$ . See Robot::delta_torque for equations.

Implements Robot_basic.

Definition at line 63 of file comp_dqp.cpp.

void Robot::dTdqi	(	Matrix &	dRot,
		ColumnVector &	dp,
		const int	i
	)

virtual

Partial derivative of the robot position (homogeneous transf.)

This function computes the partial derivatives:

$\frac{\partial{}^0 T_n}{\partial q_i} = {}^0 T_{i-1} Q_i \; {}^{i-1} T_n$

in standard notation and

$\frac{\partial{}^0 T_n}{\partial q_i} = {}^0 T_{i} Q_i \; {}^{i} T_n$

in modified notation,

with

$Q_i = \left [ \begin{array}{cccc} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array} \right ]$

for a revolute joint and

$Q_i = \left [ \begin{array}{cccc} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{array} \right ]$

for a prismatic joint.

$dRot$ and $dp$ are modified on output.

Implements Robot_basic.

Definition at line 249 of file kinemat.cpp.

ReturnMatrix Robot::dTdqi ( const int i )

virtual

Partial derivative of the robot position (homogeneous transf.)

See Robot::dTdqi(Matrix & dRot, ColumnVector & dp, const int i) for equations.

Implements Robot_basic.

Definition at line 334 of file kinemat.cpp.

ReturnMatrix Robot::G ( )

virtual

Joint torque due to gravity based on Recursive Newton-Euler formulation.

Implements Robot_basic.

Definition at line 321 of file dynamics.cpp.

ReturnMatrix Robot::inv_kin	(	const Matrix &	Tobj,
		const int	mj = `0`
	)

virtual

Overload inv_kin function.

Reimplemented from Robot_basic.

Definition at line 218 of file invkine.cpp.

ReturnMatrix Robot::inv_kin	(	const Matrix &	Tobj,
		const int	mj,
		const int	endlink,
		bool &	converge
	)

virtual

Inverse kinematics solutions.

The solution is based on the analytic inverse kinematics if robot type (familly) is Rhino or Puma, otherwise used the numerical algoritm defined in Robot_basic class.

Reimplemented from Robot_basic.

Definition at line 226 of file invkine.cpp.

ReturnMatrix Robot::inv_kin_puma	(	const Matrix &	Tobj,
		bool &	converge
	)

virtual

Analytic Puma inverse kinematics.

converge will be false if the desired end effector pose is outside robot range.

Implements Robot_basic.

Definition at line 348 of file invkine.cpp.

ReturnMatrix Robot::inv_kin_rhino	(	const Matrix &	Tobj,
		bool &	converge
	)

virtual

Analytic Rhino inverse kinematics.

converge will be false if the desired end effector pose is outside robot range.

Implements Robot_basic.

Definition at line 251 of file invkine.cpp.

ReturnMatrix Robot::inv_kin_schilling	(	const Matrix &	Tobj,
		bool &	converge
	)

virtual

Analytic Schilling inverse kinematics.

converge will be false if the desired end effector pose is outside robot range.

Implements Robot_basic.

Definition at line 508 of file invkine.cpp.

virtual ReturnMatrix Robot::jacobian ( const int ref = 0 ) const

inlinevirtual

Jacobian of mobile links expressed at frame ref.

Reimplemented from Robot_basic.

Definition at line 357 of file robot.h.

ReturnMatrix Robot::jacobian	(	const int	endlink,
		const int	ref
	)		const

virtual

Jacobian of mobile links up to endlink expressed at frame ref.

The Jacobian expressed in based frame is

$^{0}J(q) = \left[ \begin{array}{cccc} ^{0}J_1(q) & ^{0}J_2(q) & \cdots & ^{0}J_n(q) \\ \end{array} \right]$

where $^{0}J_i(q)$ is defined by

$^{0}J_i(q) = \begin{array}{cc} \left[ \begin{array}{c} z_i \times ^{i}p_n \\ z_i \\ \end{array} \right] & \textrm{rotoid joint} \end{array}$

$^{0}J_i(q) = \begin{array}{cc} \left[ \begin{array}{c} z_i \\ 0 \\ \end{array} \right] & \textrm{prismatic joint} \\ \end{array}$

Expressed in a different frame the Jacobian is obtained by

$^{i}J(q) = \left[ \begin{array}{cc} ^{0}_iR^T & 0 \\ 0 & ^{0}_iR^T \end{array} \right] {^{0}}J(q)$

Implements Robot_basic.

Definition at line 352 of file kinemat.cpp.

ReturnMatrix Robot::jacobian_dot ( const int ref = 0 ) const

virtual

Jacobian derivative of mobile joints expressed at frame ref.

The Jacobian derivative expressed in based frame is

$^{0}\dot{J}(q,\dot{q}) = \left[ \begin{array}{cccc} ^{0}\dot{J}_1(q,\dot{q}) & ^{0}\dot{J}_2(q,\dot{q}) & \cdots & ^{0}\dot{J}_n(q,\dot{q}) \\ \end{array} \right]$

where $^{0}\dot{J}_i(q,\dot{q})$ is defined by

$^{0}\dot{J}_i(q,\dot{q}) = \begin{array}{cc} \left[ \begin{array}{c} \omega_{i-1} \times z_i \\ \omega_{i-1} \times ^{i-1}p_n + z_i \times ^{i-1}\dot{p}_n \end{array} \right] & \textrm{rotoid joint} \end{array}$

$^{0}\dot{J}_i(q,\dot{q}) = \begin{array}{cc} \left[ \begin{array}{c} 0 \\ 0 \\ \end{array} \right] & \textrm{prismatic joint} \\ \end{array}$

Expressed in a different frame the Jacobian derivative is obtained by

$^{i}J(q) = \left[ \begin{array}{cc} ^{0}_iR^T & 0 \\ 0 & ^{0}_iR^T \end{array} \right] {^{0}}J(q)$

Implements Robot_basic.

Definition at line 449 of file kinemat.cpp.

void Robot::kine_pd	(	Matrix &	Rot,
		ColumnVector &	pos,
		ColumnVector &	pos_dot,
		const int	j
	)		const

virtual

Direct kinematics with velocity.

Parameters

Rot	Frame j rotation matrix w.r.t to the base frame.
pos	Frame j position vector wr.r.t to the base frame.
pos_dot	Frame j velocity vector w.r.t to the base frame.
j	Frame j. Print an error on the console if j is out of range.

Implements Robot_basic.

Definition at line 219 of file kinemat.cpp.

void Robot::robotType_inv_kin ( )

virtual

Identify inverse kinematics familly.

Identify the inverse kinematics analytic solution based on the similarity of the robot DH parameters and the DH parameters of know robots (ex: Puma, Rhino, ...). The inverse kinematics will be based on a numerical alogorithm if there is no match .

Implements Robot_basic.

Definition at line 1284 of file robot.cpp.

ReturnMatrix Robot::torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp
	)

virtual

Joint torque, without contact force, based on Recursive Newton-Euler formulation.

Implements Robot_basic.

Definition at line 135 of file dynamics.cpp.

ReturnMatrix Robot::torque	(	const ColumnVector &	q,
		const ColumnVector &	qp,
		const ColumnVector &	qpp,
		const ColumnVector &	Fext,
		const ColumnVector &	Next
	)

virtual

Joint torque based on Recursive Newton-Euler formulation.

In order to apply the RNE as presented in Murray 86, let us define the following variables (referenced in the $i^{th}$ coordinate frame if applicable):

$\sigma_i$ is the joint type; $\sigma_i = 1$ for a revolute joint and $\sigma_i = 0$ for a prismatic joint.

$z_0 = \left [ \begin{array}{ccc} 0 & 0 & 1 \end{array} \right ]^T$

$p_i = \left [ \begin{array}{ccc} a_i & d_i \sin \alpha_i & d_i \cos \alpha_i \end{array} \right ]^T$ is the position of the $i^{th}$ with respect to the $i-1^{th}$ frame.

Forward Iterations for $i=1, 2, \ldots, n$ . Initialize: $\omega_0 = \dot{\omega}_0 = 0$ and $\dot{v}_0 = - g$ .

$\omega_i = R_i^T [\omega_{i-1} + \sigma_i z_0 \dot{\theta}_i ]$

$\dot{\omega}_i = R_i^T \{ \dot{\omega}_{i-1} + \sigma_i [z_0 \ddot{\theta}_i + \omega_{i-1} \times (z_0 \dot{\theta}_i )] \}$

$\dot{v}_i = R_i^T \{ \dot{v}_{i-1} + (1 -\sigma_i) [z_0 \ddot{d}_i + 2 \omega_{i-1} \times (z_0 \dot{d}_i )] \} + \dot{\omega}_i \times p_i + \omega_i \times ( \omega_i \times p_i)$