00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108 #ifndef __cplusplus
00109 #error Must use C++ for the type Robot
00110 #endif
00111 #ifndef ROBOT_H
00112 #define ROBOT_H
00113
00114
00115
00116
00117
00118
00119 #include "utils.h"
00120
00121 #ifdef use_namespace
00122 namespace ROBOOP {
00123 using namespace NEWMAT;
00124 #endif
00125
00126 static const char header_rcsid[] __UNUSED__ = "$Id: robot.h,v 1.21 2007/11/11 23:57:24 ejt Exp $";
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136 class Link
00137 {
00138 friend class Robot_basic;
00139 friend class Robot;
00140 friend class mRobot;
00141 friend class mRobot_min_para;
00142
00143 public:
00144 Link(const int jt = 0, const Real it = 0.0, const Real id = 0.0,
00145 const Real ia = 0.0, const Real ial = 0.0, const Real theta_min = -M_PI/2,
00146 const Real theta_max = M_PI/2, const Real it_off = 0.0, const Real mass = 1.0,
00147 const Real cmx = 0.0, const Real cmy = 0.0, const Real cmz = 0.0,
00148 const Real ixx = 0.0, const Real ixy = 0.0, const Real ixz = 0.0,
00149 const Real iyy = 0.0, const Real iyz = 0.0, const Real izz = 0.0,
00150 const Real iIm = 0.0, const Real iGr = 0.0, const Real iB = 0.0,
00151 const Real iCf = 0.0, const bool dh = true, const bool min_inertial_para = false);
00152 Link(const Link & x);
00153 ~Link(){}
00154 Link & operator=(const Link & x);
00155 void transform(const Real q);
00156 bool get_DH(void) const {return DH; }
00157 int get_joint_type(void) const { return joint_type; }
00158 Real get_theta(void) const { return theta; }
00159 Real get_d(void) const { return d; }
00160 Real get_a(void) const { return a; }
00161 Real get_alpha(void) const { return alpha; }
00162 Real get_q(void) const;
00163 Real get_theta_min(void) const { return theta_min; }
00164 Real get_theta_max(void) const { return theta_max; }
00165 Real get_joint_offset(void) const { return joint_offset; }
00166 ReturnMatrix get_mc(void) { return mc; }
00167 ReturnMatrix get_r(void) { return r; }
00168 ReturnMatrix get_p(void) const { return p; }
00169 Real get_m(void) const { return m; }
00170 Real get_Im(void) const { return Im; }
00171 Real get_Gr(void) const { return Gr; }
00172 Real get_B(void) const { return B; }
00173 Real get_Cf(void) const { return Cf; }
00174 ReturnMatrix get_I(void) const { return I; }
00175 bool get_immobile(void) const { return immobile; }
00176 void set_m(const Real m_) { m = m_; }
00177 void set_mc(const ColumnVector & mc_);
00178 void set_r(const ColumnVector & r_);
00179 void set_Im(const Real Im_) { Im = Im_; }
00180 void set_B(const Real B_) { B = B_; }
00181 void set_Cf(const Real Cf_) { Cf = Cf_; }
00182 void set_I(const Matrix & I);
00183 void set_immobile(bool im) { immobile=im; }
00184
00185 Matrix R;
00186 Real qp,
00187 qpp;
00188
00189 private:
00190 int joint_type;
00191 Real theta,
00192 d,
00193 a,
00194 alpha,
00195 theta_min,
00196 theta_max,
00197 joint_offset;
00198 bool DH,
00199 min_para;
00200 ColumnVector r,
00201 p;
00202 Real m,
00203 Im,
00204 Gr,
00205 B,
00206 Cf;
00207 ColumnVector mc;
00208 Matrix I;
00209 bool immobile;
00210 };
00211
00212
00213
00214
00215
00216 class Robot_basic {
00217 friend class Robot;
00218 friend class mRobot;
00219 friend class mRobot_min_para;
00220 friend class Robotgl;
00221 friend class mRobotgl;
00222 public:
00223 explicit Robot_basic(const int ndof = 1, const bool dh_parameter = false,
00224 const bool min_inertial_para = false);
00225 explicit Robot_basic(const Matrix & initrobot_motor, const bool dh_parameter = false,
00226 const bool min_inertial_para = false);
00227 explicit Robot_basic(const Matrix & initrobot, const Matrix & initmotor,
00228 const bool dh_parameter = false, const bool min_inertial_para = false);
00229 explicit Robot_basic(const Config & robData, const std::string & robotName,
00230 const bool dh_parameter = false, const bool min_inertial_para = false);
00231 Robot_basic(const Robot_basic & x);
00232 virtual ~Robot_basic();
00233 Robot_basic & operator=(const Robot_basic & x);
00234
00235 Real get_q(const int i) const {
00236 if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00237 return links[i].get_q();
00238 }
00239 bool get_DH()const { return links[1].DH; }
00240 int get_dof()const { return dof; }
00241 int get_available_dof()const { return get_available_dof(dof); }
00242 int get_available_dof(const int endlink)const;
00243 int get_fix()const { return fix; }
00244 ReturnMatrix get_q(void)const;
00245 ReturnMatrix get_qp(void)const;
00246 ReturnMatrix get_qpp(void)const;
00247 ReturnMatrix get_available_q(void)const { return get_available_q(dof); }
00248 ReturnMatrix get_available_qp(void)const { return get_available_qp(dof); }
00249 ReturnMatrix get_available_qpp(void)const { return get_available_qpp(dof); }
00250 ReturnMatrix get_available_q(const int endlink)const;
00251 ReturnMatrix get_available_qp(const int endlink)const;
00252 ReturnMatrix get_available_qpp(const int endlink)const;
00253 void set_q(const ColumnVector & q);
00254 void set_q(const Matrix & q);
00255 void set_q(const Real q, const int i) {
00256 if(i < 1 || i > dof) error("i must be 1 <= i <= dof");
00257 links[i].transform(q);
00258 }
00259 void set_qp(const ColumnVector & qp);
00260 void set_qpp(const ColumnVector & qpp);
00261 void kine(Matrix & Rot, ColumnVector & pos)const;
00262 void kine(Matrix & Rot, ColumnVector & pos, const int j)const;
00263 ReturnMatrix kine(void)const;
00264 ReturnMatrix kine(const int j)const;
00265 ReturnMatrix kine_pd(const int ref=0)const;
00266 virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00267 ColumnVector & pos_dot, const int ref)const=0;
00268 virtual void robotType_inv_kin() = 0;
00269 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00270 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, bool & converge) {return inv_kin(Tobj,mj,dof,converge);}
00271 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00272 virtual ReturnMatrix inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge);
00273 virtual ReturnMatrix inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge);
00274 virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge) = 0;
00275 virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge) = 0;
00276 virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); }
00277 virtual ReturnMatrix jacobian(const int endlink, const int ref)const = 0;
00278 virtual ReturnMatrix jacobian_dot(const int ref=0)const = 0;
00279 ReturnMatrix jacobian_DLS_inv(const double eps, const double lambda_max, const int ref=0)const;
00280 virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i) { dTdqi(dRot,dpos,i,dof); }
00281 virtual ReturnMatrix dTdqi(const int i) { return dTdqi(i,dof); }
00282 virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink) = 0;
00283 virtual ReturnMatrix dTdqi(const int i, const int endlink) = 0;
00284 ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
00285 const ColumnVector & tau);
00286 ReturnMatrix acceleration(const ColumnVector & q, const ColumnVector & qp,
00287 const ColumnVector & tau, const ColumnVector & Fext,
00288 const ColumnVector & Next);
00289 ReturnMatrix inertia(const ColumnVector & q);
00290 virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp) = 0;
00291 virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00292 const ColumnVector & qpp) = 0;
00293 virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00294 const ColumnVector & qpp,
00295 const ColumnVector & Fext_,
00296 const ColumnVector & Next_) = 0;
00297 virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00298 const ColumnVector & qpp, const ColumnVector & dq,
00299 const ColumnVector & dqp, const ColumnVector & dqpp,
00300 ColumnVector & torque, ColumnVector & dtorque) =0;
00301 virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00302 const ColumnVector & qpp, const ColumnVector & dq,
00303 ColumnVector & torque, ColumnVector & dtorque) =0;
00304 virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00305 const ColumnVector & dqp, ColumnVector & torque,
00306 ColumnVector & dtorque) =0;
00307 ReturnMatrix dtau_dq(const ColumnVector & q, const ColumnVector & qp,
00308 const ColumnVector & qpp);
00309 ReturnMatrix dtau_dqp(const ColumnVector & q, const ColumnVector & qp);
00310 virtual ReturnMatrix G() = 0;
00311 virtual ReturnMatrix C(const ColumnVector & qp) = 0;
00312 void error(const std::string & msg1) const;
00313
00314
00315 static Real limit_range(Real x, Real min, Real max) { return x>max?max:(x<min?min:x); }
00316
00317
00318 static Real limit_angle(Real x, Real min, Real max) {
00319 if(x<min || x>max) {
00320 Real mn_dist=fabs(normalize_angle(min-x));
00321 Real mx_dist=fabs(normalize_angle(max-x));
00322 if(mn_dist<mx_dist)
00323 return min;
00324 else
00325 return max;
00326 } else
00327 return x;
00328 }
00329
00330
00331 static Real normalize_angle(Real t) { return t-rint(t/(2*M_PI))*(2*M_PI); }
00332
00333
00334 static Real homog_norm(const ColumnVector& v) {
00335 Real tot=0;
00336 for(int i=1; i<=v.nrows()-1; i++)
00337 tot+=v(i)*v(i);
00338 return sqrt(tot)/v(v.nrows());
00339 }
00340
00341 void convertFrame(Matrix& R, ColumnVector& p, int cur, int dest) const;
00342 ReturnMatrix convertFrame(int cur, int dest) const;
00343 void convertLinkToFrame(Matrix& R, ColumnVector& p, int cur, int dest) const;
00344 ReturnMatrix convertLinkToFrame(int cur, int dest) const;
00345 void convertFrameToLink(Matrix& R, ColumnVector& p, int cur, int dest) const;
00346 ReturnMatrix convertFrameToLink(int cur, int dest) const;
00347 void convertLink(Matrix& R, ColumnVector& p, int cur, int dest) const;
00348 ReturnMatrix convertLink(int cur, int dest) const;
00349
00350 ColumnVector *w, *wp, *vp, *a, *f, *f_nv, *n, *n_nv, *F, *N, *p, *pp,
00351 *dw, *dwp, *dvp, *da, *df, *dn, *dF, *dN, *dp,
00352 z0,
00353 gravity;
00354 Matrix *R;
00355 Link *links;
00356
00357 private:
00358
00359 enum EnumRobotType
00360 {
00361 DEFAULT = 0,
00362 RHINO = 1,
00363 PUMA = 2,
00364 ERS_LEG,
00365 ERS2XX_HEAD,
00366 ERS7_HEAD,
00367 PANTILT,
00368 GOOSENECK,
00369 CRABARM,
00370 };
00371 EnumRobotType robotType;
00372 int dof,
00373 fix;
00374
00375 static ReturnMatrix pack4x4(const Matrix& R, const ColumnVector& p) {
00376 Matrix T(4,4);
00377 T.SubMatrix(1,3,1,3)=R;
00378 T.SubMatrix(1,3,4,4)=p;
00379 T(4,1)=0; T(4,2)=0; T(4,3)=0; T(4,4)=1;
00380 T.Release(); return T;
00381 }
00382 };
00383
00384
00385
00386
00387
00388 class Robot : public Robot_basic
00389 {
00390 public:
00391 explicit Robot(const int ndof=1);
00392 explicit Robot(const Matrix & initrobot);
00393 explicit Robot(const Matrix & initrobot, const Matrix & initmotor);
00394 Robot(const Robot & x);
00395 explicit Robot(const std::string & filename, const std::string & robotName);
00396 explicit Robot(const Config & robData, const std::string & robotName);
00397 ~Robot(){}
00398 Robot & operator=(const Robot & x);
00399 virtual void robotType_inv_kin();
00400 virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00401 ColumnVector & pos_dot, const int ref)const;
00402 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00403 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, bool & converge) {return inv_kin(Tobj,mj,dof,converge);}
00404 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00405 virtual ReturnMatrix inv_kin_pos(const ColumnVector & Pobj, const int mj, const int endlink, const ColumnVector& Plink, bool & converge);
00406 virtual ReturnMatrix inv_kin_orientation(const Matrix & Robj, const int mj, const int endlink, bool & converge);
00407 virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00408 virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00409
00410 virtual ReturnMatrix inv_kin_ers_pos(const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool & converge);
00411 virtual ReturnMatrix inv_kin_pan_tilt(const ColumnVector & Pobj, bool & converge);
00412 virtual ReturnMatrix inv_kin_goose_neck(const Matrix & Tobj);
00413 virtual ReturnMatrix inv_kin_goose_neck_pos(const ColumnVector & Pobj);
00414 virtual ReturnMatrix goose_neck_angles(const ColumnVector & Pobj, Real phi);
00415
00416 virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); }
00417 virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00418 virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00419 virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i) { dTdqi(dRot,dpos,i,dof); }
00420 virtual ReturnMatrix dTdqi(const int i) { return dTdqi(i,dof); }
00421 virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00422 virtual ReturnMatrix dTdqi(const int i, const int endlink);
00423 virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00424 const ColumnVector & qpp);
00425 virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00426 const ColumnVector & qpp,
00427 const ColumnVector & Fext_,
00428 const ColumnVector & Next_);
00429 virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00430 virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00431 const ColumnVector & qpp, const ColumnVector & dq,
00432 const ColumnVector & dqp, const ColumnVector & dqpp,
00433 ColumnVector & ltorque, ColumnVector & dtorque);
00434 virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00435 const ColumnVector & qpp, const ColumnVector & dq,
00436 ColumnVector & torque, ColumnVector & dtorque);
00437 virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00438 const ColumnVector & dqp, ColumnVector & torque,
00439 ColumnVector & dtorque);
00440 virtual ReturnMatrix G();
00441 virtual ReturnMatrix C(const ColumnVector & qp);
00442
00443 protected:
00444 Real computeFirstERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, Real min, Real max, bool & converge);
00445 Real computeSecondERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge);
00446 Real computeThirdERSLink(int curlink, const ColumnVector & Pobj, const int endlink, const ColumnVector& Plink, bool invert, Real min, Real max, bool & converge);
00447 };
00448
00449
00450
00451
00452
00453
00454
00455 class mRobot : public Robot_basic {
00456 public:
00457 explicit mRobot(const int ndof=1);
00458 explicit mRobot(const Matrix & initrobot_motor);
00459 explicit mRobot(const Matrix & initrobot, const Matrix & initmotor);
00460 mRobot(const mRobot & x);
00461 explicit mRobot(const std::string & filename, const std::string & robotName);
00462 explicit mRobot(const Config & robData, const std::string & robotName);
00463 ~mRobot(){}
00464 mRobot & operator=(const mRobot & x);
00465 virtual void robotType_inv_kin();
00466 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00467 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, bool & converge) {return inv_kin(Tobj,mj,dof,converge);}
00468 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00469 virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00470 virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00471 virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00472 ColumnVector & pos_dot, const int ref)const;
00473 virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); }
00474 virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00475 virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00476 virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i) { dTdqi(dRot,dpos,i,dof); }
00477 virtual ReturnMatrix dTdqi(const int i) { return dTdqi(i,dof); }
00478 virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00479 virtual ReturnMatrix dTdqi(const int i, const int endlink);
00480 virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00481 const ColumnVector & qpp);
00482 virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00483 const ColumnVector & qpp,
00484 const ColumnVector & Fext_,
00485 const ColumnVector & Next_);
00486 virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00487 virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00488 const ColumnVector & qpp, const ColumnVector & dq,
00489 const ColumnVector & dqp, const ColumnVector & dqpp,
00490 ColumnVector & torque, ColumnVector & dtorque);
00491 virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00492 const ColumnVector & qpp, const ColumnVector & dq,
00493 ColumnVector & torque, ColumnVector & dtorque);
00494 virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00495 const ColumnVector & dqp, ColumnVector & torque,
00496 ColumnVector & dtorque);
00497 virtual ReturnMatrix G();
00498 virtual ReturnMatrix C(const ColumnVector & qp);
00499 };
00500
00501
00502
00503
00504
00505
00506
00507 class mRobot_min_para : public Robot_basic
00508 {
00509 public:
00510 explicit mRobot_min_para(const int ndof=1);
00511 explicit mRobot_min_para(const Matrix & dhinit);
00512 explicit mRobot_min_para(const Matrix & initrobot, const Matrix & initmotor);
00513 mRobot_min_para(const mRobot_min_para & x);
00514 explicit mRobot_min_para(const std::string & filename, const std::string & robotName);
00515 explicit mRobot_min_para(const Config & robData, const std::string & robotName);
00516 ~mRobot_min_para(){}
00517 mRobot_min_para & operator=(const mRobot_min_para & x);
00518 virtual void robotType_inv_kin();
00519 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj = 0);
00520 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, bool & converge) {return inv_kin(Tobj,mj,dof,converge);}
00521 virtual ReturnMatrix inv_kin(const Matrix & Tobj, const int mj, const int endlink, bool & converge);
00522 virtual ReturnMatrix inv_kin_rhino(const Matrix & Tobj, bool & converge);
00523 virtual ReturnMatrix inv_kin_puma(const Matrix & Tobj, bool & converge);
00524 virtual void kine_pd(Matrix & Rot, ColumnVector & pos,
00525 ColumnVector & pos_dot, const int ref=0)const;
00526 virtual ReturnMatrix jacobian(const int ref=0)const { return jacobian(dof,ref); }
00527 virtual ReturnMatrix jacobian(const int endlink, const int ref)const;
00528 virtual ReturnMatrix jacobian_dot(const int ref=0)const;
00529 virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i) { dTdqi(dRot,dpos,i,dof); }
00530 virtual ReturnMatrix dTdqi(const int i) { return dTdqi(i,dof); }
00531 virtual void dTdqi(Matrix & dRot, ColumnVector & dpos, const int i, const int endlink);
00532 virtual ReturnMatrix dTdqi(const int i, const int endlink);
00533 virtual ReturnMatrix torque(const ColumnVector & q, const ColumnVector & qp,
00534 const ColumnVector & qpp);
00535 virtual ReturnMatrix torque(const ColumnVector & q,
00536 const ColumnVector & qp,
00537 const ColumnVector & qpp,
00538 const ColumnVector & Fext_,
00539 const ColumnVector & Next_);
00540 virtual ReturnMatrix torque_novelocity(const ColumnVector & qpp);
00541 virtual void delta_torque(const ColumnVector & q, const ColumnVector & qp,
00542 const ColumnVector & qpp, const ColumnVector & dq,
00543 const ColumnVector & dqp, const ColumnVector & dqpp,
00544 ColumnVector & torque, ColumnVector & dtorque);
00545 virtual void dq_torque(const ColumnVector & q, const ColumnVector & qp,
00546 const ColumnVector & qpp, const ColumnVector & dq,
00547 ColumnVector & torque, ColumnVector & dtorque);
00548 virtual void dqp_torque(const ColumnVector & q, const ColumnVector & qp,
00549 const ColumnVector & dqp, ColumnVector & torque,
00550 ColumnVector & dtorque);
00551 virtual ReturnMatrix G();
00552 virtual ReturnMatrix C(const ColumnVector & qp);
00553 };
00554
00555 void perturb_robot(Robot_basic & robot, const double f = 0.1);
00556
00557 bool Puma_DH(const Robot_basic *robot);
00558 bool Rhino_DH(const Robot_basic *robot);
00559 bool ERS_Leg_DH(const Robot_basic *robot);
00560 bool ERS2xx_Head_DH(const Robot_basic *robot);
00561 bool ERS7_Head_DH(const Robot_basic *robot);
00562 bool PanTilt_DH(const Robot_basic *robot);
00563 bool Puma_mDH(const Robot_basic *robot);
00564 bool Rhino_mDH(const Robot_basic *robot);
00565 bool Goose_Neck_DH(const Robot_basic *robot);
00566 bool Crab_Arm_DH(const Robot_basic *robot);
00567
00568 #ifdef use_namespace
00569 }
00570 #endif
00571
00572 #endif
00573