joint.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00009 /*
00010  * joint.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.10.21
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  * get joint values, vels, accs
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  * set joint position/orientation
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  * compute relative from absolute position/orientation
00394  * call CalcPosition() first
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  * set joint values, vels, accs
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  * set joint froce/torque
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  * get joint force/torque
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  * clear joint force/torque and external force/moment
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 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:55