Homepage Demos Overview Downloads Tutorials Reference
Credits

ROBOOP::Robot Class Reference

#include <robot.h>

Inheritance diagram for ROBOOP::Robot:

Inheritance graph
[legend]
List of all members.

Detailed Description

DH notation robot class.

Definition at line 385 of file robot.h.

Public Member Functions

 Robot (const int ndof=1)
 Constructor.
 Robot (const Matrix &initrobot)
 Constructor.
 Robot (const Matrix &initrobot, const Matrix &initmotor)
 Constructor.
 Robot (const Robot &x)
 Copy constructor.
 Robot (const string &filename, const string &robotName)
 Constructor.
 Robot (const Config &robData, const string &robotName)
 ~Robot ()
 Destructor.
Robotoperator= (const Robot &x)
 Overload = operator.
virtual void robotType_inv_kin ()
 Identify inverse kinematics familly.
virtual void kine_pd (Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const
 Direct kinematics with velocity.
virtual ReturnMatrix inv_kin (const Matrix &Tobj, const int mj=0)
 Overload inv_kin function.
virtual ReturnMatrix inv_kin (const Matrix &Tobj, const int mj, bool &converge)
 Numerical inverse kinematics. See inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge).
virtual ReturnMatrix inv_kin (const Matrix &Tobj, const int mj, const int endlink, bool &converge)
 Inverse kinematics solutions.
virtual ReturnMatrix inv_kin_pos (const ColumnVector &Pobj, const int mj, const int endlink, const ColumnVector &Plink, bool &converge)
 Inverse kinematics solutions.
virtual ReturnMatrix inv_kin_orientation (const Matrix &Robj, const int mj, const int endlink, bool &converge)
 Inverse kinematics solutions.
virtual ReturnMatrix inv_kin_rhino (const Matrix &Tobj, bool &converge)
 Analytic Rhino inverse kinematics.
virtual ReturnMatrix inv_kin_puma (const Matrix &Tobj, bool &converge)
 Analytic Puma inverse kinematics.
virtual ReturnMatrix inv_kin_ers_pos (const ColumnVector &Pobj, const int endlink, const ColumnVector &Plink, bool &converge)
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 links up to endlink expressed at frame ref.
virtual ReturnMatrix jacobian_dot (const int ref=0) const
virtual void dTdqi (Matrix &dRot, ColumnVector &dpos, const int i)
 Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink).
virtual ReturnMatrix dTdqi (const int i)
 Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink).
virtual void dTdqi (Matrix &dRot, ColumnVector &dpos, const int i, const int endlink)
 Partial derivative of the robot position (homogeneous transf.).
virtual ReturnMatrix dTdqi (const int i, const int endlink)
 Partial derivative of the robot position (homogeneous transf.).
virtual ReturnMatrix torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
virtual ReturnMatrix torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &Fext_, const ColumnVector &Next_)
virtual ReturnMatrix torque_novelocity (const ColumnVector &qpp)
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)
virtual void dq_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, ColumnVector &torque, ColumnVector &dtorque)
virtual void dqp_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque)
virtual ReturnMatrix G ()
virtual ReturnMatrix C (const ColumnVector &qp)

Protected Member Functions

Real computeFirstERSLink (int curlink, const ColumnVector &Pobj, const int endlink, const ColumnVector &Plink, Real min, Real max, bool &converge)
Real computeSecondERSLink (int curlink, const ColumnVector &Pobj, const int endlink, const ColumnVector &Plink, bool invert, Real min, Real max, bool &converge)
Real computeThirdERSLink (int curlink, const ColumnVector &Pobj, const int endlink, const ColumnVector &Plink, bool invert, Real min, Real max, bool &converge)


Constructor & Destructor Documentation

ROBOOP::Robot::Robot const int  ndof = 1  )  [explicit]
 

Constructor.

Definition at line 1346 of file robot.cpp.

ROBOOP::Robot::Robot const Matrix initrobot  )  [explicit]
 

Constructor.

Definition at line 1355 of file robot.cpp.

ROBOOP::Robot::Robot const Matrix initrobot,
const Matrix initmotor
[explicit]
 

Constructor.

Definition at line 1364 of file robot.cpp.

ROBOOP::Robot::Robot const Robot x  ) 
 

Copy constructor.

Definition at line 1394 of file robot.cpp.

ROBOOP::Robot::Robot const string &  filename,
const string &  robotName
[explicit]
 

Constructor.

Definition at line 1374 of file robot.cpp.

ROBOOP::Robot::Robot const Config robData,
const string &  robotName
[explicit]
 

Definition at line 1384 of file robot.cpp.

ROBOOP::Robot::~Robot  )  [inline]
 

Destructor.

Definition at line 394 of file robot.h.


Member Function Documentation

virtual ReturnMatrix ROBOOP::Robot::C const ColumnVector qp  )  [virtual]
 

Implements ROBOOP::Robot_basic.

Real ROBOOP::Robot::computeFirstERSLink int  curlink,
const ColumnVector Pobj,
const int  endlink,
const ColumnVector Plink,
Real  min,
Real  max,
bool &  converge
[protected]
 

Definition at line 947 of file invkine.cpp.

Referenced by computeSecondERSLink(), and inv_kin_ers_pos().

Real ROBOOP::Robot::computeSecondERSLink int  curlink,
const ColumnVector Pobj,
const int  endlink,
const ColumnVector Plink,
bool  invert,
Real  min,
Real  max,
bool &  converge
[protected]
 

Definition at line 964 of file invkine.cpp.

Referenced by inv_kin_ers_pos().

