ROBOOP::Robot_basic Class Reference#include <robot.h>
Inheritance diagram for ROBOOP::Robot_basic:
[legend]List of all members.
Detailed Description
Virtual base robot class.
Definition at line 217 of file robot.h.
|
Public Member Functions |
| Robot_basic (const int ndof=1, const bool dh_parameter=false, const bool min_inertial_para=false) |
| Constructor.
|
| Robot_basic (const Matrix &initrobot_motor, const bool dh_parameter=false, const bool min_inertial_para=false) |
| Constructor.
|
| Robot_basic (const Matrix &initrobot, const Matrix &initmotor, const bool dh_parameter=false, const bool min_inertial_para=false) |
| Constructor.
|
| Robot_basic (const Config &robData, const string &robotName, const bool dh_parameter=false, const bool min_inertial_para=false) |
| Constructor.
|
| Robot_basic (const Robot_basic &x) |
| Copy constructor.
|
virtual | ~Robot_basic () |
| Destructor.
|
Robot_basic & | operator= (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.
|
void | set_q (const Matrix &q) |
| Set the joint position vector.
|
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.
|
void | kine (Matrix &Rot, ColumnVector &pos, const int j) const |
| Direct kinematics at end effector.
|
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.
|
virtual void | kine_pd (Matrix &Rot, ColumnVector &pos, ColumnVector &pos_dot, const int ref) const =0 |
virtual void | robotType_inv_kin ()=0 |
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) |
| Numerical inverse kinematics.
|
virtual ReturnMatrix | inv_kin_pos (const ColumnVector &Pobj, const int mj, const int endlink, const ColumnVector &Plink, bool &converge) |
| Numerical inverse kinematics.
|
virtual ReturnMatrix | inv_kin_orientation (const Matrix &Robj, const int mj, const int endlink, bool &converge) |
| Numerical inverse kinematics.
|
virtual ReturnMatrix | inv_kin_rhino (const Matrix &Tobj, bool &converge)=0 |
virtual ReturnMatrix | inv_kin_puma (const Matrix &Tobj, bool &converge)=0 |
virtual ReturnMatrix | jacobian (const int ref=0) const |
| Jacobian of mobile links expressed at frame ref. See jacobian(const int endlink, const int reg).
|
virtual ReturnMatrix | jacobian (const int endlink, const int ref) const =0 |
virtual ReturnMatrix | jacobian_dot (const int ref=0) const =0 |
ReturnMatrix | jacobian_DLS_inv (const double eps, const double lambda_max, const int ref=0) const |
| Inverse Jacobian based on damped least squares inverse.
|
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)=0 |
virtual ReturnMatrix | dTdqi (const int i, const int endlink)=0 |
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.
|
ReturnMatrix | inertia (const ColumnVector &q) |
| Inertia of the manipulator.
|
virtual ReturnMatrix | torque_novelocity (const ColumnVector &qpp)=0 |
virtual ReturnMatrix | torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)=0 |
virtual ReturnMatrix | torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &Fext_, const ColumnVector &Next_)=0 |
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)=0 |
virtual void | dq_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp, const ColumnVector &dq, ColumnVector &torque, ColumnVector &dtorque)=0 |
virtual void | dqp_torque (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &dqp, ColumnVector &torque, ColumnVector &dtorque)=0 |
ReturnMatrix | dtau_dq (const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp) |
| Sensitivity of the dynamics with respect to .
|
ReturnMatrix | dtau_dqp (const ColumnVector &q, const ColumnVector &qp) |
| Sensitivity of the dynamics with respect to .
|
virtual ReturnMatrix | G ()=0 |
virtual ReturnMatrix | C (const ColumnVector &qp)=0 |
void | error (const string &msg1) const |
| Print the message msg1 on the console.
|
void | convertFrame (Matrix &R, ColumnVector &p, int cur, int dest) const |
| converts one or more column-wise vectors from frame cur to frame dest
|
ReturnMatrix | convertFrame (int cur, int dest) const |
| Returns a matrix which can be used to convert from frame cur to frame dest The Matrix returned will be homogenous (4x4).
|
void | convertLinkToFrame (Matrix &R, ColumnVector &p, int cur, int dest) const |
| converts vector(s) on link cur to be vector(s) in joint reference frame dest
|
ReturnMatrix | convertLinkToFrame (int cur, int dest) const |
| Returns a matrix which can be used to convert from link frame cur to joint frame dest.
|
void | convertFrameToLink (Matrix &R, ColumnVector &p, int cur, int dest) const |
| converts vector(s) in joint referenc frame cur to be vector(s) on link dest
|
ReturnMatrix | convertFrameToLink (int cur, int dest) const |
| Returns a matrix which can be used to convert from joint reference frame cur to link frame dest.
|
void | convertLink (Matrix &R, ColumnVector &p, int cur, int dest) const |
| converts vector(s) on link cur to be vector(s) on link dest
|
ReturnMatrix | convertLink (int cur, int dest) const |
| Returns a matrix which can be used to convert from link cur to link dest.
|
Static Public Member Functions |
static Real | limit_range (Real x, Real min, Real max) |
| useful for fixing x to be at least min or at most max
|
static Real | limit_angle (Real x, Real min, Real max) |
| useful for fixing x to be at least min or at most max; handles angles so closest boundary is picked, but assumes x is normalized (-pi,pi)
|
static Real | normalize_angle (Real t) |
| ensures that t is in the range (-pi,pi) (upper boundary may not be inclusive...?)
|
static Real | homog_norm (const ColumnVector &v) |
| converts a homogenous vector to a normal vector
|
Public Attributes |
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.
|
Link * | links |
| Pointer on Link cclass.
|
Private Types |
enum | EnumRobotType {
DEFAULT = 0,
RHINO = 1,
PUMA = 2,
ERS_LEG,
ERS2XX_HEAD,
ERS7_HEAD
} |
| enum EnumRobotType More...
|
Static Private Member Functions |
static ReturnMatrix | pack4x4 (const Matrix &R, const ColumnVector &p) |
Private Attributes |
EnumRobotType | robotType |
| Robot type.
|
int | dof |
| Degree of freedom.
|
int | fix |
| Virtual link, used with modified DH notation.
|
Friends |
class | Robot |
class | mRobot |
class | mRobot_min_para |
class | Robotgl |
class | mRobotgl |
Member Enumeration Documentation
|
enum EnumRobotType
- Enumeration values:
-
DEFAULT |
Default robot familly. |
RHINO |
Rhino familly. |
PUMA |
Puma familly. |
ERS_LEG |
AIBO leg. |
ERS2XX_HEAD |
AIBO 200 series camera chain (210, 220). |
ERS7_HEAD |
AIBO 7 model camera chain. |
Definition at line 360 of file robot.h. |
Constructor & Destructor Documentation
ROBOOP::Robot_basic::Robot_basic |
( |
const int |
ndof = 1 , |
|
|
const bool |
dh_parameter = false , |
|
|
const bool |
min_inertial_para = false |
|
) |
[explicit] |
|
|
Constructor.
- Parameters:
-
| ndof: | Robot degree of freedom. |
| dh_parameter: | true if DH notation, false if modified DH notation. |
| min_inertial_para: | true inertial parameter are in minimal form. |
Allocate memory for vectors and matrix pointers. Initialize all the Links instance.
Definition at line 681 of file robot.cpp. |
ROBOOP::Robot_basic::Robot_basic |
( |
const Matrix & |
dhinit, |
|
|
const bool |
dh_parameter = false , |
|
|
const bool |
min_inertial_para = false |
|
) |
[explicit] |
|
|
Constructor.
- Parameters:
-
| dhinit: | Robot initialization matrix. |
| dh_parameter: | true if DH notation, false if modified DH notation. |
| min_inertial_para: | true inertial parameter are in minimal form. |
Allocate memory for vectors and matrix pointers. Initialize all the Links instance.
Definition at line 383 of file robot.cpp. |
ROBOOP::Robot_basic::Robot_basic |
( |
const Matrix & |
initrobot, |
|
|
const Matrix & |
initmotor, |
|
|
const bool |
dh_parameter = false , |
|
|
const bool |
min_inertial_para = false |
|
) |
[explicit] |
|
|
Constructor.
- Parameters:
-
| initrobot: | Robot initialization matrix. |
| initmotor: | Motor initialization matrix. |
| dh_parameter: | true if DH notation, false if modified DH notation. |
| min_inertial_para: | true inertial parameter are in minimal form. |
Allocate memory for vectors and matrix pointers. Initialize all the Links instance.
Definition at line 545 of file robot.cpp. |
ROBOOP::Robot_basic::Robot_basic |
( |
const Config & |
robData, |
|
|
const string & |
robotName, |
|
|
const bool |
dh_parameter = false , |
|
|
const bool |
min_inertial_para = false |
|
) |
[explicit] |
|
|
Constructor.
- Parameters:
-
| robData: | Robot configuration file. |
| robotName: | Basic section name of the configuration file. |
| dh_parameter: | DH notation (True) or modified DH notation. |
| min_inertial_para: | Minimum inertial parameters (True). |
Initialize a new robot from a configuration file.
Definition at line 828 of file robot.cpp. |
ROBOOP::Robot_basic::Robot_basic |
( |
const Robot_basic & |
x |
) |
|
|
|
Destructor.
Free all vectors and matrix memory.
Definition at line 991 of file robot.cpp. |
Member Function Documentation
|
Joints acceleration.
The robot dynamics is
then the joint acceleration is
Definition at line 108 of file dynamics.cpp. |
ReturnMatrix ROBOOP::Robot_basic::convertFrame |
( |
int |
cur, |
|
|
int |
dest |
|
) |
const |
|
|
Returns a matrix which can be used to convert from frame cur to frame dest The Matrix returned will be homogenous (4x4).
Definition at line 265 of file kinemat.cpp. |
void ROBOOP::Robot_basic::convertFrame |
( |
Matrix & |
R, |
|
|
ColumnVector & |
p, |
|
|
int |
cur, |
|
|
int |
dest |
|
) |
const |
|
ReturnMatrix ROBOOP::Robot_basic::convertFrameToLink |
( |
int |
cur, |
|
|
int |
dest |
|
) |
const |
|
|
Returns a matrix which can be used to convert from joint reference frame cur to link frame dest.
- Parameters:
-
| cur | the source link |
| dest | the target link |
Note the difference between this and convertFrame is that a link moves within the frame at its joints origin - the frame is static no matter how the joint moves within it. If we have a point on the link, then the position of that point will move within the frame as the joint moves. This function will return vector(s) of A represented as if they were connected to link dest.
The Matrix returned will be homogenous (4x4).
Definition at line 358 of file kinemat.cpp. |
void ROBOOP::Robot_basic::convertFrameToLink |
( |
Matrix & |
R, |
|
|
ColumnVector & |
p, |
|
|
int |
cur, |
|
|
int |
dest |
|
) |
const |
|
|
converts vector(s) in joint referenc frame cur to be vector(s) on link dest
- Parameters:
-
[out] | R | on return contains rotation matrix between cur and dest |
[out] | p | on return contains vector from cur to dest |
[in] | cur | the link vectors of A are currently relative to |
[in] | dest | the link vectors should be converted to |
Note the difference between this and convertFrame is that a link moves within the frame at its joints origin - the frame is static no matter how the joint moves within it. If we have a point on the link, then the position of that point will move within the frame as the joint moves. This function will return vector(s) of A represented as if they were connected to link dest.
The Matrix returned will match the homogenousness of A.
Definition at line 331 of file kinemat.cpp.
Referenced by convertFrameToLink(). |
ReturnMatrix ROBOOP::Robot_basic::convertLink |
( |
int |
cur, |
|
|
int |
dest |
|
) |
const |
|
|
Returns a matrix which can be used to convert from link cur to link dest.
- Parameters:
-
| cur | the source link |
| dest | the target link |
Note the difference between this and convertFrame is that a link moves within the frame at its joints origin - the frame is static no matter how the joint moves within it. If we have a point on the link, then the position of that point will move within the frame as the joint moves. This function will return vector(s) of A represented as if they were connected to link dest.
The Matrix returned will be homogenous (4x4).
Definition at line 416 of file kinemat.cpp. |
void ROBOOP::Robot_basic::convertLink |
( |
Matrix & |
R, |
|
|
ColumnVector & |
p, |
|
|
int |
cur, |
|
|
int |
dest |
|
) |
const |
|
|
converts vector(s) on link cur to be vector(s) on link dest
- Parameters:
-
[out] | R | on return contains rotation matrix between cur and dest |
[out] | p | on return contains vector from cur to dest |
[in] | cur | the link vectors of A are currently relative to |
[in] | dest | the link vectors should be converted to |
Note the difference between this and convertFrame is that a link moves within the frame at its joints origin - the frame is static no matter how the joint moves within it. If we have a point on the link, then the position of that point will move within the frame as the joint moves. This function will return vector(s) of A represented as if they were connected to link dest.
The Matrix returned will match the homogenousness of A.
Definition at line 378 of file kinemat.cpp.
Referenced by ROBOOP::Robot::computeFirstERSLink(), ROBOOP::Robot::computeSecondERSLink(), ROBOOP::Robot::computeThirdERSLink(), and convertLink(). |
ReturnMatrix ROBOOP::Robot_basic::convertLinkToFrame |
( |
int |
cur, |
|
|
int |
dest |
|
) |
const |
|
|
Returns a matrix which can be used to convert from link frame cur to joint frame dest.
- Parameters:
-
| cur | the source link |
| dest | the target frame |
Note the difference between this and convertFrame is that a link moves within the frame at its joints origin - the frame is static no matter how the joint moves within it. If we have a point on the link, then the position of that point will move within the frame as the joint moves. This function will return vector(s) of A represented as if they were connected to link dest.
The Matrix returned will be homogenous (4x4).
Definition at line 311 of file kinemat.cpp. |
void ROBOOP::Robot_basic::convertLinkToFrame |
( |
Matrix & |
R, |
|
|
ColumnVector & |
p, |
|
|
int |
cur, |
|
|
int |
dest |
|
) |
const |
|
|
converts vector(s) on link cur to be vector(s) in joint reference frame dest
- Parameters:
-
[out] | R | on return contains rotation matrix between cur and dest |
[out] | p | on return contains vector from cur to dest |
[in] | cur | the link vectors of A are currently relative to |
[in] | dest | the link vectors should be converted to |
Note the difference between this and convertFrame is that a link moves within the frame at its joints origin - the frame is static no matter how the joint moves within it. If we have a point on the link, then the position of that point will move within the frame as the joint moves. This function will return vector(s) of A represented as if they were connected to link dest.
The Matrix returned will match the homogenousness of A.
Definition at line 285 of file kinemat.cpp.
Referenced by convertLinkToFrame(). |
|
Sensitivity of the dynamics with respect to .
This function computes .
Definition at line 58 of file sensitiv.cpp. |
|
Sensitivity of the dynamics with respect to .
This function computes .
Definition at line 84 of file sensitiv.cpp. |
virtual ReturnMatrix ROBOOP::Robot_basic::dTdqi |
( |
const int |
i, |
|
|
const int |
endlink |
|
) |
[pure virtual] |
|
virtual void ROBOOP::Robot_basic::dTdqi |
( |
Matrix & |
dRot, |
|
|
ColumnVector & |
dpos, |
|
|
const int |
i, |
|
|
const int |
endlink |
|
) |
[pure virtual] |
|
virtual ReturnMatrix ROBOOP::Robot_basic::dTdqi |
( |
const int |
i |
) |
[inline, virtual] |
|
virtual void ROBOOP::Robot_basic::dTdqi |
( |
Matrix & |
dRot, |
|
|
ColumnVector & |
dpos, |
|
|
const int |
i |
|
) |
[inline, virtual] |
|
void ROBOOP::Robot_basic::error |
( |
const string & |
msg1 |
) |
const |
|
|
Print the message msg1 on the console.
Definition at line 1122 of file robot.cpp.
Referenced by ROBOOP::mRobot_min_para::C(), ROBOOP::mRobot::C(), ROBOOP::Robot::C(), ROBOOP::mRobot_min_para::delta_torque(), ROBOOP::mRobot::delta_torque(), ROBOOP::Robot::delta_torque(), ROBOOP::mRobot_min_para::dq_torque(), ROBOOP::mRobot::dq_torque(), ROBOOP::Robot::dq_torque(), ROBOOP::mRobot_min_para::dqp_torque(), ROBOOP::mRobot::dqp_torque(), ROBOOP::Robot::dqp_torque(), ROBOOP::mRobot_min_para::dTdqi(), ROBOOP::mRobot::dTdqi(), ROBOOP::Robot::dTdqi(), get_q(), Robot_basic(), set_q(), set_qp(), set_qpp(), ROBOOP::mRobot_min_para::torque(), ROBOOP::mRobot::torque(), ROBOOP::Robot::torque(), ROBOOP::mRobot_min_para::torque_novelocity(), ROBOOP::mRobot::torque_novelocity(), and ROBOOP::Robot::torque_novelocity(). |
virtual ReturnMatrix ROBOOP::Robot_basic::G |
( |
|
) |
[pure virtual] |
|
int ROBOOP::Robot_basic::get_available_dof |
( |
const int |
endlink |
) |
const |
|
|
Counts number of currently non-immobile links up to and including endlink.
Definition at line 1129 of file robot.cpp. |
int ROBOOP::Robot_basic::get_available_dof |
( |
|
) |
const [inline] |
|
ReturnMatrix ROBOOP::Robot_basic::get_available_q |
( |
const int |
endlink |
) |
const |
|
|
Return the joint position vector of available (non-immobile) joints up to and including endlink.
Definition at line 1169 of file robot.cpp. |
ReturnMatrix ROBOOP::Robot_basic::get_available_q |
( |
void |
|
) |
const [inline] |
|
ReturnMatrix ROBOOP::Robot_basic::get_available_qp |
( |
const int |
endlink |
) |
const |
|
|
Return the joint velocity vector of available (non-immobile) joints up to and including endlink.
Definition at line 1181 of file robot.cpp. |
ReturnMatrix ROBOOP::Robot_basic::get_available_qp |
( |
void |
|
) |
const [inline] |
|
|
Return the joint velocity vector of available (non-immobile) joints.
Definition at line 249 of file robot.h. |
ReturnMatrix ROBOOP::Robot_basic::get_available_qpp |
( |
const int |
endlink |
) |
const |
|
|
Return the joint acceleration vector of available (non-immobile) joints up to and including endlink.
Definition at line 1193 of file robot.cpp. |
ReturnMatrix ROBOOP::Robot_basic::get_available_qpp |
( |
void |
|
) |
const [inline] |
|
|
Return the joint acceleration vector of available (non-immobile) joints.
Definition at line 250 of file robot.h. |
bool ROBOOP::Robot_basic::get_DH |
( |
void |
|
) |
const [inline] |
|
|
Return true if in DH notation, false otherwise.
Definition at line 240 of file robot.h. |
int ROBOOP::Robot_basic::get_dof |
( |
|
) |
const [inline] |
|
int ROBOOP::Robot_basic::get_fix |
( |
|
) |
const [inline] |
|
Real ROBOOP::Robot_basic::get_q |
( |
const int |
i |
) |
const [inline] |
|
|
Return the joint acceleration vector.
Definition at line 1159 of file robot.cpp. |
static Real ROBOOP::Robot_basic::homog_norm |
( |
const ColumnVector & |
v |
) |
[inline, static] |
|
ReturnMatrix ROBOOP::Robot_basic::inv_kin |
( |
const Matrix & |
Tobj, |
|
|
const int |
mj, |
|
|
const int |
endlink, |
|
|
bool & |
converge |
|
) |
[virtual] |
|
|
Numerical inverse kinematics.
- Parameters:
-
| Tobj: | Transformation matrix expressing the desired end effector pose. |
| mj: | Select algorithm type, 0: based on Jacobian, 1: based on derivative of T. |
| endlink: | the link to pretend is the end effector |
| converge: | Indicate if the algorithm converge. |
The joint position vector before the inverse kinematics is returned if the algorithm does not converge.
Reimplemented in ROBOOP::Robot, ROBOOP::mRobot, and ROBOOP::mRobot_min_para.
Definition at line 90 of file invkine.cpp. |
virtual ReturnMatrix ROBOOP::Robot_basic::inv_kin |
( |
const Matrix & |
Tobj, |
|
|
const int |
mj, |
|
|
bool & |
converge |
|
) |
[inline, virtual] |
|
ReturnMatrix ROBOOP::Robot_basic::inv_kin |
( |
const Matrix & |
Tobj, |
|
|
const int |
mj = 0 |
|
) |
[virtual] |
|
ReturnMatrix ROBOOP::Robot_basic::inv_kin_orientation |
( |
const Matrix & |
Robj, |
|
|
const int |
mj, |
|
|
const int |
endlink, |
|
|
bool & |
converge |
|
) |
[virtual] |
|
|
Numerical inverse kinematics.
- Parameters:
-
| Robj: | Rotation matrix expressing the desired end effector orientation w.r.t base frame |
| mj: | Select algorithm type, 0: based on Jacobian, 1: based on derivative of T. |
| endlink: | the link to pretend is the end effector |
| converge: | Indicate if the algorithm converge. |
The joint position vector before the inverse kinematics is returned if the algorithm does not converge.
Reimplemented in ROBOOP::Robot.
Definition at line 426 of file invkine.cpp. |
|
Numerical inverse kinematics.
- Parameters:
-
| Pobj: | Vector expressing the desired end effector position; can be homogenous |
| mj: | Select algorithm type, 0: based on Jacobian, 1: based on derivative of T. |
| endlink: | the link to pretend is the end effector |
| Plink: | ignored for now |
| converge: | Indicate if the algorithm converge. |
The joint position vector before the inverse kinematics is returned if the algorithm does not converge.
Reimplemented in ROBOOP::Robot.
Definition at line 211 of file invkine.cpp. |
virtual ReturnMatrix ROBOOP::Robot_basic::inv_kin_puma |
( |
const Matrix & |
Tobj, |
|
|
bool & |
converge |
|
) |
[pure virtual] |
|
virtual ReturnMatrix ROBOOP::Robot_basic::inv_kin_rhino |
( |
const Matrix & |
Tobj, |
|
|
bool & |
converge |
|
) |
[pure virtual] |
|
virtual ReturnMatrix ROBOOP::Robot_basic::jacobian |
( |
const int |
endlink, |
|
|
const int |
ref |
|
) |
const [pure virtual] |
|
virtual ReturnMatrix ROBOOP::Robot_basic::jacobian |
( |
const int |
ref = 0 |
) |
const [inline, virtual] |
|
ReturnMatrix ROBOOP::Robot_basic::jacobian_DLS_inv |
( |
const double |
eps, |
|
|
const double |
lambda_max, |
|
|
const int |
ref = 0 |
|
) |
const |
|
|
Inverse Jacobian based on damped least squares inverse.
- Parameters:
-
| eps: | Range of singular region. |
| lambda_max: | Value to obtain a good solution in singular region. |
| ref: | Selected frame (ex: joint 4). |
The Jacobian inverse, based on damped least squares, is
where and is a damping factor and the identity matrix respectively. Based on SVD (Singular Value Decomposition) the Jacobian is , where , and are respectively the input vector, the ouput vector and the singular values ( , is the rank of J). Using the previous equations we obtain
A singular region, based on the smallest singular value, can be defined by
Definition at line 182 of file kinemat.cpp.
Referenced by ROBOOP::Clik::q_qdot(). |
virtual ReturnMatrix ROBOOP::Robot_basic::jacobian_dot |
( |
const int |
ref = 0 |
) |
const [pure virtual] |
|
ReturnMatrix ROBOOP::Robot_basic::kine |
( |
const int |
j |
) |
const |
|
|
Return the frame j direct kinematics transform matrix.
Definition at line 142 of file kinemat.cpp. |
|
Direct kinematics at end effector.
- Parameters:
-
| Rot: | Frame j orientation. |
| pos: | Frame j position. |
| j: | Selected frame. |
Definition at line 110 of file kinemat.cpp. |
ReturnMatrix ROBOOP::Robot_basic::kine_pd |
( |
const int |
j = 0 |
) |
const |
|
|
Direct kinematics with velocity.
Return a matrix. The first three columns are the frame j to the base rotation, the fourth column is the frame j w.r.t to the base postion vector and the last column is the frame j w.r.t to the base translational velocity vector. Print an error on the console if j is out of range.
Definition at line 155 of file kinemat.cpp. |
static Real ROBOOP::Robot_basic::limit_angle |
( |
Real |
x, |
|
|
Real |
min, |
|
|
Real |
max |
|
) |
[inline, static] |
|
static Real ROBOOP::Robot_basic::limit_range |
( |
Real |
x, |
|
|
Real |
min, |
|
|
Real |
max |
|
) |
[inline, static] |
|
|
useful for fixing x to be at least min or at most max
Definition at line 316 of file robot.h. |
static Real ROBOOP::Robot_basic::normalize_angle |
( |
Real |
t |
) |
[inline, static] |
|
virtual void ROBOOP::Robot_basic::robotType_inv_kin |
( |
|
) |
[pure virtual] |
|
void ROBOOP::Robot_basic::set_q |
( |
const Real |
q, |
|
|
const int |
i |
|
) |
[inline] |
|
|
< Set joint i position.
Definition at line 256 of file robot.h. |
void ROBOOP::Robot_basic::set_q |
( |
const Matrix & |
q |
) |
|
|
|
Set the joint position vector.
Set the joint position vector and recalculate the orientation matrix R and the position vector p (see Link class). Print an error if the size of q is incorrect.
Definition at line 1205 of file robot.cpp. |
|
Set the joint position vector.
Set the joint position vector and recalculate the orientation matrix R and the position vector p (see Link class). Print an error if the size of q is incorrect.
Definition at line 1272 of file robot.cpp.
Referenced by ROBOOP::mRobot_min_para::delta_torque(), ROBOOP::mRobot::delta_torque(), ROBOOP::Robot::delta_torque(), ROBOOP::mRobot_min_para::dq_torque(), ROBOOP::mRobot::dq_torque(), ROBOOP::Robot::dq_torque(), ROBOOP::mRobot_min_para::dqp_torque(), ROBOOP::mRobot::dqp_torque(), ROBOOP::Robot::dqp_torque(), ROBOOP::Clik::endeff_pos_ori_err(), inertia(), inv_kin(), ROBOOP::Robot::inv_kin_ers_pos(), inv_kin_orientation(), inv_kin_pos(), ROBOOP::mRobot_min_para::inv_kin_puma(), ROBOOP::mRobot::inv_kin_puma(), ROBOOP::Robot::inv_kin_puma(), ROBOOP::mRobot_min_para::inv_kin_rhino(), ROBOOP::mRobot::inv_kin_rhino(), ROBOOP::Robot::inv_kin_rhino(), ROBOOP::Clik::q_qdot(), ROBOOP::Dynamics::set_robot_on_first_point_of_splines(), ROBOOP::mRobot_min_para::torque(), ROBOOP::mRobot::torque(), and ROBOOP::Robot::torque(). |
void ROBOOP::Robot_basic::set_qp |
( |
const ColumnVector & |
qp |
) |
|
|
void ROBOOP::Robot_basic::set_qpp |
( |
const ColumnVector & |
qpp |
) |
|
|
|
Set the joint acceleration vector.
Definition at line 1326 of file robot.cpp. |
Friends And Related Function Documentation
friend class mRobotgl [friend] |
|
friend class Robot [friend] |
|
friend class Robotgl [friend] |
|
Member Data Documentation
|
Degree of freedom.
Definition at line 371 of file robot.h.
Referenced by acceleration(), dtau_dq(), dtau_dqp(), dTdqi(), get_available_dof(), get_available_q(), get_available_qp(), get_available_qpp(), get_dof(), get_q(), get_qp(), get_qpp(), inertia(), inv_kin(), inv_kin_orientation(), inv_kin_pos(), jacobian(), kine(), operator=(), Robot_basic(), set_q(), set_qp(), and set_qpp(). |
|
Pointer on Link cclass.
Definition at line 356 of file robot.h.
Referenced by convertFrame(), convertFrameToLink(), convertLink(), convertLinkToFrame(), get_DH(), get_q(), get_qp(), get_qpp(), inv_kin(), inv_kin_orientation(), inv_kin_pos(), operator=(), Robot_basic(), set_q(), set_qp(), set_qpp(), and ~Robot_basic(). |
|
Axis vector at each joint.
Definition at line 351 of file robot.h.
Referenced by ROBOOP::mRobot_min_para::delta_torque(), ROBOOP::mRobot::delta_torque(), ROBOOP::Robot::delta_torque(), ROBOOP::mRobot_min_para::dq_torque(), ROBOOP::mRobot::dq_torque(), ROBOOP::Robot::dq_torque(), ROBOOP::mRobot_min_para::dqp_torque(), ROBOOP::mRobot::dqp_torque(), ROBOOP::Robot::dqp_torque(), operator=(), and Robot_basic(). |
The documentation for this class was generated from the following files:
|