integ.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  * integ.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.07.11
12  */
13 
14 #include "chain.h"
15 
24 int Chain::IntegrateAdaptive(double& timestep, int step, double min_timestep, double max_integ_error)
25 {
27  int i;
28  double half_step = timestep * 0.5;
29  if(step == 0)
30  {
31  // save derivatives at t
32  // save initial value/vel and compute value/vel at t+dt
33  // set value/vel in the middle
34  for(i=0; i<n_value; i++)
35  {
36  j_value_dot[0][i] = *all_value_dot[i];
37  init_value[i] = *all_value[i];
38  j_value_dot[1][i] = init_value[i] + j_value_dot[0][i] * timestep;
39  *all_value[i] += j_value_dot[0][i] * half_step;
40  }
41  for(i=0; i<n_dof; i++)
42  {
43  j_acc_p[0][i] = *all_vel_dot[i];
44  init_vel[i] = *all_vel[i];
45  j_acc_p[1][i] = init_vel[i] + j_acc_p[0][i] * timestep;
46  *all_vel_dot[i] += j_acc_p[0][i] * half_step;
47  }
48  }
49  else if(step == 1)
50  {
51  // compute and save value/vel after timestep
52  double d, value_error = 0.0, vel_error = 0.0, total_error;
53  for(i=0; i<n_value; i++)
54  {
55  d = *all_value[i] + *all_value_dot[i] * half_step - j_value_dot[1][i];
56  value_error += d*d;
57  }
58  for(i=0; i<n_dof; i++)
59  {
60  d = *all_vel[i] + *all_vel_dot[i] * half_step - j_acc_p[1][i];
61  vel_error += d*d;
62  }
63  double new_timestep;
64  // compare x1 and x2
65  total_error = sqrt(value_error+vel_error) / (n_value+n_dof);
66  // integrate with new timestep
67  new_timestep = timestep * sqrt(max_integ_error / total_error);
68  if(new_timestep < min_timestep)
69  new_timestep = min_timestep;
70  if(new_timestep > timestep)
71  new_timestep = timestep;
72  timestep = new_timestep;
73  for(i=0; i<n_value; i++)
74  {
75  *all_value[i] = init_value[i] + j_value_dot[0][i] * timestep;
76  }
77  for(i=0; i<n_dof; i++)
78  {
79  *all_vel[i] = init_vel[i] + j_acc_p[0][i] * timestep;
80  }
81  }
83  return 0;
84 }
85 
86 int Chain::IntegrateValue(double timestep)
87 {
89  int i;
90  for(i=0; i<n_value; i++)
91  {
92  *all_value[i] += timestep * *all_value_dot[i];
93  }
95  return 0;
96 }
97 
98 int Chain::IntegrateVelocity(double timestep)
99 {
100  root->pre_integrate();
101  int i;
102  for(i=0; i<n_dof; i++)
103  {
104  *all_vel[i] += timestep * *all_vel_dot[i];
105  }
106  root->post_integrate();
107  return 0;
108 }
109 
110 int Chain::Integrate(double timestep)
111 {
112  root->pre_integrate();
113  int i;
114  for(i=0; i<n_value; i++)
115  {
116  *all_value[i] += timestep * *all_value_dot[i];
117  }
118  for(i=0; i<n_dof; i++)
119  {
120  *all_vel[i] += timestep * *all_vel_dot[i];
121  }
122  root->post_integrate();
123  return 0;
124 }
125 
126 int Chain::IntegrateRK4Value(double timestep, int step)
127 {
128  root->pre_integrate();
129  int i;
130  // save derivatives
131  for(i=0; i<n_value; i++)
132  {
133  j_value_dot[step][i] = *all_value_dot[i];
134  }
135  // prepare for the next step
136  if(step == 0)
137  {
138  for(i=0; i<n_value; i++)
139  {
140  init_value[i] = *all_value[i]; // save init value
141  *all_value[i] += j_value_dot[0][i] * timestep * 0.5;
142  }
143  }
144  else if(step == 1)
145  {
146  for(i=0; i<n_value; i++)
147  {
148  *all_value[i] = init_value[i] + j_value_dot[1][i] * timestep * 0.5;
149  }
150  }
151  else if(step == 2)
152  {
153  for(i=0; i<n_value; i++)
154  {
155  *all_value[i] = init_value[i] + j_value_dot[2][i] * timestep;
156  }
157  }
158  else if(step == 3)
159  {
160  for(i=0; i<n_value; i++)
161  {
162  *all_value[i] = init_value[i] +
163  (j_value_dot[0][i] + 2.0*(j_value_dot[1][i] + j_value_dot[2][i]) + j_value_dot[3][i]) * timestep / 6.0;
164  }
165  }
166  root->post_integrate();
167  return 0;
168 }
169 
170 int Chain::IntegrateRK4Velocity(double timestep, int step)
171 {
172  root->pre_integrate();
173  int i;
174  // save derivatives
175  for(i=0; i<n_dof; i++)
176  {
177  j_acc_p[step][i] = *all_vel_dot[i];
178  }
179  // prepare for the next step
180  if(step == 0)
181  {
182  for(i=0; i<n_dof; i++)
183  {
184  init_vel[i] = *all_vel[i]; // save init vel
185  *all_vel[i] += j_acc_p[0][i] * timestep * 0.5;
186  }
187  }
188  else if(step == 1)
189  {
190  for(i=0; i<n_dof; i++)
191  {
192  *all_vel[i] = init_vel[i] + j_acc_p[1][i] * timestep * 0.5;
193  }
194  }
195  else if(step == 2)
196  {
197  for(i=0; i<n_dof; i++)
198  {
199  *all_vel[i] = init_vel[i] + j_acc_p[2][i] * timestep;
200  }
201  }
202  else if(step == 3)
203  {
204  for(i=0; i<n_dof; i++)
205  {
206  *all_vel[i] = init_vel[i] +
207  (j_acc_p[0][i] + 2.0*(j_acc_p[1][i] + j_acc_p[2][i]) + j_acc_p[3][i]) * timestep / 6.0;
208  }
209  }
210  root->post_integrate();
211  return 0;
212 }
213 
214 int Chain::IntegrateRK4(double timestep, int step)
215 {
216  root->pre_integrate();
217  int i;
218  double half_step = timestep * 0.5;
219  // save derivatives
220  for(i=0; i<n_value; i++)
221  {
222  j_value_dot[step][i] = *all_value_dot[i];
223  }
224  for(i=0; i<n_dof; i++)
225  {
226  j_acc_p[step][i] = *all_vel_dot[i];
227  }
228  // prepare for the next step
229  if(step == 0)
230  {
231  for(i=0; i<n_value; i++)
232  {
233  init_value[i] = *all_value[i]; // save init value
234  *all_value[i] += j_value_dot[0][i] * half_step;
235  }
236  for(i=0; i<n_dof; i++)
237  {
238  init_vel[i] = *all_vel[i]; // save init vel
239  *all_vel[i] += j_acc_p[0][i] * half_step;
240  }
241  }
242  else if(step == 1)
243  {
244  for(i=0; i<n_value; i++)
245  {
246  *all_value[i] = init_value[i] + j_value_dot[1][i] * half_step;
247  }
248  for(i=0; i<n_dof; i++)
249  {
250  *all_vel[i] = init_vel[i] + j_acc_p[1][i] * half_step;
251  }
252  }
253  else if(step == 2)
254  {
255  for(i=0; i<n_value; i++)
256  {
257  *all_value[i] = init_value[i] + j_value_dot[2][i] * timestep;
258  }
259  for(i=0; i<n_dof; i++)
260  {
261  *all_vel[i] = init_vel[i] + j_acc_p[2][i] * timestep;
262  }
263  }
264  else if(step == 3)
265  {
266  for(i=0; i<n_value; i++)
267  {
268  *all_value[i] = init_value[i] +
269  (j_value_dot[0][i] + 2.0*(j_value_dot[1][i] + j_value_dot[2][i]) + j_value_dot[3][i]) * timestep / 6.0;
270  }
271  for(i=0; i<n_dof; i++)
272  {
273  *all_vel[i] = init_vel[i] +
274  (j_acc_p[0][i] + 2.0*(j_acc_p[1][i] + j_acc_p[2][i]) + j_acc_p[3][i]) * timestep / 6.0;
275  }
276  }
277  root->post_integrate();
278  return 0;
279 }
280 
282 {
283  // compute p_lin_vel, p_ep_dot, p_ang_vel, p_lin_acc, p_ang_acc
284  switch(j_type)
285  {
286  case JROTATE:
287  case JSLIDE:
288  break;
289  case JSPHERE:
290  p_ang_vel.mul(rel_att, rel_ang_vel);
291  p_ep_dot.angvel2epdot(rel_ep, rel_ang_vel);
292  p_ang_acc.mul(rel_att, rel_ang_acc);
293  break;
294  case JFREE:
295  p_lin_vel.mul(rel_att, rel_lin_vel);
296  p_ang_vel.mul(rel_att, rel_ang_vel);
297  p_ep_dot.angvel2epdot(rel_ep, rel_ang_vel);
298  p_lin_acc.mul(rel_att, rel_lin_acc);
299  p_ang_acc.mul(rel_att, rel_ang_acc);
300  break;
301  default:
302  break;
303  }
304  child->pre_integrate();
305  brother->pre_integrate();
306  return 0;
307 }
308 
310 {
311  // compute rel_att, rel_lin_vel, rel_ang_vel,
312  fMat33 ratt;
313  switch(j_type)
314  {
315  case JROTATE:
316  case JSLIDE:
317  SetJointValue(q);
318  SetJointVel(qd);
319  break;
320  case JSPHERE:
321  rel_ep.unit();
322  rel_att.set(rel_ep);
323  ratt.tran(rel_att);
324  rel_ang_vel.mul(ratt, p_ang_vel);
325  break;
326  case JFREE:
327  rel_ep.unit();
328  rel_att.set(rel_ep);
329  ratt.tran(rel_att);
330  rel_lin_vel.mul(ratt, p_lin_vel);
331  rel_ang_vel.mul(ratt, p_ang_vel);
332  break;
333  default:
334  break;
335  }
336  child->post_integrate();
337  brother->post_integrate();
338  return 0;
339 }
3x3 matrix class.
Definition: fMatrix3.h:29
int IntegrateValue(double timestep)
Integrate value/velocity only.
Definition: integ.cpp:86
int SetJointValue(const fVec &values)
Set all joint values.
Definition: joint.cpp:415
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
double * init_value
Definition: chain.h:462
Joint * root
Chain information.
Definition: chain.h:466
double * j_value_dot[4]
for 4-th order Runge-Kutta
Definition: chain.h:460
png_uint_32 i
Definition: png.h:2735
int SetJointVel(const fVec &vels)
Set all joint velocities/accelerations.
Definition: joint.cpp:456
int post_integrate()
Definition: integ.cpp:309
spherical (3DOF)
Definition: chain.h:43
int IntegrateRK4Value(double timestep, int step)
Definition: integ.cpp:126
int IntegrateAdaptive(double &timestep, int step, double min_timestep=DEFAULT_MIN_TIMESTEP, double max_integ_error=DEFAULT_MAX_INTEG_ERROR)
Performs Euler integration with adaptive timestep.
Definition: integ.cpp:24
int IntegrateRK4Velocity(double timestep, int step)
Definition: integ.cpp:170
double ** all_value
Pointers to the integration variables.
Definition: chain.h:455
int n_dof
Definition: chain.h:468
double ** all_vel_dot
Definition: chain.h:458
int IntegrateVelocity(double timestep)
Definition: integ.cpp:98
int n_value
Definition: chain.h:467
int IntegrateRK4(double timestep, int step)
Performs 4-th order Runge-Kutta integration.
Definition: integ.cpp:214
double * init_vel
Definition: chain.h:463
Classes for defining open/closed kinematic chains.
double ** all_value_dot
Definition: chain.h:456
prismatic (1DOF)
Definition: chain.h:42
double ** all_vel
Definition: chain.h:457
rotational (1DOF)
Definition: chain.h:41
double * j_acc_p[4]
Definition: chain.h:461
int Integrate(double timestep)
Performs Euler integration with timestep timestep [s].
Definition: integ.cpp:110
free (6DOF)
Definition: chain.h:44
int pre_integrate()
Definition: integ.cpp:281


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