ROBOOP, A Robotics Object Oriented Package in C++
mRobot Class Reference

Modified DH notation robot class. More...

#include <robot.h>

Inheritance diagram for mRobot:
Robot_basic

Public Member Functions

 mRobot (const int ndof=1)
 Constructor.
 
 mRobot (const Matrix &initrobot_motor)
 Constructor.
 
 mRobot (const Matrix &initrobot, const Matrix &initmotor)
 Constructor.
 
 mRobot (const mRobot &x)
 Copy constructor.
 
 mRobot (const std::string &filename, const std::string &robotName)
 
virtual ~mRobot ()
 Destructor.
 
virtual void robotType_inv_kin ()
 Identify inverse kinematics familly. More...
 
ReturnMatrix inv_kin (const Matrix &Tobj, const int mj=0)
 Overload inv_kin function.
 
virtual ReturnMatrix inv_kin (const Matrix &Tobj, const int mj, const int endlink, bool &converge)
 Inverse kinematics solutions. More...
 
virtual ReturnMatrix inv_kin_rhino (const Matrix &Tobj, bool &converge)
 Analytic Rhino inverse kinematics. More...
 
virtual ReturnMatrix inv_kin_puma (const Matrix &Tobj, bool &converge)
 Analytic Puma inverse kinematics. More...
 
virtual ReturnMatrix inv_kin_schilling (const Matrix &Tobj, bool &converge)
 Analytic Schilling inverse kinematics. More...
 
virtual void kine_pd (Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const
 Direct kinematics with velocity. More...
 
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
 Jacobian of mobile joints 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 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 torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
 Joint torque, without contact force, based on Recursive Newton-Euler formulation.
 
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.
 
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)
 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 ReturnMatrix G ()
 Joint torque due to gravity based on Recursive Newton-Euler formulation.
 
virtual ReturnMatrix C (const ColumnVector &qp)
 Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
 
- Public Member Functions inherited from Robot_basic
 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.
 
virtual ~Robot_basic ()
 Destructor. More...
 
Robot_basicoperator= (const Robot_basic &x)
 Overload = operator.
 
Real get_q (const int i) const
 
bool get_DH () const
 Return true if in DH notation, false otherwise.
 
int get_dof () const
 Return dof.
 
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.
 
int get_fix () const
 Return fix.
 
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 get_available_q (void) const
 Return the joint position vector of available (non-immobile) joints.
 
ReturnMatrix get_available_qp (void) const
 Return the joint velocity vector of available (non-immobile) joints.
 
ReturnMatrix get_available_qpp (void) const
 Return the joint acceleration 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 (const int endlink) const
 Return the joint velocity vector of available (non-immobile) joints up to and including endlink.
 
ReturnMatrix get_available_qpp (const int endlink) const
 Return the joint acceleration vector of available (non-immobile) joints up to and including endlink.
 
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.
 
void set_qpp (const ColumnVector &qpp)
 Set the joint acceleration vector.
 
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.
 
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. 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...
 
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. More...
 
ReturnMatrix inertia (const ColumnVector &q)
 Inertia of the manipulator.
 
ReturnMatrix dtau_dq (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
 Sensitivity of the dynamics with respect to $ q $. 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.
 

Additional Inherited Members

- Public Attributes inherited from Robot_basic
ColumnVector * w
 
ColumnVector * wp
 
ColumnVector * vp
 
ColumnVector * a
 
ColumnVector * f
 
ColumnVector * f_nv
 
ColumnVector * n
 
ColumnVector * n_nv
 
ColumnVector * F
 
ColumnVector * N
 
ColumnVector * p
 
ColumnVector * pp
 
ColumnVector * dw
 
ColumnVector * dwp
 
ColumnVector * dvp
 
ColumnVector * da
 
ColumnVector * df
 
ColumnVector * dn
 
ColumnVector * dF
 
ColumnVector * dN
 
ColumnVector * dp
 
ColumnVector z0
 Axis vector at each joint.
 
ColumnVector gravity
 Gravity vector.
 
Matrix * R
 Temprary rotation matrix.
 
Linklinks
 Pointer on Link cclass.
 

Detailed Description

Modified DH notation robot class.

Definition at line 385 of file robot.h.

Member Function Documentation

void mRobot::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 - QR_i^T \omega_i \delta \theta_i) \]

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

