update_ik.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  * update.cpp
11  * Create: Katsu Yamane, 03.07.10
12  */
13 
14 #include "ik.h"
15 
16 #ifdef MEASURE_TIME
17 #include <time.h>
18 double calc_jacobian_time = 0;
19 double solve_ik_time = 0;
20 double high_constraint_time = 0;
21 double low_constraint_time = 0;
22 #endif
23 
24 double IK::Update(double timestep)
25 {
26  double ret;
27  CalcPosition();
28 #ifdef MEASURE_TIME
29  clock_t t1 = clock();
30 #endif
31  calc_jacobian();
32  calc_feedback();
33 #ifdef MEASURE_TIME
34  clock_t t2 = clock();
35  calc_jacobian_time += (double)(t2 - t1) / (double)CLOCKS_PER_SEC;
36 #endif
37  ret = solve_ik();
38 #ifdef MEASURE_TIME
39  solve_ik_time += (double)(clock() - t2) / (double)CLOCKS_PER_SEC;
40 #endif
41  Integrate(timestep);
42  CalcPosition();
43  return ret;
44 }
45 
46 double IK::Update(double max_timestep, double min_timestep, double max_integ_error)
47 {
48  double ret;
49  double new_timestep = max_timestep;
50  for(int i=0; i<2; i++)
51  {
52  CalcPosition();
53  calc_jacobian();
54  calc_feedback();
55  ret = solve_ik();
56  IntegrateAdaptive(new_timestep, i, min_timestep, max_integ_error);
57  }
58 // cerr << new_timestep << endl;
59  CalcPosition();
60  return ret;
61 }
62 
63 /*
64  * calc_jacobian
65  */
66 int IK::calc_jacobian()
67 {
68  int i;
69  for(i=0; i<n_constraints; i++)
70  {
71  constraints[i]->calc_jacobian();
72  }
73  return 0;
74 }
75 
76 int IKConstraint::calc_jacobian()
77 {
78  J.resize(n_const, ik->NumDOF());
79  J.zero();
80  calc_jacobian(ik->Root());
81  return 0;
82 }
83 
84 int IKConstraint::calc_jacobian(Joint* cur)
85 {
86  if(!cur) return 0;
87  switch(cur->j_type)
88  {
89  case JROTATE:
90  calc_jacobian_rotate(cur);
91  break;
92  case JSLIDE:
93  calc_jacobian_slide(cur);
94  break;
95  case JSPHERE:
96  calc_jacobian_sphere(cur);
97  break;
98  case JFREE:
99  calc_jacobian_free(cur);
100  break;
101  default:
102  break;
103  }
104  calc_jacobian(cur->child);
105  calc_jacobian(cur->brother);
106  return 0;
107 }
108 
109 /*
110  * calc_feedback
111  */
112 int IK::calc_feedback()
113 {
114  int i;
115  for(i=0; i<n_constraints; i++)
116  {
117  constraints[i]->calc_feedback();
118  }
119  return 0;
120 }
121 
122 /*
123  * solve_ik
124  */
125 int IK::copy_jacobian()
126 {
127  int i, m;
128  for(m=0; m<N_PRIORITY_TYPES; m++)
129  n_const[m] = 0;
130  n_all_const = 0;
131  // count number of constraints
132  for(i=0; i<n_constraints; i++)
133  {
134  constraints[i]->count_constraints();
135  }
136  if(n_all_const == 0) return 0;
137  // ¹ÔÎë!E¥Ù¥¯¥È¥ë¤ê¹à
138  for(m=0; m<N_PRIORITY_TYPES; m++)
139  {
140  if(n_const[m] > 0)
141  {
142  J[m].resize(n_const[m], n_dof);
143  J[m].zero();
144  fb[m].resize(n_const[m]);
145  fb[m].zero();
146  weight[m].resize(n_const[m]);
147  weight[m] = 1.0;
148  }
149  }
150  // ³Æ¹´Áå"Ëá§ãR¥Ó¥¢¥ó¡¦¥Õ¥£¡¼¥É¥Ð¥Ã¥¯ÁæÁ٤깸¤â"
151  // Âå"¡¦ÇësÎë!E¥Ù¥¯¥È¥ë£õ"¡¦
152  for(i=0; i<n_constraints; i++)
153  constraints[i]->copy_jacobian();
154  return 0;
155 }
156 
157 double IK::solve_ik()
158 {
159  int i, j;
160  double current_max_condnum = -1.0;
161  copy_jacobian();
162  if(n_all_const > 0)
163  {
165  // check rank when HIGH_IF_POSSIBLE constraints have high priority
166  int n_high_const;
167  fMat Jhigh;
168  fMat wJhigh;
169  fVec fb_high;
170  fVec weight_high;
171  int* is_high_const = 0;
172  double cond_number = 1.0;
173 // cerr << "---" << endl;
174 // cerr << n_const[HIGH_PRIORITY] << " " << n_const[HIGH_IF_POSSIBLE] << " " << n_const[LOW_PRIORITY] << endl;
175  if(n_const[HIGH_IF_POSSIBLE] > 0)
176  {
177  is_high_const = new int [n_const[HIGH_IF_POSSIBLE]];
178  // initialize
179  for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
180  is_high_const[i] = true;
181  // search
182  int search_phase = 0;
183  while(1)
184  {
185  n_high_const = n_const[HIGH_PRIORITY];
186  for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
187  {
188  if(is_high_const[i]) n_high_const++;
189  }
190  Jhigh.resize(n_high_const, n_dof);
191  wJhigh.resize(n_high_const, n_dof);
192  fb_high.resize(n_high_const);
193  weight_high.resize(n_high_const);
194  if(n_const[HIGH_PRIORITY] > 0)
195  {
196  // set fb and J of higher priority pins
197  for(i=0; i<n_const[HIGH_PRIORITY]; i++)
198  {
199  fb_high(i) = fb[HIGH_PRIORITY](i);
200  weight_high(i) = weight[HIGH_PRIORITY](i);
201  for(j=0; j<n_dof; j++)
202  {
203  Jhigh(i, j) = J[HIGH_PRIORITY](i, j);
204  wJhigh(i, j) = Jhigh(i, j) * weight_high(i) / joint_weights(j);
205  }
206  }
207  }
208  int count = 0;
209  // set fb and J of medium priority pins
210  for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
211  {
212  if(is_high_const[i])
213  {
214  fb_high(n_const[HIGH_PRIORITY]+count) = fb[HIGH_IF_POSSIBLE](i);
215  weight_high(n_const[HIGH_PRIORITY]+count) = weight[HIGH_IF_POSSIBLE](i);
216  for(j=0; j<n_dof; j++)
217  {
218  Jhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j);
219  wJhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j) * weight[HIGH_IF_POSSIBLE](i) / joint_weights(j);
220  }
221  count++;
222  }
223  }
224  // singular value decomposition
225  fMat U(n_high_const, n_high_const), VT(n_dof, n_dof);
226  fVec s;
227  int s_size;
228  if(n_high_const < n_dof) s_size = n_high_const;
229  else s_size = n_dof;
230  s.resize(s_size);
231  wJhigh.svd(U, s, VT);
232  double condnum_limit = max_condnum * 100.0;
233  if(s(s_size-1) > s(0)/(max_condnum*condnum_limit))
234  cond_number = s(0) / s(s_size-1);
235  else
236  cond_number = condnum_limit;
237  if(current_max_condnum < 0.0 || cond_number > current_max_condnum)
238  {
239  current_max_condnum = cond_number;
240  }
241  if(n_high_const <= n_const[HIGH_PRIORITY]) break;
242  // remove some constraints
243  if(cond_number > max_condnum)
244  {
245  int reduced = false;
246  for(i=n_constraints-1; i>=0; i--)
247  {
248  if(constraints[i]->enabled &&
249  constraints[i]->priority == HIGH_IF_POSSIBLE &&
250  constraints[i]->i_const >= 0 &&
251  constraints[i]->GetType() == HANDLE_CONSTRAINT &&
252  is_high_const[constraints[i]->i_const])
253  {
254  IKHandle* h = (IKHandle*)constraints[i];
255  if(search_phase ||
256  (!search_phase && h->joint->DescendantDOF() > 0))
257  {
258  for(j=0; j<constraints[i]->n_const; j++)
259  {
260  is_high_const[constraints[i]->i_const + j] = false;
261  }
262  constraints[i]->is_dropped = true;
263 // cerr << "r" << flush;
264  reduced = true;
265  break;
266  }
267  }
268  }
269  if(!reduced) search_phase++;
270  }
271  else break;
272  }
273  }
274  else
275  {
276  n_high_const = n_const[HIGH_PRIORITY];
277  Jhigh.resize(n_high_const, n_dof);
278  wJhigh.resize(n_high_const, n_dof);
279  fb_high.resize(n_high_const);
280  weight_high.resize(n_high_const);
281  if(n_high_const > 0)
282  {
283  Jhigh.set(J[HIGH_PRIORITY]);
284  fb_high.set(fb[HIGH_PRIORITY]);
285  weight_high.set(weight[HIGH_PRIORITY]);
286  }
287  }
288 #if 0
289  // adjust feedback according to the condition number
291  if(current_max_condnum > max_condnum)
292  fb_high.zero();
293  else
294  {
295  double k = (current_max_condnum-max_condnum)/(1.0-max_condnum);
296  fb_high *= k;
297  cerr << current_max_condnum << ", " << k << endl;
298  }
301  if(current_max_condnum < 0.0) current_max_condnum = 1.0;
302 #endif
303  int n_low_const = n_all_const - n_high_const;
304  int low_first = 0, count = 0;
305  fMat Jlow(n_low_const, n_dof);
306  fVec fb_low(n_low_const);
307  fVec weight_low(n_low_const);
308  for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
309  {
310  if(!is_high_const[i])
311  {
312  fb_low(count) = fb[HIGH_IF_POSSIBLE](i);
313  weight_low(count) = weight[HIGH_IF_POSSIBLE](i);
314  for(j=0; j<n_dof; j++)
315  {
316  Jlow(count, j) = J[HIGH_IF_POSSIBLE](i, j);
317  }
318  count++;
319  }
320  }
321  low_first = count;
322  double* p = fb_low.data() + low_first;
323  double* q = fb[LOW_PRIORITY].data();
324  double* r = weight_low.data() + low_first;
325  double* s = weight[LOW_PRIORITY].data();
326  for(i=0; i<n_const[LOW_PRIORITY]; p++, q++, r++, s++, i++)
327  {
328 // fb_low(low_first+i) = fb[LOW_PRIORITY](i);
329  *p = *q;
330  *r = *s;
331  double* a = Jlow.data() + low_first + i;
332  int a_row = Jlow.row();
333  double* b = J[LOW_PRIORITY].data() + i;
334  int b_row = J[LOW_PRIORITY].row();
335  for(j=0; j<n_dof; a+=a_row, b+=b_row, j++)
336  {
337 // Jlow(low_first+i, j) = J[LOW_PRIORITY](i, j);
338  *a = *b;
339  }
340  }
341  if(is_high_const) delete[] is_high_const;
342 
343  fVec jvel(n_dof); // ³û¼â1¡¦x
344  fVec jvel0(n_dof); // ¹ó/¡¦¾å
345  fVec fb_low_0(n_low_const), dfb(n_low_const), y(n_dof);
346  fMat Jinv(n_dof, n_high_const), W(n_dof, n_dof), JW(n_low_const, n_dof);
347  fVec w_error(n_low_const), w_norm(n_dof);
348  // weighted
349  double damping = 0.1;
350 // w_error = 1.0;
351  w_error.set(weight_low);
352  w_norm = 1.0;
353 // w_norm.set(joint_weights);
354 // cerr << "joint_weights = " << joint_weights << endl;
355 // cerr << "weight_high = " << weight_high << endl;
356 // cerr << "weight_low = " << weight_low << endl;
357 #ifdef MEASURE_TIME
358  clock_t t1 = clock();
359 #endif
360  // ¹ó/¡¦¾å¡¦€ÅæéòÉçïàïêvZ
361  if(n_high_const > 0)
362  {
363  for(i=0; i<n_dof; i++)
364  {
365  int a_row = wJhigh.row();
366  double* a = wJhigh.data() + a_row*i;
367  int b_row = Jhigh.row();
368  double* b = Jhigh.data() + b_row*i;
369  double* c = joint_weights.data() + i;
370  double* d = weight_high.data();
371  for(j=0; j<n_high_const; a++, b++, d++, j++)
372  {
373 // wJhigh(j, i) = Jhigh(j, i) / joint_weights(i);
374  *a = *b * *d / *c;
375  }
376  }
377  fVec w_fb_high(fb_high);
378  for(i=0; i<n_high_const; i++)
379  w_fb_high(i) *= weight_high(i);
380  Jinv.p_inv(wJhigh);
381  jvel0.mul(Jinv, w_fb_high);
382  W.mul(Jinv, wJhigh);
383  for(i=0; i<n_dof; i++)
384  {
385  double* w = W.data() + i;
386  double a = joint_weights(i);
387  for(j=0; j<n_dof; w+=n_dof, j++)
388  {
389  if(i==j) *w -= 1.0;
390  *w /= -a;
391  }
392  jvel0(i) /= a;
393  }
394  JW.mul(Jlow, W);
395  }
396  else
397  {
398  jvel0.zero();
399  JW.set(Jlow);
400  }
401 #ifdef MEASURE_TIME
402  clock_t t2 = clock();
403  high_constraint_time += (double)(t2-t1)/(double)CLOCKS_PER_SEC;
404 #endif
405  // Ǥ¡¦#x¥¯¥È¥ë¹à¤ê³×Z
406  if(n_low_const > 0)
407  {
408  fb_low_0.mul(Jlow, jvel0);
409  dfb.sub(fb_low, fb_low_0);
410  y.lineq_sr(JW, w_error, w_norm, damping, dfb);
411  if(n_high_const > 0) jvel.mul(W, y);
412  else jvel.set(y);
413 // fVec error(n_low_const);
414 // error = dfb-Jlow*jvel;
415 // cerr << dfb*dfb << "->" << error*error << endl;
416  }
417  else
418  {
419  jvel.zero();
420  }
421  // ³û¼â1¡¦x
422  jvel += jvel0;
423 #ifdef MEASURE_TIME
424  clock_t t3 = clock();
425  low_constraint_time += (double)(t3-t2)/(double)CLOCKS_PER_SEC;
426 #endif
427  SetJointVel(jvel);
428 // cerr << fb_high - Jhigh * jvel << endl;
429  }
430  if(current_max_condnum < 0.0) current_max_condnum = 1.0;
431  return current_max_condnum;
432 }
433 
434 int IKConstraint::count_constraints()
435 {
436  i_const = -1;
437  if(enabled)
438  {
439  i_const = ik->n_const[priority];
440  ik->n_const[priority] += n_const;
441  ik->n_all_const += n_const;
442  }
443  return 0;
444 }
445 
446 int IKConstraint::copy_jacobian()
447 {
448  if(i_const < 0) return -1;
449  int n_dof = ik->NumDOF();
450  int i, j, m;
451  is_dropped = false;
452  for(m=0; m<IK::N_PRIORITY_TYPES; m++)
453  {
454  if(priority == (IK::Priority)m && enabled)
455  {
456  fMat& targetJ = ik->J[m];
457  int target_row = targetJ.row();
458  int row = J.row();
459  double *a, *b;
460  for(i=0; i<n_const; i++)
461  {
462  ik->fb[m](i_const+i) = fb(i);
463  if(weight.size() == n_const)
464  ik->weight[m](i_const+i) = weight(i);
465  else
466  ik->weight[m](i_const+i) = 1.0;
467  a = targetJ.data() + i_const + i;
468  b = J.data() + i;
469  for(j=0; j<n_dof; a+=target_row, b+=row, j++)
470  {
471 // targetJ(i_const+i, j) = J(i, j);
472  *a = *b;
473  }
474  }
475  break;
476  }
477  }
478  return 0;
479 }
480 
481 
IK::n_const
int n_const[N_PRIORITY_TYPES]
number of constraints of each type
Definition: ik.h:238
IK::n_constraints
int n_constraints
number of current constraints
Definition: ik.h:224
autoplay.c
int c
Definition: autoplay.py:16
IK::solve_ik
double solve_ik()
compute the joint velocity
Definition: update_ik.cpp:157
Joint::child
Joint * child
pointer to the child joint
Definition: chain.h:685
fMat::resize
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
IK::Priority
Priority
Definition: ik.h:57
IKHandle
Position constraint.
Definition: ik.h:416
fMat::row
int row() const
Returns the number of rows.
Definition: fMatrix.h:158
IK::constraints
IKConstraint ** constraints
list of current constraints
Definition: ik.h:230
ik.h
Inverse kinematics (UTPoser) class.
ret
RTC::ReturnCode_t ret(RTC::Local::ReturnCode_t r)
IK::HIGH_PRIORITY
strictly satisfied
Definition: ik.h:58
w
w
i
png_uint_32 i
Definition: png.h:2735
IKConstraint::count_constraints
int count_constraints()
Definition: update_ik.cpp:434
Chain::SetJointVel
int SetJointVel(const fVec &vels)
Set all joint velocities/accelerations.
Definition: joint.cpp:456
b
long b
Definition: jpegint.h:371
IK::J
fMat J[N_PRIORITY_TYPES]
Jacobian matrix of each type.
Definition: ik.h:235
IKConstraint::i_const
int i_const
feedback velocity (n_const)
Definition: ik.h:404
JSPHERE
spherical (3DOF)
Definition: chain.h:43
IK::n_all_const
int n_all_const
Definition: ik.h:239
fMat::mul
void mul(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1217
IK::joint_weights
fVec joint_weights
joint weights
Definition: ik.h:245
fMat::zero
void zero()
Creates a zero matrix.
Definition: fMatrix.h:249
Chain::CalcPosition
void CalcPosition()
Forward kinematics.
Definition: fk.cpp:16
Chain::IntegrateAdaptive
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
fVec::set
void set(double *_d)
Sets all elements.
Definition: fMatrix.h:533
IKConstraint::calc_feedback
virtual int calc_feedback()=0
compute the feedback velocity
IKConstraint::joint
Joint * joint
target joint
Definition: ik.h:391
fVec::mul
void mul(const fVec &vec, double d)
Definition: fMatrix.cpp:1620
Chain::n_dof
int n_dof
Definition: chain.h:468
IKConstraint::calc_jacobian
virtual int calc_jacobian()
Computes the constraint Jacobian.
Definition: update_ik.cpp:76
IK::HIGH_IF_POSSIBLE
strictly satisfied if possible
Definition: ik.h:59
j
def j(str, encoding="cp932")
Joint::j_type
JointType j_type
joint type
Definition: chain.h:694
IK::calc_jacobian
int calc_jacobian()
compute the Jacobian matrix
Definition: update_ik.cpp:66
a
string a
row
png_bytepp row
Definition: png.h:1759
IKConstraint::is_dropped
int is_dropped
index in the constraints with the same priority
Definition: ik.h:406
IK::LOW_PRIORITY
lower priority
Definition: ik.h:60
fMat::svd
int svd(fMat &U, fVec &Sigma, fMat &VT)
singular value decomposition
Definition: fMatrix.cpp:733
fVec::resize
void resize(int i)
Change the size.
Definition: fMatrix.h:511
fMat::data
double * data() const
Returns the pointer to the first element.
Definition: fMatrix.h:193
fVec
Vector of generic size.
Definition: fMatrix.h:491
Joint::brother
Joint * brother
pointer to the brother joint
Definition: chain.h:684
IK::max_condnum
double max_condnum
maximum condition number
Definition: ik.h:242
IK::Update
double Update(double timestep)
Definition: update_ik.cpp:24
JSLIDE
prismatic (1DOF)
Definition: chain.h:42
fVec::zero
void zero()
Creates a zero vector.
Definition: fMatrix.h:575
Joint::DescendantDOF
int DescendantDOF()
Total DOF of the descendants (end link side).
Definition: chain.cpp:441
IK::HANDLE_CONSTRAINT
position/orientation of link
Definition: ik.h:48
fMat::set
void set(double *_d)
Sets the values from an array.
Definition: fMatrix.h:187
IKConstraint::copy_jacobian
int copy_jacobian()
copy each constraint Jacobian to the whole Jacobian matrix
Definition: update_ik.cpp:446
JROTATE
rotational (1DOF)
Definition: chain.h:41
IKConstraint::n_const
int n_const
weight
Definition: ik.h:398
Joint
The class for representing a joint.
Definition: chain.h:538
IK::copy_jacobian
int copy_jacobian()
Definition: update_ik.cpp:125
fVec::size
int size() const
Size of the vector (same as row()).
Definition: fMatrix.h:507
y
* y
Definition: IceUtils.h:97
IK::calc_feedback
int calc_feedback()
Definition: update_ik.cpp:112
IK::weight
fVec weight[N_PRIORITY_TYPES]
weight of each type
Definition: ik.h:237
Chain::Integrate
int Integrate(double timestep)
Performs Euler integration with timestep timestep [s].
Definition: integ.cpp:110
JFREE
free (6DOF)
Definition: chain.h:44
fMat
Matrix of generic size. The elements are stored in a one-dimensional array in row-major order...
Definition: fMatrix.h:46
fVec::lineq_sr
int lineq_sr(const fMat &A, fVec &w_err, fVec &w_norm, double k, const fVec &b)
Definition: fMatrix.cpp:869
IK::fb
fVec fb[N_PRIORITY_TYPES]
constraint error of each type
Definition: ik.h:236

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