psim.h
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  */
00016 #ifndef __PSIM_H__
00017 #define __PSIM_H__
00018 
00019 #include <chain.h>
00020 #include <fMatrix3.h>
00021 #include <list>
00022 #include <vector>
00023 
00024 class pLink;
00025 class pSim;
00026 
00027 struct JointInfo;
00028 typedef std::list<class pSubChain*> subchain_list;
00029 typedef std::vector<Joint*> joint_list;
00030 typedef std::vector<class pJoint*> p_joint_list;
00031 
00033 #define N_FRIC_CONE_DIV 8
00034 
00035 //#define VERBOSE
00036 
00037 // Some definitions for test and logging.
00038 // Divide-and-Conquer Algorithm (Featherstone 1999) test 
00039 //#define USE_DCA
00040 // use normal Lemke method
00041 //#define USE_NORMAL_LEMKE
00042 // measure CPU time for collision simulation
00043 // check timing for parallel processing
00044 //#define TIMING_CHECK
00045 
00046 #ifdef USE_MPI
00047 #include <mpi.h>
00048 #endif
00049 
00057 enum PSIM_AXIS
00058 {
00059         PSIM_AXIS_NULL = -1,
00060         PSIM_AXIS_X, 
00061         PSIM_AXIS_Y,
00062         PSIM_AXIS_Z,
00063         PSIM_AXIS_RX,
00064         PSIM_AXIS_RY,
00065         PSIM_AXIS_RZ,
00066 };
00067 
00068 #ifdef USE_MPI
00069 
00070 enum {
00071         PSIM_TAG_LAMBDA = 100,
00072         PSIM_TAG_ACC,
00073         PSIM_TAG_FORCE,
00074 };
00075 #endif
00076 
00081 class pJoint
00082 {
00083         friend class pSim;
00084         friend class pLink;
00085         friend class pSubChain;
00086 public:
00087         pJoint(Joint* _joint, Joint* _link_side) {
00088                 joint = _joint;
00089                 link_side = _link_side;
00090                 if(!link_side || joint == link_side) parent_side = false;
00091                 else parent_side = true;
00092                 J.resize(6, 6);
00093                 Jdot.resize(6);
00094                 J.identity();
00095                 Jdot.zero();
00096                 f_final.resize(6);
00097                 f_final.zero();
00098                 acc_final.resize(6);
00099                 acc_final.zero();
00100                 dvel.resize(6);
00101                 dvel.zero();
00102                 colf_final.resize(6);
00103                 colf_final.zero();
00104                 vel_final.resize(6);
00105                 vel_final.zero();
00106                 plink = 0;
00107                 subchain = 0;
00108         }
00109         
00110         ~pJoint() {
00111         }
00112 
00113         pJoint* Pair() {
00114                 return pair;
00115         }
00116         Joint* GetJoint() {
00117                 return joint;
00118         }
00119         int ParentSide() {
00120                 return parent_side;
00121         }
00122 
00123 private:
00124         void calc_jacobian();
00125         void calc_jdot();
00126         void dump(ostream& ost);
00127 
00128         Joint* joint;
00129         Joint* link_side;  
00130         pLink* plink;
00131         pJoint* pair;
00132         int parent_side;  
00133         pSubChain* subchain;  
00134 
00135         fMat J;
00136         fVec Jdot;
00137         fVec f_final;    // 6
00138         fVec acc_final;  // 6
00139 
00140         void calc_dvel();
00141         fVec dvel;
00142         fVec colf_final;
00143         fVec vel_final;
00144 };
00145 
00150 class pLink
00151 {
00152         friend class pSim;
00153         friend class pJoint;
00154         friend class pSubChain;
00155 public:
00156         pLink(Joint* _joint) {
00157                 joint = _joint;
00158                 M.resize(6, 6);
00159                 Minv.resize(6, 6);
00160                 c.resize(6);
00161                 acc.resize(6);
00162                 M.zero();
00163                 Minv.zero();
00164                 c.zero();
00165                 acc.zero();
00166                 subchain = 0;
00167         }
00168         
00169         ~pLink() {
00170         }
00171 
00172 private:
00173         void calc_inertia();
00174         void calc_acc(const fVec3& g0);
00175         void dump(ostream& ost);
00176         
00177         Joint* joint;
00178         pSubChain* subchain;
00179 
00180         fMat M;     // 6x6
00181         fMat Minv;  // 6x6
00182         fVec c;     // 6
00183         fVec acc;   // 6
00184 };
00185 
00190 class pSubChain
00191 {
00192         friend class pSim;
00193         friend class pJoint;
00194         friend class pLink;
00195 public:
00196         pSubChain(pSim* _sim, pSubChain* _p, pJoint* _p0, pJoint* _p1) {
00197                 sim = _sim;
00198                 parent = _p;
00199                 children[0] = 0;
00200                 children[1] = 0;
00201                 last_pjoints[0] = _p0;
00202                 last_pjoints[1] = _p1;
00203                 last_joint = last_pjoints[0]->joint;
00204                 last_index[0] = last_index[1] = -1;
00205                 axis = PSIM_AXIS_NULL;
00206                 if(last_joint->j_type == JROTATE)
00207                 {
00208                         if(last_joint->axis(0) > 0.95) axis = PSIM_AXIS_RX;
00209                         else if(last_joint->axis(1) > 0.95) axis = PSIM_AXIS_RY;
00210                         else if(last_joint->axis(2) > 0.95) axis = PSIM_AXIS_RZ;
00211                 }
00212                 else if(last_joint->j_type == JSLIDE)
00213                 {
00214                         if(last_joint->axis(0) > 0.95) axis = PSIM_AXIS_X;
00215                         else if(last_joint->axis(1) > 0.95) axis = PSIM_AXIS_Y;
00216                         else if(last_joint->axis(2) > 0.95) axis = PSIM_AXIS_Z;
00217                 }
00218                 outer_joints = 0;
00219                 outer_joints_origin = 0;
00220                 outer_joints_index = 0;
00221                 n_outer_joints = 0;
00222                 links = 0;
00223                 n_links = 0;
00224                 Lambda = 0;
00225                 acc_temp = 0;
00226                 vel_temp = 0;
00227                 n_dof = last_joint->n_dof;
00228                 n_const = 6 - n_dof;
00229                 joint_index = 0;
00230                 const_index = 0;
00231                 if(n_dof > 0) joint_index = new int [n_dof];
00232                 if(n_const > 0) const_index = new int [n_const];
00233                 int count, i;
00234                 if(last_joint->t_given)
00235                 {
00236                         switch(last_joint->j_type)
00237                         {
00238                         case JROTATE:
00239                         case JSLIDE:
00240                                 count = 0;
00241                                 for(i=0; i<6; i++)
00242                                 {
00243                                         if(i == (int)axis) joint_index[0] = i;
00244                                         else
00245                                         {
00246                                                 const_index[count] = i;
00247                                                 count++;
00248                                         }
00249                                 }
00250                                 break;
00251                         case JSPHERE:
00252                                 const_index[0] = 0;
00253                                 const_index[1] = 1;
00254                                 const_index[2] = 2;
00255                                 joint_index[0] = 3;
00256                                 joint_index[1] = 4;
00257                                 joint_index[2] = 5;
00258                                 break;
00259                         case JFREE:
00260                                 for(i=0; i<6; i++) joint_index[i] = i;
00261                                 break;
00262                         case JFIXED:
00263                                 for(i=0; i<6; i++) const_index[i] = i;
00264                                 break;
00265                         default:
00266                                 break;
00267                         }
00268                 }
00269                 else
00270                 {
00271                         for(i=0; i<6; i++) const_index[i] = i;
00272                 }
00273 #ifdef USE_MPI
00274                 rank = 0;
00275 #endif
00276         }
00277         pSubChain(pSim* _sim, pSubChain* _p, pLink* _pl) {
00278                 sim = _sim;
00279                 parent = _p;
00280                 children[0] = 0;
00281                 children[1] = 0;
00282                 last_pjoints[0] = 0;
00283                 last_pjoints[1] = 0;
00284                 last_joint = 0;
00285                 last_index[0] = last_index[1] = -1;
00286                 axis = PSIM_AXIS_NULL;
00287                 outer_joints = 0;
00288                 outer_joints_origin = 0;
00289                 outer_joints_index = 0;
00290                 n_outer_joints = 0;
00291                 links = new pLink* [1];
00292                 links[0] = _pl;
00293                 n_links = 1;
00294                 Lambda = 0;
00295                 acc_temp = 0;
00296                 vel_temp = 0;
00297                 n_dof = 6;
00298                 n_const = 0;
00299                 joint_index = 0;
00300                 const_index = 0;
00301 #ifdef USE_MPI
00302                 rank = 0;
00303 #endif
00304         }
00305         
00306         ~pSubChain() {
00307                 if(outer_joints)
00308                 {
00309                         int i;
00310                         for(i=0; i<n_outer_joints; i++)
00311                                 delete[] Lambda[i];
00312                         delete[] Lambda;
00313                         delete[] acc_temp;
00314                         delete[] vel_temp;
00315                         delete[] outer_joints;
00316                         delete[] outer_joints_origin;
00317                         delete[] outer_joints_index;
00318                 }
00319                 if(joint_index) delete[] joint_index;
00320                 if(const_index) delete[] const_index;
00321                 if(links) delete[] links;
00322                 if(children[0]) delete children[0];
00323                 if(children[1] && children[1] != children[0]) delete children[1];
00324         }
00325 
00326 private:
00327         int get_outer_index(pJoint* pj) {
00328                 int i;
00329                 for(i=0; i<n_outer_joints; i++)
00330                         if(pj == outer_joints[i]) return i;
00331                 return -1;
00332         }
00333 
00334         void init();
00335 
00336         int total_cost();
00337         int num_leaves();
00338         int schedule_depth();
00339 
00340         pSim* sim;
00341         pSubChain* parent;
00342         pSubChain* children[2];
00343         pJoint* last_pjoints[2];  // [0] is child side, [1] is parent side
00344         Joint* last_joint;
00345         PSIM_AXIS axis;
00346         int last_index[2];  // index of last_pjoints in outer_joints of children
00347 
00348         pJoint** outer_joints;
00349         int* outer_joints_origin;
00350         int* outer_joints_index;
00351         int n_outer_joints;
00352         pLink** links;
00353         int n_links;
00354 
00355         int n_dof;
00356         int n_const;
00357         int* const_index;
00358         int* joint_index;
00359 
00360         void calc_inertia();
00361         void calc_inertia_leaf();
00362         void calc_inertia_body();
00363 #ifdef USE_MPI
00364         void recv_inertia();
00365         void send_inertia(int dest);
00366 #endif
00367 
00368         void calc_acc();
00369         void calc_acc_leaf();
00370         void calc_acc_body();
00371 #ifdef USE_MPI
00372         void recv_acc();
00373         void send_acc(int dest);
00374 #endif
00375 
00376         void disassembly();
00377         void disassembly_body();
00378         void disassembly_leaf();
00379 #ifdef USE_MPI
00380         void recv_force();
00381         void send_force(int dest);
00382 #endif
00383         
00384         fMat P;          // 6x6
00385 #ifndef USE_DCA
00386         fMat Gamma;      // n_const x n_const
00387         fMat Gamma_inv;  // n_const x n_const
00388 #endif
00389         fMat** Lambda;   // matrix of (n_outer_joints x n_outer_joints) matrices with size 6x6
00390 
00391         fVec da6;
00392         fMat W, IW;
00393         fVec* acc_temp;  // vector of (n_outer_joints) vectors with size 6x1
00394         fVec tau;        // n_dof x 1; joint torque
00395         fVec f_temp;     // n_const x 1
00396         fVec acc_final;  // n_dof x 1
00397 #ifdef USE_DCA
00398         fMat Vhat;
00399         fMat SVS;
00400 #endif
00401 
00402         void calc_dvel();
00403         void calc_dvel_leaf();
00404         void calc_dvel_body();
00405         void col_disassembly();
00406         void col_disassembly_body();
00407         fVec* vel_temp;
00408         fVec colf_temp;
00409         
00410         void dump(ostream& ost);
00411 
00412         // compute contact force based on LCP
00413         // only at the root of the schedule
00414         int calc_contact_force(double timestep);
00415 
00416         void clear_f_final();
00417 
00418 #ifdef USE_MPI
00419         int assign_processes(int start_rank, int end_rank);
00420         int create_types(int* n_proc_joints, int** lengths, MPI_Aint** disps, MPI_Datatype** oldtypes);
00421         void scatter_acc();
00422         int rank;
00423         MPI_Datatype parent_lambda_type;
00424         MPI_Datatype parent_acc_type;
00425         MPI_Datatype parent_force_type;
00426 #endif
00427 };
00428 
00429 struct JointInfo
00430 {
00431         JointInfo() {
00432                 pjoints[0] = pjoints[1] = 0;
00433                 plink = 0;
00434         }
00435         ~JointInfo() {
00436         }
00437         
00438         pJoint* pjoints[2];
00439         pLink* plink;
00440 };
00441 
00446 class HRPBASE_EXPORT pSim
00447         : virtual public Chain
00448 {
00449         friend class pJoint;
00450         friend class pLink;
00451         friend class pSubChain;
00452 public:
00454 
00458 #ifdef USE_MPI
00459         pSim(int _rank = 0): Chain() {
00460 #else
00461         pSim(): Chain() {
00462 #endif
00463                 joint_info = 0;
00464                 subchains = 0;
00465                 for(int i=0; i<N_FRIC_CONE_DIV; i++)
00466                 {
00467                         double ang = 2.0*i*PI/N_FRIC_CONE_DIV;
00468                         cone_dir[i](0) = cos(ang);
00469                         cone_dir[i](1) = sin(ang);
00470                         cone_dir[i](2) = 0.0;
00471                 }
00472 #ifdef USE_MPI
00473 #ifdef TIMING_CHECK
00474                 inertia_wait_time = 0.0;
00475                 acc_wait_time = 0.0;
00476                 force_wait_time = 0.0;
00477 #endif
00478                 rank = _rank;
00479                 all_acc_types = 0;
00480 #endif
00481         }
00482         ~pSim() {
00483                 if(joint_info)
00484                 {
00485                         for(int i=0; i<n_joint; i++)
00486                         {
00487                                 delete joint_info[i].pjoints[0];
00488                                 delete joint_info[i].pjoints[1];
00489                                 if(joint_info[i].plink) delete joint_info[i].plink;
00490                         }
00491                         delete[] joint_info;
00492                 }
00493                 if(subchains) delete subchains;
00494 #ifdef USE_MPI
00495                 if(all_acc_types) delete[] all_acc_types;
00496 #ifdef TIMING_CHECK
00497                 cerr << "[" << rank << "] inertia_wait_time = " << inertia_wait_time << endl;
00498                 cerr << "[" << rank << "] acc_wait_time = " << acc_wait_time << endl;
00499                 cerr << "[" << rank << "] force_wait_time = " << force_wait_time << endl;
00500 #endif
00501 #endif
00502         }
00503 
00504         virtual void Clear();
00505 
00507         int Update();
00508 
00510 
00517         int Update(double timestep, std::vector<class SDContactPair*>& sdContactPairs);
00518 
00520         int Schedule();
00521 
00523         int Schedule(Joint** joints);
00524 
00526         int AutoSchedule(int max_procs);
00527 
00528 #ifdef USE_MPI
00529 
00530         int AssignProcesses(int max_procs = 1);
00531 #endif
00532 
00534         void DumpSchedule(ostream& ost);
00535 
00537         int TotalCost();
00538         int NumLeaves();
00539         int ScheduleDepth();
00540 
00542         int ConstraintForces(fVec& cf);
00543 
00544         void GetPJoint(Joint* _joint, pJoint* _pjoints[2]) {
00545                 _pjoints[0] = joint_info[_joint->i_joint].pjoints[0];
00546                 _pjoints[1] = joint_info[_joint->i_joint].pjoints[1];
00547         }
00548 
00549 protected:
00550 #ifdef SEGA
00551         virtual int init();
00552 #else
00553         virtual int init(SceneGraph* sg);
00554 #endif
00555         int myinit();
00556         int init_contact();
00557         virtual int clear_data();
00558         virtual int clear_contact();
00559 
00560 private:
00561         pSubChain* default_schedule(pSubChain* p, Joint* j);
00562         void default_schedule_virtual(Joint* j);
00563         void setup_pjoint(Joint* j);
00564         void setup_pjoint_virtual(Joint* j);
00565         void calc_consts();
00566 
00567         void update_position();
00568         void update_velocity();
00569         void disassembly();
00570 
00571         // collision
00572         void update_collision();
00573         void calc_dvel();
00574         void col_disassembly();
00575 
00576         // contact force computation based on LCP
00577         joint_list contact_vjoints;
00578         std::vector<fVec3> contact_relvels;
00579         std::vector<double> fric_coefs;
00580 
00581         joint_list all_vjoints;
00582         std::vector<fMat> all_Jv;
00583         std::vector<fMat> all_Jr;
00584         std::vector<fVec3> all_jdot_v;
00585         std::vector<fVec3> all_jdot_r;
00586 
00587         fVec3 cone_dir[N_FRIC_CONE_DIV];
00588 
00589         int contact_vjoint_index(Joint* _jnt) {
00590                 int count = 0;
00591                 for(joint_list::iterator j=contact_vjoints.begin(); j!=contact_vjoints.end(); count++, j++)
00592                 {
00593                         if(_jnt == *j) return count;
00594                 }
00595                 return -1;
00596         }
00597 
00598         int build_subchain_tree(int _n_joints, Joint** joints, subchain_list& buf);
00599         void build_subchain_tree(Joint* cur_joint, subchain_list& buf);
00600         int in_subchain(pSubChain* sc, pLink* pl);
00601 
00602         JointInfo* joint_info;
00603         pSubChain* subchains;
00604 
00605 #ifdef USE_MPI
00606 #ifdef TIMING_CHECK
00607         double inertia_wait_time;
00608         double acc_wait_time;
00609         double force_wait_time;
00610 #endif
00611 
00612         void scatter_acc();
00613         MPI_Datatype* all_acc_types;
00614         int size;
00615         int rank;
00616 #endif
00617 
00618 };
00619 
00620 #endif
00621 


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