Homepage Demos Overview Downloads Tutorials Reference
Credits

ROBOOP::Link Class Reference

#include <robot.h>

List of all members.


Detailed Description

Link definitions.

A n degree of freedom (dof) serial manipulator is composed of n links. This class describe the property of a link. A n dof robot has n instance of the class Link.

Definition at line 137 of file robot.h.

Public Member Functions

 Link (const int jt=0, const Real it=0.0, const Real id=0.0, const Real ia=0.0, const Real ial=0.0, const Real theta_min=-M_PI/2, const Real theta_max=M_PI/2, const Real it_off=0.0, const Real mass=1.0, const Real cmx=0.0, const Real cmy=0.0, const Real cmz=0.0, const Real ixx=0.0, const Real ixy=0.0, const Real ixz=0.0, const Real iyy=0.0, const Real iyz=0.0, const Real izz=0.0, const Real iIm=0.0, const Real iGr=0.0, const Real iB=0.0, const Real iCf=0.0, const bool dh=true, const bool min_inertial_para=false)
 Constructor.
 Link (const Link &x)
 Copy constructor.
 ~Link ()
 Destructor.
Linkoperator= (const Link &x)
 Overload = operator.
void transform (const Real q)
 Set the rotation matrix R and the vector p.
bool get_DH (void) const
 Return DH value.
int get_joint_type (void) const
 Return the joint type.
Real get_theta (void) const
 Return theta.
Real get_d (void) const
 Return d.
Real get_a (void) const
 Return a.
Real get_alpha (void) const
 Return alpha.
Real get_q (void) const
 Return joint position (theta if joint type is rotoide, d otherwise).
Real get_theta_min (void) const
 Return theta_min.
Real get_theta_max (void) const
 Return theta_max.
Real get_joint_offset (void) const
 Return joint_offset.
ReturnMatrix get_mc (void)
 Return mc.
ReturnMatrix get_r (void)
 Return r.
ReturnMatrix get_p (void) const
 Return p.
Real get_m (void) const
 Return m.
Real get_Im (void) const
 Return Im.
Real get_Gr (void) const
 Return Gr.
Real get_B (void) const
 Return B.
Real get_Cf (void) const
 Return Cf.
ReturnMatrix get_I (void) const
 Return I.
bool get_immobile (void) const
 Return immobile.
void set_m (const Real m_)
 Set m.
void set_mc (const ColumnVector &mc_)
 Set the mass $\times$ center of gravity position.
void set_r (const ColumnVector &r_)
 Set the center of gravity position.
void set_Im (const Real Im_)
 Set Im.
void set_B (const Real B_)
 Set B.
void set_Cf (const Real Cf_)
 Set Cf.
void set_I (const Matrix &I)
 Set the inertia matrix.
void set_immobile (bool im)
 Set immobile.

Public Attributes

Matrix R
 Orientation matrix of actual link w.r.t to previous link.
Real qp
 Joint velocity.
Real qpp
 Joint acceleration.

Private Attributes

int joint_type
 Joint type.
Real theta
 theta DH parameter.
Real d
 d DH parameter.
Real a
 a DH parameter.
Real alpha
 alpha DH parameter.
Real theta_min
 Min joint angle.
Real theta_max
 Max joint angle.
Real joint_offset
 Offset in joint angle (rotoide and prismatic).
bool DH
 DH notation(true) or DH modified notation.
bool min_para
 Minimum inertial parameter.
ColumnVector r
 Position of center of mass w.r.t. link coordinate system (min_para=F).
ColumnVector p
 Position vector of actual link w.r.t to previous link.
Real m
 Mass of the link.
Real Im
 Motor Inertia.
Real Gr
 Gear Ratio.
Real B
 Viscous coefficient.
Real Cf
 Coulomb fiction coefficient.
ColumnVector mc
 Mass $\times$ center of gravity (used if min_para = true).
Matrix I
 Inertia matrix w.r.t. center of mass and link coordinate system orientation.
