00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014 #include "chain.h"
00015
00016 double Joint::TotalMass()
00017 {
00018 if(!child) return mass;
00019 return (mass + child->total_mass());
00020 }
00021
00022 double Joint::total_mass()
00023 {
00024 if(!child) return mass;
00025 double b = 0.0;
00026 if(brother) b = brother->total_mass();
00027 return mass + b + child->total_mass();
00028 }
00029
00030 void Joint::SetRotateJointType(const fVec3& rpos, const fMat33& ratt, AxisIndex ai)
00031 {
00032 j_type = JROTATE;
00033 if(t_given) n_dof = 1;
00034 else n_thrust = 1;
00035 init_pos.set(rpos);
00036 init_att.set(ratt);
00037 rel_pos.set(rpos);
00038 rel_att.set(ratt);
00039 axis.zero();
00040 if(ai != AXIS_NULL) axis(ai) = 1.0;
00041 }
00042
00043 void Joint::SetSlideJointType(const fVec3& rpos, const fMat33& ratt, AxisIndex ai)
00044 {
00045 j_type = JSLIDE;
00046 if(t_given) n_dof = 1;
00047 else n_thrust = 1;
00048 init_pos.set(rpos);
00049 init_att.set(ratt);
00050 rel_pos.set(rpos);
00051 rel_att.set(ratt);
00052 axis.zero();
00053 if(ai != AXIS_NULL) axis(ai) = 1.0;
00054 }
00055
00056 void Joint::SetFixedJointType(const fVec3& rpos, const fMat33& ratt)
00057 {
00058 j_type = JFIXED;
00059 n_dof = 0;
00060 n_thrust = 0;
00061 init_pos.set(rpos);
00062 init_att.set(ratt);
00063 rel_pos.set(rpos);
00064 rel_att.set(ratt);
00065 rel_ep.set(rel_att);
00066 }
00067
00068 void Joint::SetSphereJointType(const fVec3& rpos, const fMat33& ratt)
00069 {
00070 j_type = JSPHERE;
00071 if(t_given) n_dof = 3;
00072 else n_thrust = 3;
00073 init_pos.set(rpos);
00074 init_att.set(ratt);
00075 rel_pos.set(rpos);
00076 rel_att.set(ratt);
00077 rel_ep.set(rel_att);
00078 }
00079
00080 void Joint::SetFreeJointType(const fVec3& rpos, const fMat33& ratt)
00081 {
00082 j_type = JFREE;
00083 if(t_given) n_dof = 6;
00084 else n_thrust = 6;
00085 init_pos.set(rpos);
00086 init_att.set(ratt);
00087 rel_pos.set(rpos);
00088 rel_att.set(ratt);
00089 rel_ep.set(rel_att);
00090 }
00091
00092 void Joint::UpdateJointType()
00093 {
00094 n_dof = 0;
00095 n_thrust = 0;
00096 switch(j_type)
00097 {
00098 case JROTATE:
00099 case JSLIDE:
00100 if(t_given) n_dof = 1;
00101 else n_thrust = 1;
00102 break;
00103 case JSPHERE:
00104 if(t_given) n_dof = 3;
00105 else n_thrust = 3;
00106 break;
00107 case JFREE:
00108 if(t_given) n_dof = 6;
00109 else n_thrust = 6;
00110 break;
00111 case JFIXED:
00112 break;
00113 default:
00114 break;
00115 }
00116 init_pos.set(rel_pos);
00117 init_att.set(rel_att);
00118 rel_ep.set(rel_att);
00119 }
00120
00121
00122
00123
00124 int Chain::GetJointValue(fVec& values)
00125 {
00126 values.resize(n_value);
00127 root->get_joint_value(values);
00128 return 0;
00129 }
00130
00131 int Joint::get_joint_value(fVec& values)
00132 {
00133 static fVec3 p;
00134 static fMat33 r;
00135 static fEulerPara ep;
00136 if(i_value >= 0)
00137 {
00138 switch(j_type)
00139 {
00140 case JROTATE:
00141 case JSLIDE:
00142 GetJointValue(values(i_value));
00143 break;
00144 case JSPHERE:
00145 GetJointValue(r);
00146 ep.set(r);
00147 values(i_value) = ep(0);
00148 values(i_value+1) = ep(1);
00149 values(i_value+2) = ep(2);
00150 values(i_value+3) = ep(3);
00151 break;
00152 case JFREE:
00153 GetJointValue(p, r);
00154 ep.set(r);
00155 values(i_value) = p(0);
00156 values(i_value+1) = p(1);
00157 values(i_value+2) = p(2);
00158 values(i_value+3) = ep(0);
00159 values(i_value+4) = ep(1);
00160 values(i_value+5) = ep(2);
00161 values(i_value+6) = ep(3);
00162 break;
00163 default:
00164 break;
00165 }
00166 }
00167 child->get_joint_value(values);
00168 brother->get_joint_value(values);
00169 return 0;
00170 }
00171
00172 int Chain::GetJointVel(fVec& vels)
00173 {
00174 vels.resize(n_dof);
00175 root->get_joint_vel(vels);
00176 return 0;
00177 }
00178
00179 int Joint::get_joint_vel(fVec& vels)
00180 {
00181 static fVec3 pd, rd;
00182 if(i_dof >= 0)
00183 {
00184 switch(j_type)
00185 {
00186 case JROTATE:
00187 case JSLIDE:
00188 GetJointVel(vels(i_dof));
00189 break;
00190 case JSPHERE:
00191 GetJointVel(rd);
00192 vels(i_dof) = rd(0);
00193 vels(i_dof+1) = rd(1);
00194 vels(i_dof+2) = rd(2);
00195 break;
00196 case JFREE:
00197 GetJointVel(pd, rd);
00198 vels(i_dof) = pd(0);
00199 vels(i_dof+1) = pd(1);
00200 vels(i_dof+2) = pd(2);
00201 vels(i_dof+3) = rd(0);
00202 vels(i_dof+4) = rd(1);
00203 vels(i_dof+5) = rd(2);
00204 break;
00205 default:
00206 break;
00207 }
00208 }
00209 brother->get_joint_vel(vels);
00210 child->get_joint_vel(vels);
00211 return 0;
00212 }
00213
00214 int Chain::GetJointAcc(fVec& accs)
00215 {
00216 accs.resize(n_dof);
00217 root->get_joint_acc(accs);
00218 return 0;
00219 }
00220
00221 int Joint::get_joint_acc(fVec& accs)
00222 {
00223 static fVec3 pdd, rdd;
00224 if(i_dof >= 0)
00225 {
00226 switch(j_type)
00227 {
00228 case JROTATE:
00229 case JSLIDE:
00230 GetJointAcc(accs(i_dof));
00231 break;
00232 case JSPHERE:
00233 GetJointAcc(rdd);
00234 accs(i_dof) = rdd(0);
00235 accs(i_dof+1) = rdd(1);
00236 accs(i_dof+2) = rdd(2);
00237 break;
00238 case JFREE:
00239 GetJointAcc(pdd, rdd);
00240 accs(i_dof) = pdd(0);
00241 accs(i_dof+1) = pdd(1);
00242 accs(i_dof+2) = pdd(2);
00243 accs(i_dof+3) = rdd(0);
00244 accs(i_dof+4) = rdd(1);
00245 accs(i_dof+5) = rdd(2);
00246 break;
00247 default:
00248 break;
00249 }
00250 }
00251 brother->get_joint_acc(accs);
00252 child->get_joint_acc(accs);
00253 return 0;
00254 }
00255
00256 int Joint::GetJointValue(double& _q)
00257 {
00258 if(j_type == JROTATE || j_type == JSLIDE)
00259 {
00260 _q = q;
00261 }
00262 else
00263 {
00264 return -1;
00265 }
00266 return 0;
00267 }
00268
00269 int Joint::GetJointVel(double& _qd)
00270 {
00271 if(j_type == JROTATE || j_type == JSLIDE)
00272 {
00273 _qd = qd;
00274 }
00275 else
00276 {
00277 return -1;
00278 }
00279 return 0;
00280 }
00281
00282 int Joint::GetJointAcc(double& _qdd)
00283 {
00284 if(j_type == JROTATE || j_type == JSLIDE)
00285 {
00286 _qdd = qdd;
00287 }
00288 else
00289 {
00290 return -1;
00291 }
00292 return 0;
00293 }
00294
00295 int Joint::GetJointValue(fMat33& r)
00296 {
00297 if(j_type == JSPHERE || j_type == JFREE)
00298 {
00299 r.set(rel_att);
00300 }
00301 else
00302 {
00303 return -1;
00304 }
00305 return 0;
00306 }
00307
00308 int Joint::GetJointVel(fVec3& rd)
00309 {
00310 if(j_type == JSPHERE || j_type == JFREE)
00311 {
00312 rd.set(rel_ang_vel);
00313 }
00314 else
00315 {
00316 return -1;
00317 }
00318 return 0;
00319 }
00320
00321 int Joint::GetJointAcc(fVec3& rdd)
00322 {
00323 if(j_type == JSPHERE || j_type == JFREE)
00324 {
00325 rdd.set(rel_ang_acc);
00326 }
00327 else
00328 {
00329 return -1;
00330 }
00331 return 0;
00332 }
00333
00334 int Joint::GetJointValue(fVec3& p, fMat33& r)
00335 {
00336 if(j_type == JFREE || j_type == JFIXED)
00337 {
00338 p.set(rel_pos);
00339 r.set(rel_att);
00340 }
00341 else
00342 {
00343 return -1;
00344 }
00345 return 0;
00346 }
00347
00348 int Joint::GetJointVel(fVec3& pd, fVec3& rd)
00349 {
00350 if(j_type == JFREE)
00351 {
00352 pd.set(rel_lin_vel);
00353 rd.set(rel_ang_vel);
00354 }
00355 else
00356 {
00357 return -1;
00358 }
00359 return 0;
00360 }
00361
00362 int Joint::GetJointAcc(fVec3& pdd, fVec3& rdd)
00363 {
00364 if(j_type == JFREE)
00365 {
00366 pdd.set(rel_lin_acc);
00367 rdd.set(rel_ang_acc);
00368 }
00369 else
00370 {
00371 return -1;
00372 }
00373 return 0;
00374 }
00375
00376
00377
00378
00379 int Joint::SetJointPosition(const fVec3& p)
00380 {
00381 rel_pos.set(p);
00382 return 0;
00383 }
00384
00385 int Joint::SetJointOrientation(const fMat33& r)
00386 {
00387 rel_att.set(r);
00388 rel_ep.set(r);
00389 return 0;
00390 }
00391
00392
00393
00394
00395
00396 int Chain::set_abs_position_orientation(Joint* jnt, const fVec3& abs_pos, const fMat33& abs_att)
00397 {
00398 if(!jnt->parent) return -1;
00399 fVec3 p_pos(jnt->parent->abs_pos);
00400 fMat33 p_att(jnt->parent->abs_att);
00401 static fVec3 pp, r_pos;
00402 static fMat33 tR, r_att;
00403 pp.sub(abs_pos, p_pos);
00404 tR.tran(p_att);
00405 r_pos.mul(tR, pp);
00406 r_att.mul(tR, abs_att);
00407 jnt->SetJointPosition(r_pos);
00408 jnt->SetJointOrientation(r_att);
00409 return 0;
00410 }
00411
00412
00413
00414
00415 int Chain::SetJointValue(const fVec& values)
00416 {
00417 if(values.size() != n_value)
00418 {
00419 cerr << "Chain::SetJointValue: error - size of the argument should be n_value (" << n_value << ")" << endl;
00420 return -1;
00421 }
00422 root->set_joint_value(values);
00423 return 0;
00424 }
00425
00426 int Joint::set_joint_value(const fVec& values)
00427 {
00428 static fVec3 p;
00429 static fEulerPara ep;
00430 if(i_value >= 0)
00431 {
00432 switch(j_type)
00433 {
00434 case JROTATE:
00435 case JSLIDE:
00436 SetJointValue(values(i_value));
00437 break;
00438 case JSPHERE:
00439 ep.set(values(i_value), values(i_value+1), values(i_value+2), values(i_value+3));
00440 SetJointValue(ep);
00441 break;
00442 case JFREE:
00443 p.set(values(i_value), values(i_value+1), values(i_value+2));
00444 ep.set(values(i_value+3), values(i_value+4), values(i_value+5), values(i_value+6));
00445 SetJointValue(p, ep);
00446 break;
00447 default:
00448 break;
00449 }
00450 }
00451 child->set_joint_value(values);
00452 brother->set_joint_value(values);
00453 return 0;
00454 }
00455
00456 int Chain::SetJointVel(const fVec& vels)
00457 {
00458 if(vels.size() != n_dof)
00459 {
00460 cerr << "Chain::SetJointVel: error - size of the argument should be n_dof (" << n_dof << ")" << endl;
00461 return -1;
00462 }
00463 root->set_joint_vel(vels);
00464 return 0;
00465 }
00466
00467 int Joint::set_joint_vel(const fVec& vels)
00468 {
00469 static fVec3 pd;
00470 static fVec3 rd;
00471 if(i_dof >= 0)
00472 {
00473 switch(j_type)
00474 {
00475 case JROTATE:
00476 case JSLIDE:
00477 SetJointVel(vels(i_dof));
00478 break;
00479 case JSPHERE:
00480 rd.set(vels(i_dof), vels(i_dof+1), vels(i_dof+2));
00481 SetJointVel(rd);
00482 break;
00483 case JFREE:
00484 pd.set(vels(i_dof), vels(i_dof+1), vels(i_dof+2));
00485 rd.set(vels(i_dof+3), vels(i_dof+4), vels(i_dof+5));
00486 SetJointVel(pd, rd);
00487 break;
00488 default:
00489 break;
00490 }
00491 }
00492 child->set_joint_vel(vels);
00493 brother->set_joint_vel(vels);
00494 return 0;
00495 }
00496
00497 int Chain::SetJointAcc(const fVec& accs)
00498 {
00499 if(accs.size() != n_dof)
00500 {
00501 cerr << "Chain::SetJointAcc: error - size of the argument should be n_dof (" << n_dof << ")" << endl;
00502 return -1;
00503 }
00504 root->set_joint_acc(accs);
00505 return 0;
00506 }
00507
00508 int Joint::set_joint_acc(const fVec& accs)
00509 {
00510 static fVec3 pdd;
00511 static fVec3 rdd;
00512 if(i_dof >= 0)
00513 {
00514 switch(j_type)
00515 {
00516 case JROTATE:
00517 case JSLIDE:
00518 SetJointAcc(accs(i_dof));
00519 break;
00520 case JSPHERE:
00521 rdd.set(accs(i_dof), accs(i_dof+1), accs(i_dof+2));
00522 SetJointAcc(rdd);
00523 break;
00524 case JFREE:
00525 pdd.set(accs(i_dof), accs(i_dof+1), accs(i_dof+2));
00526 rdd.set(accs(i_dof+3), accs(i_dof+4), accs(i_dof+5));
00527 SetJointAcc(pdd, rdd);
00528 break;
00529 default:
00530 break;
00531 }
00532 }
00533 child->set_joint_acc(accs);
00534 brother->set_joint_acc(accs);
00535 return 0;
00536 }
00537
00538 int Joint::SetJointValue(double _q)
00539 {
00540 q = _q;
00541 if(j_type == JROTATE)
00542 {
00543 static fMat33 tmp;
00544 tmp.rot2mat(axis, q);
00545 rel_att.mul(init_att, tmp);
00546 rel_ep.set(rel_att);
00547 }
00548 else if(j_type == JSLIDE)
00549 {
00550 static fVec3 tmp, tmp1;
00551 tmp.mul(q, axis);
00552 tmp1.mul(rel_att, tmp);
00553 rel_pos.add(tmp1, init_pos);
00554 }
00555 else
00556 {
00557 return -1;
00558 }
00559 return 0;
00560 }
00561
00562 int Joint::SetJointVel(double _qd)
00563 {
00564 qd = _qd;
00565 if(j_type == JROTATE)
00566 {
00567 rel_ang_vel.mul(qd, axis);
00568 }
00569 else if(j_type == JSLIDE)
00570 {
00571 rel_lin_vel.mul(qd, axis);
00572 }
00573 else
00574 {
00575 return -1;
00576 }
00577 return 0;
00578 }
00579
00580 int Joint::SetJointAcc(double _qdd)
00581 {
00582 qdd = _qdd;
00583 if(j_type == JROTATE)
00584 {
00585 rel_ang_acc.mul(qdd, axis);
00586 }
00587 else if(j_type == JSLIDE)
00588 {
00589 rel_lin_acc.mul(qdd, axis);
00590 }
00591 else
00592 {
00593 return -1;
00594 }
00595 return 0;
00596 }
00597
00598 int Joint::SetJointValue(const fMat33& r)
00599 {
00600 if(j_type == JSPHERE || j_type == JFREE)
00601 {
00602 rel_att.set(r);
00603 rel_ep.set(rel_att);
00604 }
00605 else
00606 {
00607 return -1;
00608 }
00609 return 0;
00610 }
00611
00612 int Joint::SetJointValue(const fEulerPara& ep)
00613 {
00614 if(j_type == JSPHERE || j_type == JFREE)
00615 {
00616 rel_ep.set(ep);
00617 rel_att.set(rel_ep);
00618 }
00619 else
00620 {
00621 return -1;
00622 }
00623 return 0;
00624 }
00625
00626 int Joint::SetJointVel(const fVec3& rd)
00627 {
00628 if(j_type == JSPHERE || j_type == JFREE)
00629 {
00630 rel_ang_vel.set(rd);
00631 }
00632 else
00633 {
00634 return -1;
00635 }
00636 return 0;
00637 }
00638
00639 int Joint::SetJointAcc(const fVec3& rdd)
00640 {
00641 if(j_type == JSPHERE || j_type == JFREE)
00642 {
00643 rel_ang_acc.set(rdd);
00644 }
00645 else
00646 {
00647 return -1;
00648 }
00649 return 0;
00650 }
00651
00652 int Joint::SetJointAcc(double ax, double ay, double az)
00653 {
00654 if(j_type == JSPHERE || j_type == JFREE)
00655 {
00656 rel_ang_acc(0) = ax;
00657 rel_ang_acc(1) = ay;
00658 rel_ang_acc(2) = az;
00659 }
00660 else
00661 {
00662 return -1;
00663 }
00664 return 0;
00665 }
00666
00667 int Joint::SetJointValue(const fVec3& p, const fMat33& r)
00668 {
00669 if(j_type == JFREE || j_type == JFIXED)
00670 {
00671 rel_pos.set(p);
00672 rel_att.set(r);
00673 rel_ep.set(rel_att);
00674 }
00675 else
00676 {
00677 return -1;
00678 }
00679 return 0;
00680 }
00681
00682 int Joint::SetJointValue(const fVec3& p, const fEulerPara& ep)
00683 {
00684 if(j_type == JFREE || j_type == JFIXED)
00685 {
00686 rel_pos.set(p);
00687 rel_ep.set(ep);
00688 rel_att.set(rel_ep);
00689 }
00690 else
00691 {
00692 return -1;
00693 }
00694 return 0;
00695 }
00696
00697 int Joint::SetJointVel(const fVec3& pd, const fVec3& rd)
00698 {
00699 if(j_type == JFREE)
00700 {
00701 rel_lin_vel.set(pd);
00702 rel_ang_vel.set(rd);
00703 }
00704 else
00705 {
00706 return -1;
00707 }
00708 return 0;
00709 }
00710
00711 int Joint::SetJointAcc(const fVec3& pdd, const fVec3& rdd)
00712 {
00713 if(j_type == JFREE)
00714 {
00715 rel_lin_acc.set(pdd);
00716 rel_ang_acc.set(rdd);
00717 }
00718 else
00719 {
00720 return -1;
00721 }
00722 return 0;
00723 }
00724
00725 int Joint::SetJointAcc(double lx, double ly, double lz, double ax, double ay, double az)
00726 {
00727 if(j_type == JFREE)
00728 {
00729 rel_lin_acc(0) = lx;
00730 rel_lin_acc(1) = ly;
00731 rel_lin_acc(2) = lz;
00732 rel_ang_acc(0) = ax;
00733 rel_ang_acc(1) = ay;
00734 rel_ang_acc(2) = az;
00735 }
00736 else
00737 {
00738 return -1;
00739 }
00740 return 0;
00741 }
00742
00743
00744
00745
00746 int Chain::SetJointForce(const fVec& forces)
00747 {
00748 if(forces.size() != n_dof)
00749 {
00750 cerr << "Chain::SetJointForce: error - size of the argument should be n_dof (" << n_dof << ")" << endl;
00751 return -1;
00752 }
00753 root->set_joint_force(forces);
00754 return 0;
00755 }
00756
00757 int Joint::set_joint_force(const fVec& forces)
00758 {
00759 if(i_dof >= 0)
00760 {
00761 switch(j_type)
00762 {
00763 case JROTATE:
00764 case JSLIDE:
00765 SetJointForce(forces(i_dof));
00766 break;
00767 case JSPHERE:
00768 SetJointForce(fVec3(forces(i_dof), forces(i_dof+1), forces(i_dof+2)));
00769 break;
00770 case JFREE:
00771 SetJointForce(fVec3(forces(i_dof), forces(i_dof+1), forces(i_dof+2)),
00772 fVec3(forces(i_dof+3), forces(i_dof+4), forces(i_dof+5)));
00773 break;
00774 default:
00775 break;
00776 }
00777 }
00778 child->set_joint_force(forces);
00779 brother->set_joint_force(forces);
00780 return 0;
00781 }
00782
00783 int Joint::SetJointForce(double _tau)
00784 {
00785 if(j_type == JROTATE || j_type == JSLIDE)
00786 {
00787 tau = _tau;
00788 }
00789 else
00790 {
00791 return -1;
00792 }
00793 return 0;
00794 }
00795
00796 int Joint::GetJointForce(double& _tau)
00797 {
00798 if(j_type == JROTATE || j_type == JSLIDE)
00799 {
00800 _tau = tau;
00801 }
00802 else
00803 {
00804 return -1;
00805 }
00806 return 0;
00807 }
00808
00809 int Joint::SetJointForce(const fVec3& _n3)
00810 {
00811 if(j_type == JSPHERE)
00812 {
00813 tau_n.set(_n3);
00814 }
00815 else
00816 {
00817 return -1;
00818 }
00819 return 0;
00820 }
00821
00822 int Joint::GetJointForce(fVec3& _n3)
00823 {
00824 if(j_type == JSPHERE)
00825 {
00826 _n3.set(tau_n);
00827 }
00828 else
00829 {
00830 return -1;
00831 }
00832 return 0;
00833 }
00834
00835 int Joint::SetJointForce(const fVec3& _f3, const fVec3& _n3)
00836 {
00837 if(j_type == JFREE)
00838 {
00839 tau_f.set(_f3);
00840 tau_n.set(_n3);
00841 }
00842 else
00843 {
00844 return -1;
00845 }
00846 return 0;
00847 }
00848
00849 int Joint::GetJointForce(fVec3& _f3, fVec3& _n3)
00850 {
00851 if(j_type == JFREE)
00852 {
00853 _f3.set(tau_f);
00854 _n3.set(tau_n);
00855 }
00856 else
00857 {
00858 return -1;
00859 }
00860 return 0;
00861 }
00862
00863
00864
00865
00866 int Chain::GetJointForce(fVec& forces)
00867 {
00868 if(forces.size() != n_dof)
00869 {
00870 cerr << "Chain::GetJointForce: error - size of the argument should be n_dof (" << n_dof << ")" << endl;
00871 return -1;
00872 }
00873 root->get_joint_force(forces);
00874 return 0;
00875 }
00876
00877 int Joint::get_joint_force(fVec& forces)
00878 {
00879 if(i_dof >= 0)
00880 {
00881 switch(j_type)
00882 {
00883 case JROTATE:
00884 case JSLIDE:
00885 forces(i_dof) = tau;
00886 break;
00887 case JSPHERE:
00888 forces(i_dof) = tau_n(0);
00889 forces(i_dof+1) = tau_n(1);
00890 forces(i_dof+2) = tau_n(2);
00891 break;
00892 case JFREE:
00893 forces(i_dof) = tau_f(0);
00894 forces(i_dof+1) = tau_f(1);
00895 forces(i_dof+2) = tau_f(2);
00896 forces(i_dof+3) = tau_n(0);
00897 forces(i_dof+4) = tau_n(1);
00898 forces(i_dof+5) = tau_n(2);
00899 break;
00900 default:
00901 break;
00902 }
00903 }
00904 child->get_joint_force(forces);
00905 brother->get_joint_force(forces);
00906 return 0;
00907 }
00908
00909
00910
00911
00912 int Chain::ClearJointForce()
00913 {
00914 root->clear_joint_force();
00915 return 0;
00916 }
00917
00918 int Joint::clear_joint_force()
00919 {
00920 tau = 0.0;
00921 tau_f.zero();
00922 tau_n.zero();
00923 brother->clear_joint_force();
00924 child->clear_joint_force();
00925 return 0;
00926 }
00927
00928 int Chain::ClearExtForce()
00929 {
00930 root->clear_ext_force();
00931 return 0;
00932 }
00933
00934 int Joint::clear_ext_force()
00935 {
00936 ext_force.zero();
00937 ext_moment.zero();
00938 brother->clear_ext_force();
00939 child->clear_ext_force();
00940 return 0;
00941 }