schedule.cpp
Go to the documentation of this file.
1 /*
2  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
3  * All rights reserved. This program is made available under the terms of the
4  * Eclipse Public License v1.0 which accompanies this distribution, and is
5  * available at http://www.eclipse.org/legal/epl-v10.html
6  * Contributors:
7  * The University of Tokyo
8  */
14 #include "psim.h"
15 #include <dp.h>
16 
17 #define TINY_MASS 1.0e-16
18 #define N_2_COEF 1.6 // alpha
19 #define N_COEF 1.0 // beta
20 #define DOF_COEF -1.0 // gamma
21 #define CONST_COEF 14.4 // delta
22 
23 static pSim* sim = 0;
24 static int max_procs = 0;
25 
26 #include <fstream>
27 ofstream logfile("schedule.log");
28 //static ofstream logfile;
29 
30 // special handling of nodes with only one process
31 // 1. cost of the node: the cost to assemble all internal joints (constuctor)
32 // 2. assigning schedule children: no schedule children will be assigned (find_available_parent)
33 // 3. A* cost: always zero (calc_astar_cost)
34 // 4. converting the node to schedule: add all joints
36  : public dpNode
37 {
38 public:
39  dpScheduleNode(dpScheduleNode* potential_parent,
40  Joint* _last_joint, const fVec& _proc_costs,
41  dpScheduleNode* _schedule_parent, int child_id,
42  int _first_proc, int _last_proc,
43  const joint_list& org_internal_joints,
44  const p_joint_list& org_outer_joints): dpNode() {
45  last_joint = _last_joint;
46  if(last_joint)
47  {
49  }
50  else
51  {
52  last_pjoints[0] = 0;
53  last_pjoints[1] = 0;
54  }
55  schedule_parent = _schedule_parent;
56  first_proc = _first_proc;
57  last_proc = _last_proc;
58  if(schedule_parent)
59  {
62  {
63  set_outer_joints(schedule_parent->last_pjoints[child_id], org_outer_joints, outer_joints);
64  }
65  }
66  else
67  {
68  schedule_depth = -1;
69  for(p_joint_list::const_iterator j=org_outer_joints.begin(); j!=org_outer_joints.end(); j++)
70  {
71  outer_joints.push_back(*j);
72  }
73  }
74  single_joint_cost = 0.0;
76  proc_costs.set(_proc_costs);
77  mycost = 0.0;
78  if(last_joint)
79  {
80  logfile << "new node: " << last_joint->name << " (schedule depth = " << schedule_depth << flush;
81  logfile << ", procs = [" << first_proc << ", " << last_proc << "]" << flush;
83  logfile << ", schedule_parent = " << schedule_parent->last_joint->name << ")" << endl;
84  else
85  logfile << ")" << endl;
86  // split org_internal_joints by last_joint and find outer_joints
88  // compute costs
89  if(last_proc - first_proc == 1)
90  {
91  joint_list added_joints;
93  }
94  else
95  {
97  }
98  }
99  else
100  {
101  for(joint_list::const_iterator i=org_internal_joints.begin(); i!=org_internal_joints.end(); i++)
102  {
103  internal_joints[0].push_back(*i);
104  }
105  }
106  logfile << " single_joint_cost = " << single_joint_cost << endl;
107  for(int i=first_proc; i<last_proc; i++)
108  {
110  }
112  if(potential_parent)
113  {
114  mycost -= potential_parent->proc_costs.max_value();
115  }
116  logfile << " proc_costs = " << tran(proc_costs) << endl;
117  logfile << " mycost = " << mycost << endl;
118  }
120  }
121 
123  return proc_costs;
124  }
125 
127  return last_joint;
128  }
130  return schedule_depth;
131  }
132  int FirstProc() {
133  return first_proc;
134  }
135  int LastProc() {
136  return last_proc;
137  }
138  int ListAllInternalJoints(joint_list& all_internal);
139 
140 protected:
141  int list_all_internal_joints_sub(Joint* cur, joint_list& all_internal);
142  int list_all_internal_joints_rev(Joint* cur, joint_list& all_internal);
143  int split_internal_joints(Joint* _last_joint, const joint_list& org_internal_joints, joint_list& _internal_joints_0, joint_list& _internal_joints_1);
144  int set_outer_joints(pJoint* last_pjoint, const p_joint_list& org_outer_joints, p_joint_list& new_outer_joints);
145 
146  dpScheduleNode* find_available_parent(int target_depth, int& child_id);
147  dpScheduleNode* find_available_parent_sub(dpScheduleNode* cur, int target_depth, dpNode* cur_leaf, int& child_id);
148  // functions that should be defined in the subclass
149  // create and add child nodes
150  int create_child_nodes(dpNode**& nodes) {
151  if(is_goal()) return 0;
152  logfile << "create_child_node [" << TotalAstarCost() << "] (joints = " << flush;
153  dpNode* n;
154  for(n=this; n; n=n->Parent())
155  {
157  if(s->last_joint)
158  logfile << "[" << s->last_joint->name << "/" << s->schedule_depth << "]" << flush;
159  }
160  logfile << ")" << endl;
161  // last joint
162  if(depth == sim->NumJoint()-1)
163  {
164  logfile << "create goal (0)" << endl;
165  nodes = new dpNode* [1];
166  nodes[0] = new dpScheduleNode(this, 0, proc_costs, this, 0, first_proc, last_proc, joint_list(), p_joint_list());
167  return 1;
168  }
169  // find the schedule_parent to add
170  dpScheduleNode* target_parent = 0;
171  int child_id = -1;
172  target_parent = find_available_parent(schedule_depth-1, child_id);
173  if(!target_parent)
174  {
175  // try next depth
176  target_parent = find_available_parent(schedule_depth, child_id);
177  // no parent to add: add a goal
178  if(!target_parent)
179  {
180  logfile << "create goal (1)" << endl;
181  nodes = new dpNode* [1];
182  nodes[0] = new dpScheduleNode(this, 0, proc_costs, this, 0, first_proc, last_proc, joint_list(), p_joint_list());
183  return 1;
184  }
185  }
187  if(target_parent->last_joint)
188  {
189  logfile << "target_parent = " << target_parent->last_joint->name << ", schedule_depth = " << schedule_depth << ", child_id = " << child_id << endl;
190  }
191  else
192  {
193  logfile << "target_parent = null, schedule_depth = " << schedule_depth << ", child_id = " << child_id << endl;
194  }
195  int _first_proc = 0, _last_proc = max_procs;
196  if(child_id == 0)
197  {
198  if(target_parent->internal_joints[1].size() > 0)
199  {
200  _first_proc = target_parent->first_proc;
201  _last_proc = target_parent->first_proc + (target_parent->last_proc-target_parent->first_proc)/2;
202  }
203  else
204  {
205  _first_proc = target_parent->first_proc;
206  _last_proc = target_parent->last_proc;
207  }
208  }
209  else
210  {
211  if(target_parent->internal_joints[0].size() > 0)
212  {
213  _first_proc = target_parent->first_proc + (target_parent->last_proc-target_parent->first_proc)/2;
214  _last_proc = target_parent->last_proc;
215  }
216  else
217  {
218  _first_proc = target_parent->first_proc;
219  _last_proc = target_parent->last_proc;
220  }
221  }
222  joint_list& internal_joints = target_parent->internal_joints[child_id];
223  // only one process available: apply default schedule
224  if(_last_proc - _first_proc == 1)
225  {
226  nodes = new dpNode* [1];
227  // set child's last_joint
228  Joint* next_last = 0;
229  if(target_parent->last_joint)
230  {
231  joint_list& in_joints = target_parent->internal_joints[child_id];
232  for(joint_list::iterator i=in_joints.begin(); i!=in_joints.end(); i++)
233  {
234  if(*i == target_parent->last_joint->parent ||
235  (*i)->parent == target_parent->last_joint ||
236  (*i)->parent == target_parent->last_joint->parent)
237  {
238  next_last = *i;
239  break;
240  }
241  }
242  }
243  else
244  {
245  next_last = sim->Root();
246  }
247  dpScheduleNode* n = new dpScheduleNode(this, next_last, proc_costs, target_parent, child_id, _first_proc, _last_proc, internal_joints, target_parent->outer_joints);
248  nodes[0] = (dpNode*)n;
249  return 1;
250  }
251  // more than two processors available
252  int n_internal_joints = internal_joints.size();
253  nodes = new dpNode* [n_internal_joints];
254  joint_list::iterator j;
255  int i;
256  for(i=0, j=internal_joints.begin(); j!=internal_joints.end(); i++, j++)
257  {
258  dpScheduleNode* n = new dpScheduleNode(this, *j, proc_costs, target_parent, child_id, _first_proc, _last_proc, internal_joints, target_parent->outer_joints);
259 #if 1 // always generate two non-empty subchains
260  if(n->internal_joints[0].size() == 0 ||
261  n->internal_joints[1].size() == 0)
262  {
263  delete n;
264  nodes[i] = 0;
265  }
266  else
267 #endif
268  {
269  nodes[i] = (dpNode*)n;
270  }
271  }
272  return n_internal_joints;
273  }
274  // compute (local) cost
275  double calc_cost() {
276  return mycost;
277  }
278  // (optional) compute A-star cost
279  double calc_astar_cost(dpNode* potential_parent) {
280  double c = 0.0;
281  logfile << "calc_astar_cost(" << flush;
282  if(last_joint)
283  {
284  logfile << last_joint->name << "/" << schedule_depth << ")" << endl;
285  p_joint_list new_outer_joints;
286  if(last_proc - first_proc == 1)
287  {
288 #if 0
289  double tmp = calc_min_subchain_cost(outer_joints);
290  logfile << " one process: " << tmp << endl;
291  if(tmp > c) c = tmp;
292 #endif
293  }
294  else
295  {
296  // child 0
297  if(internal_joints[0].size() > 0)
298  {
299  int n_myprocs = last_proc - first_proc;
300  joint_list added_joints;
301  if(internal_joints[1].size() > 0)
302  {
303  n_myprocs = (last_proc - first_proc)/2;
304  }
305  set_outer_joints(last_pjoints[0], outer_joints, new_outer_joints);
306  double tmp = calc_min_subchain_cost(new_outer_joints, added_joints)/n_myprocs;
307  logfile << " child 0: " << tmp << " [" << n_myprocs << "]" << endl;
308  if(tmp > c) c = tmp;
309  }
310  // child 1
311  if(internal_joints[1].size() > 0)
312  {
313  joint_list added_joints;
314  int n_myprocs = last_proc - first_proc;
315  if(internal_joints[0].size() > 0)
316  {
317  n_myprocs -= (last_proc-first_proc)/2;
318  }
319  set_outer_joints(last_pjoints[1], outer_joints, new_outer_joints);
320  double tmp = calc_min_subchain_cost(new_outer_joints, added_joints)/n_myprocs;
321  logfile << " child 1: " << tmp << " [" << n_myprocs << "]" << endl;
322  if(tmp > c) c = tmp;
323  }
324  }
325  }
326  else
327  {
328  logfile << "default)" << endl;
329  }
330  logfile << " final = " << c << endl;
331  return c;
332  }
333  // check if the states are same
334  int same_state(dpNode* refnode) {
335  return false;
336  }
337  // check if the state is goal
338  int is_goal() {
339  return ((!last_joint && depth > 0) || depth == sim->NumJoint());
340  }
341 
342 private:
345 
350 
352  double mycost;
355 
356  double calc_single_joint_cost(int n_dof, int n_outer) {
357 // return INVERSE_UNIT*(6-n_dof)*(6-n_dof)*(6-n_dof) + n_outer*n_outer;
358  return N_2_COEF*n_outer*n_outer + N_COEF*n_outer + DOF_COEF*n_dof + CONST_COEF;
359  }
360 
361  double calc_min_subchain_cost(const p_joint_list& _outer_joints, joint_list& added_joints);
362  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);
363 
364  double add_to_child_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side);
365  double add_to_parent_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side);
366 };
367 
369 {
370 #if 0
371  all_internal.clear();
372  if(!last_joint) return 0;
373  if(last_proc - first_proc > 1)
374  {
375  all_internal.push_back(last_joint);
376  return 0;
377  }
378  pJoint* top_pjoint = 0;
379  // find the top joint
380  for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
381  {
382  if(!(*j)->ParentSide())
383  {
384  top_pjoint = *j;
385  break;
386  }
387  }
388  if(!top_pjoint)
389  {
390  list_all_internal_joints_rev(sim->Root(), all_internal);
391  }
392  else
393  {
394  list_all_internal_joints_sub(top_pjoint->GetJoint()->child, all_internal);
395  }
396 #else
397  if(last_proc - first_proc == 1)
398  {
399  calc_min_subchain_cost(outer_joints, all_internal);
400  }
401  else if(last_joint)
402  {
403  all_internal.push_back(last_joint);
404  }
405 #endif
406  return 0;
407 }
408 
410 {
411  if(!cur) return 0;
412  pJoint* pjoints[2];
413  sim->GetPJoint(cur, pjoints);
414  for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
415  {
416  if(pjoints[1] == *j)
417  {
418  return 0;
419  }
420  }
421  all_internal.push_back(cur);
422  list_all_internal_joints_rev(cur->child, all_internal);
423  list_all_internal_joints_rev(cur->brother, all_internal);
424  return 0;
425 }
426 
428 {
429  if(!cur) return 0;
430  pJoint* pjoints[2];
431  sim->GetPJoint(cur, pjoints);
432  for(p_joint_list::const_iterator j=outer_joints.begin(); j!=outer_joints.end(); j++)
433  {
434  if(pjoints[1] == *j)
435  {
436  return 0;
437  }
438  }
439  list_all_internal_joints_sub(cur->child, all_internal);
440  list_all_internal_joints_sub(cur->brother, all_internal);
441  all_internal.push_back(cur);
442  return 0;
443 }
444 
445 double dpScheduleNode::calc_min_subchain_cost(const p_joint_list& _outer_joints, joint_list& added_joints)
446 {
447  pJoint* top_pjoint = 0;
448  logfile << "=== calc_min_subchain_cost ===" << endl;
449  logfile << "outer = [" << flush;
450  // find the top joint
451  for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
452  {
453  logfile << " " << (*j)->GetJoint()->name << flush;
454  if(!(*j)->ParentSide())
455  {
456  top_pjoint = *j;
457 // break;
458  }
459  }
460  logfile << "]" << endl;
461 
462  int* n_outer_parent_side = new int [sim->NumJoint()];
463  int* n_outer_child_side = new int [sim->NumJoint()];
464  for(int i=0; i<sim->NumJoint(); i++)
465  {
466  n_outer_parent_side[i] = 0;
467  n_outer_child_side[i] = 0;
468  }
469  for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
470  {
471  if((*j)->ParentSide())
472  {
473  Joint* cur = (*j)->GetJoint();
474  for(Joint* p=cur->parent; p; p=p->parent)
475  {
476  n_outer_child_side[p->i_joint]++;
477  }
478  }
479  }
480  for(int i=0; i<sim->NumJoint(); i++)
481  {
482  n_outer_parent_side[i] = _outer_joints.size() - n_outer_child_side[i];
483 // logfile << sim->FindJoint(i)->name << ": parent_side = " << n_outer_parent_side[i] << ", child_side = " << n_outer_child_side[i] << endl;
484  }
485  double ret = 0.0;
486  added_joints.clear();
487  if(!top_pjoint)
488  {
489  ret = calc_min_subchain_cost_sub(sim->Root(), _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
490  }
491  else
492  {
493  joint_list sorted_child;
494  for(Joint* c = top_pjoint->GetJoint()->child; c; c=c->brother)
495  {
496  int done = false;
497  for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
498  {
499  if(n_outer_child_side[c->i_joint] <= n_outer_child_side[(*j)->i_joint])
500  {
501  done = true;
502  sorted_child.insert(j, c);
503  break;
504  }
505  }
506  if(!done) sorted_child.push_back(c);
507  }
508  for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
509  {
510  ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
511  }
512  }
513  delete[] n_outer_parent_side;
514  delete[] n_outer_child_side;
515  return ret;
516 }
517 
518 int is_in(const joint_list& jlist, Joint* jnt)
519 {
520  for(joint_list::const_iterator i=jlist.begin(); i!=jlist.end(); i++)
521  {
522  if(*i == jnt) return true;
523  }
524  return false;
525 }
526 
527 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)
528 {
529  if(!cur) return 0.0;
530 // logfile << "calc_min_subchain_cost_sub(" << cur->name << ") ->" << endl;
531  pJoint* pjoints[2];
532  sim->GetPJoint(cur, pjoints);
533  for(p_joint_list::const_iterator j=_outer_joints.begin(); j!=_outer_joints.end(); j++)
534  {
535  if(pjoints[1] == *j)
536  {
537  return 0.0;
538  }
539  }
540  double ret = 0.0;
541  int descend = false;
542  int my_outer = n_outer_child_side[cur->i_joint];
543  if(n_outer_parent_side[cur->i_joint] < n_outer_child_side[cur->i_joint])
544  {
545  descend = true;
546  my_outer = n_outer_parent_side[cur->i_joint];
547  }
548  // if not in the descending order, this joint should be processed later
549  if(!descend)
550  {
551  joint_list sorted_child;
552  for(Joint* c = cur->child; c; c=c->brother)
553  {
554  int done = false;
555  for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
556  {
557  if(n_outer_child_side[c->i_joint] <= n_outer_child_side[(*j)->i_joint])
558  {
559  done = true;
560  sorted_child.insert(j, c);
561  break;
562  }
563  }
564  if(!done) sorted_child.push_back(c);
565  }
566  for(joint_list::iterator j=sorted_child.begin(); j!=sorted_child.end(); j++)
567  {
568  ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
569  }
570  ret += add_to_parent_side(cur, added_joints, n_outer_parent_side, n_outer_child_side);
571  }
572  // if in descending order, sort the children in the descending order of
573  // outer joints
574  else
575  {
576  joint_list smaller_child, larger_child;
577  for(Joint* c=cur->child; c; c=c->brother)
578  {
579  int n_child_outer = n_outer_child_side[c->i_joint];
580  if(n_child_outer <= my_outer)
581  {
582  // add to smaller_child
583  int done = false;
584  for(joint_list::iterator i=smaller_child.begin(); i!=smaller_child.end(); i++)
585  {
586  int n = n_outer_child_side[(*i)->i_joint];
587  if(n_child_outer <= n)
588  {
589  smaller_child.insert(i, c);
590  done = true;
591  break;
592  }
593  }
594  if(!done)
595  {
596  smaller_child.push_back(c);
597  }
598  }
599  else
600  {
601  // add to larger_child
602  int done = false;
603  for(joint_list::iterator i=larger_child.begin(); i!=larger_child.end(); i++)
604  {
605  int n = n_outer_child_side[(*i)->i_joint];
606  if(n_child_outer <= n)
607  {
608  larger_child.insert(i, c);
609  done = true;
610  break;
611  }
612  }
613  if(!done)
614  {
615  larger_child.push_back(c);
616  }
617  }
618  }
619 // logfile << " smaller: " << endl;
620  for(joint_list::iterator j=smaller_child.begin(); j!=smaller_child.end(); j++)
621  {
622 // logfile << " " << (*j)->name << endl;
623  ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
624  }
625  ret += add_to_child_side(cur, added_joints, n_outer_parent_side, n_outer_child_side);
626 // logfile << " larger: " << endl;
627  for(joint_list::iterator j=larger_child.begin(); j!=larger_child.end(); j++)
628  {
629 // logfile << " " << (*j)->name << endl;
630  ret += calc_min_subchain_cost_sub(*j, _outer_joints, n_outer_parent_side, n_outer_child_side, added_joints);
631  }
632  }
633 // logfile << "<- calc_min_subchain_cost_sub(" << cur->name << ")" << endl;
634  return ret;
635 }
636 
637 double dpScheduleNode::add_to_child_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side)
638 {
639  double ret = 0.0;
640  int n_outer = n_outer_parent_side[cur->i_joint];
641  int n_link_outer = 0;
642  for(Joint* c=cur->child; c; c=c->brother)
643  {
644  if(!is_in(added_joints, c))
645  {
646  n_link_outer++;
647  }
648  }
649  added_joints.push_back(cur);
650  ret = calc_single_joint_cost(cur->n_dof, n_outer+n_link_outer);
651  logfile << " adding " << cur->name << ", n_outer = " << n_outer << ", n_link_outer = " << n_link_outer << ", cost = " << ret << endl;
652  return ret;
653 }
654 
655 double dpScheduleNode::add_to_parent_side(Joint* cur, joint_list& added_joints, int* n_outer_parent_side, int* n_outer_child_side)
656 {
657  double ret = 0.0;
658  int n_outer = n_outer_child_side[cur->i_joint];
659  int n_link_outer = 0;
660  if(cur->parent && !is_in(added_joints, cur->parent))
661  {
662  n_link_outer++;
663  }
664  if(cur->parent)
665  {
666  for(Joint* c=cur->parent->child; c; c=c->brother)
667  {
668  if(!is_in(added_joints, c) && c!=cur)
669  {
670  n_link_outer++;
671  }
672  }
673  }
674  added_joints.push_back(cur);
675  ret = calc_single_joint_cost(cur->n_dof, n_outer+n_link_outer);
676  logfile << " adding " << cur->name << ", n_outer = " << n_outer << ", n_link_outer = " << n_link_outer << ", cost = " << ret << endl;
677  return ret;
678 }
679 
680 dpScheduleNode* dpScheduleNode::find_available_parent(int target_depth, int& child_id)
681 {
682  dpScheduleNode* s, *ret = 0;
683  for(s=this; s; s=(dpScheduleNode*)s->parent)
684  {
685  if((ret = find_available_parent_sub(s, target_depth, this, child_id)))
686  return ret;
687  }
688  return 0;
689 }
690 
691 dpScheduleNode* dpScheduleNode::find_available_parent_sub(dpScheduleNode* cur, int target_depth, dpNode* cur_leaf, int& child_id)
692 {
693  if(!cur) return 0;
694  if(cur->last_proc - cur->first_proc == 1) return 0;
695  if(cur->schedule_depth == target_depth)
696  {
697  if(cur->internal_joints[0].size() > 0)
698  {
699  int failed = false;
700  joint_list::iterator f;
701  for(f=cur->internal_joints[0].begin(); f!=cur->internal_joints[0].end(); f++)
702  {
703  dpNode* n;
704  for(n=cur_leaf; n; n=n->Parent())
705  {
707  if(*f == s->last_joint)
708  {
709  failed = true;
710  break;
711  }
712  }
713  if(failed) break;
714  }
715  if(!failed)
716  {
717  child_id = 0;
718  return cur;
719  }
720  }
721  if(cur->internal_joints[1].size() > 0)
722  {
723  int failed = false;
724  joint_list::iterator f;
725  for(f=cur->internal_joints[1].begin(); f!=cur->internal_joints[1].end(); f++)
726  {
727  dpNode* n;
728  for(n=cur_leaf; n; n=n->Parent())
729  {
731  if(*f == s->last_joint)
732  {
733  failed = true;
734  break;
735  }
736  }
737  if(failed) break;
738  }
739  if(!failed)
740  {
741  child_id = 1;
742  return cur;
743  }
744  }
745  }
746  return 0;
747 }
748 
749 static void get_all_joints(Joint* cur, joint_list& all_joints, joint_list& all_vjoints)
750 {
751  if(!cur) return;
752  if(cur->real)
753  {
754  all_vjoints.push_back(cur);
755  }
756  else
757  {
758  all_joints.push_back(cur);
759  }
760  get_all_joints(cur->child, all_joints, all_vjoints);
761  get_all_joints(cur->brother, all_joints, all_vjoints);
762  return;
763 }
764 
765 int pSim::AutoSchedule(int _max_procs)
766 {
767  logfile << "AutoSchedule(max_procs = " << _max_procs << ")" << endl;
768  // set static variables ->
769  sim = this;
770  max_procs = _max_procs;
771  // <-
772  joint_list init_internal_joints; // all joints
773  p_joint_list init_outer_joints; // virtual joints only
774  joint_list all_vjoints;
775  // list all joints
776  get_all_joints(root, init_internal_joints, all_vjoints);
777  // set init_outer_joints
778  for(joint_list::iterator j=all_vjoints.begin(); j!=all_vjoints.end(); j++)
779  {
780  pJoint* v_pjoints[2];
781  GetPJoint(*j, v_pjoints);
782  init_outer_joints.push_back(v_pjoints[0]);
783  init_outer_joints.push_back(v_pjoints[1]);
784  }
785  dpMain* dp = new dpMain;
786  fVec init_proc_costs(max_procs);
787  init_proc_costs.zero();
788  dpScheduleNode* start_node = new dpScheduleNode(0, 0, init_proc_costs, 0, 0, 0, max_procs, init_internal_joints, init_outer_joints);
789 
790  dp->SetStartNode(start_node);
791  dp->Search(-1, 1);
792  dpNode* goal = dp->BestGoal();
793  if(!goal)
794  {
795  logfile << "pSim::AutoSchedule: goal not found" << endl;
796  delete dp;
797  return -1;
798  }
799  cerr << "goal found" << endl;
800  cerr << "cost = " << goal->TotalCost() << endl;
801  fVec& proc_costs = ((dpScheduleNode*)goal)->ProcCosts();
802  cerr << "proc_costs = " << tran(proc_costs) << endl;
803  Joint** joints = new Joint* [n_joint+all_vjoints.size()];
804  dpNode* n;
805  int count = 0;
806  for(n=goal->Parent(); n; n=n->Parent())
807  {
809  if(s->LastJoint())
810  {
811  cerr << "[" << s->LastJoint()->name << "/" << s->ScheduleDepth() << "] cost = " << n->TotalCost() << ", procs = [" << s->FirstProc() << ", " << s->LastProc() << "]" << endl;
812  }
813  else
814  {
815  cerr << "[default/" << s->ScheduleDepth() << "]" << endl;
816  }
817  joint_list all_internal;
818  s->ListAllInternalJoints(all_internal);
819  logfile << "joints = " << endl;
820  for(joint_list::iterator j=all_internal.begin(); j!=all_internal.end(); j++)
821  {
822  logfile << " " << count << " " << (*j)->name << endl;
823  joints[count] = *j;
824  count++;
825  }
826  }
827  logfile << "current count = " << count << ", number of vjoints = " << all_vjoints.size() << ", total n_joint = " << n_joint << endl;
828  logfile << "joints = " << endl;
829  for(int i=0; i<count; i++)
830  {
831  logfile << "[" << i << "] = " << joints[i]->name << endl;
832  }
833  for(joint_list::iterator j=all_vjoints.begin(); j!=all_vjoints.end(); j++)
834  {
835  joints[count] = *j;
836  logfile << "[" << count << "] = " << joints[count]->name << endl;
837  count++;
838  }
839 #if 1
840  Schedule(joints);
841 #endif
842  delete[] joints;
843  delete dp;
844  return 0;
845 }
846 
847 int dpScheduleNode::split_internal_joints(Joint* _last_joint, const joint_list& org_internal_joints, joint_list& _internal_joints_0, joint_list& _internal_joints_1)
848 {
849  _internal_joints_0.clear();
850  _internal_joints_1.clear();
851  joint_list::const_iterator j;
852 // logfile << "-- split at " << _last_joint->name << endl;
853  for(j=org_internal_joints.begin(); j!=org_internal_joints.end(); j++)
854  {
855 // logfile << " " << (*j)->name << " goes to " << flush;
856  if(*j == _last_joint)
857  {
858 // logfile << " none" << endl;
859  }
860  else if((*j)->isAscendant(_last_joint))
861  {
862  _internal_joints_0.push_back(*j);
863 // logfile << " 0" << endl;
864  }
865  else
866  {
867  _internal_joints_1.push_back(*j);
868 // logfile << " 1" << endl;
869  }
870  }
871 // logfile << "split end" << endl;
872  return 0;
873 }
874 
875 int dpScheduleNode::set_outer_joints(pJoint* last_pjoint, const p_joint_list& org_outer_joints, p_joint_list& new_outer_joints)
876 {
877  new_outer_joints.clear();
878  new_outer_joints.push_back(last_pjoint);
879  p_joint_list::const_iterator p;
880  int child_id = (last_pjoint->ParentSide()) ? 1 : 0;
881  for(p=org_outer_joints.begin(); p!=org_outer_joints.end(); p++)
882  {
883  if((*p)->GetJoint()->isAscendant(last_pjoint->GetJoint()))
884  {
885  if(child_id == 0)
886  new_outer_joints.push_back(*p);
887  }
888  else if(child_id == 1)
889  {
890  new_outer_joints.push_back(*p);
891  }
892  }
893  return 0;
894 }
895 
896 #ifdef USE_MPI
897 
900 int pSim::AssignProcesses(int max_procs)
901 {
902  size = max_procs;
903  subchains->assign_processes(0, max_procs);
904 
905  all_acc_types = new MPI_Datatype [max_procs];
906  int** lengths = new int* [max_procs];
907  MPI_Aint** disps = new MPI_Aint* [max_procs];
908  MPI_Datatype** oldtypes = new MPI_Datatype* [max_procs];
909  int* n_proc_joints = new int [max_procs];
910  int i;
911  for(i=0; i<max_procs; i++)
912  {
913  lengths[i] = new int [3*n_joint];
914  disps[i] = new MPI_Aint [3*n_joint];
915  oldtypes[i] = new MPI_Datatype [3*n_joint];
916  n_proc_joints[i] = 0;
917  }
918  subchains->create_types(n_proc_joints, lengths, disps, oldtypes);
919  for(i=0; i<max_procs; i++)
920  {
921  MPI_Type_create_struct(n_proc_joints[i], lengths[i], disps[i], oldtypes[i], all_acc_types+i);
922  MPI_Type_commit(all_acc_types+i);
923  logfile << "[" << rank << "]: all_acc_types[" << i << "] = " << all_acc_types[i] << endl;
924  }
925 
926  for(i=0; i<max_procs; i++)
927  {
928  delete[] lengths[i];
929  delete[] disps[i];
930  delete[] oldtypes[i];
931  }
932  delete[] lengths;
933  delete[] disps;
934  delete[] oldtypes;
935  delete[] n_proc_joints;
936  return 0;
937 }
938 
939 // available ranks: start_rank -> end_rank-1
940 int pSubChain::assign_processes(int start_rank, int end_rank)
941 {
942  rank = start_rank;
943  // assign different processes to the children
944  if(children[0] && children[1] && children[0] != children[1] &&
945  children[0]->last_joint && children[1]->last_joint &&
946  end_rank > start_rank+1)
947  {
948  int n_my_procs = end_rank - start_rank;
949  int n_half_procs = n_my_procs/2;
950  children[0]->assign_processes(start_rank, start_rank+n_half_procs);
951  children[1]->assign_processes(start_rank+n_half_procs, end_rank);
952  }
953  // otherwise assign the same process
954  else
955  {
956  if(children[0])
957  {
958  children[0]->assign_processes(start_rank, end_rank);
959  }
960  if(children[0] != children[1] && children[1])
961  {
962  children[1]->assign_processes(start_rank, end_rank);
963  }
964  }
965  return 0;
966 }
967 
968 int pSubChain::create_types(int* n_proc_joints, int** _lengths, MPI_Aint** _disps, MPI_Datatype** _oldtypes)
969 {
970  if(!this) return 0;
971  // types for sending to parent
972  if(parent && rank != parent->rank)
973  {
974  logfile << "[" << sim->rank << "] create_types " << last_joint->name << endl;
975  int n_mat = n_outer_joints * n_outer_joints;
976  int count = 0;
977  int* lengths = new int [n_mat];
978  MPI_Aint* disps = new int [n_mat];
979  MPI_Datatype* oldtypes = new int [n_mat];
980  for(int i=0; i<n_outer_joints; i++)
981  {
982  for(int j=0; j<n_outer_joints; j++)
983  {
984  lengths[count] = 36;
985  oldtypes[count] = MPI_DOUBLE;
986  MPI_Get_address(Lambda[i][j].data(), disps+count);
987  count++;
988  }
989  }
990  MPI_Type_create_struct(n_mat, lengths, disps, oldtypes, &parent_lambda_type);
991  MPI_Type_commit(&parent_lambda_type);
992  logfile << "[" << sim->rank << "]: parent_lambda_type = " << parent_lambda_type << endl;
993  // acc
994  for(int i=0; i<n_outer_joints; i++)
995  {
996  lengths[i] = 6;
997  oldtypes[i] = MPI_DOUBLE;
998  MPI_Get_address(acc_temp[i].data(), disps+i);
999  count++;
1000  }
1001  MPI_Type_create_struct(n_outer_joints, lengths, disps, oldtypes, &parent_acc_type);
1002  MPI_Type_commit(&parent_acc_type);
1003  logfile << "[" << sim->rank << "]: parent_acc_type = " << parent_acc_type << endl;
1004  delete[] lengths;
1005  delete[] disps;
1006  delete[] oldtypes;
1007  }
1008  if(children[0] && rank != children[0]->rank || children[1] && rank != children[1]->rank)
1009  {
1010  int n_array = n_outer_joints + 2;
1011  int* lengths = new int [n_array];
1012  MPI_Aint* disps = new int [n_array];
1013  MPI_Datatype* oldtypes = new int [n_array];
1014  // force
1015  for(int i=0; i<n_outer_joints; i++)
1016  {
1017  lengths[i] = 6;
1018  oldtypes[i] = MPI_DOUBLE;
1019  MPI_Get_address(outer_joints[i]->f_final.data(), disps+i);
1020  }
1021  // f_final
1022  lengths[n_outer_joints] = 6;
1023  lengths[n_outer_joints+1] = 6;
1024  oldtypes[n_outer_joints] = MPI_DOUBLE;
1025  oldtypes[n_outer_joints+1] = MPI_DOUBLE;
1026  MPI_Get_address(last_pjoints[0]->f_final.data(), disps+n_outer_joints);
1027  MPI_Get_address(last_pjoints[1]->f_final.data(), disps+n_outer_joints+1);
1028  MPI_Type_create_struct(n_outer_joints+2, lengths, disps, oldtypes, &parent_force_type);
1029  MPI_Type_commit(&parent_force_type);
1030  logfile << "[" << sim->rank << "]: parent_force_type = " << parent_force_type << endl;
1031  delete[] lengths;
1032  delete[] disps;
1033  delete[] oldtypes;
1034  }
1035  // acc
1036  if(last_joint && last_joint->n_dof > 0)
1037  {
1038  int index = n_proc_joints[rank];
1039  _oldtypes[rank][index] = MPI_DOUBLE;
1040  _oldtypes[rank][index+1] = MPI_DOUBLE;
1041  _oldtypes[rank][index+2] = MPI_DOUBLE;
1042  MPI_Get_address(acc_final.data(), _disps[rank]+index);
1043  MPI_Get_address(last_pjoints[0]->f_final.data(), _disps[rank]+index+1);
1044  MPI_Get_address(last_pjoints[1]->f_final.data(), _disps[rank]+index+2);
1045  _lengths[rank][index] = acc_final.size();
1046  _lengths[rank][index+1] = 6;
1047  _lengths[rank][index+2] = 6;
1048  n_proc_joints[rank] += 3;
1049  }
1050  children[0]->create_types(n_proc_joints, _lengths, _disps, _oldtypes);
1051  if(children[0] != children[1]) children[1]->create_types(n_proc_joints, _lengths, _disps, _oldtypes);
1052  return 0;
1053 }
1054 
1055 #endif
1056 
1061 {
1062  if(subchains) delete subchains;
1063  subchains = 0;
1064  // first attach subchains ignoring the virtual links
1065  subchains = default_schedule(0, root);
1066  // insert subchains for the virtual links
1067  default_schedule_virtual(root);
1068  subchains->init();
1069  return 0;
1070 }
1071 
1073 {
1074  if(!j) return 0;
1075  if(j->i_joint < 0) return 0; // OK?
1076  if(j->real) return 0; // skip virtual links for now
1077  // skip mass-less end points (but include space)
1078  if(j->mass < TINY_MASS && j->parent)
1079  {
1080  pSubChain* c1 = default_schedule(p, j->brother);
1081  if(c1) return c1;
1082  pSubChain* c0 = default_schedule(p, j->child);
1083  return c0;
1084  }
1085  // create subchain for j
1086  pJoint* pj0, *pj1;
1087  pj0 = joint_info[j->i_joint].pjoints[0];
1088  pj1 = joint_info[j->i_joint].pjoints[1];
1089  pSubChain* sc = new pSubChain(this, p, pj0, pj1);
1090  // children subchains
1091  pSubChain* c0 = 0, *c1 = 0;
1092  // brother or parent link
1093  if(j->brother)
1094  {
1095  c1 = default_schedule(sc, j->brother);
1096  }
1097  if(!c1 && j->parent && j->parent->i_joint >= 0)
1098  {
1099  pLink* pl = joint_info[j->parent->i_joint].plink;
1100  c1 = new pSubChain(this, sc, pl);
1101  }
1102  // children
1103  if(j->child && !j->child->real)
1104  {
1105  c0 = default_schedule(sc, j->child);
1106  }
1107  if(!c0 && j->i_joint >= 0)
1108  {
1109  pLink* pl = joint_info[j->i_joint].plink;
1110  c0 = new pSubChain(this, sc, pl);
1111  }
1112  sc->children[0] = c0;
1113  sc->children[1] = c1;
1114  return sc;
1115 }
1116 
1118 {
1119  if(!j) return;
1120  if(j->i_joint < 0) return;
1121  // virtual but not contact
1122  if(j->real && contact_vjoint_index(j) < 0)
1123  {
1124  pJoint* pj0, *pj1;
1125  pj0 = joint_info[j->i_joint].pjoints[0];
1126  pj1 = joint_info[j->i_joint].pjoints[1];
1127  pSubChain* sc = new pSubChain(this, subchains, pj0, pj1);
1128  // insert sc between subchains and its child
1129  // same child for [0] and [1]
1130  sc->children[0] = subchains->children[0];
1131  sc->children[1] = subchains->children[0];
1132  // subchains->children[1] must be null
1133  subchains->children[0]->parent = sc;
1134  subchains->children[0] = sc;
1135  }
1136  default_schedule_virtual(j->brother);
1137  default_schedule_virtual(j->child);
1138 }
1139 
1141 {
1142  if(!this) return;
1143  children[0]->init();
1144  if(children[0] != children[1]) children[1]->init();
1145  if(Lambda)
1146  {
1147  for(int i=0; i<n_outer_joints; i++) delete[] Lambda[i];
1148  delete[] Lambda;
1149  }
1150  Lambda = 0;
1151  if(acc_temp) delete[] acc_temp;
1152  acc_temp = 0;
1153  if(vel_temp) delete[] vel_temp;
1154  vel_temp = 0;
1155  n_outer_joints = 0;
1156  if(outer_joints) delete[] outer_joints;
1157  if(outer_joints_origin) delete[] outer_joints_origin;
1158  if(outer_joints_index) delete[] outer_joints_index;
1159  outer_joints = 0;
1160  outer_joints_origin = 0;
1161  outer_joints_index = 0;
1162  // consists of a single link
1163  if(!last_joint && n_links == 1)
1164  {
1165  int i;
1166  // count outer joints
1167  Joint* p = links[0]->joint;
1168  links[0]->subchain = this;
1169  n_outer_joints = 1; // parent
1170  Joint* cur;
1171  for(cur=p->child; cur; cur=cur->brother)
1172  {
1173  if(cur->real || cur->mass > TINY_MASS)
1174  n_outer_joints++; // children
1175  }
1176  // virtual links
1177  for(i=0; i<sim->n_joint; i++)
1178  {
1179  if(sim->joint_info[i].pjoints[0]->joint->real == links[0]->joint)
1180  n_outer_joints++;
1181  }
1182  outer_joints = new pJoint* [n_outer_joints];
1183  outer_joints_origin = new int [n_outer_joints];
1184  outer_joints_index = new int [n_outer_joints];
1185  outer_joints[0] = sim->joint_info[p->i_joint].pjoints[0]; // child side
1186  outer_joints_origin[0] = -1;
1187  outer_joints_index[0] = -1;
1188  n_outer_joints = 1;
1189  for(cur=p->child; cur; cur=cur->brother)
1190  {
1191  if(cur->real || cur->mass > TINY_MASS)
1192  {
1193  outer_joints[n_outer_joints] = sim->joint_info[cur->i_joint].pjoints[1]; // parent side
1194  outer_joints[n_outer_joints]->subchain = this;
1195  outer_joints_origin[n_outer_joints] = -1;
1196  outer_joints_index[n_outer_joints] = -1;
1197  n_outer_joints++;
1198  }
1199  }
1200  for(i=0; i<sim->n_joint; i++)
1201  {
1202  if(sim->joint_info[i].pjoints[0]->joint->real == links[0]->joint)
1203  {
1204  outer_joints[n_outer_joints] = sim->joint_info[i].pjoints[0]; // child side
1205  outer_joints[n_outer_joints]->subchain = this;
1206  outer_joints_origin[n_outer_joints] = -1;
1207  outer_joints_index[n_outer_joints] = -1;
1208  n_outer_joints++;
1209  }
1210  }
1211  }
1212  // gather information from children subchains
1213  else
1214  {
1215  int i, count;
1216  // last_index
1217  if(children[0]) last_index[0] = children[0]->get_outer_index(last_pjoints[0]);
1218  if(children[1]) last_index[1] = children[1]->get_outer_index(last_pjoints[1]);
1219  // outer joints
1220  n_outer_joints = children[0]->n_outer_joints - 1;
1221  if(children[0] == children[1])
1222  n_outer_joints--;
1223  else if(children[1])
1224  n_outer_joints += children[1]->n_outer_joints - 1;
1225  if(n_outer_joints > 0)
1226  {
1227  outer_joints = new pJoint* [n_outer_joints];
1228  outer_joints_origin = new int [n_outer_joints];
1229  outer_joints_index = new int [n_outer_joints];
1230  count = 0;
1231  for(i=0; i<children[0]->n_outer_joints; i++)
1232  {
1233  if(children[0]->outer_joints[i]->joint != last_joint)
1234  {
1235  outer_joints[count] = children[0]->outer_joints[i];
1236  outer_joints_origin[count] = 0;
1237  if(children[0] == children[1] &&
1238  outer_joints[count]->joint->real &&
1239  outer_joints[count]->joint->real != outer_joints[count]->link_side) // virtual side
1240  {
1241  outer_joints_origin[count] = 1;
1242  }
1243  outer_joints_index[count] = i;
1244  count++;
1245  }
1246  }
1247  if(children[1] && children[0] != children[1])
1248  {
1249  for(i=0; i<children[1]->n_outer_joints; i++)
1250  {
1251  if(children[1]->outer_joints[i]->joint != last_joint)
1252  {
1253  outer_joints[count] = children[1]->outer_joints[i];
1254  outer_joints_origin[count] = 1;
1255  outer_joints_index[count] = i;
1256  count++;
1257  }
1258  }
1259  }
1260  }
1261  // links
1262  n_links = children[0]->n_links;
1263  if(children[1] && children[0] != children[1])
1264  n_links += children[1]->n_links;
1265  if(links) delete[] links;
1266  links = 0;
1267  links = new pLink* [n_links];
1268  count = 0;
1269  for(i=0; i<children[0]->n_links; i++)
1270  {
1271  links[count] = children[0]->links[i];
1272  count++;
1273  }
1274  if(children[1] && children[0] != children[1])
1275  {
1276  for(i=0; i<children[1]->n_links; i++)
1277  {
1278  links[count] = children[1]->links[i];
1279  count++;
1280  }
1281  }
1282  }
1283  // create matrices and vectors
1284  P.resize(6, 6);
1285  P.zero();
1286  da6.resize(6);
1287  da6.zero();
1288  W.resize(6, 6);
1289  W.zero();
1290  IW.resize(6, 6);
1291  IW.zero();
1292 #ifdef USE_DCA
1293  Vhat.resize(6,6);
1294  Vhat.zero();
1295  SVS.resize(n_dof, n_dof);
1296  SVS.zero();
1297 #else
1298  Gamma.resize(n_const, n_const);
1299  Gamma.zero();
1300  Gamma_inv.resize(n_const, n_const);
1301  Gamma_inv.zero();
1302 #endif
1303  f_temp.resize(n_const);
1304  f_temp.zero();
1305  colf_temp.resize(n_const);
1306  colf_temp.zero();
1307  tau.resize(n_dof);
1308  tau.zero();
1309  acc_final.resize(n_dof);
1310  acc_final.zero();
1311  if(n_outer_joints > 0)
1312  {
1313  int i, j;
1314  Lambda = new fMat* [n_outer_joints];
1315  acc_temp = new fVec [n_outer_joints];
1316  vel_temp = new fVec [n_outer_joints];
1317  for(i=0; i<n_outer_joints; i++)
1318  {
1319  Lambda[i] = new fMat [n_outer_joints];
1320  for(j=0; j<n_outer_joints; j++)
1321  {
1322  Lambda[i][j].resize(6, 6);
1323  Lambda[i][j].zero();
1324  }
1325  acc_temp[i].resize(6);
1326  acc_temp[i].zero();
1327  vel_temp[i].resize(6);
1328  vel_temp[i].zero();
1329  }
1330  }
1331 }
1332 
1336 int pSim::Schedule(Joint** joints)
1337 {
1338  if(subchains) delete subchains;
1339  subchains = 0;
1341  build_subchain_tree(n_joint, joints, buf);
1342  if(buf.size() == 1)
1343  {
1344  subchains = *(buf.begin());
1345  subchains->init();
1346  return 0;
1347  }
1348  cerr << "pSim::Schedule(joints): error- invalid joint order" << endl;
1349  return -1;
1350 }
1351 
1352 int pSim::build_subchain_tree(int _n_joints, Joint** joints, subchain_list& buf)
1353 {
1354  int i;
1355  for(i=0; i<_n_joints; i++)
1356  {
1357  cerr << "build_subchain_tree " << joints[i]->name << endl;
1358  build_subchain_tree(joints[i], buf);
1359  }
1360  return 0;
1361 }
1362 
1364 {
1365  JointInfo& jinfo = joint_info[cur_joint->i_joint];
1366  pJoint* pj0 = jinfo.pjoints[0];
1367  pJoint* pj1 = jinfo.pjoints[1];
1368  int pj0_done = false;
1369  int pj1_done = false;
1370  pSubChain* to_remove[2] = {0, 0};
1371  pSubChain* myp = new pSubChain(this, 0, pj0, pj1);
1372  subchain_list::iterator i;
1373  for(i=buf.begin(); i!=buf.end(); i++)
1374  {
1375  if(!pj0_done && in_subchain(*i, pj0->plink))
1376  {
1377  myp->children[0] = *i;
1378  (*i)->parent = myp;
1379  pj0_done = true;
1380  if(!to_remove[0]) to_remove[0] = *i;
1381  else if(!to_remove[1]) to_remove[1] = *i;
1382  }
1383  if(!pj1_done && in_subchain(*i, pj1->plink))
1384  {
1385  myp->children[1] = *i;
1386  (*i)->parent = myp;
1387  pj1_done = true;
1388  if(!to_remove[0]) to_remove[0] = *i;
1389  else if(!to_remove[1]) to_remove[1] = *i;
1390  }
1391  }
1392  if(!pj0_done && pj0->plink)
1393  {
1394  pSubChain* newp = new pSubChain(this, myp, pj0->plink);
1395  myp->children[0] = newp;
1396  }
1397  if(!pj1_done && pj1->plink)
1398  {
1399  pSubChain* newp = new pSubChain(this, myp, pj1->plink);
1400  myp->children[1] = newp;
1401  }
1402  if(to_remove[0]) buf.remove(to_remove[0]);
1403  if(to_remove[1]) buf.remove(to_remove[1]);
1404  buf.push_front(myp);
1405 }
1406 
1408 {
1409  if(!sc) return 0;
1410  if(sc->links && sc->links[0] == pl) return 1;
1411  if(in_subchain(sc->children[0], pl)) return 1;
1412  if(sc->children[0] != sc->children[1] &&
1413  in_subchain(sc->children[1], pl)) return 1;
1414  return 0;
1415 }
int ScheduleDepth()
Definition: schedule.cpp:129
pLink * plink
Definition: psim.h:130
double mass
mass
Definition: chain.h:704
int c
Definition: autoplay.py:16
double TotalAstarCost()
Definition: dp.h:82
dpScheduleNode * find_available_parent(int target_depth, int &child_id)
Definition: schedule.cpp:680
Joint * child
pointer to the child joint
Definition: chain.h:685
std::list< class pSubChain * > subchain_list
Definition: psim.h:27
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
int list_all_internal_joints_sub(Joint *cur, joint_list &all_internal)
Definition: schedule.cpp:427
int build_subchain_tree(int _n_joints, Joint **joints, subchain_list &buf)
Definition: schedule.cpp:1352
int n_dof
degrees of freedom (0/1/3/6)
Definition: chain.h:715
double TotalCost()
Definition: dp.h:76
std::vector< Joint * > joint_list
Definition: psim.h:29
pSubChain * children[2]
Definition: psim.h:342
int list_all_internal_joints_rev(Joint *cur, joint_list &all_internal)
Definition: schedule.cpp:409
double calc_cost()
Compute the local cost at the node.
Definition: schedule.cpp:275
pJoint * pjoints[2]
Definition: psim.h:438
#define DOF_COEF
Definition: schedule.cpp:20
static int max_procs
Definition: schedule.cpp:24
int same_state(dpNode *refnode)
Check if the node&#39;s state is the same as refnode. (to remove duplicates)
Definition: schedule.cpp:334
png_uint_32 size
Definition: png.h:1521
double calc_astar_cost(dpNode *potential_parent)
Compute the A-star cost at the node (optional).
Definition: schedule.cpp:279
Forward dynamics computation based on Assembly-Disassembly Algorithm.
dpScheduleNode(dpScheduleNode *potential_parent, Joint *_last_joint, const fVec &_proc_costs, dpScheduleNode *_schedule_parent, int child_id, int _first_proc, int _last_proc, const joint_list &org_internal_joints, const p_joint_list &org_outer_joints)
Definition: schedule.cpp:39
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
int Search(int _max_nodes=-1, int _max_goals=-1)
Dijkstra or A* search: find the node with the smallest cost.
Definition: dp.cpp:17
double calc_min_subchain_cost(const p_joint_list &_outer_joints, joint_list &added_joints)
Definition: schedule.cpp:445
int ParentSide()
Definition: psim.h:119
Base class for generic discrete search.
double add_to_child_side(Joint *cur, joint_list &added_joints, int *n_outer_parent_side, int *n_outer_child_side)
Definition: schedule.cpp:637
int n_joint
Definition: chain.h:469
png_uint_32 i
Definition: png.h:2735
int ListAllInternalJoints(joint_list &all_internal)
Definition: schedule.cpp:368
static void get_all_joints(Joint *cur, joint_list &all_joints, joint_list &all_vjoints)
Definition: schedule.cpp:749
#define N_2_COEF
Definition: schedule.cpp:18
Joint * GetJoint()
Definition: psim.h:116
dpNode * parent
Definition: dp.h:160
void SetStartNode(dpNode *_n)
Set the initial node for search.
Definition: dp.h:303
Joint * joint
Definition: psim.h:128
#define TINY_MASS
Definition: schedule.cpp:17
#define N_COEF
Definition: schedule.cpp:19
void set(double *_d)
Sets all elements.
Definition: fMatrix.h:533
friend class dpMain
Definition: dp.h:32
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)
Definition: schedule.cpp:527
fVec & ProcCosts()
Definition: schedule.cpp:122
def j(str, encoding="cp932")
list index
char * name
joint name (including the character name)
Definition: chain.h:691
joint_list internal_joints[2]
Definition: schedule.cpp:349
Joint * Root()
Definition: chain.h:151
double single_joint_cost
Definition: schedule.cpp:351
void resize(int i)
Change the size.
Definition: fMatrix.h:511
pSubChain * default_schedule(pSubChain *p, Joint *j)
Definition: schedule.cpp:1072
ofstream logfile("schedule.log")
Vector of generic size.
Definition: fMatrix.h:491
Joint * brother
pointer to the brother joint
Definition: chain.h:684
png_bytep buf
Definition: png.h:2729
Definition: dp.h:286
std::vector< class pJoint * > p_joint_list
Definition: psim.h:30
#define CONST_COEF
Definition: schedule.cpp:21
Joint * LastJoint()
Definition: schedule.cpp:126
pSubChain * parent
Definition: psim.h:341
Class for representing "handle"; two pJoint instances are attached to both sides of each joint...
Definition: psim.h:81
int is_in(const joint_list &jlist, Joint *jnt)
Definition: schedule.cpp:518
dpNode * Parent()
Definition: dp.h:52
fMat tran(const fMat &mat)
Definition: fMatrix.cpp:1013
int is_goal()
Check if the state is goal.
Definition: schedule.cpp:338
void zero()
Creates a zero vector.
Definition: fMatrix.h:575
static pSim * sim
Definition: schedule.cpp:23
int create_child_nodes(dpNode **&nodes)
Create (potential) child nodes.
Definition: schedule.cpp:150
void init()
Definition: schedule.cpp:1140
p_joint_list outer_joints
Definition: schedule.cpp:348
int NumJoint()
Total number of joints.
Definition: chain.h:347
int in_subchain(pSubChain *sc, pLink *pl)
Definition: schedule.cpp:1407
int split_internal_joints(Joint *_last_joint, const joint_list &org_internal_joints, joint_list &_internal_joints_0, joint_list &_internal_joints_1)
Definition: schedule.cpp:847
pLink ** links
Definition: psim.h:352
int Schedule()
Creates default schedule, which is optimized for serial computation.
Definition: schedule.cpp:1060
JSAMPIMAGE data
Definition: jpeglib.h:945
Main class for forward dynamics computation.
Definition: psim.h:446
The class for representing a joint.
Definition: chain.h:538
Joint * real
pointer to the real (connected) joint; for closed chains.
Definition: chain.h:687
int i_joint
index of the joint
Definition: chain.h:722
double max_value()
Returns the maximum value.
Definition: fMatrix.cpp:1478
Joint * last_joint
Definition: schedule.cpp:343
dpScheduleNode * schedule_parent
Definition: schedule.cpp:347
int AutoSchedule(int max_procs)
Automatic scheduling for max_procs processors.
Definition: schedule.cpp:765
int set_outer_joints(pJoint *last_pjoint, const p_joint_list &org_outer_joints, p_joint_list &new_outer_joints)
Definition: schedule.cpp:875
dpNode * BestGoal(dpNode *ref=0)
Extract the goal with the smallest cost (if any).
Definition: dp.h:336
JointInfo * joint_info
Definition: psim.h:602
Definition: dp.h:29
void default_schedule_virtual(Joint *j)
Definition: schedule.cpp:1117
Joint * parent
pointer to the parent joint
Definition: chain.h:683
double add_to_parent_side(Joint *cur, joint_list &added_joints, int *n_outer_parent_side, int *n_outer_child_side)
Definition: schedule.cpp:655
void GetPJoint(Joint *_joint, pJoint *_pjoints[2])
Definition: psim.h:544
double calc_single_joint_cost(int n_dof, int n_outer)
Definition: schedule.cpp:356
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
pJoint * last_pjoints[2]
Definition: schedule.cpp:344
Node for schedule tree; represents a partial chain.
Definition: psim.h:190
dpScheduleNode * find_available_parent_sub(dpScheduleNode *cur, int target_depth, dpNode *cur_leaf, int &child_id)
Definition: schedule.cpp:691
int depth
Definition: dp.h:168


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat Apr 13 2019 02:14:25