init.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  * init.cpp
00011  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.18
00012  */
00013 
00014 #include "chain.h"
00015 
00016 /*
00017  * initialize
00018  */
00019 #ifdef SEGA
00020 int Chain::init()
00021 #else
00022 int Chain::init(SceneGraph* sg)
00023 #endif
00024 {
00025         // reset pointers
00026         if(all_value) delete[] all_value;
00027         if(all_value_dot) delete[] all_value_dot;
00028         if(all_vel) delete[] all_vel;
00029         if(all_vel_dot) delete[] all_vel_dot;
00030         if(j_acc_p[0]) delete[] j_acc_p[0];
00031         if(j_acc_p[1]) delete[] j_acc_p[1];
00032         if(j_acc_p[2]) delete[] j_acc_p[2];
00033         if(j_acc_p[3]) delete[] j_acc_p[3];
00034         if(j_value_dot[0]) delete[] j_value_dot[0];
00035         if(j_value_dot[1]) delete[] j_value_dot[1];
00036         if(j_value_dot[2]) delete[] j_value_dot[2];
00037         if(j_value_dot[3]) delete[] j_value_dot[3];
00038         if(init_value) delete[] init_value;
00039         if(init_vel) delete[] init_vel;
00040         n_value = 0;
00041         n_dof = 0;
00042         n_thrust = 0;
00043         n_joint = 0;
00044         all_value = 0;
00045         all_value_dot = 0;
00046         all_vel = 0;
00047         all_vel_dot = 0;
00048         j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0;
00049         j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0;
00050         init_value = 0;
00051         init_vel = 0;
00052         if(!root) return 0;
00053         // initialize
00054 #ifndef SEGA
00055         if(sg) set_relative_positions(sg);
00056 #endif
00057         root->init();
00058         if(n_value > 0)
00059         {
00060                 all_value = new double* [n_value];
00061                 all_value_dot = new double* [n_value];
00062                 j_value_dot[0] = new double [n_value];
00063                 j_value_dot[1] = new double [n_value];
00064                 j_value_dot[2] = new double [n_value];
00065                 j_value_dot[3] = new double [n_value];
00066                 init_value = new double [n_value];
00067         }
00068         if(n_dof > 0)
00069         {
00070                 all_vel = new double* [n_dof];
00071                 all_vel_dot = new double* [n_dof];
00072                 j_acc_p[0] = new double [n_dof];
00073                 j_acc_p[1] = new double [n_dof];
00074                 j_acc_p[2] = new double [n_dof];
00075                 j_acc_p[3] = new double [n_dof];
00076                 init_vel = new double [n_dof];
00077         }
00078         root->init_arrays();
00079         CalcPosition();
00080         root->init_virtual();
00081 #ifndef SEGA
00082         if(sg)
00083         {
00084                 init_scale(sg);
00085         }
00086         apply_scale();
00087 #endif
00088         return 0;
00089 }
00090 
00091 #ifndef SEGA
00092 void Chain::init_scale(SceneGraph* sg)
00093 {
00094         init_scale_sub(sg->getNodes());
00095 }
00096 
00097 void Chain::init_scale_sub(Node* node)
00098 {
00099         if(!node) return;
00100         char* name = 0;
00101         if(node->isTransformNode() && (name = node->getName()) && strstr(name, ScaleString))
00102         {
00103                 char* jointname = name + strlen(ScaleString);
00104                 float scale[3];
00105                 ((TransformNode*)node)->getScale(scale);
00106                 scale_object _s;
00107                 _s.set_joint_name(jointname, 0);  // jointname includes character name
00108                 _s.scale = scale[0];
00109                 add_scale_object(_s);
00110         }
00111         init_scale_sub(node->next());
00112         init_scale_sub(node->getChildNodes());
00113 }
00114 
00115 void Chain::ApplyGeomScale(SceneGraph* sg)
00116 {
00117     apply_geom_scale(sg, root);
00118 }
00119 
00120 void Chain::apply_geom_scale(SceneGraph* sg, Joint* cur)
00121 {
00122     if(!cur) return;
00123     TransformNode* mytrans = sg->findTransformNode(cur->name);
00124     // mytrans uses new scale to enlarge the geometry
00125     if(mytrans)
00126     {
00127                 float new_scale = cur->cur_scale;
00128 //        mytrans->setScale(new_scale, new_scale, new_scale);
00129     }
00130     apply_geom_scale(sg, cur->brother);
00131     apply_geom_scale(sg, cur->child);
00132 }
00133 #endif
00134 
00135 void Joint::init()
00136 {
00137 //      if(!realname)
00138         {
00139         if(parent) n_root_dof = parent->n_root_dof;
00140         if(t_given)
00141         {
00142                 i_value = chain->n_value;
00143                 i_dof = chain->n_dof;
00144                 i_thrust = -1;
00145                 n_root_dof += n_dof;
00146         }
00147         else
00148         {
00149                 i_value = -1;
00150                 i_dof = -1;
00151                 i_thrust = chain->n_thrust;
00152                 n_root_dof += n_thrust;
00153         }
00154         i_joint = chain->n_joint;
00155         chain->n_joint++;
00156         switch(j_type)
00157         {
00158         case JROTATE:
00159         case JSLIDE:
00160                 if(t_given)
00161                 {
00162                         chain->n_value++;
00163                         chain->n_dof++;
00164                 }
00165                 else
00166                 {
00167                         chain->n_thrust++;
00168                 }
00169                 break;
00170         case JSPHERE:
00171                 if(t_given)
00172                 {
00173                         chain->n_value += 4; // Euler Parameters
00174                         chain->n_dof += 3;
00175                 }
00176                 else
00177                 {
00178                         chain->n_thrust += 3;
00179                 }
00180                 break;
00181         case JFREE:
00182                 if(t_given)
00183                 {
00184                         chain->n_value += 7;
00185                         chain->n_dof += 6;
00186                 }
00187                 else
00188                 {
00189                         chain->n_thrust += 6;
00190                 }
00191                 break;
00192         case JFIXED:
00193                 break;
00194         default:
00195                 cerr << "warning: joint type not set for " << name << endl;
00196                 break;
00197         }
00198 
00199         }
00200         child->init();
00201         brother->init();
00202 }
00203 
00204 void Joint::init_arrays()
00205 {
00206 //      if(!realname)
00207         {
00208                 
00209         int i;
00210         switch(j_type)
00211         {
00212         case JROTATE:
00213         case JSLIDE:
00214                 if(t_given)
00215                 {
00216                         chain->all_value[i_value] = &q;
00217                         chain->all_value_dot[i_value] = &qd;
00218                         chain->all_vel[i_dof] = &qd;
00219                         chain->all_vel_dot[i_dof] = &qdd;
00220                 }
00221                 break;
00222         case JSPHERE:
00223                 if(t_given)
00224                 {
00225                         for(i=0; i<4; i++)
00226                         {
00227                                 chain->all_value[i_value+i] = &rel_ep(i);
00228                                 chain->all_value_dot[i_value+i] = &p_ep_dot(i);
00229                         }
00230                         for(i=0; i<3; i++)
00231                         {
00232                                 chain->all_vel[i_dof+i] = &p_ang_vel(i);
00233                                 chain->all_vel_dot[i_dof+i] = &p_ang_acc(i);
00234                         }
00235                 }
00236                 break;
00237         case JFREE:
00238                 if(t_given)
00239                 {
00240                         for(i=0; i<3; i++)
00241                         {
00242                                 chain->all_value[i_value+i] = &rel_pos(i);
00243                                 chain->all_value_dot[i_value+i] = &p_lin_vel(i);
00244                         }
00245                         for(i=0; i<4; i++)
00246                         {
00247                                 chain->all_value[i_value+3+i] = &rel_ep(i);
00248                                 chain->all_value_dot[i_value+3+i] = &p_ep_dot(i);
00249                         }
00250                         for(i=0; i<3; i++)
00251                         {
00252                                 chain->all_vel[i_dof+i] = &p_lin_vel(i);
00253                                 chain->all_vel_dot[i_dof+i] = &p_lin_acc(i);
00254                         }
00255                         for(i=0; i<3; i++)
00256                         {
00257                                 chain->all_vel[i_dof+3+i] = &p_ang_vel(i);
00258                                 chain->all_vel_dot[i_dof+3+i] = &p_ang_acc(i);
00259                         }
00260                 }
00261                 break;
00262         default:
00263                 break;
00264         }
00265 
00266         }
00267         child->init_arrays();
00268         brother->init_arrays();
00269 }
00270 
00271 void Joint::init_virtual()
00272 {
00273         _init_virtual();
00274         brother->init_virtual();
00275         child->init_virtual();
00276 }
00277 
00278 void Joint::_init_virtual()
00279 {
00280         if(!realname) return;
00281         real = chain->FindJoint(realname);
00282         if(!real)
00283         {
00284                 cerr << "warning: could not find real joint " << realname << " of " << name << endl;
00285                 return;
00286         }
00287         if(real->real)
00288         {
00289                 cerr << "error: real joint " << realname << " of " << name << " is also virtual" << endl;
00290                 real = 0;
00291                 return;
00292         }
00293         // relative position & orientation in real's frame
00294         fVec3 pp;
00295         fMat33 Rt, IR;
00296         Rt.tran(real->abs_att);
00297         pp.sub(abs_pos, real->abs_pos);
00298         rpos_real.mul(Rt, pp);
00299         ratt_real.mul(Rt, abs_att);
00300         // set mass, COM, inertia in virtual joint's frame
00301         mass = real->mass;
00302         pp.sub(real->loc_com, rpos_real);  // vector from origin to com, in real joint's frame
00303         loc_com.mul(pp, ratt_real);  // transform to local frame
00304         IR.mul(real->inertia, ratt_real);
00305         inertia.mul(tran(ratt_real), IR);
00306 #if 0  // debug
00307         cerr << "-- " << name << endl;
00308         cerr << "rpos_real = " << rpos_real << endl;
00309         cerr << "ratt_real = " << ratt_real << endl;
00310         cerr << "mass = " << mass << endl;
00311         cerr << "loc_com = " << loc_com << endl;
00312         cerr << "inertia = " << inertia << endl;
00313 #endif
00314 }


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