bool immobile
 true if the joint is to be considered locked - ignored for inverse kinematics, but can still be reassigned through transform

Friends

class Robot_basic
class Robot
class mRobot
class mRobot_min_para


Constructor & Destructor Documentation

ROBOOP::Link::Link const int  jt = 0,
const Real  it = 0.0,
const Real  id = 0.0,
const Real  ia = 0.0,
const Real  ial = 0.0,
const Real  it_min = -M_PI/2,
const Real  it_max = M_PI/2,
const Real  it_off = 0.0,
const Real  mass = 1.0,
const Real  cmx = 0.0,
const Real  cmy = 0.0,
const Real  cmz = 0.0,
const Real  ixx = 0.0,
const Real  ixy = 0.0,
const Real  ixz = 0.0,
const Real  iyy = 0.0,
const Real  iyz = 0.0,
const Real  izz = 0.0,
const Real  iIm = 0.0,
const Real  iGr = 0.0,
const Real  iB = 0.0,
const Real  iCf = 0.0,
const bool  dh = true,
const bool  min_inertial_parameters = false
 

Constructor.

Definition at line 122 of file robot.cpp.

ROBOOP::Link::Link const Link x  ) 
 

Copy constructor.

Definition at line 220 of file robot.cpp.

ROBOOP::Link::~Link  )  [inline]
 

Destructor.

Definition at line 154 of file robot.h.


Member Function Documentation

Real ROBOOP::Link::get_a void   )  const [inline]
 

Return a.

Definition at line 161 of file robot.h.

Referenced by ROBOOP::Robot::delta_torque(), ROBOOP::Robot::dq_torque(), ROBOOP::Robot::dqp_torque(), ROBOOP::Puma_DH(), ROBOOP::Puma_mDH(), ROBOOP::Rhino_DH(), ROBOOP::Rhino_mDH(), and ROBOOP::Robot_basic::set_q().

Real ROBOOP::Link::get_alpha void   )  const [inline]
 

Return alpha.

Definition at line 162 of file robot.h.

Referenced by ROBOOP::ERS2xx_Head_DH(), ROBOOP::ERS7_Head_DH(), ROBOOP::ERS_Leg_DH(), ROBOOP::Puma_DH(), ROBOOP::Puma_mDH(), ROBOOP::Rhino_DH(), and ROBOOP::Rhino_mDH().

Real ROBOOP::Link::get_B void   )  const [inline]
 

Return B.

Definition at line 173 of file robot.h.

Referenced by ROBOOP::perturb_robot().

Real ROBOOP::Link::get_Cf void   )  const [inline]
 

Return Cf.

Definition at line 174 of file robot.h.

Referenced by ROBOOP::perturb_robot().

Real ROBOOP::Link::get_d void   )  const [inline]
 

Return d.

Definition at line 160 of file robot.h.

Referenced by ROBOOP::Robot_basic::convertFrameToLink(), ROBOOP::Robot_basic::convertLink(), ROBOOP::Robot_basic::convertLinkToFrame(), ROBOOP::Robot::delta_torque(), ROBOOP::Robot::dq_torque(), ROBOOP::Robot::dqp_torque(), ROBOOP::Puma_DH(), ROBOOP::Puma_mDH(), ROBOOP::Rhino_DH(), ROBOOP::Rhino_mDH(), and ROBOOP::Robot_basic::set_q().

bool ROBOOP::Link::get_DH void   )  const [inline]
 

Return DH value.

Definition at line 157 of file robot.h.

Real ROBOOP::Link::get_Gr void   )  const [inline]
 

Return Gr.

Definition at line 172 of file robot.h.

ReturnMatrix ROBOOP::Link::get_I void   )  const [inline]
 

Return I.

Definition at line 175 of file robot.h.

Referenced by ROBOOP::perturb_robot().

Real ROBOOP::Link::get_Im void   )  const [inline]
 

Return Im.

Definition at line 171 of file robot.h.

