chain.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  * chain.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.18
00012  */
00013 
00014 #include "chain.h"
00015 #include <string.h>
00016 
00017 char* CharName(const char* _name)
00018 {
00019         char* ret = (char*)strrchr(_name, charname_separator);
00020         if(ret) return ret+1;
00021         return 0;
00022 }
00023 
00024 int Chain::SaveStatus(fVec& value, fVec& vel, fVec& acc)
00025 {
00026         value.resize(n_value);
00027         vel.resize(n_dof);
00028         acc.resize(n_dof);
00029         GetJointValue(value);
00030         GetJointVel(vel);
00031         GetJointAcc(acc);
00032         return 0;
00033 }
00034 
00035 int Chain::SetStatus(const fVec& values, const fVec& vels, const fVec& accs)
00036 {
00037         SetJointValue(values);
00038         SetJointVel(vels);
00039         SetJointAcc(accs);
00040         return 0;
00041 }
00042 
00043 /*
00044  * constructors and destructors
00045  */
00046 Chain::Chain()
00047 {
00048         root = NULL;
00049         n_value = 0;
00050         n_dof = 0;
00051         n_thrust = 0;
00052         n_joint = 0;
00053         in_create_chain = false;
00054         all_value = 0;
00055         all_value_dot = 0;
00056         all_vel = 0;
00057         all_vel_dot = 0;
00058         j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0;
00059         j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0;
00060         init_value = 0;
00061         init_vel = 0;
00062         do_connect = false;
00063 }
00064 
00065 Chain::~Chain()
00066 {
00067         if(root) delete root;
00068         if(all_value) delete[] all_value;
00069         if(all_value_dot) delete[] all_value_dot;
00070         if(all_vel) delete[] all_vel;
00071         if(all_vel_dot) delete[] all_vel_dot;
00072         if(j_acc_p[0]) delete[] j_acc_p[0];
00073         if(j_acc_p[1]) delete[] j_acc_p[1];
00074         if(j_acc_p[2]) delete[] j_acc_p[2];
00075         if(j_acc_p[3]) delete[] j_acc_p[3];
00076         if(j_value_dot[0]) delete[] j_value_dot[0];
00077         if(j_value_dot[1]) delete[] j_value_dot[1];
00078         if(j_value_dot[2]) delete[] j_value_dot[2];
00079         if(j_value_dot[3]) delete[] j_value_dot[3];
00080         if(init_value) delete[] init_value;
00081         if(init_vel) delete[] init_vel;
00082 #ifndef SEGA
00083         clear_scale_object_list();
00084 #endif
00085 }
00086 
00087 void Chain::Clear()
00088 {
00089         if(root) delete root;
00090         root = 0;
00091         n_value = 0;
00092         n_dof = 0;
00093         n_thrust = 0;
00094         n_joint = 0;
00095         if(all_value) delete[] all_value;
00096         all_value = 0;
00097         if(all_value_dot) delete[] all_value_dot;
00098         all_value_dot = 0;
00099         if(all_vel) delete[] all_vel;
00100         all_vel = 0;
00101         if(all_vel_dot) delete[] all_vel_dot;
00102         all_vel_dot = 0;
00103         if(j_acc_p[0]) delete[] j_acc_p[0];
00104         if(j_acc_p[1]) delete[] j_acc_p[1];
00105         if(j_acc_p[2]) delete[] j_acc_p[2];
00106         if(j_acc_p[3]) delete[] j_acc_p[3];
00107         if(j_value_dot[0]) delete[] j_value_dot[0];
00108         if(j_value_dot[1]) delete[] j_value_dot[1];
00109         if(j_value_dot[2]) delete[] j_value_dot[2];
00110         if(j_value_dot[3]) delete[] j_value_dot[3];
00111         if(init_value) delete[] init_value;
00112         if(init_vel) delete[] init_vel;
00113         j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0;
00114         j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0;
00115         init_value = 0;
00116         init_vel = 0;
00117         in_create_chain = false;
00118         do_connect = false;
00119 }
00120 
00121 Joint::Joint()
00122 {
00123         name = 0;
00124         basename = 0;
00125         realname = 0;
00126         cur_scale = 1.0;
00127         clear();
00128 }
00129 
00130 Joint::Joint(const char* _name, JointType jt, 
00131                          const fVec3& rpos, const fMat33& ratt, AxisIndex ai,
00132                          int _t_given)
00133 {
00134         name = 0;
00135         basename = 0;
00136         realname = 0;
00137         cur_scale = 1.0;
00138         clear();
00139         if(_name)
00140         {
00141                 name = new char [strlen(_name) + 1];
00142                 strcpy(name, _name);
00143                 char* charname = strrchr(name, charname_separator);
00144                 if(charname) *charname = '\0';
00145                 basename = new char [strlen(name) + 1];
00146                 strcpy(basename, name);
00147                 if(charname) *charname = charname_separator;
00148         }
00149         t_given = _t_given;
00150         switch(jt)
00151         {
00152         case JFIXED:
00153                 SetFixedJointType(rpos, ratt);
00154                 break;
00155         case JROTATE:
00156                 SetRotateJointType(rpos, ratt, ai);
00157                 break;
00158         case JSLIDE:
00159                 SetSlideJointType(rpos, ratt, ai);
00160                 break;
00161         case JSPHERE:
00162                 SetSphereJointType(rpos, ratt);
00163                 break;
00164         case JFREE:
00165                 SetFreeJointType(rpos, ratt);
00166                 break;
00167         case JUNKNOWN:
00168                 break;
00169         }
00170 }
00171 
00172 Joint::Joint(JointData* jdata, const char* charname)
00173 {
00174         name = 0;
00175         basename = 0;
00176         realname = 0;
00177         cur_scale = 1.0;
00178         clear();
00179         if(jdata->name)
00180         {
00181                 if(charname)
00182                 {
00183                         name = new char [strlen(jdata->name) + strlen(charname) + 2];
00184                         sprintf(name, "%s%c%s", jdata->name, charname_separator, charname);
00185                         basename = new char [strlen(jdata->name) + 1];
00186                         strcpy(basename, jdata->name);
00187                 }
00188                 else
00189                 {
00190                         name = new char [strlen(jdata->name) + 1];
00191                         strcpy(name, jdata->name);
00192                         // character name included?
00193                         char* _ch = strrchr(name, charname_separator);
00194                         if(_ch) *_ch = '\0';
00195                         basename = new char [strlen(name) + 1];
00196                         strcpy(basename, name);
00197                         if(_ch) *_ch = charname_separator;
00198                 }
00199         }
00200         mass = jdata->mass;
00201         inertia.set(jdata->inertia);
00202         loc_com.set(jdata->com);
00203         t_given = jdata->t_given;
00204         switch(jdata->j_type)
00205         {
00206         case JFIXED:
00207                 SetFixedJointType(jdata->rel_pos, jdata->rel_att);
00208                 break;
00209         case JROTATE:
00210                 SetRotateJointType(jdata->rel_pos, jdata->rel_att, jdata->axis_index);
00211                 break;
00212         case JSLIDE:
00213                 SetSlideJointType(jdata->rel_pos, jdata->rel_att, jdata->axis_index);
00214                 break;
00215         case JSPHERE:
00216                 SetSphereJointType(jdata->rel_pos, jdata->rel_att);
00217                 break;
00218         case JFREE:
00219                 SetFreeJointType(jdata->rel_pos, jdata->rel_att);
00220                 break;
00221         default:
00222                 break;
00223         }
00224 }
00225 
00226 void Joint::clear()
00227 {
00228         if(name) delete[] name;
00229         if(basename) delete[] basename;
00230         if(realname) delete[] realname;
00231         name = 0;
00232         basename = 0;
00233         realname = 0;
00234         chain = NULL;
00235         parent = NULL;
00236         brother = NULL;
00237         child = NULL;
00238         real = NULL;
00239         rpos_real.zero();
00240         ratt_real.identity();
00241         j_type = JUNKNOWN;
00242         t_given = true;
00243         n_dof = 0;
00244         n_thrust = 0;
00245         n_root_dof = 0;
00246 
00247         q = qd = qdd = 0.0;
00248         axis.zero();
00249         init_pos.zero();
00250         init_att.identity();
00251         rel_lin_vel.zero();
00252         rel_ang_vel.zero();
00253         rel_lin_acc.zero();
00254         rel_ang_acc.zero();
00255 
00256         rel_pos.zero();
00257         rel_att.identity();
00258         mass = 0.0;
00259         inertia.zero();
00260         loc_com.zero();
00261         gear_ratio = 1.0;
00262         rotor_inertia = 0.0;
00263         i_value = -1;
00264         i_dof = -1;
00265         i_thrust = -1;
00266         i_joint = -1;
00267 
00268         abs_pos.zero();
00269         abs_att.identity();
00270         loc_lin_vel.zero();
00271         loc_ang_vel.zero();
00272         loc_lin_acc.zero();
00273         loc_ang_acc.zero();
00274         loc_com_acc.zero();
00275         loc_com_vel.zero();
00276 
00277         p_lin_vel.zero();
00278         p_ep_dot.zero();
00279         p_ang_vel.zero();
00280         p_lin_acc.zero();
00281         p_ang_acc.zero();
00282 
00283         ext_force.zero();
00284         ext_moment.zero();
00285         joint_f.zero();
00286         joint_n.zero();
00287 
00288         tau=0.0;
00289 
00290 }
00291 
00292 Joint::~Joint()
00293 {
00294         if(name) delete[] name;
00295         if(basename) delete[] basename;
00296         if(realname) delete[] realname;
00297         if(brother) delete brother;
00298         if(child) delete child;
00299 }
00300 
00301 void Joint::SetJointData(JointData* jdata, const char* charname)
00302 {
00303         clear();
00304         if(jdata->name)
00305         {
00306                 name = new char [strlen(jdata->name) + 1];
00307                 strcpy(name, jdata->name);
00308         }
00309         mass = jdata->mass;
00310         inertia.set(jdata->inertia);
00311         loc_com.set(jdata->com);
00312         t_given = jdata->t_given;
00313         switch(jdata->j_type)
00314         {
00315         case JFIXED:
00316                 SetFixedJointType(jdata->rel_pos, jdata->rel_att);
00317                 break;
00318         case JROTATE:
00319                 SetRotateJointType(jdata->rel_pos, jdata->rel_att, jdata->axis_index);
00320                 break;
00321         case JSLIDE:
00322                 SetSlideJointType(jdata->rel_pos, jdata->rel_att, jdata->axis_index);
00323                 break;
00324         case JSPHERE:
00325                 SetSphereJointType(jdata->rel_pos, jdata->rel_att);
00326                 break;
00327         case JFREE:
00328                 SetFreeJointType(jdata->rel_pos, jdata->rel_att);
00329                 break;
00330         default:
00331                 break;
00332         }
00333 }
00334 
00335 /*
00336  * utilities
00337  */
00338 int Chain::GetJointNameList(char**& jnames)
00339 {
00340         jnames = NULL;
00341         if(in_create_chain)
00342         {
00343                 cerr << "Chain::GetJointNameList - error: cannot be called between BeginCreateChain() and EndCreateChain()" << endl;
00344                 return -1;
00345         }
00346         if(n_joint > 0)
00347         {
00348                 jnames = new char* [n_joint];
00349                 root->get_joint_name_list(jnames);
00350         }
00351         return n_joint;
00352 }
00353 
00354 void Joint::get_joint_name_list(char** jnames)
00355 {
00356         if(i_joint >= 0)
00357         {
00358                 jnames[i_joint] = new char [strlen(name) + 1];
00359                 strcpy(jnames[i_joint], name);
00360         }
00361         child->get_joint_name_list(jnames);
00362         brother->get_joint_name_list(jnames);
00363 }
00364 
00365 int Chain::GetJointList(Joint**& joints)
00366 {
00367         joints = NULL;
00368         if(in_create_chain)
00369         {
00370                 cerr << "Chain::GetJointList - error: cannot be called between BeginCreateChain() and EndCreateChain()" << endl;
00371                 return -1;
00372         }
00373         if(n_joint > 0)
00374         {
00375                 joints = new Joint* [n_joint];
00376                 root->get_joint_list(joints);
00377         }
00378         return n_joint;
00379 }
00380 
00381 void Joint::get_joint_list(Joint** joints)
00382 {
00383         if(i_joint >= 0 && !real)
00384         {
00385                 joints[i_joint] = this;
00386         }
00387         child->get_joint_list(joints);
00388         brother->get_joint_list(joints);
00389 }
00390 
00391 Joint* Chain::FindJoint(const char* n, const char* charname)
00392 {
00393         return root->find_joint(n, charname);
00394 }
00395 
00396 Joint* Joint::find_joint(const char* n, const char* charname)
00397 {
00398         if(charname)
00399         {
00400                 char* mych = CharName();
00401                 if(mych
00402                    && !strcmp(basename, n)
00403                    && !strcmp(mych, charname)) return this;
00404         }
00405         else
00406         {
00407                 if(!strcmp(name, n)) return this;
00408         }
00409         Joint* ret;
00410         if((ret = child->find_joint(n, charname))) return ret;
00411         if((ret = brother->find_joint(n, charname))) return ret;
00412         return NULL;
00413 }
00414 
00415 Joint* Chain::FindJoint(int _id)
00416 {
00417         return root->find_joint(_id);
00418 }
00419 
00420 Joint* Joint::find_joint(int _id)
00421 {
00422         if(i_joint == _id) return this;
00423         Joint* ret;
00424         if((ret = child->find_joint(_id))) return ret;
00425         if((ret = brother->find_joint(_id))) return ret;
00426         return NULL;
00427 }
00428 
00429 Joint* Chain::FindCharacterRoot(const char* charname)
00430 {
00431         if(!root) return 0;
00432         Joint* j;
00433         for(j=root->child; j; j=j->brother)
00434         {
00435                 char* ch = j->CharName();
00436                 if(ch && !strcmp(ch, charname)) return j;
00437         }
00438         return 0;
00439 }
00440 
00441 int Joint::DescendantDOF()
00442 {
00443         return child->descendant_dof();
00444 }
00445 
00446 int Joint::descendant_dof()
00447 {
00448         int ret1 = brother->descendant_dof();
00449         int ret2 = child->descendant_dof();
00450         return (ret1 + ret2 + n_dof);
00451 }
00452 
00453 int Joint::DescendantNumJoints()
00454 {
00455         return (1+child->descendant_num_joints());
00456 }
00457 
00458 int Joint::descendant_num_joints()
00459 {
00460         int ret1 = brother->descendant_num_joints();
00461         int ret2 = child->descendant_num_joints();
00462         return (ret1 + ret2 + 1);
00463 }
00464 
00465 int Joint::isDescendant(Joint* target)
00466 {
00467         return is_descendant(child, target);
00468 }
00469 
00470 int Joint::is_descendant(Joint* cur, Joint* target)
00471 {
00472         if(!cur) return false;
00473         if(cur == target) return true;
00474         if(is_descendant(cur->brother, target)) return true;
00475         if(is_descendant(cur->child, target)) return true;
00476         return false;
00477 }
00478 
00479 int Joint::isAscendant(Joint* target)
00480 {
00481         Joint* p;
00482         for(p=this; p; p=p->parent)
00483         {
00484                 if(p == target) return true;
00485         }
00486         return false;
00487 }


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:15