jacobi.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  * jacobi.cpp
11  * Create: Katsu Yamane, Univ. of Tokyo, 03.10.21
12  */
13 
14 #include <cassert>
15 #include <cstdlib>
16 #include "chain.h"
17 
18 double Chain::ComJacobian(fMat& J, fVec3& com, const char* charname)
19 {
20  double m;
21  J.resize(3, n_dof);
22  J.zero();
23  com.zero();
24  m = root->com_jacobian(J, com, charname);
25  com /= m;
26  J /= m;
27  return m;
28 }
29 
30 double Joint::com_jacobian(fMat& J, fVec3& com, const char* charname)
31 {
32  double b_mass, c_mass;
33  fVec3 b_com(0,0,0), c_com(0,0,0);
34 // cerr << name << ": " << i_chain_dof << endl;
35  c_mass = child->com_jacobian(J, c_com, charname);
36  // check if this joint should be included
37  int is_target = false;
38  if(!charname)
39  {
40  is_target = true;
41  }
42  else
43  {
44  char* my_chname = CharName();
45  if(my_chname && !strcmp(my_chname, charname))
46  {
47  is_target = true;
48  }
49  }
50  // add myself
51  if(is_target)
52  {
53  static fVec3 abs_com_pos, my_com;
54  abs_com_pos.mul(abs_att, loc_com);
55  abs_com_pos += abs_pos;
56  c_mass += mass;
57  my_com.mul(abs_com_pos, mass);
58  c_com += my_com;
59 
60  if(n_dof > 0)
61  {
62  int i;
63  // compute Jacobian
64  static fVec3 ms; // vector from child com to joint origin, minus s
65  static fMat33 msX; // cross matrix of vector ms
66  static fVec3 msXaxis;
67  ms = c_com - (c_mass * abs_pos);
68  msX.cross(ms);
69  static fMat33 msXatt;
70  msXatt.mul(msX, abs_att);
71  msXatt *= -1.0;
72  switch(j_type)
73  {
74  case JROTATE:
75  msXaxis.mul(msXatt, axis);
76  J(0, i_dof) = msXaxis(0);
77  J(1, i_dof) = msXaxis(1);
78  J(2, i_dof) = msXaxis(2);
79  break;
80  case JSLIDE:
81  // msXaxis.mul(msX, axis);
82  msXaxis.mul(abs_att, axis);
83  msXaxis *= c_mass;
84  J(0, i_dof) = msXaxis(0);
85  J(1, i_dof) = msXaxis(1);
86  J(2, i_dof) = msXaxis(2);
87  break;
88  case JSPHERE:
89  for(i=0; i<3; i++)
90  {
91  J(0, i_dof+i) = msXatt(0, i);
92  J(1, i_dof+i) = msXatt(1, i);
93  J(2, i_dof+i) = msXatt(2, i);
94  }
95  break;
96  case JFREE:
97  for(i=0; i<3; i++)
98  {
99  J(0, i_dof+i) = c_mass * abs_att(0, i);
100  J(1, i_dof+i) = c_mass * abs_att(1, i);
101  J(2, i_dof+i) = c_mass * abs_att(2, i);
102  J(0, i_dof+3+i) = msXatt(0, i);
103  J(1, i_dof+3+i) = msXatt(1, i);
104  J(2, i_dof+3+i) = msXatt(2, i);
105  }
106  break;
107  case JFIXED:
108  break;
109  case JUNKNOWN:
110  abort ();
111  }
112  }
113  }
114  b_mass = brother->com_jacobian(J, b_com, charname);
115  com.add(c_com, b_com);
116  return c_mass + b_mass;
117 }
118 
119 int Joint::CalcJacobian(fMat& J)
120 {
121  J.resize(6, chain->n_dof);
122  J.zero();
123  calc_jacobian(J, this);
124  return 0;
125 }
126 
127 int Joint::calc_jacobian(fMat& J, Joint* target)
128 {
129  switch(j_type)
130  {
131  case JROTATE:
132  if(t_given) calc_jacobian_rotate(J, target);
133  break;
134  case JSLIDE:
135  if(t_given) calc_jacobian_slide(J, target);
136  break;
137  case JSPHERE:
138  if(t_given) calc_jacobian_sphere(J, target);
139  break;
140  case JFREE:
141  if(t_given) calc_jacobian_free(J, target);
142  break;
143  default:
144  break;
145  }
146  parent->calc_jacobian(J, target);
147  return 0;
148 }
149 
150 int Joint::calc_jacobian_rotate(fMat& J, Joint* target)
151 {
152  static fVec3 axis0, pp, lin;
153  axis0.mul(abs_att, axis);
154  pp.sub(target->abs_pos, abs_pos);
155  lin.cross(axis0, pp);
156  double* a = J.data() + 6*i_dof;
157  *a = lin(0);
158  *(++a) = lin(1);
159  *(++a) = lin(2);
160  *(++a) = axis0(0);
161  *(++a) = axis0(1);
162  *(++a) = axis0(2);
163 /* J(0, i_dof) = lin(0);
164  J(1, i_dof) = lin(1);
165  J(2, i_dof) = lin(2);
166  J(3, i_dof) = axis0(0);
167  J(4, i_dof) = axis0(1);
168  J(5, i_dof) = axis0(2);
169 */ return 0;
170 }
171 
172 int Joint::calc_jacobian_slide(fMat& J, Joint* target)
173 {
174  double* a = J.data() + 6*i_dof;
175  static fVec3 axis0;
176  axis0.mul(abs_att, axis);
177  *a = axis0(0);
178  *(++a) = axis0(1);
179  *(++a) = axis0(2);
180  *(++a) = 0.0;
181  *(++a) = 0.0;
182  *(++a) = 0.0;
183 /* J(0, i_dof) = axis0(0);
184  J(1, i_dof) = axis0(1);
185  J(2, i_dof) = axis0(2);
186  J(3, i_dof) = 0.0;
187  J(4, i_dof) = 0.0;
188  J(5, i_dof) = 0.0;
189 */ return 0;
190 }
191 
192 int Joint::calc_jacobian_sphere(fMat& J, Joint* target)
193 {
194  static fVec3 axis, axis0, pp, lin;
195  double* a = J.data() + 6*i_dof;
196  axis.zero();
197  for(int i=0; i<3; a+=6, i++)
198  {
199  axis(i) = 1.0;
200  axis0.mul(abs_att, axis);
201  pp.sub(target->abs_pos, abs_pos);
202  lin.cross(axis0, pp);
203  *a = lin(0);
204  *(a+1) = lin(1);
205  *(a+2) = lin(2);
206  *(a+3) = axis0(0);
207  *(a+4) = axis0(1);
208  *(a+5) = axis0(2);
209 /* J(0, i_dof+i) = lin(0);
210  J(1, i_dof+i) = lin(1);
211  J(2, i_dof+i) = lin(2);
212  J(3, i_dof+i) = axis0(0);
213  J(4, i_dof+i) = axis0(1);
214  J(5, i_dof+i) = axis0(2);
215 */ axis(i) = 0.0;
216  }
217  return 0;
218 }
219 
220 int Joint::calc_jacobian_free(fMat& J, Joint* target)
221 {
222  double* a = J.data() + 6*i_dof;
223  static fVec3 axis, axis0, pp, lin;
224  int i;
225  axis.zero();
226  for(i=0; i<3; a+=6, i++)
227  {
228  axis(i) = 1.0;
229  axis0.mul(abs_att, axis);
230  pp.sub(target->abs_pos, abs_pos);
231  lin.cross(axis0, pp);
232  *a = axis0(0);
233  *(a+1) = axis0(1);
234  *(a+2) = axis0(2);
235  *(a+3) = 0.0;
236  *(a+4) = 0.0;
237  *(a+5) = 0.0;
238  *(a+18) = lin(0);
239  *(a+19) = lin(1);
240  *(a+20) = lin(2);
241  *(a+21) = axis0(0);
242  *(a+22) = axis0(1);
243  *(a+23) = axis0(2);
244 /* J(0, i_dof+i) = axis0(0);
245  J(1, i_dof+i) = axis0(1);
246  J(2, i_dof+i) = axis0(2);
247  J(3, i_dof+i) = 0.0;
248  J(4, i_dof+i) = 0.0;
249  J(5, i_dof+i) = 0.0;
250  J(0, i_dof+3+i) = lin(0);
251  J(1, i_dof+3+i) = lin(1);
252  J(2, i_dof+3+i) = lin(2);
253  J(3, i_dof+3+i) = axis0(0);
254  J(4, i_dof+3+i) = axis0(1);
255  J(5, i_dof+3+i) = axis0(2);
256 */ axis(i) = 0.0;
257  }
258  return 0;
259 }
260 
261 int Joint::CalcJdot(fVec& jdot)
262 {
263  static fVec save_acc, zero_acc;
264  static fVec3 space_acc;
265  save_acc.resize(chain->n_dof);
266  zero_acc.resize(chain->n_dof);
267  jdot.resize(6);
268  zero_acc.zero();
269 
270  // save gravity and joint accelerations
271  space_acc.set(chain->root->loc_lin_acc);
272  chain->GetJointAcc(save_acc);
273  // clear gravity and joint accelerations
274  chain->root->loc_lin_acc.zero();
275  chain->SetJointAcc(zero_acc);
276  // update acceleration
277  chain->CalcAcceleration();
278  // jdot obtained
279  jdot(0) = loc_lin_acc(0);
280  jdot(1) = loc_lin_acc(1);
281  jdot(2) = loc_lin_acc(2);
282  jdot(3) = loc_ang_acc(0);
283  jdot(4) = loc_ang_acc(1);
284  jdot(5) = loc_ang_acc(2);
285  // restore original accelerations
286  chain->root->loc_lin_acc.set(space_acc);
287  chain->SetJointAcc(save_acc);
288  chain->CalcAcceleration();
289  return 0;
290 }
291 
292 /*
293  * 2nd-order derivatives
294  */
295 // J: array of n_dof fMat's
296 int Joint::CalcJacobian2(fMat* J)
297 {
298  int i;
299  for(i=0; i<chain->n_dof; i++)
300  {
301  J[i].resize(6, chain->n_dof);
302  J[i].zero();
303  }
304  calc_jacobian_2(J, this);
305  return 0;
306 }
307 
308 // first layer
309 // compute J[*](1...6, i_dof)
310 int Joint::calc_jacobian_2(fMat* J, Joint* target)
311 {
312  if(j_type == JROTATE)
313  {
314  target->calc_jacobian_2_rotate_sub(J, target, this);
315  }
316  else if(j_type == JSLIDE)
317  {
318  target->calc_jacobian_2_slide_sub(J, target, this);
319  }
320  else if(j_type == JSPHERE)
321  {
322  target->calc_jacobian_2_sphere_sub(J, target, this);
323  }
324  else if(j_type == JFREE)
325  {
326  target->calc_jacobian_2_free_sub(J, target, this);
327  }
328  parent->calc_jacobian_2(J, target);
329  return 0;
330 }
331 
332 // main computation
333 // compute J[i_dof](1...6, j1->i_dof)
334 int Joint::calc_jacobian_2_rotate_sub(fMat* J, Joint* target, Joint* j1)
335 {
336  assert(j1->j_type == JROTATE);
337  static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
338  if(j_type == JROTATE)
339  {
340  calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, axis, i_dof);
341  }
342  else if(j_type == JSLIDE)
343  {
344  calc_jacobian_2_rotate_slide(J, target, j1, j1->axis, j1->i_dof, axis, i_dof);
345  }
346  else if(j_type == JSPHERE)
347  {
348  calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, x, i_dof);
349  calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, y, i_dof+1);
350  calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, z, i_dof+2);
351  }
352  else if(j_type == JFREE)
353  {
354  calc_jacobian_2_rotate_slide(J, target, j1, j1->axis, j1->i_dof, x, i_dof);
355  calc_jacobian_2_rotate_slide(J, target, j1, j1->axis, j1->i_dof, y, i_dof+1);
356  calc_jacobian_2_rotate_slide(J, target, j1, j1->axis, j1->i_dof, z, i_dof+2);
357  calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, x, i_dof+3);
358  calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, y, i_dof+4);
359  calc_jacobian_2_rotate_rotate(J, target, j1, j1->axis, j1->i_dof, z, i_dof+5);
360  }
361  parent->calc_jacobian_2_rotate_sub(J, target, j1);
362  return 0;
363 }
364 
365 int Joint::calc_jacobian_2_slide_sub(fMat* J, Joint* target, Joint* j1)
366 {
367  assert(j1->j_type == JSLIDE);
368  static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
369  if(j_type == JROTATE)
370  {
371  calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, axis, i_dof);
372  }
373  else if(j_type == JSLIDE)
374  {
375  }
376  else if(j_type == JSPHERE)
377  {
378  calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, x, i_dof);
379  calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, y, i_dof+1);
380  calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, z, i_dof+2);
381  }
382  else if(j_type == JFREE)
383  {
384  calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, x, i_dof+3);
385  calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, y, i_dof+4);
386  calc_jacobian_2_slide_rotate(J, target, j1, j1->axis, j1->i_dof, z, i_dof+5);
387  }
388  parent->calc_jacobian_2_slide_sub(J, target, j1);
389  return 0;
390 }
391 
392 int Joint::calc_jacobian_2_sphere_sub(fMat* J, Joint* target, Joint* j1)
393 {
394  assert(j1->j_type == JSPHERE);
395  static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
396  if(j_type == JROTATE)
397  {
398  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, axis, i_dof);
399  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, axis, i_dof);
400  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, axis, i_dof);
401  }
402  else if(j_type == JSLIDE)
403  {
404  calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof, axis, i_dof);
405  calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+1, axis, i_dof);
406  calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+2, axis, i_dof);
407  }
408  else if(j_type == JSPHERE)
409  {
410  // my x axis
411  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, x, i_dof);
412  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, x, i_dof);
413  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, x, i_dof);
414  // my y axis
415  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, y, i_dof+1);
416  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, y, i_dof+1);
417  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, y, i_dof+1);
418  // my z axis
419  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, z, i_dof+2);
420  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, z, i_dof+2);
421  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, z, i_dof+2);
422  }
423  else if(j_type == JFREE)
424  {
425  // my x trans
426  calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof, x, i_dof);
427  calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+1, x, i_dof);
428  calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+2, x, i_dof);
429  // my y trans
430  calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof, y, i_dof+1);
431  calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+1, y, i_dof+1);
432  calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+2, y, i_dof+1);
433  // my z trans
434  calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof, z, i_dof+2);
435  calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+1, z, i_dof+2);
436  calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+2, z, i_dof+2);
437  // my x rot
438  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, x, i_dof+3);
439  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, x, i_dof+3);
440  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, x, i_dof+3);
441  // my y rot
442  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, y, i_dof+4);
443  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, y, i_dof+4);
444  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, y, i_dof+4);
445  // my z rot
446  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof, z, i_dof+5);
447  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+1, z, i_dof+5);
448  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+2, z, i_dof+5);
449  }
450  parent->calc_jacobian_2_sphere_sub(J, target, j1);
451  return 0;
452 }
453 
454 int Joint::calc_jacobian_2_free_sub(fMat* J, Joint* target, Joint* j1)
455 {
456  assert(j1->j_type == JFREE);
457  static fVec3 x(1, 0, 0), y(0, 1, 0), z(0, 0, 1);
458  if(j_type == JROTATE)
459  {
460  calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, axis, i_dof);
461  calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, axis, i_dof);
462  calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, axis, i_dof);
463  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, axis, i_dof);
464  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, axis, i_dof);
465  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, axis, i_dof);
466  }
467  else if(j_type == JSLIDE)
468  {
469  calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof+3, axis, i_dof);
470  calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+4, axis, i_dof);
471  calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+5, axis, i_dof);
472  }
473  else if(j_type == JSPHERE)
474  {
475  // my x rot
476  calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, x, i_dof);
477  calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, x, i_dof);
478  calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, x, i_dof);
479  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, x, i_dof);
480  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, x, i_dof);
481  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, x, i_dof);
482  // my y rot
483  calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, y, i_dof+1);
484  calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, y, i_dof+1);
485  calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, y, i_dof+1);
486  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, y, i_dof+1);
487  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, y, i_dof+1);
488  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, y, i_dof+1);
489  // my z rot
490  calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, z, i_dof+2);
491  calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, z, i_dof+2);
492  calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, z, i_dof+2);
493  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, z, i_dof+2);
494  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, z, i_dof+2);
495  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, z, i_dof+2);
496  }
497  else if(j_type == JFREE)
498  {
499  // my x trans
500  calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof+3, x, i_dof);
501  calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+4, x, i_dof);
502  calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+5, x, i_dof);
503  // my y trans
504  calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof+3, y, i_dof+1);
505  calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+4, y, i_dof+1);
506  calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+5, y, i_dof+1);
507  // my z trans
508  calc_jacobian_2_rotate_slide(J, target, j1, x, j1->i_dof+3, z, i_dof+2);
509  calc_jacobian_2_rotate_slide(J, target, j1, y, j1->i_dof+4, z, i_dof+2);
510  calc_jacobian_2_rotate_slide(J, target, j1, z, j1->i_dof+5, z, i_dof+2);
511  // my x rot
512  calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, x, i_dof+3);
513  calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, x, i_dof+3);
514  calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, x, i_dof+3);
515  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, x, i_dof+3);
516  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, x, i_dof+3);
517  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, x, i_dof+3);
518  // my y rot
519  calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, y, i_dof+4);
520  calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, y, i_dof+4);
521  calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, y, i_dof+4);
522  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, y, i_dof+4);
523  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, y, i_dof+4);
524  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, y, i_dof+4);
525  // my z rot
526  calc_jacobian_2_slide_rotate(J, target, j1, x, j1->i_dof, z, i_dof+5);
527  calc_jacobian_2_slide_rotate(J, target, j1, y, j1->i_dof+1, z, i_dof+5);
528  calc_jacobian_2_slide_rotate(J, target, j1, z, j1->i_dof+2, z, i_dof+5);
529  calc_jacobian_2_rotate_rotate(J, target, j1, x, j1->i_dof+3, z, i_dof+5);
530  calc_jacobian_2_rotate_rotate(J, target, j1, y, j1->i_dof+4, z, i_dof+5);
531  calc_jacobian_2_rotate_rotate(J, target, j1, z, j1->i_dof+5, z, i_dof+5);
532  }
533  parent->calc_jacobian_2_free_sub(J, target, j1);
534  return 0;
535 }
536 
537 int Joint::calc_jacobian_2_rotate_rotate(fMat* J, Joint* target, Joint* jk, const fVec3& k_axis, int k_index, const fVec3& loc_axis, int loc_index)
538 {
539  static fVec3 out1, out2;
540  // 1st term
541  static fVec3 jk_axis, my_axis, d_jk_axis, dp, dJ;
542  jk_axis.mul(jk->abs_att, k_axis);
543  my_axis.mul(abs_att, loc_axis);
544  if(this != jk && isAscendant(jk))
545  {
546  d_jk_axis.zero();
547  }
548  else
549  {
550  d_jk_axis.cross(my_axis, jk_axis);
551  }
552  dp.sub(target->abs_pos, jk->abs_pos);
553  out1.cross(d_jk_axis, dp);
554  // 2nd term
555  static fVec3 Jt, Jk;
556  dp.sub(target->abs_pos, abs_pos);
557  Jt.cross(my_axis, dp);
558  if(this != jk && isAscendant(jk))
559  {
560  Jk.zero();
561  }
562  else
563  {
564  dp.sub(jk->abs_pos, abs_pos);
565  Jk.cross(my_axis, dp);
566  }
567  dJ.sub(Jt, Jk);
568  out2.cross(jk_axis, dJ);
569  // set
570  J[loc_index](0, k_index) = out1(0) + out2(0);
571  J[loc_index](1, k_index) = out1(1) + out2(1);
572  J[loc_index](2, k_index) = out1(2) + out2(2);
573  J[loc_index](3, k_index) = d_jk_axis(0);
574  J[loc_index](4, k_index) = d_jk_axis(1);
575  J[loc_index](5, k_index) = d_jk_axis(2);
576  return 0;
577 }
578 
579 int Joint::calc_jacobian_2_slide_rotate(fMat* J, Joint* target, Joint* jk, const fVec3& k_axis, int k_index, const fVec3& loc_axis, int loc_index)
580 {
581  static fVec3 jk_axis, my_axis, d_jk_axis;
582  jk_axis.mul(jk->abs_att, k_axis);
583  my_axis.mul(abs_att, loc_axis);
584 // if(isAscendant(jk))
585  if(this != jk && isAscendant(jk))
586  {
587  d_jk_axis.zero();
588  }
589  else
590  {
591  d_jk_axis.cross(my_axis, jk_axis);
592  }
593  J[loc_index](0, k_index) = d_jk_axis(0);
594  J[loc_index](1, k_index) = d_jk_axis(1);
595  J[loc_index](2, k_index) = d_jk_axis(2);
596  return 0;
597 }
598 
599 int Joint::calc_jacobian_2_rotate_slide(fMat* J, Joint* target, Joint* jk, const fVec3& k_axis, int k_index, const fVec3& loc_axis, int loc_index)
600 {
601  if(isAscendant(jk))
602  {
603  static fVec3 jk_axis, my_axis, out1;
604  jk_axis.mul(jk->abs_att, k_axis);
605  my_axis.mul(abs_att, loc_axis);
606  out1.cross(jk_axis, my_axis);
607  J[loc_index](0, k_index) = out1(0);
608  J[loc_index](1, k_index) = out1(1);
609  J[loc_index](2, k_index) = out1(2);
610  }
611  return 0;
612 }
613 
614 
fVec3::add
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
Joint::CalcJdot
int CalcJdot(fVec &jdot)
Jdot x thdot.
Definition: jacobi.cpp:261
fMat33
3x3 matrix class.
Definition: fMatrix3.h:29
Joint::axis
fVec3 axis
joint axis in local frame (for 1DOF joints)
Definition: chain.h:696
fMat::resize
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
Joint::calc_jacobian_free
int calc_jacobian_free(fMat &J, Joint *target)
Definition: jacobi.cpp:220
Joint::CalcJacobian
int CalcJacobian(fMat &J)
The Jacobian matrix of position/orientation w.r.t. the joint values.
Definition: jacobi.cpp:119
x
* x
Definition: IceUtils.h:98
JUNKNOWN
Definition: chain.h:39
CharName
char * CharName(const char *_name)
Extracts the character name from a joint name.
Definition: chain.cpp:17
fVec3::zero
void zero()
Creates a zero vector.
Definition: fMatrix3.h:283
Joint::calc_jacobian_2_rotate_rotate
int calc_jacobian_2_rotate_rotate(fMat *J, Joint *target, Joint *j1, const fVec3 &axis1, int index1, const fVec3 &axis2, int index2)
Definition: jacobi.cpp:537
fMat33::mul
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
fVec3::cross
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
Joint::calc_jacobian
int calc_jacobian(fMat &J, Joint *target)
Definition: jacobi.cpp:127
Chain::root
Joint * root
Chain information.
Definition: chain.h:466
fVec3::set
void set(double *v)
Set element values from array or three values.
Definition: fMatrix3.h:314
i
png_uint_32 i
Definition: png.h:2735
Joint::calc_jacobian_2_rotate_slide
int calc_jacobian_2_rotate_slide(fMat *J, Joint *target, Joint *jk, const fVec3 &k_axis, int k_index, const fVec3 &loc_axis, int loc_index)
Definition: jacobi.cpp:599
Joint::calc_jacobian_2_sphere_sub
int calc_jacobian_2_sphere_sub(fMat *J, Joint *target, Joint *j1)
Definition: jacobi.cpp:392
Joint::com_jacobian
double com_jacobian(fMat &J, fVec3 &com, const char *chname)
Definition: jacobi.cpp:30
JSPHERE
spherical (3DOF)
Definition: chain.h:43
fMat::zero
void zero()
Creates a zero matrix.
Definition: fMatrix.h:249
Joint::CalcJacobian2
int CalcJacobian2(fMat *J2)
2nd-order derivatives of the Jacobian matrix.
Definition: jacobi.cpp:296
Joint::i_dof
int i_dof
index in all DOF
Definition: chain.h:720
Chain::n_dof
int n_dof
Definition: chain.h:468
Joint::calc_jacobian_sphere
int calc_jacobian_sphere(fMat &J, Joint *target)
Definition: jacobi.cpp:192
Joint::j_type
JointType j_type
joint type
Definition: chain.h:694
Joint::calc_jacobian_rotate
int calc_jacobian_rotate(fMat &J, Joint *target)
Definition: jacobi.cpp:150
Joint::abs_att
fMat33 abs_att
absolute orientation
Definition: chain.h:742
a
string a
fMat33::cross
void cross(const fVec3 &p)
Sets spectial matrices.
Definition: fMatrix3.cpp:217
Joint::calc_jacobian_2_free_sub
int calc_jacobian_2_free_sub(fMat *J, Joint *target, Joint *j1)
Definition: jacobi.cpp:454
Joint::calc_jacobian_slide
int calc_jacobian_slide(fMat &J, Joint *target)
Definition: jacobi.cpp:172
fVec::resize
void resize(int i)
Change the size.
Definition: fMatrix.h:511
Joint::calc_jacobian_2_slide_sub
int calc_jacobian_2_slide_sub(fMat *J, Joint *target, Joint *j1)
Definition: jacobi.cpp:365
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
JFIXED
fixed (0DOF)
Definition: chain.h:40
chain.h
Classes for defining open/closed kinematic chains.
Chain::ComJacobian
double ComJacobian(fMat &J, fVec3 &com, const char *chname=0)
Computes the com Jacobian.
Definition: jacobi.cpp:18
Joint::calc_jacobian_2
int calc_jacobian_2(fMat *J, Joint *target)
Definition: jacobi.cpp:310
JSLIDE
prismatic (1DOF)
Definition: chain.h:42
Joint::calc_jacobian_2_slide_rotate
int calc_jacobian_2_slide_rotate(fMat *J, Joint *target, Joint *jk, const fVec3 &k_axis, int k_index, const fVec3 &loc_axis, int loc_index)
Definition: jacobi.cpp:579
fVec::zero
void zero()
Creates a zero vector.
Definition: fMatrix.h:575
fVec3::sub
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
fVec3
3-element vector class.
Definition: fMatrix3.h:206
JROTATE
rotational (1DOF)
Definition: chain.h:41
Joint::abs_pos
fVec3 abs_pos
absolute position
Definition: chain.h:741
fVec3::mul
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
Joint
The class for representing a joint.
Definition: chain.h:538
y
* y
Definition: IceUtils.h:97
Joint::calc_jacobian_2_rotate_sub
int calc_jacobian_2_rotate_sub(fMat *J, Joint *target, Joint *j1)
Definition: jacobi.cpp:334
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

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sat May 8 2021 02:42:38