\[ \delta \dot{v}_i = R_i^T\Big(\delta \dot{\omega}_{i-1} \times p_i + \delta \omega_{i-1} \times(\omega_{i-1} \times p_i) + \omega_{i-1} \times( \delta \omega_{i-1} \times p_i) + \delta \dot{v}_i\Big) + (1-\sigma_i)\Big(2\delta \omega_i \times z_0\dot{d}_i + 2\omega_i \times z_0 \delta \dot{d}_i + z_0 \delta \ddot{d}_i\Big) - \sigma_i QR_i^T \Big(\dot{\omega}_{i-1} \times p_i + \omega_{i-1} \times(w_{i-1}\times p_i) + \dot{v}_i\Big) \delta \theta_i + (1-\sigma_i) R_i^T \Big(\dot{\omega}_{i-1} \times p_{di} + \omega_{i-1} \times(\omega_{i-1}\times p_{di})\Big) \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 \dot{v}_i + \delta \dot{\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} R_{i+1} Qf_{i+1} \delta \theta_{i+1} \]

\[ \delta n_i = \delta N_i + R_{i+1} \delta n_{i+1} + r_i\times \delta F_i + p_{i+1} \times R_{i+1} \delta f_{i+1} + \sigma_{i+1} \Big( R_{i+1} Qn_{i+1} + p_{i+1} \times R_{i+1} Qf_{i+1} \Big) \delta \theta_{i+1} + (1-\sigma_{i+1} ) p_{di+1} p_{di+1} \times R_{i+1} f_{i+1} \delta d_{i+1} \]

\[ \delta \tau_i = \sigma \delta n_i^T z_0 + (1 - \sigma_i) \delta f_i^T z_0 \]

Implements Robot_basic.

Definition at line 287 of file delta_t.cpp.

void mRobot::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 mRobot::delta_torque for equations.

Implements Robot_basic.

Definition at line 201 of file comp_dq.cpp.

void mRobot::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 mRobot::delta_torque for equations.

Implements Robot_basic.

Definition at line 184 of file comp_dqp.cpp.

void mRobot::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} Q_i \; {}^{i} T_n \]

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 578 of file kinemat.cpp.

References threebythreeident.

ReturnMatrix mRobot::dTdqi ( const int  i)
virtual

Partial derivative of the robot position (homogeneous transf.)

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

Implements Robot_basic.

Definition at line 660 of file kinemat.cpp.

ReturnMatrix mRobot::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 601 of file invkine.cpp.

References Robot_basic::inv_kin().

ReturnMatrix mRobot::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 731 of file invkine.cpp.

ReturnMatrix mRobot::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 626 of file invkine.cpp.

ReturnMatrix mRobot::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 891 of file invkine.cpp.

ReturnMatrix mRobot::jacobian ( const int  endlink,
const int  ref 
) const
virtual

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

See Robot::jacobian for equations.

Implements Robot_basic.

Definition at line 678 of file kinemat.cpp.

ReturnMatrix mRobot::jacobian_dot ( const int  ref = 0) const
virtual

Jacobian derivative of mobile joints expressed at frame ref.

See Robot::jacobian_dot for equations.

Implements Robot_basic.

Definition at line 732 of file kinemat.cpp.

void mRobot::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 548 of file kinemat.cpp.

void mRobot::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 1350 of file robot.cpp.

References Robot_basic::DEFAULT, Robot_basic::PUMA, Puma_mDH(), Robot_basic::RHINO, Rhino_mDH(), Robot_basic::robotType, Robot_basic::SCHILLING, and Schilling_mDH().

Referenced by mRobot().

ReturnMatrix mRobot::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, 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-1} & -d_i sin \alpha_{i-1} & d_i cos \alpha_{i-1} \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 R_i^T\omega_{i-1}\times z_0 \dot{\theta}_i + \sigma_iz_0\ddot{\theta}_i \]

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

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

\[ \dot{v}_{ci} = \dot{\omega}_i\times r_i + \omega_i\times(\omega_i\times r_i) + \dot{v}_i \]

\[ F_i = m_i \dot{v}_{ci} \]

\[ N_i = I_{ci}\ddot{\omega}_i\ + \omega_i \times I_{ci}\omega_i \]

\[ f_i = R_{i+1}f_{i+1} + F_i \]

\[ n_i = N_i + R_{i+1} n_{i+1} + r_i \times F_i + p_{i+1}\times R_{i+1}f_{i+1} \]

\[ \tau_i = \sigma_i n_i^T z_0 + (1 - \sigma_i) f_i^T z_0 \]

Implements Robot_basic.

Definition at line 418 of file dynamics.cpp.

References sign().