Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
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
00036
00037
00038
00039
00040
00041
00042
00043
00044
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;
00138 fVec acc_final;
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;
00181 fMat Minv;
00182 fVec c;
00183 fVec acc;
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];
00344 Joint* last_joint;
00345 PSIM_AXIS axis;
00346 int last_index[2];
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;
00385 #ifndef USE_DCA
00386 fMat Gamma;
00387 fMat Gamma_inv;
00388 #endif
00389 fMat** Lambda;
00390
00391 fVec da6;
00392 fMat W, IW;
00393 fVec* acc_temp;
00394 fVec tau;
00395 fVec f_temp;
00396 fVec acc_final;
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
00413
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
00572 void update_collision();
00573 void calc_dvel();
00574 void col_disassembly();
00575
00576
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