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 #include "robot.h"
00055
00056 #ifdef use_namespace
00057 namespace ROBOOP {
00058 using namespace NEWMAT;
00059 #endif
00060
00061 static const char rcsid[] __UNUSED__ = "$Id: delta_t.cpp,v 1.6 2005/07/26 03:22:09 ejt Exp $";
00062
00063 void Robot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00064 const ColumnVector & qpp, const ColumnVector & dq,
00065 const ColumnVector & dqp, const ColumnVector & dqpp,
00066 ColumnVector & ltorque, ColumnVector & dtorque)
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
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 int i;
00156 Matrix Rt, temp;
00157 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00158 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00159 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00160 if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00161 if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00162 if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00163 ltorque = ColumnVector(dof);
00164 dtorque = ColumnVector(dof);
00165 set_q(q);
00166
00167 vp[0] = gravity;
00168 ColumnVector z0(3);
00169 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00170 Matrix Q(3,3);
00171 Q = 0.0;
00172 Q(1,2) = -1.0;
00173 Q(2,1) = 1.0;
00174 for(i = 1; i <= dof; i++)
00175 {
00176 Rt = links[i].R.t();
00177 p[i] = ColumnVector(3);
00178 p[i](1) = links[i].get_a();
00179 p[i](2) = links[i].get_d() * Rt(2,3);
00180 p[i](3) = links[i].get_d() * Rt(3,3);
00181 if(links[i].get_joint_type() != 0)
00182 {
00183 dp[i] = ColumnVector(3);
00184 dp[i](1) = 0.0;
00185 dp[i](2) = Rt(2,3);
00186 dp[i](3) = Rt(3,3);
00187 }
00188 if(links[i].get_joint_type() == 0)
00189 {
00190 w[i] = Rt*(w[i-1] + z0*qp(i));
00191 dw[i] = Rt*(dw[i-1] + z0*dqp(i)
00192 - Q*(w[i-1] + z0*qp(i))*dq(i));
00193 wp[i] = Rt*(wp[i-1] + z0*qpp(i)
00194 + CrossProduct(w[i-1],z0*qp(i)));
00195 dwp[i] = Rt*(dwp[i-1] + z0*dqpp(i)
00196 + CrossProduct(dw[i-1],z0*qp(i))
00197 + CrossProduct(w[i-1],z0*dqp(i))
00198 - Q*(wp[i-1]+z0*qpp(i)+CrossProduct(w[i-1],z0*qp(i)))
00199 *dq(i));
00200 vp[i] = CrossProduct(wp[i],p[i])
00201 + CrossProduct(w[i],CrossProduct(w[i],p[i]))
00202 + Rt*(vp[i-1]);
00203 dvp[i] = CrossProduct(dwp[i],p[i])
00204 + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00205 + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00206 + Rt*(dvp[i-1] - Q*vp[i-1]*dq(i));
00207 }
00208 else
00209 {
00210 w[i] = Rt*w[i-1];
00211 dw[i] = Rt*dw[i-1];
00212 wp[i] = Rt*wp[i-1];
00213 dwp[i] = Rt*dwp[i-1];
00214 vp[i] = Rt*(vp[i-1] + z0*qpp(i)
00215 + 2.0*CrossProduct(w[i-1],z0*qp(i)))
00216 + CrossProduct(wp[i],p[i])
00217 + CrossProduct(w[i],CrossProduct(w[i],p[i]));
00218 dvp[i] = Rt*(dvp[i-1] + z0*dqpp(i)
00219 + 2.0*(CrossProduct(dw[i-1],z0*qp(i))
00220 + CrossProduct(w[i-1],z0*dqp(i))))
00221 + CrossProduct(dwp[i],p[i])
00222 + CrossProduct(dw[i],CrossProduct(w[i],p[i]))
00223 + CrossProduct(w[i],CrossProduct(dw[i],p[i]))
00224 + (CrossProduct(wp[i],dp[i])
00225 + CrossProduct(w[i],CrossProduct(w[i],dp[i])))
00226 *dq(i);
00227 }
00228 a[i] = CrossProduct(wp[i],links[i].r)
00229 + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00230 + vp[i];
00231 da[i] = CrossProduct(dwp[i],links[i].r)
00232 + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00233 + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00234 + dvp[i];
00235 }
00236
00237 for(i = dof; i >= 1; i--)
00238 {
00239 F[i] = a[i] * links[i].m;
00240 dF[i] = da[i] * links[i].m;
00241 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00242 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00243 + CrossProduct(w[i],links[i].I*dw[i]);
00244 if(i == dof)
00245 {
00246 f[i] = F[i];
00247 df[i] = dF[i];
00248 n[i] = CrossProduct(p[i],f[i])
00249 + CrossProduct(links[i].r,F[i]) + N[i];
00250 dn[i] = CrossProduct(p[i],df[i])
00251 + CrossProduct(links[i].r,dF[i]) + dN[i];
00252 if(links[i].get_joint_type() != 0)
00253 dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00254 }
00255 else
00256 {
00257 f[i] = links[i+1].R*f[i+1] + F[i];
00258 df[i] = links[i+1].R*df[i+1] + dF[i];
00259 if(links[i].get_joint_type() == 0)
00260 df[i] += Q*links[i+1].R*f[i+1]*dq(i+1);
00261 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i],f[i])
00262 + CrossProduct(links[i].r,F[i]) + N[i];
00263 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i],df[i])
00264 + CrossProduct(links[i].r,dF[i]) + dN[i];
00265 if(links[i].get_joint_type() == 0)
00266 dn[i] += Q*links[i+1].R*n[i+1]*dq(i+1);
00267 else
00268 dn[i] += CrossProduct(dp[i],f[i])*dq(i);
00269 }
00270
00271 if(links[i].get_joint_type() == 0)
00272 {
00273 temp = ((z0.t()*links[i].R)*n[i]);
00274 ltorque(i) = temp(1,1);
00275 temp = ((z0.t()*links[i].R)*dn[i]);
00276 dtorque(i) = temp(1,1);
00277 }
00278 else
00279 {
00280 temp = ((z0.t()*links[i].R)*f[i]);
00281 ltorque(i) = temp(1,1);
00282 temp = ((z0.t()*links[i].R)*df[i]);
00283 dtorque(i) = temp(1,1);
00284 }
00285 }
00286 }
00287
00288
00289 void mRobot::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00290 const ColumnVector & qpp, const ColumnVector & dq,
00291 const ColumnVector & dqp, const ColumnVector & dqpp,
00292 ColumnVector & ltorque, ColumnVector & dtorque)
00293
00294
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 int i;
00384 Matrix Rt, temp;
00385 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00386 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00387 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00388 if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00389 if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00390 if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00391 ltorque = ColumnVector(dof);
00392 dtorque = ColumnVector(dof);
00393 set_q(q);
00394
00395 vp[0] = gravity;
00396 ColumnVector z0(3);
00397 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00398 Matrix Q(3,3);
00399 Q = 0.0;
00400 Q(1,2) = -1.0;
00401 Q(2,1) = 1.0;
00402 for(i = 1; i <= dof; i++)
00403 {
00404 Rt = links[i].R.t();
00405 p[i] = links[i].p;
00406 if(links[i].get_joint_type() != 0)
00407 {
00408 dp[i] = ColumnVector(3);
00409 dp[i](1) = 0.0;
00410 dp[i](2) = Rt(2,3);
00411 dp[i](3) = Rt(3,3);
00412 }
00413 if(links[i].get_joint_type() == 0)
00414 {
00415 w[i] = Rt*w[i-1] + z0*qp(i);
00416 dw[i] = Rt*dw[i-1] + z0*dqp(i)
00417 - Q*Rt*w[i-1]*dq(i);
00418 wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00419 + z0*qpp(i);
00420 dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00421 + CrossProduct(Rt*w[i-1],z0*dqp(i))
00422 - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
00423 + z0*dqpp(i);
00424 vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00425 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00426 + vp[i-1]);
00427 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00428 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00429 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00430 - Q*Rt*(CrossProduct(wp[i-1],p[i])
00431 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00432 }
00433 else
00434 {
00435 w[i] = Rt*w[i-1];
00436 dw[i] = Rt*dw[i-1];
00437 wp[i] = Rt*wp[i-1];
00438 dwp[i] = Rt*dwp[i-1];
00439 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00440 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00441 + z0*qpp(i) + 2.0*CrossProduct(w[i],z0*qp(i));
00442
00443 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00444 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00445 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00446 + (CrossProduct(wp[i-1],dp[i])
00447 + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00448 + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
00449 + z0*dqpp(i);
00450 }
00451 a[i] = CrossProduct(wp[i],links[i].r)
00452 + CrossProduct(w[i],CrossProduct(w[i],links[i].r))
00453 + vp[i];
00454 da[i] = CrossProduct(dwp[i],links[i].r)
00455 + CrossProduct(dw[i],CrossProduct(w[i],links[i].r))
00456 + CrossProduct(w[i],CrossProduct(dw[i],links[i].r))
00457 + dvp[i];
00458 }
00459
00460 for(i = dof; i >= 1; i--) {
00461 F[i] = a[i] * links[i].m;
00462 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i]);
00463 dF[i] = da[i] * links[i].m;
00464 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00465 + CrossProduct(w[i],links[i].I*dw[i]);
00466
00467 if(i == dof)
00468 {
00469 f[i] = F[i];
00470 df[i] = dF[i];
00471 n[i] = CrossProduct(links[i].r,F[i]) + N[i];
00472 dn[i] = dN[i] + CrossProduct(links[i].r,dF[i]);
00473 }
00474 else
00475 {
00476 f[i] = links[i+1].R*f[i+1] + F[i];
00477 df[i] = links[i+1].R*df[i+1] + dF[i];
00478 if(links[i+1].get_joint_type() == 0)
00479 df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00480
00481 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00482 + CrossProduct(links[i].r,F[i]) + N[i];
00483 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00484 + CrossProduct(links[i].r,dF[i]) + dN[i];
00485 if(links[i+1].get_joint_type() == 0)
00486 dn[i] += (links[i+1].R*Q*n[i+1] +
00487 CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00488 else
00489 dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00490 }
00491
00492 if(links[i].get_joint_type() == 0)
00493 {
00494 temp = z0.t()*n[i];
00495 ltorque(i) = temp(1,1);
00496 temp = z0.t()*dn[i];
00497 dtorque(i) = temp(1,1);
00498 }
00499 else
00500 {
00501 temp = z0.t()*f[i];
00502 ltorque(i) = temp(1,1);
00503 temp = z0.t()*df[i];
00504 dtorque(i) = temp(1,1);
00505 }
00506 }
00507 }
00508
00509 void mRobot_min_para::delta_torque(const ColumnVector & q, const ColumnVector & qp,
00510 const ColumnVector & qpp, const ColumnVector & dq,
00511 const ColumnVector & dqp, const ColumnVector & dqpp,
00512 ColumnVector & ltorque, ColumnVector & dtorque)
00513
00514
00515
00516
00517
00518 {
00519 int i;
00520 Matrix Rt, temp;
00521 if(q.Ncols() != 1 || q.Nrows() != dof) error("q has wrong dimension");
00522 if(qp.Ncols() != 1 || qp.Nrows() != dof) error("qp has wrong dimension");
00523 if(qpp.Ncols() != 1 || qpp.Nrows() != dof) error("qpp has wrong dimension");
00524 if(dq.Ncols() != 1 || dq.Nrows() != dof) error("dq has wrong dimension");
00525 if(dqp.Ncols() != 1 || dqp.Nrows() != dof) error("dqp has wrong dimension");
00526 if(dqpp.Ncols() != 1 || dqpp.Nrows() != dof) error("dqpp has wrong dimension");
00527 ltorque = ColumnVector(dof);
00528 dtorque = ColumnVector(dof);
00529 set_q(q);
00530
00531 vp[0] = gravity;
00532 ColumnVector z0(3);
00533 z0(1) = 0.0; z0(2) = 0.0; z0(3) = 1.0;
00534 Matrix Q(3,3);
00535 Q = 0.0;
00536 Q(1,2) = -1.0;
00537 Q(2,1) = 1.0;
00538 for(i = 1; i <= dof; i++)
00539 {
00540 Rt = links[i].R.t();
00541 p[i] = links[i].p;
00542 if(links[i].get_joint_type() != 0)
00543 {
00544 dp[i] = ColumnVector(3);
00545 dp[i](1) = 0.0;
00546 dp[i](2) = Rt(2,3);
00547 dp[i](3) = Rt(3,3);
00548 }
00549 if(links[i].get_joint_type() == 0)
00550 {
00551 w[i] = Rt*w[i-1] + z0*qp(i);
00552 dw[i] = Rt*dw[i-1] + z0*dqp(i)
00553 - Q*Rt*w[i-1]*dq(i);
00554 wp[i] = Rt*wp[i-1] + CrossProduct(Rt*w[i-1],z0*qp(i))
00555 + z0*qpp(i);
00556 dwp[i] = Rt*dwp[i-1] + CrossProduct(Rt*dw[i-1],z0*qp(i))
00557 + CrossProduct(Rt*w[i-1],z0*dqp(i))
00558 - (Q*Rt*wp[i-1] + CrossProduct(Q*Rt*w[i-1],z0*qp(i)))*dq(i)
00559 + z0*dqpp(i);
00560 vp[i] = Rt*(CrossProduct(wp[i-1],p[i])
00561 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i]))
00562 + vp[i-1]);
00563 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00564 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00565 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1])
00566 - Q*Rt*(CrossProduct(wp[i-1],p[i])
00567 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])) + vp[i-1])*dq(i);
00568 }
00569 else
00570 {
00571 w[i] = Rt*w[i-1];
00572 dw[i] = Rt*dw[i-1];
00573 wp[i] = Rt*wp[i-1];
00574 dwp[i] = Rt*dwp[i-1];
00575 vp[i] = Rt*(vp[i-1] + CrossProduct(wp[i-1],p[i])
00576 + CrossProduct(w[i-1],CrossProduct(w[i-1],p[i])))
00577 + z0*qpp(i)+ 2.0*CrossProduct(w[i],z0*qp(i));
00578 dvp[i] = Rt*(CrossProduct(dwp[i-1],p[i])
00579 + CrossProduct(dw[i-1],CrossProduct(w[i-1],p[i]))
00580 + CrossProduct(w[i-1],CrossProduct(dw[i-1],p[i])) + dvp[i-1]
00581 + (CrossProduct(wp[i-1],dp[i])
00582 + CrossProduct(w[i-1],CrossProduct(w[i-1],dp[i])))*dq(i))
00583 + 2*(CrossProduct(dw[i],z0*qp(i)) + CrossProduct(w[i],z0*dqp(i)))
00584 + z0*dqpp(i);
00585 }
00586 }
00587
00588 for(i = dof; i >= 1; i--) {
00589 F[i] = vp[i]*links[i].m + CrossProduct(wp[i], links[i].mc) +
00590 CrossProduct(w[i], CrossProduct(w[i], links[i].mc));
00591 dF[i] = dvp[i]*links[i].m + CrossProduct(dwp[i],links[i].mc)
00592 + CrossProduct(dw[i],CrossProduct(w[i],links[i].mc))
00593 + CrossProduct(w[i],CrossProduct(dw[i],links[i].mc));
00594 N[i] = links[i].I*wp[i] + CrossProduct(w[i],links[i].I*w[i])
00595 - CrossProduct(vp[i], links[i].mc);
00596 dN[i] = links[i].I*dwp[i] + CrossProduct(dw[i],links[i].I*w[i])
00597 + CrossProduct(w[i],links[i].I*dw[i])
00598 - CrossProduct(dvp[i],links[i].mc);
00599
00600 if(i == dof)
00601 {
00602 f[i] = F[i];
00603 df[i] = dF[i];
00604 n[i] = N[i];
00605 dn[i] = dN[i];
00606 }
00607 else
00608 {
00609 f[i] = links[i+1].R*f[i+1] + F[i];
00610 df[i] = links[i+1].R*df[i+1] + dF[i];
00611 if(links[i+1].get_joint_type() == 0)
00612 df[i] += links[i+1].R*Q*f[i+1]*dq(i+1);
00613
00614 n[i] = links[i+1].R*n[i+1] + CrossProduct(p[i+1],links[i+1].R*f[i+1])
00615 + N[i];
00616 dn[i] = links[i+1].R*dn[i+1] + CrossProduct(p[i+1],links[i+1].R*df[i+1])
00617 + dN[i];
00618 if(links[i+1].get_joint_type() == 0)
00619 dn[i] += (links[i+1].R*Q*n[i+1] +
00620 CrossProduct(p[i+1],links[i+1].R*Q*f[i+1]))*dq(i+1);
00621 else
00622 dn[i] += CrossProduct(dp[i+1],links[i+1].R*f[i+1])*dq(i+1);
00623 }
00624
00625 if(links[i].get_joint_type() == 0)
00626 {
00627 temp = z0.t()*n[i];
00628 ltorque(i) = temp(1,1);
00629 temp = z0.t()*dn[i];
00630 dtorque(i) = temp(1,1);
00631 }
00632 else
00633 {
00634 temp = z0.t()*f[i];
00635 ltorque(i) = temp(1,1);
00636 temp = z0.t()*df[i];
00637 dtorque(i) = temp(1,1);
00638 }
00639 }
00640 }
00641
00642 #ifdef use_namespace
00643 }
00644 #endif