ik.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  * ik.cpp
00011  * Create: Katsu Yamane, 03.07.10
00012  */
00013 
00014 #include "ik.h"
00015 #include <string>
00016 
00017 IK::IK(): Chain()
00018 {
00019         n_assigned_constraints = 0;
00020         n_constraints = 0;
00021         n_total_const = 0;
00022         for(int m=0; m<N_PRIORITY_TYPES; m++)
00023                 n_const[m] = 0;
00024         n_all_const = 0;
00025         max_condnum = MAX_CONDITION_NUMBER;
00026         assign_constraints(100);
00027 }
00028 
00029 IK::~IK()
00030 {
00031 #ifdef MEASURE_TIME
00032         extern int n_update;
00033         extern double calc_jacobian_time;
00034         extern double solve_ik_time;
00035         extern double high_constraint_time;
00036         extern double low_constraint_time;
00037         cerr << "n_update = " << n_update << endl;
00038         cerr << "calc_jacobian = " << calc_jacobian_time << endl;
00039         cerr << "solve_ik = " << solve_ik_time << endl;
00040         cerr << "high_constraint = " << high_constraint_time << endl;
00041         cerr << "low_constraint = " << low_constraint_time << endl;
00042 #endif
00043 
00044         for(int i=0; i<n_assigned_constraints; i++)
00045         {
00046                 if(constraints[i]) delete constraints[i];
00047         }
00048         delete[] constraints;
00049 }
00050 
00051 #ifdef SEGA
00052 int IK::init()
00053 #else
00054 int IK::init(SceneGraph* sg)
00055 #endif
00056 {
00057 #ifdef SEGA
00058         Chain::init();
00059         myinit();
00060 #else
00061         Chain::init(sg);
00062         myinit(sg);
00063 #endif
00064         return 0;
00065 }
00066 
00067 #ifdef SEGA
00068 int IK::myinit()
00069 #else
00070 int IK::myinit(SceneGraph* sg)
00071 #endif
00072 {
00073         joint_weights.resize(n_dof);
00074         joint_weights = 1.0;  // default weights
00075 #ifndef SEGA
00076         // load markers from sg
00077         if(sg)
00078         {
00079                 CalcPosition();
00080                 Joint* j;
00081                 for(j=root->child; j; j=j->brother)
00082                         load_markers(sg, j);
00083         }
00084 #endif
00085         return 0;
00086 }
00087 
00088 #ifndef SEGA
00089 int IK::load_markers(SceneGraph* sg, Joint* rj)
00090 {
00091         char* marker_top_full_name;
00092         char* char_name = rj->CharName();
00093         if(char_name)
00094         {
00095                 marker_top_full_name = new char [strlen(marker_top_name) + strlen(char_name) + 2];
00096                 sprintf(marker_top_full_name, "%s%c%s", marker_top_name, charname_separator, char_name);
00097         }
00098         else
00099         {
00100                 marker_top_full_name = new char [strlen(marker_top_name) + 1];
00101                 strcpy(marker_top_full_name, marker_top_name);
00102         }
00103         TransformNode* marker_top = sg->findTransformNode(marker_top_full_name);
00104         if(marker_top)
00105                 load_markers(marker_top);
00106         delete[] marker_top_full_name;
00107         return 0;
00108 }
00109 
00110 int IK::load_markers(TransformNode* marker_top)
00111 {
00112         // find first transform node
00113         Node* n;
00114         for(n=marker_top->getChildNodes(); n && !n->getName(); n=n->getChildNodes());
00115         if(!n) return 0;
00116         Node* tn;
00117         for(tn=n; tn; tn=tn->next())
00118         {
00119                 if(tn->isTransformNode())
00120                         add_marker((TransformNode*)tn);
00121         }
00122         return 0;
00123 }
00124 
00125 IKHandle* IK::add_marker(TransformNode* tn)
00126 {
00127         // analyze name {marker name}_{link name}:{character name}
00128         char* fullname = new char [strlen(tn->getName()) + 1];
00129         strcpy(fullname, tn->getName());
00130         char* ch_start = strrchr(fullname, charname_separator);
00131         char* body_name;
00132         if(ch_start)
00133         {
00134                 body_name = new char [ch_start - fullname + 1];
00135                 strncpy(body_name, fullname, ch_start-fullname);
00136                 body_name[ch_start-fullname] = '\0';
00137         }
00138         else  // no character name
00139         {
00140                 body_name = new char [strlen(fullname) + 1];
00141                 strcpy(body_name, fullname);
00142         }
00143         char* j_start = strrchr(body_name, joint_name_separator);
00144         if(!j_start)
00145         {
00146                 delete[] body_name;
00147                 delete[] fullname;
00148                 return 0;
00149         }
00150         char* joint_name = new char [strlen(j_start)];
00151         strcpy(joint_name, j_start+1);
00152 
00153         char* marker_name;
00154         if(ch_start)
00155         {
00156                 marker_name = new char [j_start - body_name + strlen(ch_start) + 1];
00157                 strncpy(marker_name, body_name, j_start-body_name);
00158                 marker_name[j_start-body_name] = charname_separator;
00159                 marker_name[j_start-body_name+1] = '\0';
00160                 strcat(marker_name, ch_start+1);
00161         }
00162         else
00163         {
00164                 marker_name = new char [j_start - body_name + 1];
00165                 strncpy(marker_name, body_name, j_start-body_name);
00166                 marker_name[j_start-body_name] = '\0';
00167         }
00168         Joint* joint;
00169         if(ch_start)
00170                 joint = FindJoint(joint_name, ch_start+1);
00171         else
00172                 joint = FindJoint(joint_name);
00173         if(!joint)
00174         {
00175                 delete[] body_name;
00176                 delete[] fullname;
00177                 delete[] joint_name;
00178                 delete[] marker_name;
00179                 return 0;
00180         }
00181         // create new fixed joint as a child of joint
00182         Joint* new_joint;
00183         if(ch_start)
00184         {
00185 //              char* marker_joint_name = new char [strlen(body_name) + strlen(ch_start) + 1];
00186 //              sprintf(marker_joint_name, "%s%s", body_name, ch_start);
00187                 char* marker_joint_name = new char [strlen(marker_name) + 1];
00188                 sprintf(marker_joint_name, "%s", marker_name);
00189                 new_joint = new Joint(marker_joint_name, JFIXED);
00190                 delete[] marker_joint_name;
00191         }
00192         else
00193         {
00194                 new_joint = new Joint(body_name, JFIXED);
00195         }
00196         if(AddJoint(new_joint, joint))
00197         {
00198                 delete[] body_name;
00199                 delete[] fullname;
00200                 delete[] joint_name;
00201                 delete[] marker_name;
00202                 delete new_joint;
00203                 return 0;
00204         }
00205         // set relative position
00206         float fpos[3], fatt[3][3], fscale[3];
00207         fVec3 abs_pos;
00208         fMat33 abs_att;
00209         get_abs_matrix(tn, fpos, fatt, fscale);
00210         abs_pos(0) = fpos[0];
00211         abs_pos(1) = fpos[1];
00212         abs_pos(2) = fpos[2];
00213         abs_att(0,0) = fatt[0][0];
00214         abs_att(0,1) = fatt[0][1];
00215         abs_att(0,2) = fatt[0][2];
00216         abs_att(1,0) = fatt[1][0];
00217         abs_att(1,1) = fatt[1][1];
00218         abs_att(1,2) = fatt[1][2];
00219         abs_att(2,0) = fatt[2][0];
00220         abs_att(2,1) = fatt[2][1];
00221         abs_att(2,2) = fatt[2][2];
00222         set_abs_position_orientation(new_joint, abs_pos, abs_att);
00223         // add handle constraint
00224         ConstIndex cindex[6] = {
00225                 HAVE_CONSTRAINT,
00226                 HAVE_CONSTRAINT,
00227                 HAVE_CONSTRAINT,
00228                 NO_CONSTRAINT,
00229                 NO_CONSTRAINT,
00230                 NO_CONSTRAINT,
00231         };
00232         IKHandle* h = new IKHandle(this, marker_name, new_joint, cindex, LOW_PRIORITY, 10.0);
00233         AddConstraint(h);
00234         cerr << "marker " << marker_name << " added to " << joint->name << " (abs_pos = " << abs_pos << ", rel_pos = " << new_joint->rel_pos << ")" << endl;
00235 //      cerr << "   joint pos = " << joint->abs_pos << endl;
00236         delete[] body_name;
00237         delete[] fullname;
00238         delete[] joint_name;
00239         delete[] marker_name;
00240         return h;
00241 }
00242 
00243 // global version
00244 IKHandle* IK::AddMarker(const std::string& label, const std::string& linkname, const std::string& charname, const fVec3& rel_pos)
00245 {
00246         Joint* pjoint = FindJoint(linkname.c_str(), charname.c_str());
00247         if(!pjoint) return 0;
00248         std::string fullname = label + charname_separator + charname;
00249         Joint* new_joint = new Joint(fullname.c_str(), JFIXED);
00250         if(AddJoint(new_joint, pjoint))
00251         {
00252                 delete new_joint;
00253                 return 0;
00254         }
00255         new_joint->rel_pos.set(rel_pos);
00256         // add handle constraint
00257         ConstIndex cindex[6] = {
00258                 HAVE_CONSTRAINT,
00259                 HAVE_CONSTRAINT,
00260                 HAVE_CONSTRAINT,
00261                 NO_CONSTRAINT,
00262                 NO_CONSTRAINT,
00263                 NO_CONSTRAINT,
00264         };
00265         IKHandle* h = new IKHandle(this, fullname.c_str(), new_joint, cindex, LOW_PRIORITY, 10.0);
00266         AddConstraint(h);
00267         cerr << "marker " << fullname << " added " << new_joint->rel_pos << endl;
00268         return h;
00269 }
00270 
00271 IKHandle* IK::AddMarker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos)
00272 {
00273         IKHandle* ret;
00274         in_create_chain = true;
00275         clear_data();
00276         ret = add_marker(marker_name, parent_joint, abs_pos);
00277         init(0);
00278         in_create_chain = false;
00279         return ret;
00280 }
00281 
00282 IKHandle* IK::add_marker(const char* marker_name, Joint* parent_joint, const fVec3& abs_pos)
00283 {
00284         char* fullname = new char [strlen(marker_name) + strlen(parent_joint->name) + 2];
00285         sprintf(fullname, "%s%c%s", marker_name, joint_name_separator, parent_joint->name);
00286         TransformNode* tnode = new TransformNode;
00287         tnode->setName(fullname);
00288         tnode->setTranslation(abs_pos(0), abs_pos(1), abs_pos(2));
00289         IKHandle* ret = add_marker(tnode);
00290         delete[] fullname;
00291         delete tnode;
00292         return ret;
00293 }
00294 
00295 int IK::EditMarker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos)
00296 {
00297         int ret;
00298         in_create_chain = true;
00299         clear_data();
00300         ret = edit_marker(marker, body_name, parent_joint, abs_pos);
00301         init(0);
00302         in_create_chain = false;
00303         return ret;
00304 }
00305 
00306 int IK::edit_marker(IKHandle* marker, const char* body_name, Joint* parent_joint, const fVec3& abs_pos)
00307 {
00308         // change name
00309         char* ch_start = strrchr(parent_joint->name, charname_separator);
00310         if(marker->joint_name) delete[] marker->joint_name;
00311         marker->joint_name = 0;
00312         if(ch_start)
00313         {
00314                 marker->joint_name = new char [strlen(body_name) + strlen(ch_start) + 1];
00315                 sprintf(marker->joint_name, "%s%s", body_name, ch_start);
00316                 cerr << "new name = " << marker->joint_name << endl;
00317         }
00318         else
00319         {
00320                 marker->joint_name = new char [strlen(body_name) + 1];
00321                 strcpy(marker->joint_name, body_name);
00322         }
00323         marker->joint->SetName(marker->joint_name);
00324         // set parent joint
00325         if(marker->joint->parent == parent_joint) return 0;
00326         // disconnect and connect
00327         cerr << "fixed to " << parent_joint->name << endl;
00328         RemoveJoint(marker->joint);
00329         AddJoint(marker->joint, parent_joint);
00330         fMat33 abs_att;
00331         abs_att.identity();
00332         set_abs_position_orientation(marker->joint, abs_pos, abs_att);
00333         return 0;
00334 }
00335 
00336 int IK::SaveMarkers(const char* _fname)
00337 {
00338         CalcPosition();
00339         SceneGraph* sg = new SceneGraph;
00340         int i;
00341         TransformNode* top_tnode = new TransformNode;
00342         top_tnode->setName(marker_top_name);
00343         sg->addNode(top_tnode);
00344         for(i=0; i<n_constraints; i++)
00345         {
00346                 if(constraints[i]->GetType() == HANDLE_CONSTRAINT)
00347                         save_marker(top_tnode, (IKHandle*)constraints[i]);
00348         }
00349         char* fname = new char [strlen(_fname) + 1];
00350         strcpy(fname, _fname);
00351         sg->save(fname);
00352         delete sg;
00353         delete[] fname;
00354         return 0;
00355 }
00356 
00357 int IK::save_marker(TransformNode* top_tnode, IKHandle* h)
00358 {
00359         char* char_name = h->joint->CharName();
00360         char* t_name;
00361         if(char_name)
00362         {
00363                 int marker_name_length = strlen(h->joint_name) - strlen(char_name) - 1;
00364                 int parent_name_length = strlen(h->joint->parent->name) - strlen(char_name) - 1;
00365                 char* marker_name = new char [marker_name_length + 1];
00366                 char* parent_name = new char [parent_name_length + 1];
00367                 t_name = new char [marker_name_length + parent_name_length + 2];
00368                 strncpy(marker_name, h->joint_name, marker_name_length);
00369                 marker_name[marker_name_length] = '\0';
00370                 strncpy(parent_name, h->joint->parent->name, parent_name_length);
00371                 parent_name[parent_name_length] = '\0';
00372                 sprintf(t_name, "%s%c%s", marker_name, joint_name_separator, parent_name);
00373                 delete[] marker_name;
00374                 delete[] parent_name;
00375         }
00376         else
00377         {
00378                 t_name = new char [strlen(h->joint_name) + strlen(h->joint->parent->name) + 2];
00379                 sprintf(t_name, "%s%c%s", h->joint_name, joint_name_separator, h->joint->parent->name);
00380         }
00381         CalcPosition();
00382         TransformNode* tnode = new TransformNode();
00383         tnode->setName(t_name);
00384         tnode->setTranslation(h->joint->abs_pos(0), h->joint->abs_pos(1), h->joint->abs_pos(2));
00385         top_tnode->addChildNode(tnode);
00386         delete[] t_name;
00387         return 0;
00388 }
00389 
00390 #endif
00391 
00392 int IK::NumConstraints(ConstType t)
00393 {
00394         int count = 0;
00395         for(int i=0; i<n_constraints; i++)
00396         {
00397                 if(constraints[i]->GetType() == t) count++;
00398         }
00399         return count;
00400 }
00401 
00402 int IK::assign_constraints(int _n)
00403 {
00404         cerr << "assign_constraints(" << _n << ")" << endl;
00405         int i;
00406         IKConstraint** save = 0;
00407         if(constraints && n_assigned_constraints > 0)
00408         {
00409                 save = new IKConstraint* [n_assigned_constraints];
00410                 for(i=0; i<n_assigned_constraints; i++)
00411                         save[i] = constraints[i];
00412                 delete[] constraints;
00413         }
00414         constraints = 0;
00415         if(_n > 0)
00416         {
00417                 constraints = new IKConstraint* [_n];
00418                 for(i=0; i<_n; i++) constraints[i] = 0;
00419                 for(i=0; i<n_assigned_constraints; i++)
00420                         constraints[i] = save[i];
00421         }
00422         n_assigned_constraints = _n;
00423         if(save) delete[] save;
00424         cerr << "<-" << endl;
00425         return 0;
00426 }
00427 
00428 int IK::AddConstraint(IKConstraint* _constraint)
00429 {
00430 //      if(!_constraint->joint) return -1;
00431         if(n_constraints >= n_assigned_constraints)
00432                 assign_constraints(n_assigned_constraints*2);
00433         int id = n_total_const++;
00434         constraints[n_constraints++] = _constraint;
00435         _constraint->id = id;
00436         return id;
00437 }
00438 
00439 int IK::RemoveConstraint(int _id)
00440 {
00441         int index = ConstraintIndex(_id);
00442         if(index < 0) return -1;
00443         if(constraints[index]) delete constraints[index];
00444         constraints[index] = 0;
00445         int i;
00446         for(i=index+1; i<n_constraints; i++)
00447         {
00448                 constraints[i-1] = constraints[i];
00449         }
00450         constraints[--n_constraints] = 0;
00451         return 0;
00452 }
00453 
00454 int IK::RemoveAllConstraints()
00455 {
00456         for(int i=0; i<n_constraints; i++)
00457         {
00458                 if(constraints[i]) delete constraints[i];
00459                 constraints[i] = 0;
00460         }
00461         n_constraints = 0;
00462         return 0;
00463 }
00464 
00465 IKConstraint* IK::FindConstraint(ConstType _type, const char* jname, const char* charname)
00466 {
00467   static char fullname[256];
00468   if(charname) {
00469         sprintf(fullname, "%s%c%s", jname, charname_separator, charname);
00470   }
00471   else {
00472         strcpy(fullname, jname);
00473   }
00474         for(int i=0; i<n_constraints; i++)
00475         {
00476                 if(constraints[i]->GetType() == _type &&
00477                    !strcmp(fullname, constraints[i]->joint_name))
00478                         return constraints[i];
00479         }
00480         return 0;
00481 }
00482 
00483 IKConstraint* IK::FindConstraint(int _id)
00484 {
00485         for(int i=0; i<n_constraints; i++)
00486         {
00487                 if(constraints[i]->id == _id)
00488                         return constraints[i];
00489         }
00490         return 0;
00491 }
00492 
00493 int IK::ConstraintID(ConstType _type, const char* jname)
00494 {
00495         for(int i=0; i<n_constraints; i++)
00496         {
00497                 if(constraints[i]->GetType() == _type &&
00498                    !strcmp(jname, constraints[i]->joint_name))
00499                         return constraints[i]->id;
00500         }
00501         return -1;
00502 }
00503 
00504 int IK::ConstraintID(int _index)
00505 {
00506         if(_index < 0 || _index >= n_constraints)
00507                 return -1;
00508         return constraints[_index]->id;
00509 }
00510 
00511 int IK::ConstraintIndex(ConstType _type, const char* jname)
00512 {
00513         for(int i=0; i<n_constraints; i++)
00514         {
00515                 if(constraints[i]->GetType() == _type &&
00516                    !strcmp(jname, constraints[i]->joint_name))
00517                         return i;
00518         }
00519         return -1;
00520 }
00521 
00522 int IK::ConstraintIndex(int _id)
00523 {
00524         for(int i=0; i<n_constraints; i++)
00525         {
00526                 if(constraints[i]->id == _id)
00527                         return i;
00528         }
00529         return -1;
00530 }
00531 
00532 int IK::ResetAllConstraints()
00533 {
00534         int i;
00535         for(i=0; i<n_constraints; i++)
00536                 constraints[i]->Reset();
00537         return 0;
00538 }
00539 
00540 int IK::ResetConstraints(ConstType t)
00541 {
00542         int i;
00543         for(i=0; i<n_constraints; i++)
00544         {
00545                 if(constraints[i]->GetType() == t)
00546                         constraints[i]->Reset();
00547         }
00548         return 0;
00549 }
00550 
00551 int IK::SetJointWeight(const char* jname, double _weight)
00552 {
00553         double weight = _weight;
00554         if(weight < MIN_JOINT_WEIGHT) weight = MIN_JOINT_WEIGHT;
00555         Joint* jnt = FindJoint(jname);
00556         if(!jnt) return -1;
00557         int i;
00558         for(i=0; i<jnt->n_dof; i++)
00559         {
00560                 joint_weights(i+jnt->i_dof) = weight;
00561         }
00562         return 0;
00563 }
00564 
00565 int IK::SetJointWeight(const char* jname, const fVec& _weight)
00566 {
00567         Joint* jnt = FindJoint(jname);
00568         if(!jnt) return -1;
00569         int i;
00570         for(i=0; i<jnt->n_dof && i<_weight.size(); i++)
00571         {
00572                 if(_weight(i) >= MIN_JOINT_WEIGHT)
00573                         joint_weights(i+jnt->i_dof) = _weight(i);
00574         }
00575         return 0;
00576 }
00577 
00578 int IK::SetDesireGain(const char* jname, double _gain)
00579 {
00580         if(_gain < 0.0) return -1;
00581         IKDesire* des = (IKDesire*)FindConstraint(DESIRE_CONSTRAINT, jname);
00582         if(!des) return -1;
00583         des->gain = _gain;
00584         return 0;
00585 }
00586 
00587 int IK::EnableDesire(const char* jname)
00588 {
00589         IKDesire* des = (IKDesire*)FindConstraint(DESIRE_CONSTRAINT, jname);
00590         if(!des) return -1;
00591         des->Enable();
00592         cerr << "desire constraint at " << jname << " enabled" << endl;
00593         return 0;
00594 }
00595 
00596 int IK::DisableDesire(const char* jname)
00597 {
00598         IKDesire* des = (IKDesire*)FindConstraint(DESIRE_CONSTRAINT, jname);
00599         if(!des) return -1;
00600         des->Disable();
00601         cerr << "desire constraint at " << jname << " disabled" << endl;
00602         return 0;
00603 }
00604 
00605 void IK::SetCharacterScale(double _scale, const char* charname)
00606 {
00607         SetConstraintScale(_scale, charname);
00608         // set joint weights
00609         joint_weights.resize(n_dof);
00610         joint_weights = 1.0;
00611         set_character_scale(root, _scale, charname);
00612 }
00613 
00614 void IK::SetConstraintScale(double _scale, const char* charname)
00615 {
00616         int i;
00617         for(i=0; i<n_constraints; i++)
00618         {
00619                 constraints[i]->SetCharacterScale(_scale, charname);
00620         }
00621 }
00622 
00623 void IK::set_character_scale(Joint* jnt, double _scale, const char* charname)
00624 {
00625         if(!jnt) return;
00626         if((!charname || strstr(jnt->name, charname)) && jnt->n_dof > 0)
00627         {
00628                 // rotation -> s
00629                 switch(jnt->j_type)
00630                 {
00631                 case JROTATE:
00632                         joint_weights(jnt->i_dof) = _scale;
00633                         break;
00634                 case JSPHERE:
00635                         joint_weights(jnt->i_dof) = _scale;
00636                         joint_weights(jnt->i_dof+1) = _scale;
00637                         joint_weights(jnt->i_dof+2) = _scale;
00638                         break;
00639                 case JFREE:
00640                         joint_weights(jnt->i_dof+3) = _scale;
00641                         joint_weights(jnt->i_dof+4) = _scale;
00642                         joint_weights(jnt->i_dof+5) = _scale;
00643                         break;
00644                 default:
00645                         break;
00646                 }
00647         }
00648         set_character_scale(jnt->brother, _scale, charname);
00649         set_character_scale(jnt->child, _scale, charname);
00650 }
00651 
00652 void IKHandle::SetCharacterScale(double _scale, const char* charname)
00653 {
00654         if(!joint) return;
00655         if(n_const == 0) return;
00656         if(charname && !strstr(joint->name, charname)) return;  // not a strict check
00657         int i;
00658         weight.resize(n_const);
00659         weight = 1.0;
00660         int count = 0;
00661 //      double s1 = 1.0/_scale;
00662         double s1 = _scale;
00663         for(i=0; i<6; i++)
00664         {
00665                 if(const_index[i] == IK::HAVE_CONSTRAINT)
00666                 {
00667                         // rotation -> 1/s
00668                         if(i >= 3) weight(count) = s1;
00669                         count++;
00670                 }
00671         }
00672 }
00673 
00674 void IKDesire::SetCharacterScale(double _scale, const char* charname)
00675 {
00676         if(!joint) return;
00677         if(n_const == 0) return;
00678         if(charname && !strstr(joint->name, charname)) return;  // not a strict check
00679         weight.resize(n_const);
00680         if(_scale < 1.0)
00681                 weight = _scale;
00682         else
00683                 weight = 1.0;
00684 #if 0
00685 //      double s1 = 1.0/_scale;
00686         double s1 = _scale;
00687         // rotation -> 1/s
00688         switch(joint->j_type)
00689         {
00690         case JROTATE:
00691                 weight(0) = s1;
00692                 break;
00693         case JSPHERE:
00694                 weight(0) = s1;
00695                 weight(1) = s1;
00696                 weight(2) = s1;
00697                 break;
00698         case JFREE:
00699                 weight(3) = s1;
00700                 weight(4) = s1;
00701                 weight(5) = s1;
00702                 break;
00703         default:
00704                 break;
00705         }
00706 #endif
00707 }


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:54