init.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  */
9 /*
10  * init.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.06.18
12  */
13 
14 #include "chain.h"
15 
16 /*
17  * initialize
18  */
19 #ifdef SEGA
20 int Chain::init()
21 #else
22 int Chain::init(SceneGraph* sg)
23 #endif
24 {
25  // reset pointers
26  if(all_value) delete[] all_value;
27  if(all_value_dot) delete[] all_value_dot;
28  if(all_vel) delete[] all_vel;
29  if(all_vel_dot) delete[] all_vel_dot;
30  if(j_acc_p[0]) delete[] j_acc_p[0];
31  if(j_acc_p[1]) delete[] j_acc_p[1];
32  if(j_acc_p[2]) delete[] j_acc_p[2];
33  if(j_acc_p[3]) delete[] j_acc_p[3];
34  if(j_value_dot[0]) delete[] j_value_dot[0];
35  if(j_value_dot[1]) delete[] j_value_dot[1];
36  if(j_value_dot[2]) delete[] j_value_dot[2];
37  if(j_value_dot[3]) delete[] j_value_dot[3];
38  if(init_value) delete[] init_value;
39  if(init_vel) delete[] init_vel;
40  n_value = 0;
41  n_dof = 0;
42  n_thrust = 0;
43  n_joint = 0;
44  all_value = 0;
45  all_value_dot = 0;
46  all_vel = 0;
47  all_vel_dot = 0;
48  j_acc_p[0] = j_acc_p[1] = j_acc_p[2] = j_acc_p[3] = 0;
49  j_value_dot[0] = j_value_dot[1] = j_value_dot[2] = j_value_dot[3] = 0;
50  init_value = 0;
51  init_vel = 0;
52  if(!root) return 0;
53  // initialize
54 #ifndef SEGA
55  if(sg) set_relative_positions(sg);
56 #endif
57  root->init();
58  if(n_value > 0)
59  {
60  all_value = new double* [n_value];
61  all_value_dot = new double* [n_value];
62  j_value_dot[0] = new double [n_value];
63  j_value_dot[1] = new double [n_value];
64  j_value_dot[2] = new double [n_value];
65  j_value_dot[3] = new double [n_value];
66  init_value = new double [n_value];
67  }
68  if(n_dof > 0)
69  {
70  all_vel = new double* [n_dof];
71  all_vel_dot = new double* [n_dof];
72  j_acc_p[0] = new double [n_dof];
73  j_acc_p[1] = new double [n_dof];
74  j_acc_p[2] = new double [n_dof];
75  j_acc_p[3] = new double [n_dof];
76  init_vel = new double [n_dof];
77  }
78  root->init_arrays();
79  CalcPosition();
80  root->init_virtual();
81 #ifndef SEGA
82  if(sg)
83  {
84  init_scale(sg);
85  }
86  apply_scale();
87 #endif
88  return 0;
89 }
90 
91 #ifndef SEGA
92 void Chain::init_scale(SceneGraph* sg)
93 {
94  init_scale_sub(sg->getNodes());
95 }
96 
97 void Chain::init_scale_sub(Node* node)
98 {
99  if(!node) return;
100  char* name = 0;
101  if(node->isTransformNode() && (name = node->getName()) && strstr(name, ScaleString))
102  {
103  char* jointname = name + strlen(ScaleString);
104  float scale[3];
105  ((TransformNode*)node)->getScale(scale);
106  scale_object _s;
107  _s.set_joint_name(jointname, 0); // jointname includes character name
108  _s.scale = scale[0];
109  add_scale_object(_s);
110  }
111  init_scale_sub(node->next());
112  init_scale_sub(node->getChildNodes());
113 }
114 
115 void Chain::ApplyGeomScale(SceneGraph* sg)
116 {
117  apply_geom_scale(sg, root);
118 }
119 
120 void Chain::apply_geom_scale(SceneGraph* sg, Joint* cur)
121 {
122  if(!cur) return;
123  TransformNode* mytrans = sg->findTransformNode(cur->name);
124  // mytrans uses new scale to enlarge the geometry
125  if(mytrans)
126  {
127  float new_scale = cur->cur_scale;
128 // mytrans->setScale(new_scale, new_scale, new_scale);
129  }
130  apply_geom_scale(sg, cur->brother);
131  apply_geom_scale(sg, cur->child);
132 }
133 #endif
134 
136 {
137 // if(!realname)
138  {
139  if(parent) n_root_dof = parent->n_root_dof;
140  if(t_given)
141  {
142  i_value = chain->n_value;
143  i_dof = chain->n_dof;
144  i_thrust = -1;
145  n_root_dof += n_dof;
146  }
147  else
148  {
149  i_value = -1;
150  i_dof = -1;
151  i_thrust = chain->n_thrust;
152  n_root_dof += n_thrust;
153  }
154  i_joint = chain->n_joint;
155  chain->n_joint++;
156  switch(j_type)
157  {
158  case JROTATE:
159  case JSLIDE:
160  if(t_given)
161  {
162  chain->n_value++;
163  chain->n_dof++;
164  }
165  else
166  {
167  chain->n_thrust++;
168  }
169  break;
170  case JSPHERE:
171  if(t_given)
172  {
173  chain->n_value += 4; // Euler Parameters
174  chain->n_dof += 3;
175  }
176  else
177  {
178  chain->n_thrust += 3;
179  }
180  break;
181  case JFREE:
182  if(t_given)
183  {
184  chain->n_value += 7;
185  chain->n_dof += 6;
186  }
187  else
188  {
189  chain->n_thrust += 6;
190  }
191  break;
192  case JFIXED:
193  break;
194  default:
195  cerr << "warning: joint type not set for " << name << endl;
196  break;
197  }
198 
199  }
200  child->init();
201  brother->init();
202 }
203 
205 {
206 // if(!realname)
207  {
208 
209  int i;
210  switch(j_type)
211  {
212  case JROTATE:
213  case JSLIDE:
214  if(t_given)
215  {
216  chain->all_value[i_value] = &q;
217  chain->all_value_dot[i_value] = &qd;
218  chain->all_vel[i_dof] = &qd;
219  chain->all_vel_dot[i_dof] = &qdd;
220  }
221  break;
222  case JSPHERE:
223  if(t_given)
224  {
225  for(i=0; i<4; i++)
226  {
227  chain->all_value[i_value+i] = &rel_ep(i);
228  chain->all_value_dot[i_value+i] = &p_ep_dot(i);
229  }
230  for(i=0; i<3; i++)
231  {
232  chain->all_vel[i_dof+i] = &p_ang_vel(i);
233  chain->all_vel_dot[i_dof+i] = &p_ang_acc(i);
234  }
235  }
236  break;
237  case JFREE:
238  if(t_given)
239  {
240  for(i=0; i<3; i++)
241  {
242  chain->all_value[i_value+i] = &rel_pos(i);
243  chain->all_value_dot[i_value+i] = &p_lin_vel(i);
244  }
245  for(i=0; i<4; i++)
246  {
247  chain->all_value[i_value+3+i] = &rel_ep(i);
248  chain->all_value_dot[i_value+3+i] = &p_ep_dot(i);
249  }
250  for(i=0; i<3; i++)
251  {
252  chain->all_vel[i_dof+i] = &p_lin_vel(i);
253  chain->all_vel_dot[i_dof+i] = &p_lin_acc(i);
254  }
255  for(i=0; i<3; i++)
256  {
257  chain->all_vel[i_dof+3+i] = &p_ang_vel(i);
258  chain->all_vel_dot[i_dof+3+i] = &p_ang_acc(i);
259  }
260  }
261  break;
262  default:
263  break;
264  }
265 
266  }
267  child->init_arrays();
268  brother->init_arrays();
269 }
270 
272 {
273  _init_virtual();
274  brother->init_virtual();
275  child->init_virtual();
276 }
277 
279 {
280  if(!realname) return;
281  real = chain->FindJoint(realname);
282  if(!real)
283  {
284  cerr << "warning: could not find real joint " << realname << " of " << name << endl;
285  return;
286  }
287  if(real->real)
288  {
289  cerr << "error: real joint " << realname << " of " << name << " is also virtual" << endl;
290  real = 0;
291  return;
292  }
293  // relative position & orientation in real's frame
294  fVec3 pp;
295  fMat33 Rt, IR;
296  Rt.tran(real->abs_att);
297  pp.sub(abs_pos, real->abs_pos);
298  rpos_real.mul(Rt, pp);
299  ratt_real.mul(Rt, abs_att);
300  // set mass, COM, inertia in virtual joint's frame
301  mass = real->mass;
302  pp.sub(real->loc_com, rpos_real); // vector from origin to com, in real joint's frame
303  loc_com.mul(pp, ratt_real); // transform to local frame
304  IR.mul(real->inertia, ratt_real);
305  inertia.mul(tran(ratt_real), IR);
306 #if 0 // debug
307  cerr << "-- " << name << endl;
308  cerr << "rpos_real = " << rpos_real << endl;
309  cerr << "ratt_real = " << ratt_real << endl;
310  cerr << "mass = " << mass << endl;
311  cerr << "loc_com = " << loc_com << endl;
312  cerr << "inertia = " << inertia << endl;
313 #endif
314 }
#define IR(x)
Integer representation of a floating-point value.
Definition: IceFPU.h:18
3x3 matrix class.
Definition: fMatrix3.h:29
virtual int init(SceneGraph *sg)
Initialize the parameters.
Definition: init.cpp:22
Joint * child
pointer to the child joint
Definition: chain.h:685
void init_scale_sub(Node *node)
Definition: init.cpp:97
void _init_virtual()
Definition: init.cpp:278
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
Definition: fMatrix3.cpp:607
png_infop png_charpp name
Definition: png.h:2382
Joint * root
Chain information.
Definition: chain.h:466
double cur_scale
Definition: chain.h:843
png_uint_32 i
Definition: png.h:2735
int n_thrust
total DOF of the joints with t_given = false
Definition: chain.h:470
void init()
Definition: init.cpp:135
void ApplyGeomScale(SceneGraph *sg)
Definition: init.cpp:115
spherical (3DOF)
Definition: chain.h:43
int n_dof
Definition: chain.h:468
char * name
joint name (including the character name)
Definition: chain.h:691
void apply_geom_scale(SceneGraph *sg, Joint *cur)
Definition: init.cpp:120
void init_scale(SceneGraph *sg)
Definition: init.cpp:92
XML utility functions.
Definition: chain.h:483
Joint * brother
pointer to the brother joint
Definition: chain.h:684
fixed (0DOF)
Definition: chain.h:40
Classes for defining open/closed kinematic chains.
prismatic (1DOF)
Definition: chain.h:42
fMat tran(const fMat &mat)
Definition: fMatrix.cpp:1013
void init_virtual()
Definition: init.cpp:271
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
void set_joint_name(const char *_joint_name, const char *_char_name)
Definition: chain.h:494
3-element vector class.
Definition: fMatrix3.h:206
rotational (1DOF)
Definition: chain.h:41
The class for representing a joint.
Definition: chain.h:538
void add_scale_object(const scale_object &_s)
free (6DOF)
Definition: chain.h:44
void init_arrays()
Definition: init.cpp:204


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