psim.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 <assert.h>
16 
18 {
19  int i;
20  for(i=0; i<n_joint; i++)
21  {
22  fVec& f_final = joint_info[i].pjoints[0]->f_final;
23  int idx = 6*joint_info[i].pjoints[0]->joint->i_joint;
24  cf(idx) = f_final(0);
25  cf(idx+1) = f_final(1);
26  cf(idx+2) = f_final(2);
27  cf(idx+3) = f_final(3);
28  cf(idx+4) = f_final(4);
29  cf(idx+5) = f_final(5);
30  }
31  return 0;
32 }
33 
35 {
36  return subchains->total_cost();
37 }
38 
40 {
41  if(!this) return 0;
42  // cost is proportional to square of n_outer_joints
43  int ret = 0;
44  if(n_links > 1) ret = n_outer_joints*n_outer_joints;
45  ret += children[0]->total_cost();
46  if(children[0] != children[1])
47  ret += children[1]->total_cost();
48  return ret;
49 }
50 
52 {
53  return subchains->schedule_depth();
54 }
55 
57 {
58  if(!this || n_links == 1) return 0;
59  int ret0 = 0, ret1 = 0;
60  if(children[0]) ret0 = children[0]->schedule_depth();
61  if(children[1]) ret1 = children[1]->schedule_depth();
62  return (ret0 > ret1) ? (ret0+1) : (ret1+1);
63 }
64 
66 {
67  return subchains->num_leaves();
68 }
69 
71 {
72  if(!this || n_links == 1) return 0;
73  if(children[0] && children[1] &&
74  children[0]->n_links == 1 && children[1]->n_links == 1)
75  {
76  return 1;
77  }
78  int ret = children[0]->num_leaves();
79  if(children[0] != children[1])
80  ret += children[1]->num_leaves();
81  return ret;
82 }
83 
88 {
89  if(joint_info)
90  {
91  for(int i=0; i<n_joint; i++)
92  {
93  delete joint_info[i].pjoints[0];
94  delete joint_info[i].pjoints[1];
95  if(joint_info[i].plink) delete joint_info[i].plink;
96  }
97  delete[] joint_info;
98  }
99  if(subchains) delete subchains;
100  joint_info = 0;
101  subchains = 0;
102  Chain::Clear();
103 }
104 
106 {
107  if(joint_info)
108  {
109  for(int i=0; i<n_joint; i++)
110  {
111  delete joint_info[i].pjoints[0];
112  delete joint_info[i].pjoints[1];
113  if(joint_info[i].plink) delete joint_info[i].plink;
114  }
115  delete[] joint_info;
116  }
117  if(subchains) delete subchains;
118  joint_info = 0;
119  subchains = 0;
120  return Chain::clear_data();
121 }
122 
124 {
125  int n_contacts = contact_vjoints.size();
126  if(n_contacts == 0) return 0;
127  int org_n_joint = n_joint;
128  JointInfo* jinfo_save = joint_info;
129  for(joint_list::iterator j=contact_vjoints.begin(); j!=contact_vjoints.end(); j++)
130  {
131  RemoveJoint(*j);
132  delete *j;
133  }
134  // clear and init Chain's data
136 #ifdef SEGA
137  Chain::init();
138 #else
139  Chain::init(0);
140 #endif
141  joint_info = new JointInfo [n_joint];
142  for(int i=0; i<org_n_joint; i++)
143  {
144  if(contact_vjoint_index(jinfo_save[i].pjoints[0]->joint) < 0)
145  {
146  Joint* j = jinfo_save[i].pjoints[0]->joint;
147  joint_info[j->i_joint].pjoints[0] = jinfo_save[i].pjoints[0];
148  joint_info[j->i_joint].pjoints[1] = jinfo_save[i].pjoints[1];
149  joint_info[j->i_joint].plink = jinfo_save[i].plink;
150  }
151  else
152  {
153  delete jinfo_save[i].pjoints[0];
154  delete jinfo_save[i].pjoints[1];
155  if(jinfo_save[i].plink) delete jinfo_save[i].plink;
156  }
157  }
158  if(jinfo_save) delete[] jinfo_save;
159  contact_vjoints.clear();
160  contact_relvels.clear();
161  fric_coefs.clear();
162  all_vjoints.clear();
163  all_Jv.clear();
164  all_Jr.clear();
165  all_jdot_v.clear();
166  all_jdot_r.clear();
167  return 0;
168 }
169 
174 {
175  int n_contacts = contact_vjoints.size();
176 // if(n_contacts == 0) return 0;
177  int org_n_joint = n_joint;
178  // clear and init Chain's data
180 #ifdef SEGA
181  Chain::init();
182 #else
183  Chain::init(0);
184 #endif
185  // create pjoint and plink for contact joints
186  JointInfo* jinfo_save = joint_info;
187  joint_info = new JointInfo [n_joint];
188  for(int i=0; i<org_n_joint; i++)
189  {
190  int new_index = jinfo_save[i].pjoints[0]->joint->i_joint;
191  joint_info[new_index].pjoints[0] = jinfo_save[i].pjoints[0];
192  joint_info[new_index].pjoints[1] = jinfo_save[i].pjoints[1];
193  joint_info[new_index].plink = jinfo_save[i].plink;
194  }
195  // contact joints
196  for(int i=0; i<n_contacts; i++)
197  {
198  Joint* j = contact_vjoints[i];
199  assert(j->real); // must be virtual joint
200  pJoint* pj0 = new pJoint(j, j->real);
201  pJoint* pj1 = new pJoint(j, j->parent);
202  pj0->plink = joint_info[j->real->i_joint].plink;
203  if(j->parent)
204  pj1->plink = joint_info[j->parent->i_joint].plink;
205  pj1->pair = pj0;
206  joint_info[j->i_joint].pjoints[0] = pj0;
207  joint_info[j->i_joint].pjoints[1] = pj1;
208  joint_info[j->i_joint].plink = 0;
209  }
210  if(jinfo_save) delete[] jinfo_save;
211  subchains->init();
212  return 0;
213 }
214 
215 #ifdef SEGA
216 int pSim::init()
217 #else
218 int pSim::init(SceneGraph* sg)
219 #endif
220 {
221 #ifdef SEGA
222  int ret = Chain::init();
223 #else
224  int ret = Chain::init(sg);
225 #endif
226  if(ret) return ret;
227  myinit();
228  return 0;
229 }
230 
232 {
233  if(joint_info) delete[] joint_info;
234  joint_info = 0;
235  if(n_joint == 0) return 0;
239  calc_consts();
240  return 0;
241 }
242 
244 {
245  if(!j) return;
246  if(j->real) return; // process virtual links later
247  if(j->i_joint >= 0)
248  {
249  pJoint* pj0 = new pJoint(j, j);
250  pJoint* pj1 = new pJoint(j, j->parent);
251  pLink* pl = new pLink(j);
252  pj0->plink = pl;
253  if(j->parent)
254  pj1->plink = joint_info[j->parent->i_joint].plink;
255  pj0->pair = pj1;
256  pj1->pair = pj0;
257  joint_info[j->i_joint].pjoints[0] = pj0;
258  joint_info[j->i_joint].pjoints[1] = pj1;
259  joint_info[j->i_joint].plink = pl;
260  }
261  setup_pjoint(j->brother);
262  setup_pjoint(j->child);
263 }
264 
266 {
267  if(!j) return;
268  if(j->real)
269  {
270  pJoint* pj0 = new pJoint(j, j->real);
271  pJoint* pj1 = new pJoint(j, j->parent);
272 // pJoint* pj0 = new pJoint(j, j->parent);
273 // pJoint* pj1 = new pJoint(j, j->real);
274  pj0->plink = joint_info[j->real->i_joint].plink;
275  if(j->parent)
276  pj1->plink = joint_info[j->parent->i_joint].plink;
277  pj1->pair = pj0;
278  joint_info[j->i_joint].pjoints[0] = pj0;
279  joint_info[j->i_joint].pjoints[1] = pj1;
280  joint_info[j->i_joint].plink = 0;
281  }
284 }
285 
287 {
288  int i;
289  for(i=0; i<n_joint; i++)
290  {
291  if(joint_info[i].plink) joint_info[i].plink->calc_inertia();
292  }
293 }
294 
296 {
297  if(joint->n_root_dof == 0) return;
298 // if(!joint->parent) return; // space
299  static fMat33 m11, m12, m22; // m21 equals to m12
300  double sx = joint->loc_com(0), sy = joint->loc_com(1), sz = joint->loc_com(2);
301  fMat33 scross(0, -sz, sy,
302  sz, 0, -sx,
303  -sy, sx, 0);
304  m11.identity();
305  m11 *= joint->mass;
306  m12.mul(joint->mass, scross);
307  m22.mul(scross, scross);
308  m22 *= -joint->mass;
309  m22 += joint->inertia;
310  if(joint->j_type == JROTATE)
311  {
312  static fVec3 n2J;
313  static fMat33 n2Jmat;
314  n2J.mul(joint->gear_ratio*joint->gear_ratio*joint->rotor_inertia, joint->axis);
315  n2Jmat.mul(n2J, n2J);
316  m22 += n2Jmat;
317  }
318  int i, j;
319  for(i=0; i<3; i++)
320  {
321  for(j=0; j<3; j++)
322  {
323  M(i, j) = m11(i, j);
324  M(i, j+3) = -m12(i, j);
325  M(i+3, j) = m12(i, j);
326  M(i+3, j+3) = m22(i, j);
327  }
328  }
329  Minv.inv_posv(M);
330 // Minv.inv_porfs(M);
331 }
332 
333 /*
334  * Dump
335  */
336 void pSim::DumpSchedule(ostream& ost)
337 {
338  subchains->dump(ost);
339 }
340 
341 void pSubChain::dump(ostream& ost)
342 {
343  if(!this) return;
344  int i;
345  ost << "-- pSubChain ->" << endl;
346  if(last_pjoints[0]) ost << "\tlast: " << last_pjoints[0]->joint->name << endl;
347  else ost << "\tsingle link" << endl;
348  if(parent) ost << "\tparent: " << parent->last_pjoints[0]->joint->name << endl;
349  ost << "\touter: " << n_outer_joints << endl;
350  for(i=0; i<n_outer_joints; i++)
351  outer_joints[i]->dump(ost);
352  ost << "\tlinks: " << n_links << endl;
353  for(i=0; i<n_links; i++)
354  links[i]->dump(ost);
355 #ifdef USE_MPI
356  ost << "\trank: " << rank << endl;
357 #endif
358  ost << "<-" << endl;
359  children[0]->dump(ost);
360  if(children[1] != children[0]) children[1]->dump(ost);
361 }
362 
363 void pJoint::dump(ostream& ost)
364 {
365  ost << "\t\t" << joint->name << "->" << link_side->name << endl;
366 }
367 
368 void pLink::dump(ostream& ost)
369 {
370  ost << "\t\t" << joint->name << endl;
371 }
virtual int clear_contact()
Definition: psim.cpp:123
int ScheduleDepth()
Definition: psim.cpp:51
pLink * plink
Definition: psim.h:130
3x3 matrix class.
Definition: fMatrix3.h:29
virtual int init(SceneGraph *sg)
Initialize the parameters.
Definition: init.cpp:22
ret1
joint_list contact_vjoints
Definition: psim.h:577
Joint * child
pointer to the child joint
Definition: chain.h:685
int contact_vjoint_index(Joint *_jnt)
Definition: psim.h:589
pJoint * pjoints[2]
Definition: psim.h:438
virtual int clear_data()
Clear arrays only; don&#39;t delete joints.
Definition: psim.cpp:105
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
virtual void Clear()
Definition: psim.cpp:87
void dump(ostream &ost)
Definition: psim.cpp:363
int RemoveJoint(Joint *j)
disconnect joint j from its parent
Definition: edit.cpp:32
Forward dynamics computation based on Assembly-Disassembly Algorithm.
int ConstraintForces(fVec &cf)
Extract the constraint forces.
Definition: psim.cpp:17
pJoint * pair
Definition: psim.h:131
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
int TotalCost()
Approximate indicators of total computational cost.
Definition: psim.cpp:34
Joint * root
Chain information.
Definition: chain.h:466
void identity()
Identity matrix.
Definition: fMatrix3.cpp:230
int n_joint
Definition: chain.h:469
png_uint_32 i
Definition: png.h:2735
void calc_consts()
Definition: psim.cpp:286
friend class pLink
Definition: psim.h:450
Joint * joint
Definition: psim.h:128
virtual int init(SceneGraph *sg)
Initialize the parameters.
Definition: psim.cpp:218
fVec f_final
Definition: psim.h:137
std::vector< fVec3 > all_jdot_r
Definition: psim.h:585
int init_contact()
Definition: psim.cpp:173
friend class pJoint
Definition: psim.h:449
pSubChain * subchains
Definition: psim.h:603
std::vector< fVec3 > contact_relvels
Definition: psim.h:578
pLink * plink
Definition: psim.h:439
int myinit()
Definition: psim.cpp:231
std::vector< fVec3 > all_jdot_v
Definition: psim.h:584
void DumpSchedule(ostream &ost)
Dump the schedule information to ost.
Definition: psim.cpp:336
void dump(const SDOPackage::NVList &nv)
Vector of generic size.
Definition: fMatrix.h:491
Joint * brother
pointer to the brother joint
Definition: chain.h:684
Class for representing "handle"; two pJoint instances are attached to both sides of each joint...
Definition: psim.h:81
int schedule_depth()
Definition: psim.cpp:56
int total_cost()
Definition: psim.cpp:39
ret0
void init()
Definition: schedule.cpp:1140
std::vector< double > fric_coefs
Definition: psim.h:579
virtual int clear_data()
Clear arrays only; don&#39;t delete joints.
Definition: vary.cpp:127
3-element vector class.
Definition: fMatrix3.h:206
joint_list all_vjoints
Definition: psim.h:581
rotational (1DOF)
Definition: chain.h:41
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
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
int NumLeaves()
Definition: psim.cpp:65
JointInfo * joint_info
Definition: psim.h:602
std::vector< fMat > all_Jr
Definition: psim.h:583
int num_leaves()
Definition: psim.cpp:70
std::vector< fMat > all_Jv
Definition: psim.h:582
Joint * parent
pointer to the parent joint
Definition: chain.h:683
virtual void Clear()
Remove all joints and clear all parameters.
Definition: chain.cpp:87
void dump(ostream &ost)
Definition: psim.cpp:341
void setup_pjoint_virtual(Joint *j)
Definition: psim.cpp:265
void setup_pjoint(Joint *j)
Definition: psim.cpp:243


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05