psim.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  */
00014 #include "psim.h"
00015 #include <assert.h>
00016 
00017 int pSim::ConstraintForces(fVec& cf)
00018 {
00019         int i;
00020         for(i=0; i<n_joint; i++)
00021         {
00022                 fVec& f_final = joint_info[i].pjoints[0]->f_final;
00023                 int idx = 6*joint_info[i].pjoints[0]->joint->i_joint;
00024                 cf(idx) = f_final(0);
00025                 cf(idx+1) = f_final(1);
00026                 cf(idx+2) = f_final(2);
00027                 cf(idx+3) = f_final(3);
00028                 cf(idx+4) = f_final(4);
00029                 cf(idx+5) = f_final(5);
00030         }
00031         return 0;
00032 }
00033 
00034 int pSim::TotalCost()
00035 {
00036         return subchains->total_cost();
00037 }
00038 
00039 int pSubChain::total_cost()
00040 {
00041         if(!this) return 0;
00042         // cost is proportional to square of n_outer_joints
00043         int ret = 0;
00044         if(n_links > 1) ret = n_outer_joints*n_outer_joints;
00045         ret += children[0]->total_cost();
00046         if(children[0] != children[1])
00047                 ret += children[1]->total_cost();
00048         return ret;
00049 }
00050 
00051 int pSim::ScheduleDepth()
00052 {
00053         return subchains->schedule_depth();
00054 }
00055 
00056 int pSubChain::schedule_depth()
00057 {
00058         if(!this || n_links == 1) return 0;
00059         int ret0 = 0, ret1 = 0;
00060         if(children[0]) ret0 = children[0]->schedule_depth();
00061         if(children[1]) ret1 = children[1]->schedule_depth();
00062         return (ret0 > ret1) ? (ret0+1) : (ret1+1);
00063 }
00064 
00065 int pSim::NumLeaves()
00066 {
00067         return subchains->num_leaves();
00068 }
00069 
00070 int pSubChain::num_leaves()
00071 {
00072         if(!this || n_links == 1) return 0;
00073         if(children[0] && children[1] &&
00074            children[0]->n_links == 1 && children[1]->n_links == 1)
00075         {
00076                 return 1;
00077         }
00078         int ret = children[0]->num_leaves();
00079         if(children[0] != children[1])
00080                 ret += children[1]->num_leaves();
00081         return ret;
00082 }
00083 
00087 void pSim::Clear()
00088 {
00089         if(joint_info) 
00090         {
00091                 for(int i=0; i<n_joint; i++)
00092                 {
00093                         delete joint_info[i].pjoints[0];
00094                         delete joint_info[i].pjoints[1];
00095                         if(joint_info[i].plink) delete joint_info[i].plink;
00096                 }
00097                 delete[] joint_info;
00098         }
00099         if(subchains) delete subchains;
00100         joint_info = 0;
00101         subchains = 0;
00102         Chain::Clear();
00103 }
00104 
00105 int pSim::clear_data()
00106 {
00107         if(joint_info)
00108         {
00109                 for(int i=0; i<n_joint; i++)
00110                 {
00111                         delete joint_info[i].pjoints[0];
00112                         delete joint_info[i].pjoints[1];
00113                         if(joint_info[i].plink) delete joint_info[i].plink;
00114                 }
00115                 delete[] joint_info;
00116         }
00117         if(subchains) delete subchains;
00118         joint_info = 0;
00119         subchains = 0;
00120         return Chain::clear_data();
00121 }
00122 
00123 int pSim::clear_contact()
00124 {
00125         int n_contacts = contact_vjoints.size();
00126         if(n_contacts == 0) return 0;
00127         int org_n_joint = n_joint;
00128         JointInfo* jinfo_save = joint_info;
00129         for(joint_list::iterator j=contact_vjoints.begin(); j!=contact_vjoints.end(); j++)
00130         {
00131                 RemoveJoint(*j);
00132                 delete *j;
00133         }
00134         // clear and init Chain's data
00135         Chain::clear_data();
00136 #ifdef SEGA
00137         Chain::init();
00138 #else
00139         Chain::init(0);
00140 #endif
00141         joint_info = new JointInfo [n_joint];
00142         for(int i=0; i<org_n_joint; i++)
00143         {
00144                 if(contact_vjoint_index(jinfo_save[i].pjoints[0]->joint) < 0)
00145                 {
00146                         Joint* j = jinfo_save[i].pjoints[0]->joint;
00147                         joint_info[j->i_joint].pjoints[0] = jinfo_save[i].pjoints[0];
00148                         joint_info[j->i_joint].pjoints[1] = jinfo_save[i].pjoints[1];
00149                         joint_info[j->i_joint].plink = jinfo_save[i].plink;
00150                 }
00151                 else
00152                 {
00153                         delete jinfo_save[i].pjoints[0];
00154                         delete jinfo_save[i].pjoints[1];
00155                         if(jinfo_save[i].plink) delete jinfo_save[i].plink;
00156                 }
00157         }
00158         if(jinfo_save) delete[] jinfo_save;
00159         contact_vjoints.clear();
00160         contact_relvels.clear();
00161         fric_coefs.clear();
00162         all_vjoints.clear();
00163         all_Jv.clear();
00164         all_Jr.clear();
00165         all_jdot_v.clear();
00166         all_jdot_r.clear();
00167         return 0;
00168 }
00169 
00173 int pSim::init_contact()
00174 {
00175         int n_contacts = contact_vjoints.size();
00176 //      if(n_contacts == 0) return 0;
00177         int org_n_joint = n_joint;
00178         // clear and init Chain's data
00179         Chain::clear_data();
00180 #ifdef SEGA
00181         Chain::init();
00182 #else
00183         Chain::init(0);
00184 #endif
00185         // create pjoint and plink for contact joints
00186         JointInfo* jinfo_save = joint_info;
00187         joint_info = new JointInfo [n_joint];
00188         for(int i=0; i<org_n_joint; i++)
00189         {
00190                 int new_index = jinfo_save[i].pjoints[0]->joint->i_joint;
00191                 joint_info[new_index].pjoints[0] = jinfo_save[i].pjoints[0];
00192                 joint_info[new_index].pjoints[1] = jinfo_save[i].pjoints[1];
00193                 joint_info[new_index].plink = jinfo_save[i].plink;
00194         }
00195         // contact joints
00196         for(int i=0; i<n_contacts; i++)
00197         {
00198                 Joint* j = contact_vjoints[i];
00199                 assert(j->real);  // must be virtual joint
00200                 pJoint* pj0 = new pJoint(j, j->real);
00201                 pJoint* pj1 = new pJoint(j, j->parent);
00202                 pj0->plink = joint_info[j->real->i_joint].plink;
00203                 if(j->parent)
00204                         pj1->plink = joint_info[j->parent->i_joint].plink;
00205                 pj1->pair = pj0;
00206                 joint_info[j->i_joint].pjoints[0] = pj0;
00207                 joint_info[j->i_joint].pjoints[1] = pj1;
00208                 joint_info[j->i_joint].plink = 0;
00209         }
00210         if(jinfo_save) delete[] jinfo_save;
00211         subchains->init();
00212         return 0;
00213 }
00214 
00215 #ifdef SEGA
00216 int pSim::init()
00217 #else
00218 int pSim::init(SceneGraph* sg)
00219 #endif
00220 {
00221 #ifdef SEGA
00222         int ret = Chain::init();
00223 #else
00224         int ret = Chain::init(sg);
00225 #endif
00226         if(ret) return ret;
00227         myinit();
00228         return 0;
00229 }
00230 
00231 int pSim::myinit()
00232 {
00233         if(joint_info) delete[] joint_info;
00234         joint_info = 0;
00235         if(n_joint == 0) return 0;
00236         joint_info = new JointInfo[n_joint];
00237         setup_pjoint(root);
00238         setup_pjoint_virtual(root);
00239         calc_consts();
00240         return 0;
00241 }
00242 
00243 void pSim::setup_pjoint(Joint* j)
00244 {
00245         if(!j) return;
00246         if(j->real) return;  // process virtual links later
00247         if(j->i_joint >= 0)
00248         {
00249                 pJoint* pj0 = new pJoint(j, j);
00250                 pJoint* pj1 = new pJoint(j, j->parent);
00251                 pLink* pl = new pLink(j);
00252                 pj0->plink = pl;
00253                 if(j->parent)
00254                         pj1->plink = joint_info[j->parent->i_joint].plink;
00255                 pj0->pair = pj1;
00256                 pj1->pair = pj0;
00257                 joint_info[j->i_joint].pjoints[0] = pj0;
00258                 joint_info[j->i_joint].pjoints[1] = pj1;
00259                 joint_info[j->i_joint].plink = pl;
00260         }
00261         setup_pjoint(j->brother);
00262         setup_pjoint(j->child);
00263 }
00264 
00265 void pSim::setup_pjoint_virtual(Joint* j)
00266 {
00267         if(!j) return;
00268         if(j->real)
00269         {
00270                 pJoint* pj0 = new pJoint(j, j->real);
00271                 pJoint* pj1 = new pJoint(j, j->parent);
00272 //              pJoint* pj0 = new pJoint(j, j->parent);
00273 //              pJoint* pj1 = new pJoint(j, j->real);
00274                 pj0->plink = joint_info[j->real->i_joint].plink;
00275                 if(j->parent)
00276                         pj1->plink = joint_info[j->parent->i_joint].plink;
00277                 pj1->pair = pj0;
00278                 joint_info[j->i_joint].pjoints[0] = pj0;
00279                 joint_info[j->i_joint].pjoints[1] = pj1;
00280                 joint_info[j->i_joint].plink = 0;
00281         }
00282         setup_pjoint_virtual(j->brother);
00283         setup_pjoint_virtual(j->child);
00284 }
00285 
00286 void pSim::calc_consts()
00287 {
00288         int i;
00289         for(i=0; i<n_joint; i++)
00290         {
00291                 if(joint_info[i].plink) joint_info[i].plink->calc_inertia();
00292         }
00293 }
00294 
00295 void pLink::calc_inertia()
00296 {
00297         if(joint->n_root_dof == 0) return;
00298 //      if(!joint->parent) return;  // space
00299         static fMat33 m11, m12, m22;  // m21 equals to m12
00300         double sx = joint->loc_com(0), sy = joint->loc_com(1), sz = joint->loc_com(2);
00301         fMat33 scross(0, -sz, sy,
00302                                   sz, 0, -sx,
00303                                   -sy, sx, 0);
00304         m11.identity();
00305         m11 *= joint->mass;
00306         m12.mul(joint->mass, scross);
00307         m22.mul(scross, scross);
00308         m22 *= -joint->mass;
00309         m22 += joint->inertia;
00310         if(joint->j_type == JROTATE)
00311         {
00312                 static fVec3 n2J;
00313                 static fMat33 n2Jmat;
00314                 n2J.mul(joint->gear_ratio*joint->gear_ratio*joint->rotor_inertia, joint->axis);
00315                 n2Jmat.mul(n2J, n2J);
00316                 m22 += n2Jmat;
00317         }
00318         int i, j;
00319         for(i=0; i<3; i++)
00320         {
00321                 for(j=0; j<3; j++)
00322                 {
00323                         M(i, j) = m11(i, j);
00324                         M(i, j+3) = -m12(i, j);
00325                         M(i+3, j) = m12(i, j);
00326                         M(i+3, j+3) = m22(i, j);
00327                 }
00328         }
00329         Minv.inv_posv(M);
00330 //      Minv.inv_porfs(M);
00331 }
00332 
00333 /*
00334  * Dump
00335  */
00336 void pSim::DumpSchedule(ostream& ost)
00337 {
00338         subchains->dump(ost);
00339 }
00340 
00341 void pSubChain::dump(ostream& ost)
00342 {
00343         if(!this) return;
00344         int i;
00345         ost << "-- pSubChain ->" << endl;
00346         if(last_pjoints[0]) ost << "\tlast: " << last_pjoints[0]->joint->name << endl;
00347         else ost << "\tsingle link" << endl;
00348         if(parent) ost << "\tparent: " << parent->last_pjoints[0]->joint->name << endl;
00349         ost << "\touter: " << n_outer_joints << endl;
00350         for(i=0; i<n_outer_joints; i++)
00351                 outer_joints[i]->dump(ost);
00352         ost << "\tlinks: " << n_links << endl;
00353         for(i=0; i<n_links; i++)
00354                 links[i]->dump(ost);
00355 #ifdef USE_MPI
00356         ost << "\trank: " << rank << endl;
00357 #endif
00358         ost << "<-" << endl;
00359         children[0]->dump(ost);
00360         if(children[1] != children[0]) children[1]->dump(ost);
00361 }
00362 
00363 void pJoint::dump(ostream& ost)
00364 {
00365         ost << "\t\t" << joint->name << "->" << link_side->name << endl;
00366 }
00367 
00368 void pLink::dump(ostream& ost)
00369 {
00370         ost << "\t\t" << joint->name << endl;
00371 }


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