Modified DH notation robot class. More...
#include <robot.h>
Public Member Functions | |
virtual ReturnMatrix | C (const ColumnVector &qp) |
Joint torque due to centrifugal and Corriolis 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. | |
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. | |
virtual void | dqp_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque) |
Delta torque due to delta joint velocity. | |
virtual void | dTdqi (Matrix &dRot, ColumnVector &dp, const int i) |
Partial derivative of the robot position (homogeneous transf.) | |
virtual ReturnMatrix | dTdqi (const int i) |
Partial derivative of the robot position (homogeneous transf.) | |
virtual ReturnMatrix | G () |
Joint torque due to gravity based on Recursive Newton-Euler formulation. | |
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. | |
virtual ReturnMatrix | inv_kin_puma (const Matrix &Tobj, bool &converge) |
Analytic Puma inverse kinematics. | |
virtual ReturnMatrix | inv_kin_rhino (const Matrix &Tobj, bool &converge) |
Analytic Rhino inverse kinematics. | |
virtual ReturnMatrix | inv_kin_schilling (const Matrix &Tobj, bool &converge) |
Analytic Schilling inverse kinematics. | |
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. | |
virtual ReturnMatrix | jacobian_dot (const int ref=0) const |
Jacobian derivative of mobile joints expressed at frame ref. | |
virtual void | kine_pd (Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const |
Direct kinematics with velocity. | |
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 void | robotType_inv_kin () |
Identify inverse kinematics familly. | |
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. | |
virtual ReturnMatrix | torque_novelocity (const ColumnVector &qpp) |
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation. | |
virtual | ~mRobot () |
Destructor. |
mRobot::mRobot | ( | const int | ndof = 1 | ) |
mRobot::mRobot | ( | const Matrix & | initrobot_motor | ) |
mRobot::mRobot | ( | const Matrix & | initrobot, |
const Matrix & | initmotor | ||
) |
mRobot::mRobot | ( | const mRobot & | x | ) |
mRobot::mRobot | ( | const std::string & | filename, |
const std::string & | robotName | ||
) |
virtual mRobot::~mRobot | ( | ) | [inline, virtual] |
ReturnMatrix mRobot::C | ( | const ColumnVector & | qp | ) | [virtual] |
Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
Implements Robot_basic.
Definition at line 636 of file dynamics.cpp.
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
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
Forward Iterations for . Initialize: .
Backward Iterations for . Initialize: .
Implements Robot_basic.
Definition at line 291 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 . See mRobot::delta_torque for equations.
Implements Robot_basic.
Definition at line 205 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 . See mRobot::delta_torque for equations.
Implements Robot_basic.
Definition at line 188 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:
with
for a revolute joint and
for a prismatic joint.
and are modified on output.
Implements Robot_basic.
Definition at line 582 of file kinemat.cpp.
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 664 of file kinemat.cpp.
ReturnMatrix mRobot::G | ( | ) | [virtual] |
Joint torque due to gravity based on Recursive Newton-Euler formulation.
Implements Robot_basic.
Definition at line 599 of file dynamics.cpp.
ReturnMatrix mRobot::inv_kin | ( | const Matrix & | Tobj, |
const int | mj = 0 |
||
) | [virtual] |
Overload inv_kin function.
Reimplemented from Robot_basic.
Definition at line 617 of file invkine.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 625 of file invkine.cpp.
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 755 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 650 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 915 of file invkine.cpp.
virtual ReturnMatrix mRobot::jacobian | ( | const int | ref = 0 | ) | const [inline, virtual] |
Jacobian of mobile links expressed at frame ref.
Reimplemented from Robot_basic.
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 682 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 736 of file kinemat.cpp.
void mRobot::kine_pd | ( | Matrix & | Rot, |
ColumnVector & | pos, | ||
ColumnVector & | pos_dot, | ||
const int | j | ||
) | const [virtual] |
Direct kinematics with velocity.
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 552 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.
ReturnMatrix mRobot::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 412 of file dynamics.cpp.
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 coordinate frame if applicable):
is the joint type; for a revolute joint and for a prismatic joint.
is the position of the $i^{th}$ with respect to the $i-1^{th}$ frame.
Forward Iterations for . Initialize: .
Backward Iterations for . Initialize: .
Implements Robot_basic.
Definition at line 422 of file dynamics.cpp.
ReturnMatrix mRobot::torque_novelocity | ( | const ColumnVector & | qpp | ) | [virtual] |
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
Implements Robot_basic.
Definition at line 555 of file dynamics.cpp.