Referenced by ROBOOP::perturb_robot().

bool ROBOOP::Link::get_immobile void   )  const [inline]
 

Return immobile.

Definition at line 176 of file robot.h.

Referenced by ROBOOP::mRobot_min_para::dTdqi(), ROBOOP::mRobot::dTdqi(), and ROBOOP::Robot::dTdqi().

Real ROBOOP::Link::get_joint_offset void   )  const [inline]
 

Return joint_offset.

Definition at line 166 of file robot.h.

Referenced by ROBOOP::Robot_basic::convertFrameToLink(), ROBOOP::Robot_basic::convertLink(), and ROBOOP::Robot_basic::convertLinkToFrame().

int ROBOOP::Link::get_joint_type void   )  const [inline]
 

Return the joint type.

Definition at line 158 of file robot.h.

Referenced by ROBOOP::mRobot_min_para::C(), ROBOOP::mRobot::C(), ROBOOP::Robot::C(), ROBOOP::Robot_basic::convertFrameToLink(), ROBOOP::Robot_basic::convertLink(), ROBOOP::Robot_basic::convertLinkToFrame(), 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(), ROBOOP::ERS2xx_Head_DH(), ROBOOP::ERS7_Head_DH(), ROBOOP::ERS_Leg_DH(), ROBOOP::mRobot_min_para::G(), ROBOOP::mRobot::G(), ROBOOP::Robot::G(), ROBOOP::Robot_basic::inv_kin(), ROBOOP::Robot_basic::inv_kin_orientation(), ROBOOP::Puma_DH(), ROBOOP::Puma_mDH(), ROBOOP::Rhino_DH(), ROBOOP::Rhino_mDH(), 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().

Real ROBOOP::Link::get_m void   )  const [inline]
 

Return m.

Definition at line 170 of file robot.h.

Referenced by ROBOOP::perturb_robot().

ReturnMatrix ROBOOP::Link::get_mc void   )  [inline]
 

Return mc.

Definition at line 167 of file robot.h.

ReturnMatrix ROBOOP::Link::get_p void   )  const [inline]
 

Return p.

Definition at line 169 of file robot.h.

Real ROBOOP::Link::get_q void   )  const
 

Return joint position (theta if joint type is rotoide, d otherwise).

The joint offset is removed from the value.

Definition at line 339 of file robot.cpp.

Referenced by ROBOOP::Robot::computeFirstERSLink(), ROBOOP::Robot::computeSecondERSLink(), ROBOOP::Robot::computeThirdERSLink(), and ROBOOP::Robot_basic::get_q().

ReturnMatrix ROBOOP::Link::get_r void   )  [inline]
 

Return r.

Definition at line 168 of file robot.h.

Real ROBOOP::Link::get_theta void   )  const [inline]
 

Return theta.

Definition at line 159 of file robot.h.

Referenced by ROBOOP::ERS2xx_Head_DH(), ROBOOP::ERS7_Head_DH(), and ROBOOP::ERS_Leg_DH().

Real ROBOOP::Link::get_theta_max void   )  const [inline]
 

Return theta_max.

Definition at line 165 of file robot.h.

Real ROBOOP::Link::get_theta_min void   )  const [inline]
 

Return theta_min.

Definition at line 164 of file robot.h.

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

Overload = operator.

Definition at line 249 of file robot.cpp.

void ROBOOP::Link::set_B const Real  B_  )  [inline]
 

Set B.

Definition at line 181 of file robot.h.

Referenced by ROBOOP::perturb_robot().

void ROBOOP::Link::set_Cf const Real  Cf_  )  [inline]
 

Set Cf.

Definition at line 182 of file robot.h.

Referenced by ROBOOP::perturb_robot().

void ROBOOP::Link::set_I const Matrix I_  ) 
 

Set the inertia matrix.

Definition at line 375 of file robot.cpp.

Referenced by ROBOOP::perturb_robot().

