schedule.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 <dp.h>
00016 
00017 #define TINY_MASS 1.0e-16
00018 #define N_2_COEF 1.6 // alpha
00019 #define N_COEF 1.0 // beta
00020 #define DOF_COEF -1.0 // gamma
00021 #define CONST_COEF 14.4 // delta
00022 
00023 static pSim* sim = 0;
00024 static int max_procs = 0;
00025 
00026 #include <fstream>
00027 ofstream logfile("schedule.log");
00028 //static ofstream logfile;
00029 
00030 // special handling of nodes with only one process
00031 // 1. cost of the node: the cost to assemble all internal joints (constuctor)
00032 // 2. assigning schedule children: no schedule children will be assigned (find_available_parent)
00033 // 3. A* cost: always zero (calc_astar_cost)
00034 // 4. converting the node to schedule: add all joints
00035 class dpScheduleNode
00036         : public dpNode
00037 {
00038 public:
00039         dpScheduleNode(dpScheduleNode* potential_parent,
00040                                    Joint* _last_joint, const fVec& _proc_costs,
00041                                    dpScheduleNode* _schedule_parent, int child_id,
00042                                    int _first_proc, int _last_proc,
00043                                    const joint_list& org_internal_joints,
00044                                    const p_joint_list& org_outer_joints): dpNode() {
00045                 last_joint = _last_joint;
00046                 if(last_joint)
00047                 {
00048                         sim->GetPJoint(last_joint, last_pjoints);
00049                 }
00050                 else
00051                 {
00052                         last_pjoints[0] = 0;
00053                         last_pjoints[1] = 0;
00054                 }
00055                 schedule_parent = _schedule_parent;
00056                 first_proc = _first_proc;
00057                 last_proc = _last_proc;
00058                 if(schedule_parent)
00059                 {
00060                         schedule_depth = schedule_parent->schedule_depth + 1;
00061                         if(schedule_parent->last_joint)
00062                         {
00063                                 set_outer_joints(schedule_parent->last_pjoints[child_id], org_outer_joints, outer_joints);
00064                         }
00065                 }
00066                 else
00067                 {
00068                         schedule_depth = -1;
00069                         for(p_joint_list::const_iterator j=org_outer_joints.begin(); j!=org_outer_joints.end(); j++)
00070                         {
00071                                 outer_joints.push_back(*j);
00072                         }
00073                 }
00074                 single_joint_cost = 0.0;
00075                 proc_costs.resize(max_procs);
00076                 proc_costs.set(_proc_costs);
00077                 mycost = 0.0;
00078                 if(last_joint)
00079                 {
00080                         logfile << "new node: " << last_joint->name << " (schedule depth = " << schedule_depth << flush;
00081                         logfile << ", procs = [" << first_proc << ", " << last_proc << "]" << flush;
00082                         if(schedule_parent && schedule_parent->last_joint)
00083                                 logfile << ", schedule_parent = " << schedule_parent->last_joint->name << ")" << endl;
00084                         else
00085                                 logfile << ")" << endl;
00086                         // split org_internal_joints by last_joint and find outer_joints 
00087                         split_internal_joints(last_joint, org_internal_joints, internal_joints[0], internal_joints[1]);
00088                         // compute costs
00089                         if(last_proc - first_proc == 1)
00090                         {
00091                                 joint_list added_joints;
00092                                 single_joint_cost = calc_min_subchain_cost(outer_joints, added_joints);
00093                         }
00094                         else
00095                         {
00096                                 single_joint_cost = calc_single_joint_cost(last_joint->n_dof, outer_joints.size());
00097                         }
00098                 }
00099                 else
00100                 {
00101                         for(joint_list::const_iterator i=org_internal_joints.begin(); i!=org_internal_joints.end(); i++)
00102                         {
00103                                 internal_joints[0].push_back(*i);
00104                         }
00105                 }
00106                 logfile << "  single_joint_cost = " << single_joint_cost << endl;
00107                 for(int i=first_proc; i<last_proc; i++)
00108                 {
00109                         proc_costs(i) += single_joint_cost;
00110                 }
00111                 mycost = proc_costs.max_value();
00112                 if(potential_parent)
00113                 {
00114                         mycost -= potential_parent->proc_costs.max_value();
00115                 }
00116                 logfile << "  proc_costs = " << tran(proc_costs) << endl;
00117                 logfile << "  mycost = " << mycost << endl;
00118         }
00119         ~dpScheduleNode() {
00120         }
00121 
00122         fVec& ProcCosts() {
00123                 return proc_costs;
00124         }
00125         
00126         Joint* LastJoint() {
00127                 return last_joint;
00128         }
00129         int ScheduleDepth() {
00130                 return schedule_depth;
00131         }
00132         int FirstProc() {
00133                 return first_proc;
00134         }
00135         int LastProc() {
00136                 return last_proc;
00137         }
00138         int ListAllInternalJoints(joint_list& all_internal);
00139         
00140 protected:
00141         int list_all_internal_joints_sub(Joint* cur, joint_list& all_internal);
00142         int list_all_internal_joints_rev(Joint* cur, joint_list& all_internal);
00143         int split_internal_joints(Joint* _last_joint, const joint_list& org_internal_joints, joint_list& _internal_joints_0, joint_list& _internal_joints_1);
00144         int set_outer_joints(pJoint* last_pjoint, const p_joint_list& org_outer_joints, p_joint_list& new_outer_joints);
00145 
00146         dpScheduleNode* find_available_parent(int target_depth, int& child_id);
00147         dpScheduleNode* find_available_parent_sub(dpScheduleNode* cur, int target_depth, dpNode* cur_leaf, int& child_id);
00148         // functions that should be defined in the subclass
00149         // create and add child nodes
00150         int create_child_nodes(dpNode**& nodes) {
00151                 if(is_goal()) return 0;
00152                 logfile << "create_child_node [" << TotalAstarCost() << "] (joints = " << flush;
00153                 dpNode* n;
00154                 for(n=this; n; n=n->Parent())
00155                 {
00156                         dpScheduleNode* s = (dpScheduleNode*)n;
00157                         if(s->last_joint)
00158                                 logfile << "[" << s->last_joint->name << "/" << s->schedule_depth << "]" << flush;
00159                 }
00160                 logfile << ")" << endl;
00161                 // last joint
00162                 if(depth == sim->NumJoint()-1)
00163                 {
00164                         logfile << "create goal (0)" << endl;
00165                         nodes = new dpNode* [1];
00166                         nodes[0] = new dpScheduleNode(this, 0, proc_costs, this, 0, first_proc, last_proc, joint_list(), p_joint_list());
00167                         return 1;
00168                 }
00169                 // find the schedule_parent to add
00170                 dpScheduleNode* target_parent = 0;
00171                 int child_id = -1;
00172                 target_parent = find_available_parent(schedule_depth-1, child_id);
00173                 if(!target_parent)
00174                 {
00175                         // try next depth
00176                         target_parent = find_available_parent(schedule_depth, child_id);
00177                         // no parent to add: add a goal
00178                         if(!target_parent)
00179                         {
00180                                 logfile << "create goal (1)" << endl;
00181                                 nodes = new dpNode* [1];
00182                                 nodes[0] = new dpScheduleNode(this, 0, proc_costs, this, 0, first_proc, last_proc, joint_list(), p_joint_list());
00183                                 return 1;
00184                         }
00185                 }
00187                 if(target_parent->last_joint)
00188                 {
00189                         logfile << "target_parent = " << target_parent->last_joint->name << ", schedule_depth = " << schedule_depth << ", child_id = " << child_id << endl;
00190                 }
00191                 else
00192                 {
00193                         logfile << "target_parent = null, schedule_depth = " << schedule_depth << ", child_id = " << child_id << endl;
00194                 }
00195                 int _first_proc = 0, _last_proc = max_procs;
00196                 if(child_id == 0)
00197                 {
00198                         if(target_parent->internal_joints[1].size() > 0)
00199                         {
00200                                 _first_proc = target_parent->first_proc;
00201                                 _last_proc = target_parent->first_proc + (target_parent->last_proc-target_parent->first_proc)/2;
00202                         }
00203                         else
00204                         {
00205                                 _first_proc = target_parent->first_proc;
00206                                 _last_proc = target_parent->last_proc;
00207                         }
00208                 }
00209                 else
00210                 {
00211                         if(target_parent->internal_joints[0].size() > 0)
00212                         {
00213                                 _first_proc = target_parent->first_proc + (target_parent->last_proc-target_parent->first_proc)/2;
00214                                 _last_proc = target_parent->last_proc;
00215                         }
00216                         else
00217                         {
00218                                 _first_proc = target_parent->first_proc;
00219                                 _last_proc = target_parent->last_proc;
00220                         }
00221                 }
00222                 joint_list& internal_joints = target_parent->internal_joints[child_id];
00223                 // only one process available: apply default schedule
00224                 if(_last_proc - _first_proc == 1)
00225                 {
00226                         nodes = new dpNode* [1];
00227                         // set child's last_joint
00228                         Joint* next_last = 0;
00229                         if(target_parent->last_joint)
00230                         {
00231                                 joint_list& in_joints = target_parent->internal_joints[child_id];
00232                                 for(joint_list::iterator i=in_joints.begin(); i!=in_joints.end(); i++)
00233                                 {
00234                                         if(*i == target_parent->last_joint->parent ||
00235                                            (*i)->parent == target_parent->last_joint ||
00236                                            (*i)->parent == target_parent->last_joint->parent)
00237                                         {
00238                                                 next_last = *i;
00239                                                 break;
00240                                         }
00241                                 }
00242                         }
00243                         else
00244                         {
00245                                 next_last = sim->Root();
00246                         }
00247                         dpScheduleNode* n = new dpScheduleNode(this, next_last, proc_costs, target_parent, child_id, _first_proc, _last_proc, internal_joints, target_parent->outer_joints);
00248                         nodes[0] = (dpNode*)n;
00249                         return 1;
00250                 }
00251                 // more than two processors available
00252                 int n_internal_joints = internal_joints.size();
00253                 nodes = new dpNode* [n_internal_joints];
00254                 joint_list::iterator j;
00255                 int i;
00256                 for(i=0, j=internal_joints.begin(); j!=internal_joints.end(); i++, j++)
00257                 {
00258                         dpScheduleNode* n = new dpScheduleNode(this, *j, proc_costs, target_parent, child_id, _first_proc, _last_proc, internal_joints, target_parent->outer_joints);
00259 #if 1  // always generate two non-empty subchains
00260                         if(n->internal_joints[0].size() == 0 ||
00261                            n->internal_joints[1].size() == 0)
00262                         {
00263                                 delete n;
00264                                 nodes[i] = 0;
00265                         }
00266                         else
00267 #endif
00268                         {
00269                                 nodes[i] = (dpNode*)n;
00270                         }
00271                 }
00272                 return n_internal_joints;
00273         }
00274         // compute (local) cost
00275         double calc_cost() {
00276                 return mycost;
00277         }
00278         // (optional) compute A-star cost
00279         double calc_astar_cost(dpNode* potential_parent) {
00280                 double c = 0.0;
00281                 logfile << "calc_astar_cost(" << flush;
00282                 if(last_joint)
00283                 {
00284                         logfile << last_joint->name << "/" << schedule_depth << ")" << endl;
00285                         p_joint_list new_outer_joints;
00286                         if(last_proc - first_proc == 1)
00287                         {
00288 #if 0
00289                                 double tmp = calc_min_subchain_cost(outer_joints);
00290                                 logfile << "  one process: " << tmp << endl;
00291                                 if(tmp > c) c = tmp;
00292 #endif
00293                         }
00294                         else
00295                         {
00296                                 // child 0
00297                                 if(internal_joints[0].size() > 0)
00298                                 {
00299                                         int n_myprocs = last_proc - first_proc;
00300                                         joint_list added_joints;
00301                                         if(internal_joints[1].size() > 0)
00302                                         {
00303                                                 n_myprocs = (last_proc - first_proc)/2;
00304                                         }
00305                                         set_outer_joints(last_pjoints[0], outer_joints, new_outer_joints);
00306                                         double tmp = calc_min_subchain_cost(new_outer_joints, added_joints)/n_myprocs;
00307                                         logfile << "  child 0: " << tmp << " [" << n_myprocs << "]" << endl;
00308                                         if(tmp > c) c = tmp;
00309                                 }
00310                                 // child 1
00311                                 if(internal_joints[1].size() > 0)
00312                                 {
00313                                         joint_list added_joints;
00314                                         int n_myprocs = last_proc - first_proc;
00315                                         if(internal_joints[0].size() > 0)
00316                                         {
00317                                                 n_myprocs -= (last_proc-first_proc)/2;
00318                                         }
00319                                         set_outer_joints(last_pjoints[1], outer_joints, new_outer_joints);
00320                                         double tmp = calc_min_subchain_cost(new_outer_joints, added_joints)/n_myprocs;
00321                                         logfile << "  child 1: " << tmp << " [" << n_myprocs << "]" << endl;
00322                                         if(tmp > c) c = tmp;
00323                                 }
00324                         }
00325                 }
00326                 else
00327                 {
00328                         logfile << "default)" << endl;
00329                 }
00330                 logfile << " final = " << c << endl;
00331                 return c;
00332         }
00333         // check if the states are same
00334         int same_state(dpNode* refnode) {
00335                 return false;
00336         }
00337         // check if the state is goal
00338         int is_goal() {
00339                 return ((!last_joint && depth > 0) || depth == sim->NumJoint());
00340         }
00341 
00342 private:
00343         Joint* last_joint;
00344         pJoint* last_pjoints[2];
00345 
00346         int schedule_depth;
00347         dpScheduleNode* schedule_parent;
00348         p_joint_list outer_joints;
00349         joint_list internal_joints[2];
00350 
00351         double single_joint_cost;
00352         double mycost;
00353         fVec proc_costs;
00354         int first_proc, last_proc;
00355 
00356         double calc_single_joint_cost(int n_dof, int n_outer) {
00357 //              return INVERSE_UNIT*(6-n_dof)*(6-n_dof)*(6-n_dof) + n_outer*n_outer;
00358                 return N_2_COEF*n_outer*n_outer + N_COEF*n_outer + DOF_COEF*n_dof + CONST_COEF;
00359         }
00360 
00361         double calc_min_subchain_cost(const p_joint_list& _outer_joints, joint_list& added_joints);
00362         double calc_min_subchain_cost_sub(Joint* cur, const p_joint_list& _outer_joints, int* n_outer_parent_side, int* n_outer_child_side, joint_list& added_joints);
00363 
00364         double add_to_child_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side);
00365         double add_to_parent_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side);
00366 };
00367 
00368 int dpScheduleNode::ListAllInternalJoints(joint_list& all_internal)
00369 {
00370 #if 0
00371         all_internal.clear();
00372         if(!last_joint) return 0;
00373         if(last_proc - first_proc > 1)
00374         {
00375                 all_internal.push_back(last_joint);
00376                 return 0;
00377         }
00378         pJoint* top_pjoint = 0;
00379         // find the top joint
00380         for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
00381         {
00382                 if(!(*j)->ParentSide())
00383                 {
00384                         top_pjoint = *j;
00385                         break;
00386                 }
00387         }
00388         if(!top_pjoint)
00389         {
00390                 list_all_internal_joints_rev(sim->Root(), all_internal);
00391         }
00392         else
00393         {
00394                 list_all_internal_joints_sub(top_pjoint->GetJoint()->child, all_internal);
00395         }
00396 #else
00397         if(last_proc - first_proc == 1)
00398         {
00399                 calc_min_subchain_cost(outer_joints, all_internal);
00400         }
00401         else if(last_joint)
00402         {
00403                 all_internal.push_back(last_joint);
00404         }
00405 #endif
00406         return 0;
00407 }
00408 
00409 int dpScheduleNode::list_all_internal_joints_rev(Joint* cur, joint_list& all_internal)
00410 {
00411         if(!cur) return 0;
00412         pJoint* pjoints[2];
00413         sim->GetPJoint(cur, pjoints);
00414         for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
00415         {
00416                 if(pjoints[1] == *j)
00417                 {
00418                         return 0;
00419                 }
00420         }
00421         all_internal.push_back(cur);
00422         list_all_internal_joints_rev(cur->child, all_internal);
00423         list_all_internal_joints_rev(cur->brother, all_internal);
00424         return 0;
00425 }
00426 
00427 int dpScheduleNode::list_all_internal_joints_sub(Joint* cur, joint_list& all_internal)
00428 {
00429         if(!cur) return 0;
00430         pJoint* pjoints[2];
00431         sim->GetPJoint(cur, pjoints);
00432         for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
00433         {
00434                 if(pjoints[1] == *j)
00435                 {
00436                         return 0;
00437                 }
00438         }
00439         list_all_internal_joints_sub(cur->child, all_internal);
00440         list_all_internal_joints_sub(cur->brother, all_internal);
00441         all_internal.push_back(cur);
00442         return 0;
00443 }
00444 
00445 double dpScheduleNode::calc_min_subchain_cost(const p_joint_list& _outer_joints, joint_list& added_joints)
00446 {
00447         pJoint* top_pjoint = 0;
00448         logfile << "=== calc_min_subchain_cost ===" << endl;
00449         logfile << "outer = [" << flush;
00450         // find the top joint
00451         for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
00452         {
00453                 logfile << " " << (*j)->GetJoint()->name << flush;
00454                 if(!(*j)->ParentSide())
00455                 {
00456                         top_pjoint = *j;
00457 //                      break;
00458                 }
00459         }
00460         logfile << "]" << endl;
00461 
00462         int* n_outer_parent_side = new int [sim->NumJoint()];
00463         int* n_outer_child_side = new int [sim->NumJoint()];
00464         for(int i=0; i<sim->NumJoint(); i++)
00465         {
00466                 n_outer_parent_side[i] = 0;
00467                 n_outer_child_side[i] = 0;
00468         }
00469         for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
00470         {
00471                 if((*j)->ParentSide())
00472                 {
00473                         Joint* cur = (*j)->GetJoint();
00474                         for(Joint* p=cur->parent; p; p=p->parent)
00475                         {
00476                                 n_outer_child_side[p->i_joint]++;
00477                         }
00478                 }
00479         }
00480         for(int i=0; i<sim->NumJoint(); i++)
00481         {
00482                 n_outer_parent_side[i] = _outer_joints.size() - n_outer_child_side[i];
00483 //              logfile << sim->FindJoint(i)->name << ": parent_side = " << n_outer_parent_side[i] << ", child_side = " << n_outer_child_side[i] << endl;
00484         }
00485         double ret = 0.0;
00486         added_joints.clear();
00487         if(!top_pjoint)
00488         {
00489                 ret = calc_min_subchain_cost_sub(sim->Root(), _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00490         }
00491         else
00492         {
00493                 joint_list sorted_child;
00494                 for(Joint* c = top_pjoint->GetJoint()->child; c; c=c->brother)
00495                 {
00496                         int done = false;
00497                         for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
00498                         {
00499                                 if(n_outer_child_side[c->i_joint] <= n_outer_child_side[(*j)->i_joint])
00500                                 {
00501                                         done = true;
00502                                         sorted_child.insert(j, c);
00503                                         break;
00504                                 }
00505                         }
00506                         if(!done) sorted_child.push_back(c);
00507                 }
00508                 for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
00509                 {
00510                         ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00511                 }
00512         }
00513         delete[] n_outer_parent_side;
00514         delete[] n_outer_child_side;
00515         return ret;
00516 }
00517 
00518 int is_in(const joint_list& jlist, Joint* jnt)
00519 {
00520         for(joint_list::const_iterator i=jlist.begin(); i!=jlist.end(); i++)
00521         {
00522                 if(*i == jnt) return true;
00523         }
00524         return false;
00525 }
00526 
00527 double dpScheduleNode::calc_min_subchain_cost_sub(Joint* cur, const p_joint_list& _outer_joints, int* n_outer_parent_side, int* n_outer_child_side, joint_list& added_joints)
00528 {
00529         if(!cur) return 0.0;
00530 //      logfile << "calc_min_subchain_cost_sub(" << cur->name << ") ->" << endl;
00531         pJoint* pjoints[2];
00532         sim->GetPJoint(cur, pjoints);
00533         for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
00534         {
00535                 if(pjoints[1] == *j)
00536                 {
00537                         return 0.0;
00538                 }
00539         }
00540         double ret = 0.0;
00541         int descend = false;
00542         int my_outer = n_outer_child_side[cur->i_joint];
00543         if(n_outer_parent_side[cur->i_joint] < n_outer_child_side[cur->i_joint])
00544         {
00545                 descend = true;
00546                 my_outer = n_outer_parent_side[cur->i_joint];
00547         }
00548         // if not in the descending order, this joint should be processed later
00549         if(!descend)
00550         {
00551                 joint_list sorted_child;
00552                 for(Joint* c = cur->child; c; c=c->brother)
00553                 {
00554                         int done = false;
00555                         for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
00556                         {
00557                                 if(n_outer_child_side[c->i_joint] <= n_outer_child_side[(*j)->i_joint])
00558                                 {
00559                                         done = true;
00560                                         sorted_child.insert(j, c);
00561                                         break;
00562                                 }
00563                         }
00564                         if(!done) sorted_child.push_back(c);
00565                 }
00566                 for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
00567                 {
00568                         ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00569                 }
00570                 ret += add_to_parent_side(cur, added_joints, n_outer_parent_side, n_outer_child_side);
00571         }
00572         // if in descending order, sort the children in the descending order of
00573         // outer joints
00574         else
00575         {
00576                 joint_list smaller_child, larger_child;
00577                 for(Joint* c=cur->child; c; c=c->brother)
00578                 {
00579                         int n_child_outer = n_outer_child_side[c->i_joint];
00580                         if(n_child_outer <= my_outer)
00581                         {
00582                                 // add to smaller_child
00583                                 int done = false;
00584                                 for(joint_list::iterator i=smaller_child.begin(); i!=smaller_child.end(); i++)
00585                                 {
00586                                         int n = n_outer_child_side[(*i)->i_joint];
00587                                         if(n_child_outer <= n)
00588                                         {
00589                                                 smaller_child.insert(i, c);
00590                                                 done = true;
00591                                                 break;
00592                                         }
00593                                 }
00594                                 if(!done)
00595                                 {
00596                                         smaller_child.push_back(c);
00597                                 }
00598                         }
00599                         else
00600                         {
00601                                 // add to larger_child
00602                                 int done = false;
00603                                 for(joint_list::iterator i=larger_child.begin(); i!=larger_child.end(); i++)
00604                                 {
00605                                         int n = n_outer_child_side[(*i)->i_joint];
00606                                         if(n_child_outer <= n)
00607                                         {
00608                                                 larger_child.insert(i, c);
00609                                                 done = true;
00610                                                 break;
00611                                         }
00612                                 }
00613                                 if(!done)
00614                                 {
00615                                         larger_child.push_back(c);
00616                                 }
00617                         }
00618                 }
00619 //              logfile << "  smaller: " << endl;
00620                 for(joint_list::iterator j=smaller_child.begin(); j!=smaller_child.end(); j++)
00621                 {
00622 //                      logfile << "   " << (*j)->name << endl;
00623                         ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00624                 }
00625                 ret += add_to_child_side(cur, added_joints, n_outer_parent_side, n_outer_child_side);
00626 //              logfile << "  larger: " << endl;
00627                 for(joint_list::iterator j=larger_child.begin(); j!=larger_child.end(); j++)
00628                 {
00629 //                      logfile << "   " << (*j)->name << endl;
00630                         ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
00631                 }
00632         }
00633 //      logfile << "<- calc_min_subchain_cost_sub(" << cur->name << ")" << endl;
00634         return ret;
00635 }
00636 
00637 double dpScheduleNode::add_to_child_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side)
00638 {
00639         double ret = 0.0;
00640         int n_outer = n_outer_parent_side[cur->i_joint];
00641         int n_link_outer = 0;
00642         for(Joint* c=cur->child; c; c=c->brother)
00643         {
00644                 if(!is_in(added_joints, c))
00645                 {
00646                         n_link_outer++;
00647                 }
00648         }
00649         added_joints.push_back(cur);
00650         ret = calc_single_joint_cost(cur->n_dof, n_outer+n_link_outer);
00651         logfile << " adding " << cur->name << ", n_outer = " << n_outer << ", n_link_outer = " << n_link_outer << ", cost = " << ret << endl;
00652         return ret;
00653 }
00654 
00655 double dpScheduleNode::add_to_parent_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side)
00656 {
00657         double ret = 0.0;
00658         int n_outer = n_outer_child_side[cur->i_joint];
00659         int n_link_outer = 0;
00660         if(cur->parent && !is_in(added_joints, cur->parent))
00661         {
00662                 n_link_outer++;
00663         }
00664         if(cur->parent)
00665         {
00666                 for(Joint* c=cur->parent->child; c; c=c->brother)
00667                 {
00668                         if(!is_in(added_joints, c) && c!=cur)
00669                         {
00670                                 n_link_outer++;
00671                         }
00672                 }
00673         }
00674         added_joints.push_back(cur);
00675         ret = calc_single_joint_cost(cur->n_dof, n_outer+n_link_outer);
00676         logfile << " adding " << cur->name << ", n_outer = " << n_outer << ", n_link_outer = " << n_link_outer << ", cost = " << ret << endl;
00677         return ret;
00678 }
00679 
00680 dpScheduleNode* dpScheduleNode::find_available_parent(int target_depth, int& child_id)
00681 {
00682         dpScheduleNode* s, *ret = 0;
00683         for(s=this; s; s=(dpScheduleNode*)s->parent)
00684         {
00685                 if((ret = find_available_parent_sub(s, target_depth, this, child_id)))
00686                         return ret;
00687         }
00688         return 0;
00689 }
00690 
00691 dpScheduleNode* dpScheduleNode::find_available_parent_sub(dpScheduleNode* cur, int target_depth, dpNode* cur_leaf, int& child_id)
00692 {
00693         if(!cur) return 0;
00694         if(cur->last_proc - cur->first_proc == 1) return 0;
00695         if(cur->schedule_depth == target_depth)
00696         {
00697                 if(cur->internal_joints[0].size() > 0)
00698                 {
00699                         int failed = false;
00700                         joint_list::iterator f;
00701                         for(f=cur->internal_joints[0].begin(); f!=cur->internal_joints[0].end(); f++)
00702                         {
00703                                 dpNode* n;
00704                                 for(n=cur_leaf; n; n=n->Parent())
00705                                 {
00706                                         dpScheduleNode* s = (dpScheduleNode*)n;
00707                                         if(*f == s->last_joint)
00708                                         {
00709                                                 failed = true;
00710                                                 break;
00711                                         }
00712                                 }
00713                                 if(failed) break;
00714                         }
00715                         if(!failed)
00716                         {
00717                                 child_id = 0;
00718                                 return cur;
00719                         }
00720                 }
00721                 if(cur->internal_joints[1].size() > 0)
00722                 {
00723                         int failed = false;
00724                         joint_list::iterator f;
00725                         for(f=cur->internal_joints[1].begin(); f!=cur->internal_joints[1].end(); f++)
00726                         {
00727                                 dpNode* n;
00728                                 for(n=cur_leaf; n; n=n->Parent())
00729                                 {
00730                                         dpScheduleNode* s = (dpScheduleNode*)n;
00731                                         if(*f == s->last_joint)
00732                                         {
00733                                                 failed = true;
00734                                                 break;
00735                                         }
00736                                 }
00737                                 if(failed) break;
00738                         }
00739                         if(!failed)
00740                         {
00741                                 child_id = 1;
00742                                 return cur;
00743                         }
00744                 }
00745         }
00746         return 0;
00747 }
00748 
00749 static void get_all_joints(Joint* cur, joint_list& all_joints, joint_list& all_vjoints)
00750 {
00751         if(!cur) return;
00752         if(cur->real)
00753         {
00754                 all_vjoints.push_back(cur);
00755         }
00756         else
00757         {
00758                 all_joints.push_back(cur);
00759         }
00760         get_all_joints(cur->child, all_joints, all_vjoints);
00761         get_all_joints(cur->brother, all_joints, all_vjoints);
00762         return;
00763 }
00764 
00765 int pSim::AutoSchedule(int _max_procs)
00766 {
00767         logfile << "AutoSchedule(max_procs = " << _max_procs << ")" << endl;
00768         // set static variables ->
00769         sim = this;
00770         max_procs = _max_procs;
00771         // <-
00772         joint_list init_internal_joints;  // all joints
00773         p_joint_list init_outer_joints;  // virtual joints only
00774         joint_list all_vjoints;
00775         // list all joints
00776         get_all_joints(root, init_internal_joints, all_vjoints);
00777         // set init_outer_joints
00778         for(joint_list::iterator j=all_vjoints.begin(); j!=all_vjoints.end(); j++)
00779         {
00780                 pJoint* v_pjoints[2];
00781                 GetPJoint(*j, v_pjoints);
00782                 init_outer_joints.push_back(v_pjoints[0]);
00783                 init_outer_joints.push_back(v_pjoints[1]);
00784         }
00785         dpMain* dp = new dpMain;
00786         fVec init_proc_costs(max_procs);
00787         init_proc_costs.zero();
00788         dpScheduleNode* start_node = new dpScheduleNode(0, 0, init_proc_costs, 0, 0, 0, max_procs, init_internal_joints, init_outer_joints);
00789 
00790         dp->SetStartNode(start_node);
00791         dp->Search(-1, 1);
00792         dpNode* goal = dp->BestGoal();
00793         if(!goal)
00794         {
00795                 logfile << "pSim::AutoSchedule: goal not found" << endl;
00796                 delete dp;
00797                 return -1;
00798         }
00799         cerr << "goal found" << endl;
00800         cerr << "cost = " << goal->TotalCost() << endl;
00801         fVec& proc_costs = ((dpScheduleNode*)goal)->ProcCosts();
00802         cerr << "proc_costs = " << tran(proc_costs) << endl;
00803         Joint** joints = new Joint* [n_joint+all_vjoints.size()];
00804         dpNode* n;
00805         int count = 0;
00806         for(n=goal->Parent(); n; n=n->Parent())
00807         {
00808                 dpScheduleNode* s = (dpScheduleNode*)n;
00809                 if(s->LastJoint())
00810                 {
00811                         cerr << "[" << s->LastJoint()->name << "/" << s->ScheduleDepth() << "] cost = " << n->TotalCost() << ", procs = [" << s->FirstProc() << ", " << s->LastProc() << "]" << endl;
00812                 }
00813                 else
00814                 {
00815                         cerr << "[default/" << s->ScheduleDepth() << "]" << endl;
00816                 }
00817                 joint_list all_internal;
00818                 s->ListAllInternalJoints(all_internal);
00819                 logfile << "joints =  " << endl;
00820                 for(joint_list::iterator j=all_internal.begin(); j!=all_internal.end(); j++)
00821                 {
00822                         logfile << "  " << count << " " << (*j)->name << endl;
00823                         joints[count] = *j;
00824                         count++;
00825                 }
00826         }
00827         logfile << "current count = " << count << ", number of vjoints = " << all_vjoints.size() << ", total n_joint = " << n_joint << endl;
00828         logfile << "joints = " << endl;
00829         for(int i=0; i<count; i++)
00830         {
00831                 logfile << "[" << i << "] = " << joints[i]->name << endl;
00832         }
00833         for(joint_list::iterator j=all_vjoints.begin(); j!=all_vjoints.end(); j++)
00834         {
00835                 joints[count] = *j;
00836                 logfile << "[" << count << "] = " << joints[count]->name << endl;
00837                 count++;
00838         }
00839 #if 1
00840         Schedule(joints);
00841 #endif
00842         delete[] joints;
00843         delete dp;
00844         return 0;
00845 }
00846 
00847 int dpScheduleNode::split_internal_joints(Joint* _last_joint, const joint_list& org_internal_joints, joint_list& _internal_joints_0, joint_list& _internal_joints_1)
00848 {
00849         _internal_joints_0.clear();
00850         _internal_joints_1.clear();
00851         joint_list::const_iterator j;
00852 //      logfile << "-- split at " << _last_joint->name << endl;
00853         for(j=org_internal_joints.begin(); j!=org_internal_joints.end(); j++)
00854         {
00855 //              logfile << "    " << (*j)->name << " goes to " << flush;
00856                 if(*j == _last_joint)
00857                 {
00858 //                      logfile << " none" << endl;
00859                 }
00860                 else if((*j)->isAscendant(_last_joint))
00861                 {
00862                         _internal_joints_0.push_back(*j);
00863 //                      logfile << " 0" << endl;
00864                 }
00865                 else
00866                 {
00867                         _internal_joints_1.push_back(*j);
00868 //                      logfile << " 1" << endl;
00869                 }
00870         }
00871 //      logfile << "split end" << endl;
00872         return 0;
00873 }
00874 
00875 int dpScheduleNode::set_outer_joints(pJoint* last_pjoint, const p_joint_list& org_outer_joints, p_joint_list& new_outer_joints)
00876 {
00877         new_outer_joints.clear();
00878         new_outer_joints.push_back(last_pjoint);
00879         p_joint_list::const_iterator p;
00880         int child_id = (last_pjoint->ParentSide()) ? 1 : 0;
00881         for(p=org_outer_joints.begin(); p!=org_outer_joints.end(); p++)
00882         {
00883                 if((*p)->GetJoint()->isAscendant(last_pjoint->GetJoint()))
00884                 {
00885                         if(child_id == 0)
00886                                 new_outer_joints.push_back(*p);
00887                 }
00888                 else if(child_id == 1)
00889                 {
00890                         new_outer_joints.push_back(*p);
00891                 }
00892         }
00893         return 0;
00894 }
00895 
00896 #ifdef USE_MPI
00897 
00900 int pSim::AssignProcesses(int max_procs)
00901 {
00902         size = max_procs;
00903         subchains->assign_processes(0, max_procs);
00904 
00905         all_acc_types = new MPI_Datatype [max_procs];
00906         int** lengths = new int* [max_procs];
00907         MPI_Aint** disps = new MPI_Aint* [max_procs];
00908         MPI_Datatype** oldtypes = new MPI_Datatype* [max_procs];
00909         int* n_proc_joints = new int [max_procs];
00910         int i;
00911         for(i=0; i<max_procs; i++)
00912         {
00913                 lengths[i] = new int [3*n_joint];
00914                 disps[i] = new MPI_Aint [3*n_joint];
00915                 oldtypes[i] = new MPI_Datatype [3*n_joint];
00916                 n_proc_joints[i] = 0;
00917         }
00918         subchains->create_types(n_proc_joints, lengths, disps, oldtypes);
00919         for(i=0; i<max_procs; i++)
00920         {
00921                 MPI_Type_create_struct(n_proc_joints[i], lengths[i], disps[i], oldtypes[i], all_acc_types+i);
00922                 MPI_Type_commit(all_acc_types+i);
00923                 logfile << "[" << rank << "]: all_acc_types[" << i << "] = " << all_acc_types[i] << endl;
00924         }
00925 
00926         for(i=0; i<max_procs; i++)
00927         {
00928                 delete[] lengths[i];
00929                 delete[] disps[i];
00930                 delete[] oldtypes[i];
00931         }
00932         delete[] lengths;
00933         delete[] disps;
00934         delete[] oldtypes;
00935         delete[] n_proc_joints;
00936         return 0;
00937 }
00938 
00939 // available ranks: start_rank -> end_rank-1
00940 int pSubChain::assign_processes(int start_rank, int end_rank)
00941 {
00942         rank = start_rank;
00943         // assign different processes to the children
00944         if(children[0] && children[1] && children[0] != children[1] &&
00945            children[0]->last_joint && children[1]->last_joint &&
00946            end_rank > start_rank+1)
00947         {
00948                 int n_my_procs = end_rank - start_rank;
00949                 int n_half_procs = n_my_procs/2;
00950                 children[0]->assign_processes(start_rank, start_rank+n_half_procs);
00951                 children[1]->assign_processes(start_rank+n_half_procs, end_rank);
00952         }
00953         // otherwise assign the same process
00954         else
00955         {
00956                 if(children[0])
00957                 {
00958                         children[0]->assign_processes(start_rank, end_rank);
00959                 }
00960                 if(children[0] != children[1] && children[1])
00961                 {
00962                         children[1]->assign_processes(start_rank, end_rank);
00963                 }
00964         }
00965         return 0;
00966 }
00967 
00968 int pSubChain::create_types(int* n_proc_joints, int** _lengths, MPI_Aint** _disps, MPI_Datatype** _oldtypes)
00969 {
00970         if(!this) return 0;
00971         // types for sending to parent
00972         if(parent && rank != parent->rank)
00973         {
00974                 logfile << "[" << sim->rank << "] create_types " << last_joint->name << endl;
00975                 int n_mat = n_outer_joints * n_outer_joints;
00976                 int count = 0;
00977                 int* lengths = new int [n_mat];
00978                 MPI_Aint* disps = new int [n_mat];
00979                 MPI_Datatype* oldtypes = new int [n_mat];
00980                 for(int i=0; i<n_outer_joints; i++)
00981                 {
00982                         for(int j=0; j<n_outer_joints; j++)
00983                         {
00984                                 lengths[count] = 36;
00985                                 oldtypes[count] = MPI_DOUBLE;
00986                                 MPI_Get_address(Lambda[i][j].data(), disps+count);
00987                                 count++;
00988                         }
00989                 }
00990                 MPI_Type_create_struct(n_mat, lengths, disps, oldtypes, &parent_lambda_type);
00991                 MPI_Type_commit(&parent_lambda_type);
00992                 logfile << "[" << sim->rank << "]: parent_lambda_type = " << parent_lambda_type << endl;
00993                 // acc
00994                 for(int i=0; i<n_outer_joints; i++)
00995                 {
00996                         lengths[i] = 6;
00997                         oldtypes[i] = MPI_DOUBLE;
00998                         MPI_Get_address(acc_temp[i].data(), disps+i);
00999                         count++;
01000                 }
01001                 MPI_Type_create_struct(n_outer_joints, lengths, disps, oldtypes, &parent_acc_type);
01002                 MPI_Type_commit(&parent_acc_type);
01003                 logfile << "[" << sim->rank << "]: parent_acc_type = " << parent_acc_type << endl;
01004                 delete[] lengths;
01005                 delete[] disps;
01006                 delete[] oldtypes;
01007         }
01008         if(children[0] && rank != children[0]->rank || children[1] && rank != children[1]->rank)
01009         {
01010                 int n_array = n_outer_joints + 2;
01011                 int* lengths = new int [n_array];
01012                 MPI_Aint* disps = new int [n_array];
01013                 MPI_Datatype* oldtypes = new int [n_array];
01014                 // force
01015                 for(int i=0; i<n_outer_joints; i++)
01016                 {
01017                         lengths[i] = 6;
01018                         oldtypes[i] = MPI_DOUBLE;
01019                         MPI_Get_address(outer_joints[i]->f_final.data(), disps+i);
01020                 }
01021                 // f_final
01022                 lengths[n_outer_joints] = 6;
01023                 lengths[n_outer_joints+1] = 6;
01024                 oldtypes[n_outer_joints] = MPI_DOUBLE;
01025                 oldtypes[n_outer_joints+1] = MPI_DOUBLE;
01026                 MPI_Get_address(last_pjoints[0]->f_final.data(), disps+n_outer_joints);
01027                 MPI_Get_address(last_pjoints[1]->f_final.data(), disps+n_outer_joints+1);
01028                 MPI_Type_create_struct(n_outer_joints+2, lengths, disps, oldtypes, &parent_force_type);
01029                 MPI_Type_commit(&parent_force_type);
01030                 logfile << "[" << sim->rank << "]: parent_force_type = " << parent_force_type << endl;
01031                 delete[] lengths;
01032                 delete[] disps;
01033                 delete[] oldtypes;
01034         }
01035         // acc
01036         if(last_joint && last_joint->n_dof > 0)
01037         {
01038                 int index = n_proc_joints[rank];
01039                 _oldtypes[rank][index] = MPI_DOUBLE;
01040                 _oldtypes[rank][index+1] = MPI_DOUBLE;
01041                 _oldtypes[rank][index+2] = MPI_DOUBLE;
01042                 MPI_Get_address(acc_final.data(), _disps[rank]+index);
01043                 MPI_Get_address(last_pjoints[0]->f_final.data(), _disps[rank]+index+1);
01044                 MPI_Get_address(last_pjoints[1]->f_final.data(), _disps[rank]+index+2);
01045                 _lengths[rank][index] = acc_final.size();
01046                 _lengths[rank][index+1] = 6;
01047                 _lengths[rank][index+2] = 6;
01048                 n_proc_joints[rank] += 3;
01049         }
01050         children[0]->create_types(n_proc_joints, _lengths, _disps, _oldtypes);
01051         if(children[0] != children[1]) children[1]->create_types(n_proc_joints, _lengths, _disps, _oldtypes);
01052         return 0;
01053 }
01054 
01055 #endif
01056 
01060 int pSim::Schedule()
01061 {
01062         if(subchains) delete subchains;
01063         subchains = 0;
01064         // first attach subchains ignoring the virtual links
01065         subchains = default_schedule(0, root);
01066         // insert subchains for the virtual links
01067         default_schedule_virtual(root);
01068         subchains->init();
01069         return 0;
01070 }
01071 
01072 pSubChain* pSim::default_schedule(pSubChain* p, Joint* j)
01073 {
01074         if(!j) return 0;
01075         if(j->i_joint < 0) return 0;  // OK?
01076         if(j->real) return 0;  // skip virtual links for now
01077         // skip mass-less end points (but include space)
01078         if(j->mass < TINY_MASS && j->parent)
01079         {
01080                 pSubChain* c1 = default_schedule(p, j->brother);
01081                 if(c1) return c1;
01082                 pSubChain* c0 = default_schedule(p, j->child);
01083                 return c0;
01084         }
01085         // create subchain for j
01086         pJoint* pj0, *pj1;
01087         pj0 = joint_info[j->i_joint].pjoints[0];
01088         pj1 = joint_info[j->i_joint].pjoints[1];
01089         pSubChain* sc = new pSubChain(this, p, pj0, pj1);
01090         // children subchains
01091         pSubChain* c0 = 0, *c1 = 0;
01092         // brother or parent link
01093         if(j->brother)
01094         {
01095                 c1 = default_schedule(sc, j->brother);
01096         }
01097         if(!c1 && j->parent && j->parent->i_joint >= 0)
01098         {
01099                 pLink* pl = joint_info[j->parent->i_joint].plink;
01100                 c1 = new pSubChain(this, sc, pl);
01101         }
01102         // children
01103         if(j->child && !j->child->real)
01104         {
01105                 c0 = default_schedule(sc, j->child);
01106         }
01107         if(!c0 && j->i_joint >= 0)
01108         {
01109                 pLink* pl = joint_info[j->i_joint].plink;
01110                 c0 = new pSubChain(this, sc, pl);
01111         }
01112         sc->children[0] = c0;
01113         sc->children[1] = c1;
01114         return sc;
01115 }
01116 
01117 void pSim::default_schedule_virtual(Joint* j)
01118 {
01119         if(!j) return;
01120         if(j->i_joint < 0) return;
01121         // virtual but not contact
01122         if(j->real && contact_vjoint_index(j) < 0)
01123         {
01124                 pJoint* pj0, *pj1;
01125                 pj0 = joint_info[j->i_joint].pjoints[0];
01126                 pj1 = joint_info[j->i_joint].pjoints[1];
01127                 pSubChain* sc = new pSubChain(this, subchains, pj0, pj1);
01128                 // insert sc between subchains and its child
01129                 // same child for [0] and [1]
01130                 sc->children[0] = subchains->children[0];
01131                 sc->children[1] = subchains->children[0];
01132                 // subchains->children[1] must be null
01133                 subchains->children[0]->parent = sc;
01134                 subchains->children[0] = sc;
01135         }
01136         default_schedule_virtual(j->brother);
01137         default_schedule_virtual(j->child);
01138 }
01139 
01140 void pSubChain::init()
01141 {
01142         if(!this) return;
01143         children[0]->init();
01144         if(children[0] != children[1]) children[1]->init();
01145         if(Lambda)
01146         {
01147                 for(int i=0; i<n_outer_joints; i++) delete[] Lambda[i];
01148                 delete[] Lambda;
01149         }
01150         Lambda = 0;
01151         if(acc_temp) delete[] acc_temp;
01152         acc_temp = 0;
01153         if(vel_temp) delete[] vel_temp;
01154         vel_temp = 0;
01155         n_outer_joints = 0;
01156         if(outer_joints) delete[] outer_joints;
01157         if(outer_joints_origin) delete[] outer_joints_origin;
01158         if(outer_joints_index) delete[] outer_joints_index;
01159         outer_joints = 0;
01160         outer_joints_origin = 0;
01161         outer_joints_index = 0;
01162         // consists of a single link
01163         if(!last_joint && n_links == 1)
01164         {
01165                 int i;
01166                 // count outer joints
01167                 Joint* p = links[0]->joint;
01168                 links[0]->subchain = this;
01169                 n_outer_joints = 1;  // parent
01170                 Joint* cur;
01171                 for(cur=p->child; cur; cur=cur->brother)
01172                 {
01173                         if(cur->real || cur->mass > TINY_MASS)
01174                                 n_outer_joints++;  // children
01175                 }
01176                 // virtual links
01177                 for(i=0; i<sim->n_joint; i++)
01178                 {
01179                         if(sim->joint_info[i].pjoints[0]->joint->real == links[0]->joint)
01180                                 n_outer_joints++;
01181                 }
01182                 outer_joints = new pJoint* [n_outer_joints];
01183                 outer_joints_origin = new int [n_outer_joints];
01184                 outer_joints_index = new int [n_outer_joints];
01185                 outer_joints[0] = sim->joint_info[p->i_joint].pjoints[0];  // child side
01186                 outer_joints_origin[0] = -1;
01187                 outer_joints_index[0] = -1;
01188                 n_outer_joints = 1;
01189                 for(cur=p->child; cur; cur=cur->brother)
01190                 {
01191                         if(cur->real || cur->mass > TINY_MASS)
01192                         {
01193                                 outer_joints[n_outer_joints] = sim->joint_info[cur->i_joint].pjoints[1];  // parent side
01194                                 outer_joints[n_outer_joints]->subchain = this;
01195                                 outer_joints_origin[n_outer_joints] = -1;
01196                                 outer_joints_index[n_outer_joints] = -1;
01197                                 n_outer_joints++;
01198                         }
01199                 }
01200                 for(i=0; i<sim->n_joint; i++)
01201                 {
01202                         if(sim->joint_info[i].pjoints[0]->joint->real == links[0]->joint)
01203                         {
01204                                 outer_joints[n_outer_joints] = sim->joint_info[i].pjoints[0];  // child side
01205                                 outer_joints[n_outer_joints]->subchain = this;
01206                                 outer_joints_origin[n_outer_joints] = -1;
01207                                 outer_joints_index[n_outer_joints] = -1;
01208                                 n_outer_joints++;
01209                         }
01210                 }
01211         }
01212         // gather information from children subchains
01213         else
01214         {
01215                 int i, count;
01216                 // last_index
01217                 if(children[0]) last_index[0] = children[0]->get_outer_index(last_pjoints[0]);
01218                 if(children[1]) last_index[1] = children[1]->get_outer_index(last_pjoints[1]);
01219                 // outer joints
01220                 n_outer_joints = children[0]->n_outer_joints - 1;
01221                 if(children[0] == children[1])
01222                         n_outer_joints--;
01223                 else if(children[1])
01224                         n_outer_joints += children[1]->n_outer_joints - 1;
01225                 if(n_outer_joints > 0)
01226                 {
01227                         outer_joints = new pJoint* [n_outer_joints];
01228                         outer_joints_origin = new int [n_outer_joints];
01229                         outer_joints_index = new int [n_outer_joints];
01230                         count = 0;
01231                         for(i=0; i<children[0]->n_outer_joints; i++)
01232                         {
01233                                 if(children[0]->outer_joints[i]->joint != last_joint)
01234                                 {
01235                                         outer_joints[count] = children[0]->outer_joints[i];
01236                                         outer_joints_origin[count] = 0;
01237                                         if(children[0] == children[1] &&
01238                                            outer_joints[count]->joint->real && 
01239                                            outer_joints[count]->joint->real != outer_joints[count]->link_side)  // virtual side
01240                                         {
01241                                                 outer_joints_origin[count] = 1;
01242                                         }
01243                                         outer_joints_index[count] = i;
01244                                         count++;
01245                                 }
01246                         }
01247                         if(children[1] && children[0] != children[1])
01248                         {
01249                                 for(i=0; i<children[1]->n_outer_joints; i++)
01250                                 {
01251                                         if(children[1]->outer_joints[i]->joint != last_joint)
01252                                         {
01253                                                 outer_joints[count] = children[1]->outer_joints[i];
01254                                                 outer_joints_origin[count] = 1;
01255                                                 outer_joints_index[count] = i;
01256                                                 count++;
01257                                         }
01258                                 }
01259                         }
01260                 }
01261                 // links
01262                 n_links = children[0]->n_links;
01263                 if(children[1] && children[0] != children[1])
01264                         n_links += children[1]->n_links;
01265                 if(links) delete[] links;
01266                 links = 0;
01267                 links = new pLink* [n_links];
01268                 count = 0;
01269                 for(i=0; i<children[0]->n_links; i++)
01270                 {
01271                         links[count] = children[0]->links[i];
01272                         count++;
01273                 }
01274                 if(children[1] && children[0] != children[1])
01275                 {
01276                         for(i=0; i<children[1]->n_links; i++)
01277                         {
01278                                 links[count] = children[1]->links[i];
01279                                 count++;
01280                         }
01281                 }
01282         }
01283         // create matrices and vectors
01284         P.resize(6, 6);
01285         P.zero();
01286         da6.resize(6);
01287         da6.zero();
01288         W.resize(6, 6);
01289         W.zero();
01290         IW.resize(6, 6);
01291         IW.zero();
01292 #ifdef USE_DCA
01293         Vhat.resize(6,6);
01294         Vhat.zero();
01295         SVS.resize(n_dof, n_dof);
01296         SVS.zero();
01297 #else
01298         Gamma.resize(n_const, n_const);
01299         Gamma.zero();
01300         Gamma_inv.resize(n_const, n_const);
01301         Gamma_inv.zero();
01302 #endif
01303         f_temp.resize(n_const);
01304         f_temp.zero();
01305         colf_temp.resize(n_const);
01306         colf_temp.zero();
01307         tau.resize(n_dof);
01308         tau.zero();
01309         acc_final.resize(n_dof);
01310         acc_final.zero();
01311         if(n_outer_joints > 0)
01312         {
01313                 int i, j;
01314                 Lambda = new fMat* [n_outer_joints];
01315                 acc_temp = new fVec [n_outer_joints];
01316                 vel_temp = new fVec [n_outer_joints];
01317                 for(i=0; i<n_outer_joints; i++)
01318                 {
01319                         Lambda[i] = new fMat [n_outer_joints];
01320                         for(j=0; j<n_outer_joints; j++)
01321                         {
01322                                 Lambda[i][j].resize(6, 6);
01323                                 Lambda[i][j].zero();
01324                         }
01325                         acc_temp[i].resize(6);
01326                         acc_temp[i].zero();
01327                         vel_temp[i].resize(6);
01328                         vel_temp[i].zero();
01329                 }
01330         }
01331 }
01332 
01336 int pSim::Schedule(Joint** joints)
01337 {
01338         if(subchains) delete subchains;
01339         subchains = 0;
01340         subchain_list buf;
01341         build_subchain_tree(n_joint, joints, buf);
01342         if(buf.size() == 1)
01343         {
01344                 subchains = *(buf.begin());
01345                 subchains->init();
01346                 return 0;
01347         }
01348         cerr << "pSim::Schedule(joints): error- invalid joint order" << endl;
01349         return -1;
01350 }
01351 
01352 int pSim::build_subchain_tree(int _n_joints, Joint** joints, subchain_list& buf)
01353 {
01354         int i;
01355         for(i=0; i<_n_joints; i++)
01356         {
01357                 cerr << "build_subchain_tree " << joints[i]->name << endl;
01358                 build_subchain_tree(joints[i], buf);
01359         }
01360         return 0;
01361 }
01362 
01363 void pSim::build_subchain_tree(Joint* cur_joint, subchain_list& buf)
01364 {
01365         JointInfo& jinfo = joint_info[cur_joint->i_joint];
01366         pJoint* pj0 = jinfo.pjoints[0];
01367         pJoint* pj1 = jinfo.pjoints[1];
01368         int pj0_done = false;
01369         int pj1_done = false;
01370         pSubChain* to_remove[2] = {0, 0};
01371         pSubChain* myp = new pSubChain(this, 0, pj0, pj1);
01372         subchain_list::iterator i;
01373         for(i=buf.begin(); i!=buf.end(); i++)
01374         {
01375                 if(!pj0_done && in_subchain(*i, pj0->plink))
01376                 {
01377                         myp->children[0] = *i;
01378                         (*i)->parent = myp;
01379                         pj0_done = true;
01380                         if(!to_remove[0]) to_remove[0] = *i;
01381                         else if(!to_remove[1]) to_remove[1] = *i;
01382                 }
01383                 if(!pj1_done && in_subchain(*i, pj1->plink))
01384                 {
01385                         myp->children[1] = *i;
01386                         (*i)->parent = myp;
01387                         pj1_done = true;
01388                         if(!to_remove[0]) to_remove[0] = *i;
01389                         else if(!to_remove[1]) to_remove[1] = *i;
01390                 }
01391         }
01392         if(!pj0_done && pj0->plink)
01393         {
01394                 pSubChain* newp = new pSubChain(this, myp, pj0->plink);
01395                 myp->children[0] = newp;
01396         }
01397         if(!pj1_done && pj1->plink)
01398         {
01399                 pSubChain* newp = new pSubChain(this, myp, pj1->plink);
01400                 myp->children[1] = newp;
01401         }
01402         if(to_remove[0]) buf.remove(to_remove[0]);
01403         if(to_remove[1]) buf.remove(to_remove[1]);
01404         buf.push_front(myp);
01405 }
01406 
01407 int pSim::in_subchain(pSubChain* sc, pLink* pl)
01408 {
01409         if(!sc) return 0;
01410         if(sc->links && sc->links[0] == pl) return 1;
01411         if(in_subchain(sc->children[0], pl)) return 1;
01412         if(sc->children[0] != sc->children[1] &&
01413            in_subchain(sc->children[1], pl)) return 1;
01414         return 0;
01415 }


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