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 static const char rcsid[] = "$Id: delta_t.cpp,v 1.5 2004/09/03 15:56:33 ejt Exp $";
00056
00057 #include "robot.h"
00058
00059 #ifdef use_namespace
00060 namespace ROBOOP {
00061 using namespace NEWMAT;
00062 #endif
00063
00064
00065 void Robot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00066 const ColumnVector & qpp, const ColumnVector & dq,
00067 const ColumnVector & dqp, const ColumnVector & dqpp,
00068 ColumnVector & ltorque, ColumnVector & dtorque)
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
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
00149
00150
00151
00152
00153
00154
00155
00156 {
00157 int i;
00158 Matrix Rt, temp;
00159 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00160 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00161 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00162 if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00163 if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00164 if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00165 ltorque = ColumnVector(dof);
00166 dtorque = ColumnVector(dof);
00167 set_q(q);
00168
00169 vp[0] = gravity;
00170 ColumnVector z0(3);
00171 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00172 Matrix Q(3,3);
00173 Q = 0.0;
00174 Q(1,2) = -1.0;
00175 Q(2,1) = 1.0;
00176 for(i = 1; i <= dof; i++)
00177 {
00178 Rt = links[i].R.t();
00179 p[i] = ColumnVector(3);
00180 p[i](1) = links[i].get_a();
00181 p[i](2) = links[i].get_d() * Rt(2,3);
00182 p[i](3) = links[i].get_d() * Rt(3,3);
00183 if(links[i].get_joint_type() != 0)
00184 {
00185 dp[i] = ColumnVector(3);
00186 dp[i](1) = 0.0;
00187 dp[i](2) = Rt(2,3);
00188 dp[i](3) = Rt(3,3);
00189 }
00190 if(links[i].get_joint_type() == 0)
00191 {
00192 w[i] = Rt*(w[i-1] + z0*qp(i));
00193 dw[i] = Rt*(dw[i-1] + z0*dqp(i)
00194 - Q*(w[i-1] + z0*qp(i))*dq(i));
00195 wp[i] = Rt*(wp[i-1] + z0*qpp(i)
00196 + CrossProduct(w[i-1],z0*qp(i)));
00197 dwp[i] = Rt*(dwp[i-1] + z0*dqpp(i)
00198 + CrossProduct(dw[i-1],z0*qp(i))
00199 + CrossProduct(w[i-1],z0*dqp(i))
00200 - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
00201 *dq(i));
00202 vp[i] = CrossProduct(wp[i],p[i])
00203 + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00204 + Rt*(vp[i-1]);
00205 dvp[i] = CrossProduct(dwp[i],p[i])
00206 + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00207 + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00208 + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
00209 }
00210 else
00211 {
00212 w[i] = Rt*w[i-1];
00213 dw[i] = Rt*dw[i-1];
00214 wp[i] = Rt*wp[i-1];
00215 dwp[i] = Rt*dwp[i-1];
00216 vp[i] = Rt*(vp[i-1] + z0*qpp(i)
00217 + 2.0*CrossProduct(w[i-1],z0*qp(i)))
00218 + CrossProduct(wp[i],p[i])
00219 + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00220 dvp[i] = Rt*(dvp[i-1] + z0*dqpp(i)
00221 + 2.0*(CrossProduct(dw[i-1],z0*qp(i))
00222 + CrossProduct(w[i-1],z0*dqp(i))))
00223 + CrossProduct(dwp[i],p[i])
00224 + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00225 + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00226 + (CrossProduct(wp[i],dp[i])
00227 + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
00228 *dq(i);
00229 }
00230 a[i] = CrossProduct(wp[i],links[i].r)
00231 + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00232 + vp[i];
00233 da[i] = CrossProduct(dwp[i],links[i].r)
00234 + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00235 + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00236 + dvp[i];
00237 }
00238
00239 for(i = dof; i >= 1; i--)
00240 {
00241 F[i] = a[i] * links[i].m;
00242 dF[i] = da[i] * links[i].m;
00243 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00244 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00245 + CrossProduct(w[i],links[i].I*dw[i]);
00246 if(i == dof)
00247 {
00248 f[i] = F[i];
00249 df[i] = dF[i];
00250 n[i] = CrossProduct(p[i],f[i])
00251 + CrossProduct(links[i].r,F[i]) + N[i];
00252 dn[i] = CrossProduct(p[i],df[i])
00253 + CrossProduct(links[i].r,dF[i]) + dN[i];
00254 if(links[i].get_joint_type() != 0)
00255 dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00256 }
00257 else
00258 {
00259 f[i] = links[i+1].R*f[i+1] + F[i];
00260 df[i] = links[i+1].R*df[i+1] + dF[i];
00261 if(links[i].get_joint_type() == 0)
00262 df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
00263 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00264 + CrossProduct(links[i].r,F[i]) + N[i];
00265 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
00266 + CrossProduct(links[i].r,dF[i]) + dN[i];
00267 if(links[i].get_joint_type() == 0)
00268 dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
00269 else
00270 dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00271 }
00272
00273 if(links[i].get_joint_type() == 0)
00274 {
00275 temp = ((z0.t()*links[i].R)*n[i]);
00276 ltorque(i) = temp(1,1);
00277 temp = ((z0.t()*links[i].R)*dn[i]);
00278 dtorque(i) = temp(1,1);
00279 }
00280 else
00281 {
00282 temp = ((z0.t()*links[i].R)*f[i]);
00283 ltorque(i) = temp(1,1);
00284 temp = ((z0.t()*links[i].R)*df[i]);
00285 dtorque(i) = temp(1,1);
00286 }
00287 }
00288 }
00289
00290
00291 void mRobot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00292 const ColumnVector & qpp, const ColumnVector & dq,
00293 const ColumnVector & dqp, const ColumnVector & dqpp,
00294 ColumnVector & ltorque, ColumnVector & dtorque)
00295
00296
00297
00298
00299
00300
00301
00302
00303
00304
00305
00306
00307
00308
00309
00310
00311
00312
00313
00314
00315
00316
00317
00318
00319
00320
00321
00322
00323
00324
00325
00326
00327
00328
00329
00330
00331
00332
00333
00334
00335
00336
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366
00367
00368
00369
00370
00371
00372
00373
00374
00375
00376
00377
00378
00379
00380
00381
00382
00383
00384 {
00385 int i;
00386 Matrix Rt, temp;
00387 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00388 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00389 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00390 if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00391 if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00392 if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00393 ltorque = ColumnVector(dof);
00394 dtorque = ColumnVector(dof);
00395 set_q(q);
00396
00397 vp[0] = gravity;
00398 ColumnVector z0(3);
00399 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00400 Matrix Q(3,3);
00401 Q = 0.0;
00402 Q(1,2) = -1.0;
00403 Q(2,1) = 1.0;
00404 for(i = 1; i <= dof; i++)
00405 {
00406 Rt = links[i].R.t();
00407 p[i] = links[i].p;
00408 if(links[i].get_joint_type() != 0)
00409 {
00410 dp[i] = ColumnVector(3);
00411 dp[i](1) = 0.0;
00412 dp[i](2) = Rt(2,3);
00413 dp[i](3) = Rt(3,3);
00414 }
00415 if(links[i].get_joint_type() == 0)
00416 {
00417 w[i] = Rt*w[i-1] + z0*qp(i);
00418 dw[i] = Rt*dw[i-1] + z0*dqp(i)
00419 - Q*Rt*w[i-1]*dq(i);
00420 wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00421 + z0*qpp(i);
00422 dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00423 + CrossProduct(Rt*w[i-1],z0*dqp(i))
00424 - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
00425 + z0*dqpp(i);
00426 vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00427 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00428 + vp[i-1]);
00429 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00430 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00431 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00432 - Q*Rt*(CrossProduct(wp[i-1],p[i])
00433 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00434 }
00435 else
00436 {
00437 w[i] = Rt*w[i-1];
00438 dw[i] = Rt*dw[i-1];
00439 wp[i] = Rt*wp[i-1];
00440 dwp[i] = Rt*dwp[i-1];
00441 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00442 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00443 + z0*qpp(i) + 2.0*CrossProduct(w[i],z0*qp(i));
00444
00445 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00446 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00447 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00448 + (CrossProduct(wp[i-1],dp[i])
00449 + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00450 + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
00451 + z0*dqpp(i);
00452 }
00453 a[i] = CrossProduct(wp[i],links[i].r)
00454 + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00455 + vp[i];
00456 da[i] = CrossProduct(dwp[i],links[i].r)
00457 + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00458 + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00459 + dvp[i];
00460 }
00461
00462 for(i = dof; i >= 1; i--) {
00463 F[i] = a[i] * links[i].m;
00464 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00465 dF[i] = da[i] * links[i].m;
00466 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00467 + CrossProduct(w[i],links[i].I*dw[i]);
00468
00469 if(i == dof)
00470 {
00471 f[i] = F[i];
00472 df[i] = dF[i];
00473 n[i] = CrossProduct(links[i].r,F[i]) + N[i];
00474 dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
00475 }
00476 else
00477 {
00478 f[i] = links[i+1].R*f[i+1] + F[i];
00479 df[i] = links[i+1].R*df[i+1] + dF[i];
00480 if(links[i+1].get_joint_type() == 0)
00481 df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00482
00483 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00484 + CrossProduct(links[i].r,F[i]) + N[i];
00485 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00486 + CrossProduct(links[i].r,dF[i]) + dN[i];
00487 if(links[i+1].get_joint_type() == 0)
00488 dn[i] += (links[i+1].R*Q*n[i+1] +
00489 CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00490 else
00491 dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00492 }
00493
00494 if(links[i].get_joint_type() == 0)
00495 {
00496 temp = z0.t()*n[i];
00497 ltorque(i) = temp(1,1);
00498 temp = z0.t()*dn[i];
00499 dtorque(i) = temp(1,1);
00500 }
00501 else
00502 {
00503 temp = z0.t()*f[i];
00504 ltorque(i) = temp(1,1);
00505 temp = z0.t()*df[i];
00506 dtorque(i) = temp(1,1);
00507 }
00508 }
00509 }
00510
00511 void mRobot_min_para::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00512 const ColumnVector & qpp, const ColumnVector & dq,
00513 const ColumnVector & dqp, const ColumnVector & dqpp,
00514 ColumnVector & ltorque, ColumnVector & dtorque)
00515
00516
00517
00518
00519
00520 {
00521 int i;
00522 Matrix Rt, temp;
00523 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00524 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00525 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00526 if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00527 if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00528 if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00529 ltorque = ColumnVector(dof);
00530 dtorque = ColumnVector(dof);
00531 set_q(q);
00532
00533 vp[0] = gravity;
00534 ColumnVector z0(3);
00535 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00536 Matrix Q(3,3);
00537 Q = 0.0;
00538 Q(1,2) = -1.0;
00539 Q(2,1) = 1.0;
00540 for(i = 1; i <= dof; i++)
00541 {
00542 Rt = links[i].R.t();
00543 p[i] = links[i].p;
00544 if(links[i].get_joint_type() != 0)
00545 {
00546 dp[i] = ColumnVector(3);
00547 dp[i](1) = 0.0;
00548 dp[i](2) = Rt(2,3);
00549 dp[i](3) = Rt(3,3);
00550 }
00551 if(links[i].get_joint_type() == 0)
00552 {
00553 w[i] = Rt*w[i-1] + z0*qp(i);
00554 dw[i] = Rt*dw[i-1] + z0*dqp(i)
00555 - Q*Rt*w[i-1]*dq(i);
00556 wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00557 + z0*qpp(i);
00558 dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00559 + CrossProduct(Rt*w[i-1],z0*dqp(i))
00560 - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
00561 + z0*dqpp(i);
00562 vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00563 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00564 + vp[i-1]);
00565 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00566 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00567 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00568 - Q*Rt*(CrossProduct(wp[i-1],p[i])
00569 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00570 }
00571 else
00572 {
00573 w[i] = Rt*w[i-1];
00574 dw[i] = Rt*dw[i-1];
00575 wp[i] = Rt*wp[i-1];
00576 dwp[i] = Rt*dwp[i-1];
00577 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00578 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00579 + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00580 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00581 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00582 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00583 + (CrossProduct(wp[i-1],dp[i])
00584 + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00585 + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
00586 + z0*dqpp(i);
00587 }
00588 }
00589
00590 for(i = dof; i >= 1; i--) {
00591 F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00592 CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00593 dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
00594 + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
00595 + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
00596 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
00597 - CrossProduct(vp[i], links[i].mc);
00598 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00599 + CrossProduct(w[i],links[i].I*dw[i])
00600 - CrossProduct(dvp[i],links[i].mc);
00601
00602 if(i == dof)
00603 {
00604 f[i] = F[i];
00605 df[i] = dF[i];
00606 n[i] = N[i];
00607 dn[i] = dN[i];
00608 }
00609 else
00610 {
00611 f[i] = links[i+1].R*f[i+1] + F[i];
00612 df[i] = links[i+1].R*df[i+1] + dF[i];
00613 if(links[i+1].get_joint_type() == 0)
00614 df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00615
00616 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00617 + N[i];
00618 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00619 + dN[i];
00620 if(links[i+1].get_joint_type() == 0)
00621 dn[i] += (links[i+1].R*Q*n[i+1] +
00622 CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00623 else
00624 dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00625 }
00626
00627 if(links[i].get_joint_type() == 0)
00628 {
00629 temp = z0.t()*n[i];
00630 ltorque(i) = temp(1,1);
00631 temp = z0.t()*dn[i];
00632 dtorque(i) = temp(1,1);
00633 }
00634 else
00635 {
00636 temp = z0.t()*f[i];
00637 ltorque(i) = temp(1,1);
00638 temp = z0.t()*df[i];
00639 dtorque(i) = temp(1,1);
00640 }
00641 }
00642 }
00643
00644 #ifdef use_namespace
00645 }
00646 #endif