void ROBOOP::Link::set_Im const Real  Im_  )  [inline]
 

Set Im.

Definition at line 180 of file robot.h.

Referenced by ROBOOP::perturb_robot().

void ROBOOP::Link::set_immobile bool  im  )  [inline]
 

Set immobile.

Definition at line 184 of file robot.h.

Referenced by ROBOOP::Robot_basic::Robot_basic().

void ROBOOP::Link::set_m const Real  m_  )  [inline]
 

Set m.

Definition at line 177 of file robot.h.

Referenced by ROBOOP::perturb_robot().

void ROBOOP::Link::set_mc const ColumnVector mc_  ) 
 

Set the mass $\times$ center of gravity position.

Definition at line 362 of file robot.cpp.

void ROBOOP::Link::set_r const ColumnVector r_  ) 
 

Set the center of gravity position.

Definition at line 353 of file robot.cpp.

void ROBOOP::Link::transform const Real  q  ) 
 

Set the rotation matrix R and the vector p.

Definition at line 278 of file robot.cpp.

Referenced by ROBOOP::Robot_basic::set_q().


Friends And Related Function Documentation

friend class mRobot [friend]
 

Definition at line 141 of file robot.h.

friend class mRobot_min_para [friend]
 

Definition at line 142 of file robot.h.

friend class Robot [friend]
 

Definition at line 140 of file robot.h.

friend class Robot_basic [friend]
 

Definition at line 139 of file robot.h.


Member Data Documentation

Real ROBOOP::Link::a [private]
 

a DH parameter.

Definition at line 193 of file robot.h.

Referenced by 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(), Link(), operator=(), and transform().

Real ROBOOP::Link::alpha [private]
 

alpha DH parameter.

Definition at line 193 of file robot.h.

Referenced by Link(), and operator=().

Real ROBOOP::Link::B [private]
 

Viscous coefficient.

Definition at line 204 of file robot.h.

Referenced by ROBOOP::mRobot_min_para::C(), ROBOOP::mRobot::C(), ROBOOP::Robot::C(), operator=(), ROBOOP::mRobot_min_para::torque(), ROBOOP::mRobot::torque(), and ROBOOP::Robot::torque().

Real ROBOOP::Link::Cf [private]
 

Coulomb fiction coefficient.

Definition at line 204 of file robot.h.

Referenced by ROBOOP::mRobot_min_para::C(), ROBOOP::mRobot::C(), ROBOOP::Robot::C(), operator=(), ROBOOP::mRobot_min_para::torque(), ROBOOP::mRobot::torque(), and ROBOOP::Robot::torque().

Real ROBOOP::Link::d [private]
 

d DH parameter.

Definition at line 193 of file robot.h.

Referenced by get_q(), 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(), Link(), operator=(), and transform().

bool ROBOOP::Link::DH [private]
 

DH notation(true) or DH modified notation.

Definition at line 200 of file robot.h.

Referenced by operator=(), and ROBOOP::Robot_basic::set_q().

Real ROBOOP::Link::Gr [private]
 

Gear Ratio.

Definition at line 204 of file robot.h.

Referenced by ROBOOP::mRobot_min_para::C(), ROBOOP::mRobot::C(), ROBOOP::Robot::C(), operator=(), 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().

Matrix ROBOOP::Link::I [private]
 

Inertia matrix w.r.t. center of mass and link coordinate system orientation.

Definition at line 209 of file robot.h.

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(), Link(), operator=(), set_I(), 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().

Real ROBOOP::Link::Im [private]
 

Motor Inertia.

Definition at line 204 of file robot.h.

Referenced by operator=(), 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().

bool ROBOOP::Link::immobile [private]
 

true if the joint is to be considered locked - ignored for inverse kinematics, but can still be reassigned through transform

Definition at line 210 of file robot.h.

Referenced by ROBOOP::Robot_basic::inv_kin(), ROBOOP::Robot_basic::inv_kin_orientation(), ROBOOP::Robot_basic::inv_kin_pos(), operator=(), ROBOOP::Robot_basic::set_q(), and ROBOOP::Robot_basic::set_qp().

