120 static const char rcsid[] =
"$Id: robot.cpp,v 1.50 2006/05/16 19:24:26 gourdeau Exp $";
129 using namespace NEWMAT;
133 Real fourbyfourident[] = {1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0,0.0,0.0,0.0,0.0,1.0};
154 const Real iGr,
const Real iB,
const Real iCf,
const bool dh,
155 const bool min_inertial_parameters,
const bool immobile_):
164 joint_offset(it_off),
166 min_para(min_inertial_parameters),
237 I(1,2) =
I(2,1) = ixy;
238 I(1,3) =
I(3,1) = ixz;
240 I(2,3) =
I(3,2) = iyz;
319 cerr <<
"Link::set_r: wrong size in input vector." << endl;
328 cerr <<
"Link::set_mc: wrong size in input vector." << endl;
340 cerr <<
"Link::set_r: wrong size in input vector." << endl;
344 const bool min_inertial_para)
361 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
363 for(i = 1; i <= dhinit.
Nrows(); i++)
366 if (i == dhinit.
Nrows())
369 error(
"Fix link can only be on the last one");
375 error(
"Number of degree of freedom must be greater or equal to 1");
381 links =
new Link[dof+fix];
408 cerr <<
"Robot_basic constructor : new ran out of memory" << endl;
413 for(i = 0; i <= dof; i++)
427 for(i = 0; i <= dof+fix; i++)
436 if(dhinit.
Ncols() == 23)
438 for(i = 1; i <= dof+fix; i++)
439 links[i] =
Link((
int) dhinit(i,1), dhinit(i,2), dhinit(i,3),
440 dhinit(i,4), dhinit(i,5), dhinit(i,6), dhinit(i,7),
441 dhinit(i,8), dhinit(i,9), dhinit(i,10), dhinit(i,11),
442 dhinit(i,12), dhinit(i,13), dhinit(i,14), dhinit(i,15),
443 dhinit(i,16), dhinit(i,17), dhinit(i,18), dhinit(i,19),
444 dhinit(i,20), dhinit(i,21), dhinit(i,22),
445 dh_parameter, min_inertial_para, dhinit(i,23));
448 error(
"Initialisation Matrix does not have 23 columns.");
452 const bool dh_parameter,
const bool min_inertial_para)
470 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
473 for(i = 1; i <= initrobot.
Nrows(); i++)
474 if(initrobot(i,1) == 2)
476 if (i == initrobot.
Nrows())
479 error(
"Fix link can only be on the last one");
485 error(
"Number of degree of freedom must be greater or equal to 1");
490 links =
new Link[dof+fix];
517 cerr <<
"Robot_basic constructor : new ran out of memory" << endl;
522 for(i = 0; i <= dof; i++)
536 for(i = 0; i <= dof+fix; i++)
547 if(initmotor.
Ncols() == 4)
549 if(initrobot.
Ncols() == 19)
551 for(i = 1; i <= dof+fix; i++)
552 links[i] =
Link((
int) initrobot(i,1), initrobot(i,2), initrobot(i,3),
553 initrobot(i,4), initrobot(i,5), initrobot(i,6),
554 initrobot(i,7), initrobot(i,8), initrobot(i,9),
555 initrobot(i,10),initrobot(i,11), initrobot(i,12),
556 initrobot(i,13),initrobot(i,14), initrobot(i,15),
557 initrobot(i,16),initrobot(i,17), initrobot(i,18),
558 initmotor(i,1), initmotor(i,2), initmotor(i,3),
559 initmotor(i,4), dh_parameter, min_inertial_para,
563 error(
"Initialisation robot Matrix does not have 19 columns.");
566 error(
"Initialisation robot motor Matrix does not have 4 columns.");
570 error(
"Initialisation robot and motor matrix does not have same numbers of Rows.");
574 const bool min_inertial_para)
590 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
596 links =
new Link[dof];
623 cerr <<
"Robot_basic constructor : new ran out of memory" << endl;
627 for(i = 0; i <= dof; i++)
641 for(i = 0; i <= dof+fix; i++)
662 links =
new Link[dof+fix];
689 cerr <<
"Robot_basic constructor : new ran out of memory" << endl;
693 for(i = 0; i <= dof; i++)
707 for(i = 0; i <= dof+fix; i++)
716 for(i = 1; i <= dof+fix; i++)
717 links[i] = x.
links[i];
721 const bool dh_parameter,
const bool min_inertial_para)
737 z0(1) = z0(2) = 0.0; z0(3) = 1.0;
740 ifstream inconffile(filename.c_str(), std::ios::in);
743 error(
"Robot_basic::Robot_basic: unable to read input config file.");
746 bool motor =
false, angle_in_degree =
false;
748 if(robData.
select(robotName,
"dof", dof))
751 error(
"Robot_basic::Robot_basic: dof is less then one.");
754 error(
"Robot_basic::Robot_basic: error in extracting dof from conf file.");
757 robData.
select(robotName,
"Fix", fix);
758 robData.
select(robotName,
"Motor", motor);
759 robData.
select(robotName,
"angle_in_degree", angle_in_degree);
763 links =
new Link[dof+fix];
790 cerr <<
"Robot_basic constructor : new ran out of memory" << endl;
794 for(i = 0; i <= dof; i++)
808 for(i = 0; i <= dof+fix; i++)
817 for(
int j = 1; j <= dof+fix; j++)
821 m=0, cx=0, cy=0, cz=0, Ixx=0, Ixy=0, Ixz=0, Iyy=0, Iyz=0, Izz=0,
825 string robotName_link;
827 ostr << robotName <<
"_LINK" << j;
828 robotName_link = ostr.str();
830 robData.
select(robotName_link,
"joint_type", joint_type);
831 robData.
select(robotName_link,
"theta", theta);
832 robData.
select(robotName_link,
"d",
d);
833 robData.
select(robotName_link,
"a",
a);
838 robData.
select(robotName_link,
"m",
m);
839 robData.
select(robotName_link,
"cx", cx);
840 robData.
select(robotName_link,
"cy", cy);
841 robData.
select(robotName_link,
"cz", cz);
842 robData.
select(robotName_link,
"Ixx", Ixx);
843 robData.
select(robotName_link,
"Ixy", Ixy);
844 robData.
select(robotName_link,
"Ixz", Ixz);
845 robData.
select(robotName_link,
"Iyy", Iyy);
846 robData.
select(robotName_link,
"Iyz", Iyz);
847 robData.
select(robotName_link,
"Izz", Izz);
848 robData.
select(robotName_link,
"immobile", immobile);
861 robData.
select(robotName_link,
"Im",
Im);
862 robData.
select(robotName_link,
"Gr",
Gr);
863 robData.
select(robotName_link,
"B",
B);
864 robData.
select(robotName_link,
"Cf",
Cf);
868 joint_offset,
m, cx, cy, cz, Ixx, Ixy, Ixz, Iyy, Iyz,
869 Izz,
Im,
Gr,
B,
Cf, dh_parameter, min_inertial_para);
870 links[j].set_immobile(immobile);
874 links[dof+fix] =
Link(2, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0,
875 0, 0, 0, 0, 0, 0, 0, 0, 0, dh_parameter, min_inertial_para);
895 if ( (dof != x.
dof) || (fix != x.
fix) )
928 links =
new Link[dof+fix];
955 cerr <<
"Robot_basic::operator= : new ran out of memory" << endl;
959 for(i = 0; i <= dof; i++)
973 for(i = 0; i <= dof+fix; i++)
981 for(i = 1; i <= dof+fix; i++)
982 links[i] = x.
links[i];
990 cerr << endl <<
"Robot error: " << msg1.c_str() << endl;
998 for(
int i=1; i<=endlink; i++)
1009 for(
int i = 1; i <= dof; i++)
1010 q(i) = links[i].get_q();
1019 for(
int i = 1; i <= dof; i++)
1020 qp(i) = links[i].qp;
1029 for(
int i = 1; i <= dof; i++)
1030 qpp(i) = links[i].qpp;
1040 for(
int i = 1; i <= endlink; i++)
1042 q(j++) = links[i].get_q();
1052 for(
int i = 1; i <= endlink; i++)
1054 qp(j++) = links[i].qp;
1064 for(
int i = 1; i <= endlink; i++)
1066 qpp(j) = links[i].qpp;
1079 int adof=get_available_dof();
1081 for(
int i = 1; i <= dof; i++)
1083 links[i].transform(q(i,1));
1086 p[i](1) = links[i].
get_a();
1087 p[i](2) = links[i].
get_d() * links[i].R(3,2);
1088 p[i](3) = links[i].
get_d() * links[i].R(3,3);
1093 }
else if(q.
Nrows() == 1 && q.
Ncols() == dof) {
1094 for(
int i = 1; i <= dof; i++)
1096 links[i].transform(q(1,i));
1099 p[i](1) = links[i].
get_a();
1100 p[i](2) = links[i].
get_d() * links[i].R(3,2);
1101 p[i](3) = links[i].
get_d() * links[i].R(3,3);
1106 }
else if(q.
Nrows() == adof && q.
Ncols() == 1) {
1108 for(
int i = 1; i <= dof; i++)
1110 links[i].transform(q(j++,1));
1113 p[i](1) = links[i].
get_a();
1114 p[i](2) = links[i].
get_d() * links[i].R(3,2);
1115 p[i](3) = links[i].
get_d() * links[i].R(3,3);
1120 }
else if(q.
Nrows() == 1 && q.
Ncols() == adof) {
1122 for(
int i = 1; i <= dof; i++)
1124 links[i].transform(q(1,j++));
1127 p[i](1) = links[i].
get_a();
1128 p[i](2) = links[i].
get_d() * links[i].R(3,2);
1129 p[i](3) = links[i].
get_d() * links[i].R(3,3);
1134 }
else error(
"q has the wrong dimension in set_q()");
1146 if(q.
Nrows() == dof) {
1147 for(
int i = 1; i <= dof; i++)
1149 links[i].transform(q(i));
1152 p[i](1) = links[i].
get_a();
1153 p[i](2) = links[i].
get_d() * links[i].R(3,2);
1154 p[i](3) = links[i].
get_d() * links[i].R(3,3);
1159 }
else if(q.
Nrows() == get_available_dof()) {
1161 for(
int i = 1; i <= dof; i++)
1163 links[i].transform(q(j++));
1166 p[i](1) = links[i].
get_a();
1167 p[i](2) = links[i].
get_d() * links[i].R(3,2);
1168 p[i](3) = links[i].
get_d() * links[i].R(3,3);
1173 }
else error(
"q has the wrong dimension in set_q()");
1179 if(qp.
Nrows() == dof)
1180 for(
int i = 1; i <= dof; i++)
1181 links[i].qp =
qp(i);
1182 else if(qp.
Nrows() == get_available_dof()) {
1184 for(
int i = 1; i <= dof; i++)
1186 links[i].qp =
qp(j++);
1188 error(
"qp has the wrong dimension in set_qp()");
1194 if(qpp.
Nrows() == dof)
1195 for(
int i = 1; i <= dof; i++)
1196 links[i].qpp =
qpp(i);
1198 error(
"qpp has the wrong dimension in set_qpp()");
1270 Robot::Robot(
const string & filename,
const string & robotName):
1339 mRobot::mRobot(
const string & filename,
const string & robotName):
1456 if( (f < 0) || (f > 1) )
1458 cerr <<
"perturb_robot: f is not between 0 and 1" << endl;
1466 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1468 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1470 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1472 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1476 fact = (2.0*rand()/RAND_MAX - 1)*f + 1;
1495 for (
int j = 1; j <= 5; j++)
1528 for (
int j = 1; j <= 6; j++)
1561 for (
int j = 1; j <= 6; j++)
1595 for (
int j = 1; j <= 5; j++)
1627 for (
int j = 1; j <= 6; j++)
1661 for (
int j = 1; j <= 6; j++)
1682 #ifdef use_namespace mRobot_min_para(const int ndof=1)
Constructor.
void set_B(const Real B_)
Set B.
Real get_q(void) const
Return joint position (theta if joint type is rotoide, d otherwise).
bool Schilling_DH(const Robot_basic &robot)
Return true if the robot is like a Schilling on DH notation.
int fix
Virtual link, used with modified DH notation.
ReturnMatrix get_qp(void) const
Return the joint velocity vector.
int get_dof() const
Return dof.
bool Puma_mDH(const Robot_basic &robot)
Return true if the robot is like a Puma on modified DH notation.
short read_conf(std::ifstream &inconffile)
Read a configuration file.
Link * links
Pointer on Link cclass.
mRobot(const int ndof=1)
Constructor.
ReturnMatrix get_qpp(void) const
Return the joint acceleration vector.
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, const bool immobile=false)
Constructor.
bool immobile
true if the joint is to be considered locked - ignored for inverse kinematics, but can still be reass...
ReturnMatrix get_available_qpp(void) const
Return the joint acceleration vector of available (non-immobile) joints.
Real alpha
alpha DH parameter.
Robots class definitions.
Robot_basic(const int ndof=1, const bool dh_parameter=false, const bool min_inertial_para=false)
Constructor.
void set_mc(const ColumnVector &mc_)
Set mc.
bool Schilling_mDH(const Robot_basic &robot)
Return true if the robot is like a Schilling on modified DH notation.
Real theta_max
Max joint angle.
Header file for Config class definitions.
Real get_Cf(void) const
Return Cf.
Real get_d(void) const
Return d.
Matrix I
Inertia matrix w.r.t. center of mass and link coordinate system orientation.
bool isZero(const double x)
ReturnMatrix get_q(void) const
Return the joint position vector.
bool select(const std::string §ion, const std::string ¶meter, T &value) const
Get a parameter data, of a certain section, into the string value.
Handle configuration files.
EnumRobotType robotType
Robot type.
int get_available_dof() const
Counts number of currently non-immobile links.
virtual void robotType_inv_kin()
Identify inverse kinematics familly.
bool Rhino_mDH(const Robot_basic &robot)
Return true if the robot is like a Rhino on modified DH notation.
ReturnMatrix get_I(void) const
Return I.
void set_qpp(const ColumnVector &qpp)
Set the joint acceleration vector.
void set_I(const Matrix &I)
Set I.
void set_Im(const Real Im_)
Set Im.
Real B
Viscous coefficient.
void cleanUpPointers()
Free all vectors and matrix memory.
Robot(const int ndof=1)
Constructor.
Real get_B(void) const
Return B.
void set_q(const ColumnVector &q)
Set the joint position vector.
Virtual base robot class.
static const char rcsid[]
RCS/CVS version.
void set_m(const Real m_)
Set m.
Modified DH notation robot class.
Real get_alpha(void) const
Return alpha.
int get_fix() const
Return fix.
Robot_basic & operator=(const Robot_basic &x)
Overload = operator.
Real theta_min
Min joint angle.
Matrix R
Orientation matrix of actual link w.r.t to previous link.
void transform(const Real q)
Set the rotation matrix R and the vector p.
Real get_Im(void) const
Return Im.
The usual rectangular matrix.
ReturnMatrix get_available_q(void) const
Return the joint position vector of available (non-immobile) joints.
virtual void robotType_inv_kin()
Identify inverse kinematics familly.
FloatVector FloatVector * a
Real Cf
Coulomb fiction coefficient.
void set_Cf(const Real Cf_)
Set Cf.
Modified DH notation and minimal inertial parameters robot class.
ColumnVector gravity
Gravity vector.
ReturnMatrix get_available_qp(void) const
Return the joint velocity vector of available (non-immobile) joints.
void set_qp(const ColumnVector &qp)
Set the joint velocity vector.
void set_r(const ColumnVector &r_)
Set r.
FloatVector FloatVector FloatVector * alpha
Real joint_offset
Offset in joint angle (rotoide and prismatic).
Real get_m(void) const
Return m.
virtual ~Robot_basic()
Destructor.
Real theta
theta DH parameter.
ColumnVector mc
Mass center of gravity (used if min_para = true).
ColumnVector r
Position of center of mass w.r.t. link coordinate system (min_para=F).
int dof
Degree of freedom.
bool Rhino_DH(const Robot_basic &robot)
Return true if the robot is like a Rhino on DH notation.
int get_joint_type(void) const
Return the joint type.
Real get_a(void) const
Return a.
void perturb_robot(Robot_basic &robot, const double f)
Modify a robot.
int joint_type
Joint type.
Real threebythreeident[]
Used to initialize a matrix.
bool min_para
Minimum inertial parameter.
Real fourbyfourident[]
Used to initialize a matrix.
bool Puma_DH(const Robot_basic &robot)
Return true if the robot is like a Puma on DH notation.
Real qpp
Joint acceleration.
void error(const std::string &msg1) const
Print the message msg1 on the console.
ColumnVector z0
Axis vector at each joint.
double deg2rad(const double angle_deg)
virtual void robotType_inv_kin()
Identify inverse kinematics familly.
ColumnVector p
Position vector of actual link w.r.t to previous link.
bool DH
DH notation(true) or DH modified notation.