54 static const char rcsid[] =
"$Id: bench.cpp,v 1.20 2005/07/01 16:16:35 gourdeau Exp $";
62 #define clock() GetTickCount() 70 using namespace ROBOOP;
76 {1.758, 2.8, -1.015, 0.225, 0.0, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
77 1.6021, 3.07, -0.925, 0.1125, 0.1949, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
78 -1.7580, 2.8, -1.015, -0.1125, 0.1949, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
79 -1.6021, 3.07, -0.925, -0.225, 0.0, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
80 0.0, 2.8, 2.03, -0.1125, -0.1949, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
81 0.0, 3.07, 1.85, 0.1125, -0.1949, -0.228, 3.358, 0.05, 4.237, 0.1406, 10, 12.5, 0.5, 0.35, 0.0, 0.0, 0.0,
82 0.0, 0.0, -0.114, 1.001, 0.59, 0.843, 10, 0.12, 0.04, 0.5, 0.5, 0.5, 1.5, 0.5, 0.005, 5.44, 0.443};
85 {0.2, 0.3, -0.4, 0.1, -1.4, 0.1};
87 {0.25, 0.25, -0.45, 0.07, -1.7, 0.07};
89 {3.0508, 3.2324, 3.2997, 3.4560, 3.5797, 3.6935};
91 {0.2, 0.3, -0.4, 0.1, -1.4, 0.1};
93 {-10.0, -10.0, -10, -10.0, -10, -10};
99 {126.219689, 789.968672, 0.708602, 79.122963, 81.806978, -31.61797};
106 Matrix _q(6,1), qg(6,1), _dq(6,1), _ddq(6,1), _l(6,1), To(6,1),
107 V(6,1), L(6,1), tddq(6,1);
120 platt_Stewart =
Stewart(InitPlatt);
123 platt_Stewart.
set_q(_q);
124 platt_Stewart.
set_dq(_dq);
127 cout<<
"===============================\n";
128 cout<<
"Benchmarks for Stewart platform\n";
129 cout<<
"===============================\n\n";
130 cout<<
"Inverse Kinematics :\n";
132 for(i =0; i<20000; i++)
135 cout<<
"Time : "<<((end - start)/(
Real)CLOCKS_PER_SEC)*1000.0/20000<<
"\nResult : "<<L.t()<<endl;
138 cout<<
"Forward Kinematics :\n";
139 platt_Stewart.
set_q(qg);
141 for(i =0; i<100; i++){
143 platt_Stewart.
set_q(qg);}
145 cout<<
"Time : "<<((end - start)/(
Real)CLOCKS_PER_SEC)*1000.0/100<<
"\nResult : "<<L.t()<<endl;
147 platt_Stewart.
set_q(_q);
149 cout<<
"Inverse Dynamics:\n";
151 for(i =0; i<100; i++)
152 L = platt_Stewart.
Torque();
154 cout<<
"Time : "<<((end - start)/(
Real)CLOCKS_PER_SEC)*1000.0/100<<
"\nResult : "<<L.t()<<endl;
158 cout<<
"Forward Dynamics:\n";
160 for(i =0; i<100; i++)
163 cout<<
"Time : "<<((end - start)/(
Real)CLOCKS_PER_SEC)*1000.0/100<<
"\nResult : "<<L.t()<<endl;
170 cout<<
"Forward Dynamics with actuators:\nResult : "<<L.
t()<<endl;
176 {0, 0, 0, 0,
M_PI/2.0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.35, 0, 0, 0, 0, 0, 0, 0,
177 0, 0, 0, 0.4318, 0, 0, 0, 0, 17.4, -0.3638, 0.006, 0.2275, 0.13, 0, 0, 0.524, 0, 0.539, 0, 0, 0, 0, 0,
178 0, 0, 0.15005, 0.0203, -
M_PI/2.0, 0, 0, 0, 4.8, -0.0203, -0.0141, 0.07, 0.066, 0, 0, 0.086, 0, 0.0125, 0, 0, 0, 0, 0,
179 0, 0, 0.4318, 0.0,
M_PI/2.0, 0, 0, 0, 0.82, 0, 0.019, 0, 0.0018, 0, 0, 0.0013, 0, 0.0018, 0, 0, 0, 0, 0,
180 0, 0, 0, 0.0, -
M_PI/2.0, 0, 0, 0, 0.34, 0.0, 0.0, 0.0, 0.0003, 0.0, 0.0, 0.0004, 0.0, 0.0003, 0, 0, 0, 0, 0,
181 0, 0, 0, 0, 0, 0, 0, 0, 0.09, 0.0, 0.0, 0.032, 0.00015, 0.0, 0.0, 0.00015, 0.0, 0.00004, 0, 0, 0, 0, 0};
187 Matrix initrob(6,23), thomo, temp2;
192 robot =
Robot(initrob);
196 printf(
"=================================\n");
197 printf(
"Benchmarks for serial 6 dof robot\n");
198 printf(
"=================================\n\n");
200 printf(
"Begin compute Forward Kinematics\n");
202 for (i = 1; i <=
NTRY; i++) {
204 temp2 = robot.
kine();
207 printf(
"MilliSeconds %6.2f\n", ((end - start) / (
Real)CLOCKS_PER_SEC)*1000.0/
NTRY);
213 thomo = robot.
kine();
214 printf(
"Begin compute Inverse Kinematics\n");
216 for (i = 1; i <=
NTRY; i++) {
221 printf(
"MilliSeconds %6.2f\n", ((end - start) / (
Real)CLOCKS_PER_SEC)*1000.0/
NTRY);
224 printf(
"Begin compute Jacobian\n");
226 for (i = 1; i <=
NTRY; i++) {
231 printf(
"MilliSeconds %6.2f\n", ((end - start) / (
Real)CLOCKS_PER_SEC)*1000.0/
NTRY);
234 printf(
"Begin compute Torque\n");
236 for (i = 1; i <=
NTRY; i++) {
237 temp = robot.
torque(q,q,q);
240 printf(
"MilliSeconds %6.2f\n", ((end - start) / (
Real)CLOCKS_PER_SEC)*1000.0/
NTRY);
243 printf(
"Begin acceleration\n");
245 for (i = 1; i <=
NTRY; i++) {
249 printf(
"MilliSeconds %6.2f\n", ((end - start) / (
Real)CLOCKS_PER_SEC)*1000.0/
NTRY);
void set_dq(const ColumnVector _dq)
Set the platform's speed.
void set_ddq(const ColumnVector _ddq)
Set the platform's acceleration.
virtual ReturnMatrix torque(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &qpp)
Joint torque, without contact force, based on Recursive Newton-Euler formulation. ...
ReturnMatrix ForwardDyn(const ColumnVector Torque, const Real Gravity=GRAVITY)
Return the acceleration vector of the platform (ddq)
Robots class definitions.
static const char rcsid[]
RCS/CVS version.
const Real PUMA560_data[]
ReturnMatrix acceleration(const ColumnVector &q, const ColumnVector &qp, const ColumnVector &tau)
Joints acceleration without contact force.
ReturnMatrix ForwardKine(const ColumnVector guess_q, const ColumnVector l_given, const Real tolerance=0.001)
Return the position vector of the platform (vector q)
void set_q(const ColumnVector &q)
Set the joint position vector.
TransposedMatrix t() const
void set_q(const ColumnVector _q)
Set the position of the platform.
The usual rectangular matrix.
ReturnMatrix Torque(const Real Gravity=GRAVITY)
Return the torque vector of the platform.
void kine(Matrix &Rot, ColumnVector &pos) const
Direct kinematics at end effector.
ReturnMatrix InvPosKine()
Return the lenght of the links in a vector.
Stewart class definitions.
ReturnMatrix ForwardDyn_AD(const ColumnVector Command, const Real t)
Return the acceleration of the platform (Stewart platform mechanism dynamics including actuator dynam...
ReturnMatrix inv_kin(const Matrix &Tobj, const int mj=0)
Overload inv_kin function.
virtual ReturnMatrix jacobian(const int ref=0) const
Jacobian of mobile links expressed at frame ref.