Real ROBOOP::Link::joint_offset [private]
 

Offset in joint angle (rotoide and prismatic).

Definition at line 193 of file robot.h.

Referenced by operator=().

int ROBOOP::Link::joint_type [private]
 

Joint type.

Definition at line 191 of file robot.h.

Referenced by get_q(), Link(), operator=(), and transform().

Real ROBOOP::Link::m [private]
 

Mass of the link.

Definition at line 204 of file robot.h.

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::G(), ROBOOP::mRobot::G(), ROBOOP::Robot::G(), operator=(), 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().

ColumnVector ROBOOP::Link::mc [private]
 

Mass $\times$ center of gravity (used if min_para = true).

Definition at line 208 of file robot.h.

Referenced by Link(), operator=(), and set_mc().

bool ROBOOP::Link::min_para [private]
 

Minimum inertial parameter.

Definition at line 200 of file robot.h.

Referenced by operator=().

ColumnVector ROBOOP::Link::p [private]
 

Position vector of actual link w.r.t to previous link.

Definition at line 202 of file robot.h.

Referenced by ROBOOP::Robot_basic::convertFrame(), ROBOOP::mRobot_min_para::delta_torque(), ROBOOP::mRobot::delta_torque(), ROBOOP::mRobot_min_para::dq_torque(), ROBOOP::mRobot::dq_torque(), ROBOOP::mRobot_min_para::dqp_torque(), ROBOOP::mRobot::dqp_torque(), ROBOOP::mRobot_min_para::dTdqi(), ROBOOP::mRobot::dTdqi(), ROBOOP::Robot::dTdqi(), Link(), operator=(), ROBOOP::Robot_basic::set_q(), and transform().

Real ROBOOP::Link::qp
 

Joint velocity.

Definition at line 188 of file robot.h.

Referenced by ROBOOP::Robot_basic::get_qp(), Link(), operator=(), and ROBOOP::Robot_basic::set_qp().

Real ROBOOP::Link::qpp
 

Joint acceleration.

Definition at line 188 of file robot.h.

Referenced by ROBOOP::Robot_basic::get_qpp(), Link(), operator=(), and ROBOOP::Robot_basic::set_qpp().

ColumnVector ROBOOP::Link::r [private]
 

Position of center of mass w.r.t. link coordinate system (min_para=F).

Definition at line 202 of file robot.h.

Referenced by Link(), operator=(), and set_r().

Matrix ROBOOP::Link::R
 

Orientation matrix of actual link w.r.t to previous link.

Definition at line 186 of file robot.h.

Referenced by ROBOOP::mRobot_min_para::C(), ROBOOP::mRobot::C(), ROBOOP::Robot::C(), ROBOOP::Robot_basic::convertFrame(), 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(), ROBOOP::mRobot_min_para::G(), ROBOOP::mRobot::G(), ROBOOP::Robot::G(), Link(), operator=(), ROBOOP::Robot_basic::set_q(), ROBOOP::mRobot_min_para::torque(), ROBOOP::mRobot::torque(), ROBOOP::Robot::torque(), ROBOOP::mRobot_min_para::torque_novelocity(), ROBOOP::mRobot::torque_novelocity(), ROBOOP::Robot::torque_novelocity(), and transform().

Real ROBOOP::Link::theta [private]
 

theta DH parameter.

Definition at line 193 of file robot.h.

Referenced by get_q(), Link(), operator=(), and transform().

Real ROBOOP::Link::theta_max [private]
 

Max joint angle.

Definition at line 193 of file robot.h.

Referenced by operator=().

Real ROBOOP::Link::theta_min [private]
 

Min joint angle.

Definition at line 193 of file robot.h.

Referenced by operator=().


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

ROBOOP v1.21a
Generated Tue Oct 19 14:18:30 2004 by Doxygen 1.3.9.1