Modified DH notation and minimal inertial parameters 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. More... | |
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 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 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 | kine_pd (Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref=0) const |
Direct kinematics with velocity. More... | |
mRobot_min_para (const int ndof=1) | |
Constructor. More... | |
mRobot_min_para (const Matrix &dhinit) | |
Constructor. More... | |
mRobot_min_para (const Matrix &initrobot, const Matrix &initmotor) | |
Constructor. More... | |
mRobot_min_para (const mRobot_min_para &x) | |
Copy constructor. More... | |
mRobot_min_para (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 | ~mRobot_min_para () |
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 . 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... | |
Modified DH notation and minimal inertial parameters robot class.
mRobot_min_para::mRobot_min_para | ( | const int | ndof = 1 | ) |
mRobot_min_para::mRobot_min_para | ( | const Matrix & | dhinit | ) |
mRobot_min_para::mRobot_min_para | ( | const mRobot_min_para & | x | ) |
mRobot_min_para::mRobot_min_para | ( | const std::string & | filename, |
const std::string & | robotName | ||
) |
|
inlinevirtual |
|
virtual |
Joint torque due to centrifugal and Corriolis based on Recursive Newton-Euler formulation.
Implements Robot_basic.
Definition at line 873 of file dynamics.cpp.
|
virtual |
Delta torque dynamics.
See mRobot::delta_torque for equations.
Implements Robot_basic.
Definition at line 511 of file delta_t.cpp.
|
virtual |
Delta torque due to delta joint position.
This function computes . See mRobot::delta_torque for equations.
Implements Robot_basic.
Definition at line 335 of file comp_dq.cpp.
|
virtual |
Delta torque due to delta joint velocity.
This function computes . See mRobot::delta_torque for equations.
Implements Robot_basic.
Definition at line 303 of file comp_dqp.cpp.
|
virtual |
Partial derivative of the robot position (homogeneous transf.)
This function computes the partial derivatives:
See mRobot::dTdqi(Matrix & dRot, ColumnVector & dp, const int i) for equations.
Implements Robot_basic.
Definition at line 829 of file kinemat.cpp.
|
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 887 of file kinemat.cpp.
|
virtual |
Joint torque due to gravity based on Recursive Newton-Euler formulation.
Implements Robot_basic.
Definition at line 832 of file dynamics.cpp.
|
virtual |
Overload inv_kin function.
Reimplemented from Robot_basic.
Definition at line 1023 of file invkine.cpp.
|
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 1031 of file invkine.cpp.
|
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 1150 of file invkine.cpp.
|
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 1053 of file invkine.cpp.
|
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 1311 of file invkine.cpp.
|
inlinevirtual |
Jacobian of mobile links expressed at frame ref.
Reimplemented from Robot_basic.
|
virtual |
Jacobian of mobile joints up to endlink expressed at frame ref.
See Robot::jacobian for equations.
Implements Robot_basic.
Definition at line 905 of file kinemat.cpp.
|
virtual |
Jacobian derivative of mobile joints expressed at frame ref.
See Robot::jacobian_dot for equations.
Implements Robot_basic.
Definition at line 961 of file kinemat.cpp.
|
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 799 of file kinemat.cpp.
|
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.
|
virtual |
Joint torque without contact force based on Recursive Newton-Euler formulation.
Implements Robot_basic.
Definition at line 687 of file dynamics.cpp.
|
virtual |
Joint torque based on Recursive Newton-Euler formulation.
See ReturnMatrix mRobot::torque(const ColumnVector & q, const ColumnVector & qp, const ColumnVector & qpp, const ColumnVector & Fext, const ColumnVector & Next) for the Recursive Newton-Euler formulation.
Implements Robot_basic.
Definition at line 697 of file dynamics.cpp.
|
virtual |
Joint torque. when joint velocity is 0, based on Recursive Newton-Euler formulation.
Implements Robot_basic.
Definition at line 782 of file dynamics.cpp.