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
00054
00055 static const char rcsid[] = "$Id: delta_t.cpp,v 1.17 2005/07/01 16:11:45 gourdeau 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)
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)
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)
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