Real ROBOOP::Robot::computeThirdERSLink int  curlink,
const ColumnVector Pobj,
const int  endlink,
const ColumnVector Plink,
bool  invert,
Real  min,
Real  max,
bool &  converge
[protected]
 

Definition at line 1006 of file invkine.cpp.

Referenced by inv_kin_ers_pos().

virtual void ROBOOP::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]
 

Implements ROBOOP::Robot_basic.

virtual void ROBOOP::Robot::dq_torque const ColumnVector q,
const ColumnVector qp,
const ColumnVector qpp,
const ColumnVector dq,
ColumnVector torque,
ColumnVector dtorque
[virtual]
 

Implements ROBOOP::Robot_basic.

virtual void ROBOOP::Robot::dqp_torque const ColumnVector q,
const ColumnVector qp,
const ColumnVector dqp,
ColumnVector torque,
ColumnVector dtorque
[virtual]
 

Implements ROBOOP::Robot_basic.

ReturnMatrix ROBOOP::Robot::dTdqi const int  i,
const int  endlink
[virtual]
 

Partial derivative of the robot position (homogeneous transf.).

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

Implements ROBOOP::Robot_basic.

Definition at line 539 of file kinemat.cpp.

void ROBOOP::Robot::dTdqi Matrix dRot,
ColumnVector dpos,
const int  i,
const int  endlink
[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 ROBOOP::Robot_basic.

Definition at line 454 of file kinemat.cpp.

virtual ReturnMatrix ROBOOP::Robot::dTdqi const int  i  )  [inline, virtual]
 

Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink).

Reimplemented from ROBOOP::Robot_basic.

Definition at line 413 of file robot.h.

virtual void ROBOOP::Robot::dTdqi Matrix dRot,
ColumnVector dpos,
const int  i
[inline, virtual]
 

Partial derivative of the robot position (homogeneous transf.) See dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink).

Reimplemented from ROBOOP::Robot_basic.

Definition at line 412 of file robot.h.

Referenced by dTdqi().

virtual ReturnMatrix ROBOOP::Robot::G  )  [virtual]
 

Implements ROBOOP::Robot_basic.

ReturnMatrix ROBOOP::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 ROBOOP::Robot_basic.

Definition at line 583 of file invkine.cpp.

virtual ReturnMatrix ROBOOP::Robot::inv_kin const Matrix Tobj,
const int  mj,
bool &  converge
[inline, virtual]
 

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

Reimplemented from ROBOOP::Robot_basic.

Definition at line 400 of file robot.h.

ReturnMatrix ROBOOP::Robot::inv_kin const Matrix Tobj,
const int  mj = 0
[virtual]
 

Overload inv_kin function.

Reimplemented from ROBOOP::Robot_basic.

Definition at line 575 of file invkine.cpp.

Referenced by inv_kin().

ReturnMatrix ROBOOP::Robot::inv_kin_ers_pos const ColumnVector Pobj,
const int  endlink,
const ColumnVector Plink,
bool &  converge
[virtual]
 

Definition at line 912 of file invkine.cpp.

Referenced by inv_kin_pos().

ReturnMatrix ROBOOP::Robot::inv_kin_orientation const Matrix Robj,
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 ROBOOP::Robot_basic.

Definition at line 628 of file invkine.cpp.

ReturnMatrix ROBOOP::Robot::inv_kin_pos const ColumnVector Pobj,
const int  mj,
const int  endlink,
const ColumnVector Plink,
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 ROBOOP::Robot_basic.

Definition at line 606 of file invkine.cpp.

ReturnMatrix ROBOOP::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 ROBOOP::Robot_basic.

Definition at line 750 of file invkine.cpp.

Referenced by inv_kin().

ReturnMatrix ROBOOP::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 ROBOOP::Robot_basic.

Definition at line 651 of file invkine.cpp.

Referenced by inv_kin().

ReturnMatrix ROBOOP::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 ROBOOP::Robot_basic.

Definition at line 557 of file kinemat.cpp.

virtual ReturnMatrix ROBOOP::Robot::jacobian const int  ref = 0  )  const [inline, virtual]
 

Jacobian of mobile links expressed at frame ref.

Reimplemented from ROBOOP::Robot_basic.

Definition at line 409 of file robot.h.

virtual ReturnMatrix ROBOOP::Robot::jacobian_dot const int  ref = 0  )  const [virtual]
 

Implements ROBOOP::Robot_basic.

void ROBOOP::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 ROBOOP::Robot_basic.

Definition at line 424 of file kinemat.cpp.

Robot & ROBOOP::Robot::operator= const Robot x  ) 
 

Overload = operator.

Definition at line 1398 of file robot.cpp.

void ROBOOP::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 ROBOOP::Robot_basic.

Definition at line 1405 of file robot.cpp.

Referenced by Robot().

virtual ReturnMatrix ROBOOP::Robot::torque const ColumnVector q,
const ColumnVector qp,
const ColumnVector qpp,
const ColumnVector Fext_,
const ColumnVector Next_
[virtual]
 

Implements ROBOOP::Robot_basic.

virtual ReturnMatrix ROBOOP::Robot::torque const ColumnVector q,
const ColumnVector qp,
const ColumnVector qpp
[virtual]
 

Implements ROBOOP::Robot_basic.

virtual ReturnMatrix ROBOOP::Robot::torque_novelocity const ColumnVector qpp  )  [virtual]
 

Implements ROBOOP::Robot_basic.


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

ROBOOP v1.21a
Generated Tue Aug 16 16:32:19 2005 by Doxygen 1.4.4