update.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, 04.02.25
12  */
13 
14 #include "psim.h"
15 #include <limits>
16 
17 //#define PSIM_TEST
18 
20 
21 #ifdef VERBOSE
22 #include <fstream>
23 std::ofstream update_log("update.log");
24 #endif
25 
26 #ifdef TIMING_CHECK
27 static double update_start_time = 0.0;
28 #endif
29 
30 #ifdef PSIM_TEST
31 double max_condition_number = -1.0;
32 Joint* max_condition_number_joint = 0;
33 double max_sigma_ratio = -1.0;
34 Joint* max_sigma_ratio_joint = 0;
35 fVec condition_numbers;
36 fVec sigma_ratios;
37 double total_gamma_error = 0.0;
38 #endif
39 
40 int pSim::Update()
41 {
42 #ifdef VERBOSE
43  update_log << "Update no contact" << endl;
44 #endif
45 #ifdef TIMING_CHECK
46  update_start_time = MPI_Wtime();
47 #endif
48 #ifdef PSIM_TEST
49  max_condition_number = -1.0;
50  max_sigma_ratio = -1.0;
51  max_condition_number_joint = 0;
52  max_sigma_ratio_joint = 0;
53  condition_numbers.resize(n_dof);
54  sigma_ratios.resize(n_dof);
55  condition_numbers.zero();
56  sigma_ratios.zero();
57  total_gamma_error = 0.0;
58 #endif
59  /**** assembly ****/
60  // position-dependent variables
61  update_position();
62 #if 1
63  if(do_connect)
64  {
65  //
66  // do collision computation if needed
67  //
68  update_collision();
69  // don't forget to recompute link velocities
70  CalcVelocity();
71  do_connect = false;
72  }
73 #endif
74  // velocity-dependent variables
75  update_velocity();
76 
77 #ifdef TIMING_CHECK
78  cerr << "[" << rank << "] disassembly t = " << MPI_Wtime()-update_start_time << endl;
79 #endif
80  /**** disassembly ****/
81  disassembly();
82 
83 #ifdef PSIM_TEST
84  cerr << "--- max condition number = " << max_condition_number << " at " << max_condition_number_joint->name << endl;
85  cerr << "--- max sigma ratio = " << max_sigma_ratio << " at " << max_sigma_ratio_joint->name << endl;
86  cerr << "--- total_gamma_error = " << total_gamma_error << endl;
87 // cerr << "condition_numbers = " << tran(condition_numbers) << endl;
88 // cerr << "sigma_ratios = " << tran(sigma_ratios) << endl;
89 #endif
90 #ifdef USE_MPI
91  // scatter results
92  scatter_acc();
93 #endif
94  return 0;
95 }
96 
97 #ifdef USE_MPI
98 void pSim::scatter_acc()
99 {
100  int i;
101  for(i=0; i<size; i++)
102  {
103  MPI_Bcast(MPI_BOTTOM, 1, all_acc_types[i], i, MPI_COMM_WORLD);
104  }
105  subchains->scatter_acc();
106 }
107 
108 void pSubChain::scatter_acc()
109 {
110  if(!this) return;
111  if(last_joint && last_joint->t_given)
112  {
113  switch(last_joint->j_type)
114  {
115  case JROTATE:
116  case JSLIDE:
117  last_joint->SetJointAcc(acc_final(0));
118 // cerr << last_joint->name << ": " << acc_final(0) << endl;
119  break;
120  case JSPHERE:
121  last_joint->SetJointAcc(acc_final(0), acc_final(1), acc_final(2));
122 // cerr << last_joint->name << ": " << tran(acc_final) << endl;
123  break;
124  case JFREE:
125  last_joint->SetJointAcc(acc_final(0), acc_final(1), acc_final(2),
126  acc_final(3), acc_final(4), acc_final(5));
127 // cerr << last_joint->name << ": " << tran(acc_final) << endl;
128  break;
129  }
130 #if 0
131  double* sendbuf;
132  sendbuf = last_pjoints[0]->f_final.data();
133  MPI_Bcast(sendbuf, 6, MPI_DOUBLE, rank, MPI_COMM_WORLD);
134  sendbuf = last_pjoints[1]->f_final.data();
135  MPI_Bcast(sendbuf, 6, MPI_DOUBLE, rank, MPI_COMM_WORLD);
136 #endif
137  }
138  if(children[0]) children[0]->scatter_acc();
139  if(children[1] && children[0] != children[1]) children[1]->scatter_acc();
140 }
141 #endif
142 
146 void pSim::update_position()
147 {
148  int i;
149  for(i=0; i<n_joint; i++)
150  {
151 #ifdef USE_MPI
152  if(joint_info[i].pjoints[0]->subchain && rank == joint_info[i].pjoints[0]->subchain->rank)
153 #endif
154  joint_info[i].pjoints[0]->calc_jacobian();
155 #ifdef USE_MPI
156  if(joint_info[i].pjoints[1]->subchain && rank == joint_info[i].pjoints[1]->subchain->rank)
157 #endif
158  joint_info[i].pjoints[1]->calc_jacobian();
159  }
160  // should come after computing Jacobians of all pjoints
161  subchains->calc_inertia();
162 }
163 
164 void pJoint::calc_jacobian()
165 {
166  if(parent_side)
167  {
168  static fVec3 rel_pos;
169  static fMat33 rel_att;
170  if(link_side == joint->real)
171  {
172  rel_pos.set(joint->rpos_real);
173  rel_att.set(joint->ratt_real);
174  }
175  else
176  {
177  rel_pos.set(joint->rel_pos);
178  rel_att.set(joint->rel_att);
179  }
180  static fMat33 tcross, tmpT;
181  tcross.cross(rel_pos);
182  tmpT.mul(tcross, rel_att);
183  J(0,0) = rel_att(0,0), J(0,1) = rel_att(1,0), J(0,2) = rel_att(2,0);
184  J(1,0) = rel_att(0,1), J(1,1) = rel_att(1,1), J(1,2) = rel_att(2,1);
185  J(2,0) = rel_att(0,2), J(2,1) = rel_att(1,2), J(2,2) = rel_att(2,2);
186  J(0,3) = tmpT(0,0), J(0,4) = tmpT(1,0), J(0,5) = tmpT(2,0);
187  J(1,3) = tmpT(0,1), J(1,4) = tmpT(1,1), J(1,5) = tmpT(2,1);
188  J(2,3) = tmpT(0,2), J(2,4) = tmpT(1,2), J(2,5) = tmpT(2,2);
189  J(3,0) = 0.0, J(3,1) = 0.0, J(3,2) = 0.0;
190  J(4,0) = 0.0, J(4,1) = 0.0, J(4,2) = 0.0;
191  J(5,0) = 0.0, J(5,1) = 0.0, J(5,2) = 0.0;
192  J(3,3) = rel_att(0,0), J(3,4) = rel_att(1,0), J(3,5) = rel_att(2,0);
193  J(4,3) = rel_att(0,1), J(4,4) = rel_att(1,1), J(4,5) = rel_att(2,1);
194  J(5,3) = rel_att(0,2), J(5,4) = rel_att(1,2), J(5,5) = rel_att(2,2);
195 // update_log << joint->name << ": J = " << J << endl;
196  }
197 }
198 
199 void pSubChain::calc_inertia()
200 {
201  if(!this) return;
202  if(!last_joint) // single link
203  {
204  calc_inertia_leaf();
205  }
206  else // have last joint
207  {
208  // process children first
209  children[0]->calc_inertia();
210  if(children[1] != children[0])
211  {
212  children[1]->calc_inertia();
213  }
214  calc_inertia_body();
215  }
216 }
217 
218 #ifdef USE_MPI
219 void pSubChain::recv_inertia()
220 {
221  // recv from subchain's rank
222  MPI_Status status;
223 #if 0
224  int i, j;
225  for(i=0; i<n_outer_joints; i++)
226  {
227  for(j=i; j<n_outer_joints; j++)
228  {
229  double* buf = Lambda[i][j].data();
230  MPI_Recv(buf, 36, MPI_DOUBLE, rank, PSIM_TAG_LAMBDA, MPI_COMM_WORLD, &status);
231  if(i != j)
232  {
233  Lambda[j][i].tran(Lambda[i][j]);
234  }
235  }
236  }
237 #else
238 #ifdef TIMING_CHECK
239  double time1 = MPI_Wtime();
240 #endif
241  MPI_Recv(MPI_BOTTOM, 1, parent_lambda_type, rank, PSIM_TAG_LAMBDA, MPI_COMM_WORLD, &status);
242 #ifdef TIMING_CHECK
243  double time2 = MPI_Wtime();
244  sim->inertia_wait_time += time2-time1;
245 #endif
246 #endif
247 }
248 
249 void pSubChain::send_inertia(int dest)
250 {
251  // send to dest
252 #if 0
253  int i, j;
254  for(i=0; i<n_outer_joints; i++)
255  {
256  for(j=i; j<n_outer_joints; j++)
257  {
258  double* buf = Lambda[i][j].data();
259  MPI_Send(buf, 36, MPI_DOUBLE, dest, PSIM_TAG_LAMBDA, MPI_COMM_WORLD);
260  }
261  }
262 #else
263  MPI_Send(MPI_BOTTOM, 1, parent_lambda_type, dest, PSIM_TAG_LAMBDA, MPI_COMM_WORLD);
264 #endif
265 }
266 #endif
267 
268 void pSubChain::calc_inertia_leaf()
269 {
270 #ifdef USE_MPI
271  if(sim->rank != rank) return;
272 #endif
273  int i, j;
274  // Lambda
275 // update_log << links[0]->joint->name << ": calc_inertia_leaf" << endl;
276  for(i=0; i<n_outer_joints; i++)
277  {
278  static fMat JM(6, 6);
279  if(outer_joints[i]->parent_side) // link is parent side -> compute JM
280  {
281  JM.mul(outer_joints[i]->J, outer_joints[i]->plink->Minv);
282  }
283  else // link is child side -> J is identity
284  {
285  JM.set(outer_joints[i]->plink->Minv);
286  }
287  for(j=i; j<n_outer_joints; j++) // half only
288  {
289  static fMat tJ(6, 6);
290  if(outer_joints[j]->parent_side)
291  {
292  tJ.tran(outer_joints[j]->J);
293  Lambda[i][j].mul(JM, tJ);
294  }
295  else
296  {
297  Lambda[i][j].set(JM);
298  }
299 // update_log << "Lambda[" << i << "][" << j << "] = " << Lambda[i][j] << endl;
300  if(i != j) // copy transpose
301  Lambda[j][i].tran(Lambda[i][j]);
302  }
303  }
304 #ifdef USE_MPI
305  if(parent && sim->rank != parent->rank)
306  {
307 // cerr << sim->rank << ": link (" << links[0]->joint->name << "): send_inertia to " << parent->rank << endl;
308  send_inertia(parent->rank);
309  }
310 #endif
311 }
312 
313 void pSubChain::calc_inertia_body()
314 {
315  // for space
316 // if(!children[1]) return;
317 #ifdef USE_MPI
318  if(sim->rank != rank) return;
319  if(children[0] && sim->rank != children[0]->rank)
320  {
321 // cerr << sim->rank << ": joint (" << last_joint->name << "): recv_inertia from " << children[0]->rank << endl;
322  children[0]->recv_inertia();
323  }
324  if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
325  {
326 // cerr << sim->rank << ": joint (" << last_joint->name << "): recv_inertia from " << children[1]->rank << endl;
327  children[1]->recv_inertia();
328  }
329 #endif
330  int i, j;
331 // cerr << "---- " << last_joint->name << ": calc_inertia_body" << endl;
332  // P
333  if(children[1])
334  {
335  P.add(children[0]->Lambda[last_index[0]][last_index[0]], children[1]->Lambda[last_index[1]][last_index[1]]);
336  }
337  else
338  {
339  P.set(children[0]->Lambda[last_index[0]][last_index[0]]);
340  }
341 // cerr << "Lambda[0] = " << children[0]->Lambda[last_index[0]][last_index[0]] << endl;
342 // cerr << "Lambda[1] = " << children[1]->Lambda[last_index[1]][last_index[1]] << endl;
343 #ifdef PSIM_TEST
344  {
345  fMat U1(6,6), V1T(6,6), U2(6,6), V2T(6,6);
346  fVec sigma1(6), sigma2(6);
347  children[0]->Lambda[last_index[0]][last_index[0]].svd(U1, sigma1, V1T);
348  children[1]->Lambda[last_index[1]][last_index[1]].svd(U2, sigma2, V2T);
349  double s1 = sigma1.length(), s2 = sigma2.length();
350  if(s1 > 1e-8 && s2 > 1e-8)
351  {
352  double ratio = (s1 < s2) ? s2/s1 : s1/s2;
353  if(max_sigma_ratio < 0.0 || ratio > max_sigma_ratio)
354  {
355  max_sigma_ratio = ratio;
356  max_sigma_ratio_joint = last_joint;
357  }
358  sigma_ratios(last_joint->i_dof) = ratio;
359 // cerr << last_joint->name << ": " << s1 << ", " << s2 << " -> " << ratio << endl;
360  }
361  }
362 #endif
363  if(children[0] == children[1])
364  {
365  P -= children[0]->Lambda[last_index[0]][last_index[1]];
366  P -= children[0]->Lambda[last_index[1]][last_index[0]];
367  }
368  // P should be symmetric
369 // P += tran(P);
370 // P *= 0.5;
371  P.symmetric();
372 #ifndef USE_DCA
373  // Gamma
374  if(n_const > 0)
375  {
376  for(i=0; i<n_const; i++)
377  {
378  for(j=0; j<n_const; j++)
379  Gamma(i, j) = P(const_index[i], const_index[j]);
380  }
381  if(children[0] != children[1])
382  {
383  // Gamma is symmetric, positive-definite
384  if(Gamma_inv.inv_posv(Gamma))
385  Gamma_inv.inv_svd(Gamma);
386 // Gamma_inv.inv_porfs(Gamma);
387 #ifdef PSIM_TEST
388  fMat U(n_const, n_const), VT(n_const, n_const);
389  fVec sigma(n_const);
390  Gamma.svd(U, sigma, VT);
391  double cn = sigma(0) / sigma(n_const-1);
392  if(max_condition_number < 0.0 || cn > max_condition_number)
393  {
394  max_condition_number = cn;
395  max_condition_number_joint = last_joint;
396  }
397  condition_numbers(last_joint->i_dof) = cn;
398 // cerr << "condition_number = " << cn << endl;
399 // fMat Gamma_inv2(n_const, n_const);
400 // Gamma_inv2.inv_svd(Gamma, 2000);
401 // cerr << last_joint->name << ": " << cn << endl;
402 // fMat I(n_const, n_const);
403 // I.identity();
404 // cerr << last_joint->name << ": " << I - Gamma*Gamma_inv << endl;
405 // cerr << last_joint->name << ": " << Gamma_inv - Gamma_inv2 << endl;
406 #endif
407  }
408  else
409  {
410  // Gamma may be singular
411  Gamma_inv.inv_svd(Gamma);
412 // cerr << Gamma_inv * Gamma << endl;
413  }
414  }
415  // W, IW
416  W.zero();
417  for(i=0; i<n_const; i++)
418  {
419  for(int j=0; j<n_const; j++)
420  {
421  W(const_index[i], const_index[j]) = -Gamma_inv(i, j);
422  }
423  }
424 #else // #ifndef USE_DCA
425  static fMat SV, VSV;
426  SV.resize(n_dof, 6);
427  VSV.resize(n_dof, 6);
428  Vhat.inv_posv(P);
429  if(n_dof > 0)
430  {
431  for(i=0; i<n_dof; i++)
432  {
433  for(int j=0; j<n_dof; j++)
434  {
435  SVS(i,j) = Vhat(joint_index[i], joint_index[j]);
436  }
437  for(int j=0; j<6; j++)
438  {
439  SV(i,j) = Vhat(joint_index[i], j);
440  }
441  }
442 // cerr << "SVS = " << SVS << endl;
443  VSV.lineq(SVS, SV);
444 // cerr << "VSV = " << VSV << endl;
445  W.mul(tran(SV), VSV);
446 // W *= -1.0;
447  }
448  else
449  {
450  W.zero();
451  }
452  W -= Vhat;
453 #endif
454 // cerr << "P = " << P << endl;
455 // cerr << "W = " << W << endl;
456  IW.mul(P, W);
457  for(i=0; i<6; i++)
458  {
459  IW(i, i) += 1.0;
460  }
461 // cerr << "IW = " << IW << endl;
462  // Lambda
463  if(n_const == 0)
464  {
465  for(i=0; i<n_outer_joints; i++)
466  {
467  int org_i = outer_joints_origin[i];
468  int index_i = outer_joints_index[i];
469  for(j=i; j<n_outer_joints; j++)
470  {
471  int org_j = outer_joints_origin[j];
472  int index_j = outer_joints_index[j];
473  if(children[org_i] == children[org_j])
474  {
475  Lambda[i][j].set(children[org_i]->Lambda[index_i][index_j]);
476  }
477  if(i != j)
478  {
479  Lambda[j][i].tran(Lambda[i][j]);
480  }
481  }
482  }
483  }
484  else
485  {
486  for(i=0; i<n_outer_joints; i++)
487  {
488  int org_i = outer_joints_origin[i];
489  int index_i = outer_joints_index[i];
490  fMat& Lambda_i = children[org_i]->Lambda[index_i][last_index[org_i]];
491 #ifndef USE_DCA
492  int m, n;
493  static fMat LKi, KLj, GKLj;
494  LKi.resize(6, n_const);
495  KLj.resize(n_const, 6);
496  GKLj.resize(n_const, 6);
497  for(m=0; m<6; m++)
498  {
499  for(n=0; n<n_const; n++)
500  LKi(m, n) = Lambda_i(m, const_index[n]);
501  }
502  for(j=i; j<n_outer_joints; j++)
503  {
504  int org_j = outer_joints_origin[j];
505  int index_j = outer_joints_index[j];
506  fMat& Lambda_j = children[org_j]->Lambda[last_index[org_j]][index_j];
507  for(m=0; m<n_const; m++)
508  {
509  for(n=0; n<6; n++)
510  KLj(m, n) = Lambda_j(const_index[m], n);
511  }
512  GKLj.mul(Gamma_inv, KLj);
513 // GKLj.lineq_posv(Gamma, KLj);
514  Lambda[i][j].mul(LKi, GKLj);
515  if(children[org_i] == children[org_j])
516  {
517  Lambda[i][j] -= children[org_i]->Lambda[index_i][index_j];
518  Lambda[i][j] *= -1.0;
519  }
520  if(i != j)
521  {
522  Lambda[j][i].tran(Lambda[i][j]);
523  }
524  else
525  {
526  Lambda[i][j].symmetric();
527  }
528  }
529 #else // #ifndef USE_DCA
530  for(j=i; j<n_outer_joints; j++)
531  {
532  int org_j = outer_joints_origin[j];
533  int index_j = outer_joints_index[j];
534  fMat& Lambda_j = children[org_j]->Lambda[last_index[org_j]][index_j];
535  static fMat WL(6,6);
536  WL.mul(W, Lambda_j);
537  WL *= -1.0;
538  Lambda[i][j].mul(Lambda_i, WL);
539  if(children[org_i] == children[org_j])
540  {
541  Lambda[i][j] -= children[org_i]->Lambda[index_i][index_j];
542  Lambda[i][j] *= -1.0;
543  }
544  if(i != j)
545  {
546  Lambda[j][i].tran(Lambda[i][j]);
547  }
548  else
549  {
550  Lambda[i][j].symmetric();
551  }
552  }
553 #endif
554  }
555  }
556 #ifdef USE_MPI
557  if(parent && sim->rank != parent->rank)
558  {
559 // cerr << sim->rank << ": joint (" << last_joint->name << "): send_inertia to " << parent->rank << endl;
560  send_inertia(parent->rank);
561  }
562 #endif
563 }
564 
568 void pSim::update_collision()
569 {
570  calc_dvel();
571  col_disassembly();
572 }
573 
574 void pSim::calc_dvel()
575 {
576  int i;
577  for(i=0; i<n_joint; i++)
578  {
579  joint_info[i].pjoints[0]->calc_dvel();
580  joint_info[i].pjoints[1]->calc_dvel();
581  }
582  subchains->calc_dvel();
583 }
584 
585 void pJoint::calc_dvel()
586 {
587  if(parent_side)
588  {
589  static fVec v(6);
590  v(0) = link_side->loc_lin_vel(0);
591  v(1) = link_side->loc_lin_vel(1);
592  v(2) = link_side->loc_lin_vel(2);
593  v(3) = link_side->loc_ang_vel(0);
594  v(4) = link_side->loc_ang_vel(1);
595  v(5) = link_side->loc_ang_vel(2);
596  dvel.mul(J, v);
597  }
598  else
599  {
600  dvel(0) = joint->loc_lin_vel(0);
601  dvel(1) = joint->loc_lin_vel(1);
602  dvel(2) = joint->loc_lin_vel(2);
603  dvel(3) = joint->loc_ang_vel(0);
604  dvel(4) = joint->loc_ang_vel(1);
605  dvel(5) = joint->loc_ang_vel(2);
606  }
607 }
608 
609 void pSubChain::calc_dvel()
610 {
611  if(!this) return;
612  if(!last_joint) // single link
613  {
614  calc_dvel_leaf();
615  }
616  else // have last joint
617  {
618  // process children first
619  children[0]->calc_dvel();
620  if(children[1] != children[0]) children[1]->calc_dvel();
621  calc_dvel_body();
622  }
623 }
624 
625 void pSubChain::calc_dvel_leaf()
626 {
627  int i;
628  for(i=0; i<n_outer_joints; i++)
629  {
630  vel_temp[i].set(outer_joints[i]->dvel);
631  }
632 }
633 
634 void pSubChain::calc_dvel_body()
635 {
636  if(!children[1]) return;
637  int i, j;
638  // compute f_temp
639  static fVec dv6(6), dv;
640  static fMat PK;
641  PK.resize(6, n_dof);
642  dv.resize(n_const);
643  for(i=0; i<6; i++)
644  {
645  for(j=0; j<n_dof; j++)
646  PK(i, j) = P(i, joint_index[j]);
647  }
648  // + child_side - parent_side ?
649  dv6.sub(children[0]->vel_temp[last_index[0]], children[1]->vel_temp[last_index[1]]);
650  // actually we could save some computation by
651  // selecting const rows first
652  for(i=0; i<n_const; i++)
653  dv(i) = -dv6(const_index[i]);
654 #ifndef USE_DCA // TODO: allow dca
655  colf_temp.mul(Gamma_inv, dv);
656 #endif
657  // compute new velocity at all outer joints
658  static fVec f(6);
659  f.zero();
660  for(i=0; i<n_const; i++)
661  f(const_index[i]) = colf_temp(i);
662  for(i=0; i<n_outer_joints; i++)
663  {
664  int org = outer_joints_origin[i];
665  int index = outer_joints_index[i];
666  int ilast = last_index[org];
667  vel_temp[i].mul(children[org]->Lambda[index][ilast], f);
668  if(org == 1)
669  {
670  vel_temp[i] *= -1.0;
671  }
672  vel_temp[i] += children[org]->vel_temp[index];
673  }
674 }
675 
676 void pSim::col_disassembly()
677 {
678  subchains->col_disassembly();
679 }
680 
681 void pSubChain::col_disassembly()
682 {
683  if(!this) return;
684  if(last_joint)
685  {
686  col_disassembly_body();
687  // process children last
688  children[0]->col_disassembly();
689  if(children[1] != children[0]) children[1]->col_disassembly();
690  }
691 }
692 
693 void pSubChain::col_disassembly_body()
694 {
695  // for space
696  if(!children[1]) return;
697  int i;
698  // compute final constraint force
699  static fVec KLf, Lf(6), Lf_temp[2], v(6), colf, colf_final(6);
700  KLf.resize(n_const);
701  Lf_temp[0].resize(6);
702  Lf_temp[1].resize(6);
703  colf.resize(n_const);
704  Lf_temp[0].zero();
705  Lf_temp[1].zero();
706  for(i=0; i<n_outer_joints; i++)
707  {
708  int org = outer_joints_origin[i];
709  int index = outer_joints_index[i];
710  fMat& Lambda_i = children[org]->Lambda[last_index[org]][index];
711  // first multiply and increment
712  v.mul(Lambda_i, children[org]->outer_joints[index]->colf_final);
713  Lf_temp[org] += v;
714  }
715  Lf.sub(Lf_temp[0], Lf_temp[1]);
716  // then extract rows
717  for(i=0; i<n_const; i++)
718  KLf(i) = Lf(const_index[i]);
719 #ifndef USE_DCA // TODO: allow DCA
720  colf.mul(Gamma_inv, KLf);
721 #endif
722  colf_temp -= colf;
723  colf_final.zero();
724  for(i=0; i<n_const; i++)
725  colf_final(const_index[i]) = colf_temp(i);
726  last_pjoints[0]->colf_final.set(colf_final);
727  last_pjoints[1]->colf_final.neg(colf_final);
728  // compute link velocities
729  last_pjoints[0]->vel_final.mul(children[0]->Lambda[last_index[0]][last_index[0]], last_pjoints[0]->colf_final);
730  last_pjoints[0]->vel_final += Lf_temp[0];
731  last_pjoints[0]->vel_final += children[0]->vel_temp[last_index[0]];
732  if(children[1])
733  {
734  last_pjoints[1]->vel_final.mul(children[1]->Lambda[last_index[1]][last_index[1]], last_pjoints[1]->colf_final);
735  last_pjoints[1]->vel_final += Lf_temp[1];
736  last_pjoints[1]->vel_final += children[1]->vel_temp[last_index[1]];
737  }
738  if(children[0] == children[1])
739  {
740  v.mul(children[1]->Lambda[last_index[0]][last_index[1]], last_pjoints[1]->colf_final);
741  last_pjoints[0]->vel_final += v;
742  v.mul(children[0]->Lambda[last_index[1]][last_index[0]], last_pjoints[0]->colf_final);
743  last_pjoints[1]->vel_final += v;
744  }
745  // compute joint velocity
746  v.sub(last_pjoints[0]->vel_final, last_pjoints[1]->vel_final);
747 // cerr << last_joint->name << ": v = " << tran(v) << endl;
748  switch(last_joint->j_type)
749  {
750  case JROTATE:
751  case JSLIDE:
752  last_joint->SetJointVel(v(axis));
753  break;
754  case JSPHERE:
755  last_joint->SetJointVel(fVec3(v(3), v(4), v(5)));
756  break;
757  case JFREE:
758  last_joint->SetJointVel(fVec3(v(0), v(1), v(2)), fVec3(v(3), v(4), v(5)));
759  break;
760  default:
761  break;
762  }
763 }
764 
768 void pSim::update_velocity()
769 {
770  int i;
771  for(i=0; i<n_joint; i++)
772  {
773 #ifdef USE_MPI
774  if(joint_info[i].pjoints[0]->subchain && rank == joint_info[i].pjoints[0]->subchain->rank)
775 #endif
776  {
777  joint_info[i].pjoints[0]->calc_jdot();
778  }
779 #ifdef USE_MPI
780  if(joint_info[i].pjoints[1]->subchain && rank == joint_info[i].pjoints[1]->subchain->rank)
781 #endif
782  {
783  joint_info[i].pjoints[1]->calc_jdot();
784  }
785  if(joint_info[i].plink
786 #ifdef USE_MPI
787  && joint_info[i].plink->subchain &&
788  rank == joint_info[i].plink->subchain->rank
789 #endif
790  )
791  {
792  joint_info[i].plink->calc_acc(Root()->loc_lin_acc);
793  }
794  }
795  // should come after computing Jacobians of all pjoints
796  subchains->calc_acc();
797 #ifdef TIMING_CHECK
798  cerr << "[" << rank << "] update_velocity end t = " << MPI_Wtime()-update_start_time << endl;
799 #endif
800 }
801 
802 void pJoint::calc_jdot()
803 {
804  if(parent_side)
805  {
806  static fVec3 v1, v2, v3;
807  static fVec3 rel_pos;
808  static fMat33 rel_att;
809  if(link_side == joint->real)
810  {
811  rel_pos.set(joint->rpos_real);
812  rel_att.set(joint->ratt_real);
813  }
814  else
815  {
816  rel_pos.set(joint->rel_pos);
817  rel_att.set(joint->rel_att);
818  }
819  v1.cross(link_side->loc_ang_vel, rel_pos);
820  v2.cross(link_side->loc_ang_vel, v1);
821  v3.mul(v2, rel_att);
822 
823 // if(!joint->real)
824  {
825 // v1.set(joint->loc_ang_vel);
826  v1.mul(link_side->loc_ang_vel, rel_att);
827  // linear joint motion
828  v2.cross(v1, joint->rel_lin_vel);
829  v2 *= 2.0;
830  v3 += v2;
831  // angular joint motion
832  v2.cross(v1, joint->rel_ang_vel);
833  }
834 
835  Jdot(0) = v3(0);
836  Jdot(1) = v3(1);
837  Jdot(2) = v3(2);
838  Jdot(3) = v2(0);
839  Jdot(4) = v2(1);
840  Jdot(5) = v2(2);
841 // cerr << joint->name << ": jdot = " << tran(Jdot) << endl;
842 // cerr << "rel_pos = " << rel_pos << endl;
843 // cerr << "rel_lin_vel = " << joint->rel_lin_vel << endl;
844 // cerr << "rel_ang_vel = " << joint->rel_ang_vel << endl;
845  }
846  else // child side
847  {
848  }
849 }
850 
851 void pLink::calc_acc(const fVec3& g0)
852 {
853  static fVec3 a, c1, c2, g;
854  static fVec3 v1, v2;
855  g.mul(g0, joint->abs_att);
856  // nonlinear acc
857  v1.cross(joint->loc_ang_vel, joint->loc_com);
858  a.cross(joint->loc_ang_vel, v1);
859  a += g;
860  // c1
861  c1.mul(joint->mass, a);
862  // c2
863  v1.mul(joint->inertia, joint->loc_ang_vel);
864  v2.cross(joint->loc_ang_vel, v1);
865  c2.cross(joint->loc_com, c1);
866  c2 += v2;
867  // external force/moment around com
868  c1 -= joint->ext_force;
869  c2 -= joint->ext_moment;
870 #ifdef VERBOSE
871  update_log << joint->name << ": external force/moment = " << joint->ext_force << "/" << joint->ext_moment << endl;
872 #endif
873 // v1.cross(joint->ext_force, joint->loc_com);
874 // c2 -= v1;
875  // -c
876  c(0) = -c1(0);
877  c(1) = -c1(1);
878  c(2) = -c1(2);
879  c(3) = -c2(0);
880  c(4) = -c2(1);
881  c(5) = -c2(2);
882  // bias acc
883  acc.mul(Minv, c);
884 // cerr << joint->name << ": acc = " << tran(acc) << endl;
885 }
886 
887 void pSubChain::calc_acc()
888 {
889  if(!this) return;
890  if(!last_joint) // single link
891  {
892  calc_acc_leaf();
893  }
894  else // have last joint
895  {
896  // process children first
897  children[0]->calc_acc();
898  if(children[1] != children[0])
899  {
900  children[1]->calc_acc();
901  }
902  calc_acc_body();
903  }
904 }
905 
906 #ifdef USE_MPI
907 void pSubChain::recv_acc()
908 {
909  MPI_Status status;
910 #if 0
911  int i;
912  for(i=0; i<n_outer_joints; i++)
913  {
914  double* buf = acc_temp[i].data();
915  MPI_Recv(buf, 6, MPI_DOUBLE, rank, PSIM_TAG_ACC, MPI_COMM_WORLD, &status);
916  }
917 #else
918 #ifdef TIMING_CHECK
919  double time1 = MPI_Wtime();
920  cerr << "[" << sim->rank << "] recv_acc(0) t = " << MPI_Wtime()-update_start_time << endl;
921 #endif
922  MPI_Recv(MPI_BOTTOM, 1, parent_acc_type, rank, PSIM_TAG_ACC, MPI_COMM_WORLD, &status);
923 #ifdef TIMING_CHECK
924  cerr << "[" << sim->rank << "] recv_acc(1) t = " << MPI_Wtime()-update_start_time << endl;
925  double time2 = MPI_Wtime();
926  sim->acc_wait_time += time2-time1;
927 #endif
928 #endif
929 }
930 
931 void pSubChain::send_acc(int dest)
932 {
933 #if 0
934  int i;
935  for(i=0; i<n_outer_joints; i++)
936  {
937  double* buf = acc_temp[i].data();
938  MPI_Send(buf, 6, MPI_DOUBLE, dest, PSIM_TAG_ACC, MPI_COMM_WORLD);
939  }
940 #else
941 #ifdef TIMING_CHECK
942  cerr << "[" << sim->rank << "] send_acc(0) t = " << MPI_Wtime()-update_start_time << endl;
943 #endif
944  MPI_Send(MPI_BOTTOM, 1, parent_acc_type, dest, PSIM_TAG_ACC, MPI_COMM_WORLD);
945 #ifdef TIMING_CHECK
946  cerr << "[" << sim->rank << "] send_acc(1) t = " << MPI_Wtime()-update_start_time << endl;
947 #endif
948 #endif
949 }
950 #endif
951 
952 void pSubChain::calc_acc_leaf()
953 {
954 #ifdef USE_MPI
955  if(sim->rank != rank) return;
956 #endif
957  // compute bias acc at all outer joints
958 // update_log << "--- " << links[0]->joint->name << ": calc_acc_leaf" << endl;
959  int i;
960  for(i=0; i<n_outer_joints; i++)
961  {
962  if(outer_joints[i]->parent_side)
963  {
964  acc_temp[i].mul(outer_joints[i]->J, links[0]->acc);
965  acc_temp[i] += outer_joints[i]->Jdot;
966  }
967  else
968  {
969 // acc_temp[i].set(links[0]->acc);
970  acc_temp[i].add(links[0]->acc, outer_joints[i]->Jdot);
971  }
972 // update_log << "acc_temp[" << i << "] = " << tran(acc_temp[i]) << endl;
973  }
974 #ifdef USE_MPI
975  if(parent && sim->rank != parent->rank)
976  {
977  send_acc(parent->rank);
978  }
979 #endif
980 }
981 
982 void pSubChain::calc_acc_body()
983 {
984 // if(!children[1]) return;
985 #ifdef USE_MPI
986  if(sim->rank != rank) return;
987  if(children[0] && sim->rank != children[0]->rank)
988  {
989  children[0]->recv_acc();
990  }
991  if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
992  {
993  children[1]->recv_acc();
994  }
995 #endif
996 // update_log << "--- " << last_joint->name << ": calc_acc_body" << endl;
997  int i, j;
998  // compute f_temp
999  static fVec da;
1000  static fMat PK;
1001  PK.resize(6, n_dof);
1002  da.resize(n_const);
1003  for(i=0; i<6; i++)
1004  {
1005  for(j=0; j<n_dof; j++)
1006  PK(i, j) = P(i, joint_index[j]);
1007  }
1008  if(last_joint->n_dof > 0)
1009  {
1010  switch(last_joint->j_type)
1011  {
1012  case JROTATE:
1013  case JSLIDE:
1014  tau(0) = last_joint->tau;
1015  break;
1016  case JSPHERE:
1017  tau(0) = last_joint->tau_n(0);
1018  tau(1) = last_joint->tau_n(1);
1019  tau(2) = last_joint->tau_n(2);
1020  break;
1021  case JFREE:
1022  tau(0) = last_joint->tau_f(0);
1023  tau(1) = last_joint->tau_f(1);
1024  tau(2) = last_joint->tau_f(2);
1025  tau(3) = last_joint->tau_n(0);
1026  tau(4) = last_joint->tau_n(1);
1027  tau(5) = last_joint->tau_n(2);
1028  break;
1029  default:
1030  break;
1031  }
1032  da6.mul(PK, tau);
1033  }
1034  else
1035  da6.zero();
1036 // cerr << "da6(0) = " << tran(da6) << endl;
1037  // + child_side - parent_side ?
1038  da6 += children[0]->acc_temp[last_index[0]];
1039 // cerr << "da6(1) = " << tran(da6) << endl;
1040  if(children[1])
1041  da6 -= children[1]->acc_temp[last_index[1]];
1042 // cerr << "da6(2) = " << tran(da6) << endl;
1043  // motion controlled joints
1044  if(!last_joint->t_given)
1045  {
1046  switch(last_joint->j_type)
1047  {
1048  case JROTATE:
1049  case JSLIDE:
1050  da6(axis) -= last_joint->qdd;
1051 // update_log << last_joint->name << ": qdd = " << last_joint->qdd << endl;
1052  break;
1053  case JSPHERE:
1054  da6(3) -= last_joint->rel_ang_acc(0);
1055  da6(4) -= last_joint->rel_ang_acc(1);
1056  da6(5) -= last_joint->rel_ang_acc(2);
1057  break;
1058  case JFREE:
1059  da6(0) -= last_joint->rel_lin_acc(0);
1060  da6(1) -= last_joint->rel_lin_acc(1);
1061  da6(2) -= last_joint->rel_lin_acc(2);
1062  da6(3) -= last_joint->rel_ang_acc(0);
1063  da6(4) -= last_joint->rel_ang_acc(1);
1064  da6(5) -= last_joint->rel_ang_acc(2);
1065  break;
1066  default:
1067  break;
1068  }
1069  }
1070  static fVec f(6);
1071 // cerr << "Gamma = " << Gamma << endl;
1072 // cerr << "Gamma_inv = " << Gamma_inv << endl;
1073 #if 0
1074  // actually we could save some computation by
1075  // selecting const rows first
1076  for(i=0; i<n_const; i++)
1077  da(i) = -da6(const_index[i]);
1078  f_temp.mul(Gamma_inv, da);
1079 // f_temp.lineq_posv(Gamma, da);
1080  // compute acc at all outer joints
1081  for(i=0; i<n_dof; i++)
1082  f(joint_index[i]) = tau(i);
1083  for(i=0; i<n_const; i++)
1084  f(const_index[i]) = f_temp(i);
1085 // cerr << "da = " << tran(da) << endl;
1086 // cerr << "f_temp = " << tran(f_temp) << endl;
1087 // cerr << "Gamma*f_temp - da = " << tran(Gamma*f_temp-da) << endl;
1088 #else
1089 #if 0
1090  f.mul(W, da6);
1091  for(i=0; i<n_dof; i++)
1092  {
1093  f(joint_index[i]) += tau(i);
1094  }
1095 #else
1096  static fVec db(6), Wdb(6), IWRtau(6);
1097  static fMat IWR;
1098  IWR.resize(6, n_dof);
1099  db.set(children[0]->acc_temp[last_index[0]]);
1100  if(children[1])
1101  db -= children[1]->acc_temp[last_index[1]];
1102  Wdb.mul(W, db);
1103  for(i=0; i<6; i++)
1104  {
1105  for(j=0; j<n_dof; j++)
1106  {
1107  IWR(i, j) = IW(joint_index[j], i);
1108  }
1109  }
1110  IWRtau.mul(IWR, tau);
1111 // cerr << "W = " << tran(W) << endl;
1112 // update_log << "db = " << tran(db) << endl;
1113 // cerr << "Wdb = " << tran(Wdb) << endl;
1114 // cerr << "IWRtau = " << tran(IWRtau) << endl;
1115  f.add(Wdb, IWRtau);
1116 // update_log << "f = " << tran(f) << endl;
1117 
1118 #ifdef PSIM_TEST
1119  for(i=0; i<n_const; i++)
1121  {
1122  da(i) = -da6(const_index[i]);
1123  f_temp(i) = f(const_index[i]);
1124  }
1125 // cerr << "Gamma*f_temp - da = " << tran(Gamma*f_temp-da) << endl;
1126  total_gamma_error += (Gamma*f_temp-da) * (Gamma*f_temp-da);
1128 #endif
1129 #endif
1130 #endif
1131  for(i=0; i<n_outer_joints; i++)
1132  {
1133  int org = outer_joints_origin[i];
1134  int index = outer_joints_index[i];
1135  int ilast = last_index[org];
1136  acc_temp[i].mul(children[org]->Lambda[index][ilast], f);
1137  if(org == 1)
1138  {
1139  acc_temp[i] *= -1.0;
1140  }
1141  acc_temp[i] += children[org]->acc_temp[index];
1142 // update_log << "acc_temp[" << i << "] = " << tran(acc_temp[i]) << endl;
1143  }
1144 #ifdef USE_MPI
1145  if(parent && sim->rank != parent->rank)
1146  {
1147  send_acc(parent->rank);
1148  }
1149 #endif
1150 }
1151 
1155 void pSim::disassembly()
1156 {
1157  subchains->disassembly();
1158 }
1159 
1160 void pSubChain::disassembly()
1161 {
1162  if(!this) return;
1163  if(last_joint)
1164  {
1165  disassembly_body();
1166  // process children last
1167  children[0]->disassembly();
1168  if(children[1] != children[0]) children[1]->disassembly();
1169  }
1170  else
1171  {
1172 // disassembly_leaf();
1173  }
1174 }
1175 
1176 #ifdef USE_MPI
1177 void pSubChain::recv_force()
1178 {
1179  MPI_Status status;
1180 #if 0
1181  int i;
1182  for(i=0; i<n_outer_joints; i++)
1183  {
1184  double* buf = outer_joints[i]->f_final.data();
1185  MPI_Recv(buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
1186  }
1187  double* buf;
1188  buf = last_pjoints[0]->f_final.data();
1189  MPI_Recv(buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
1190  buf = last_pjoints[1]->f_final.data();
1191  MPI_Recv(buf, 6, MPI_DOUBLE, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
1192 #else
1193 #ifdef TIMING_CHECK
1194  double time1 = MPI_Wtime();
1195 #endif
1196  MPI_Recv(MPI_BOTTOM, 1, parent_force_type, rank, PSIM_TAG_FORCE, MPI_COMM_WORLD, &status);
1197 #ifdef TIMING_CHECK
1198  double time2 = MPI_Wtime();
1199  sim->force_wait_time += time2-time1;
1200 #endif
1201 #endif
1202 }
1203 
1204 void pSubChain::send_force(int dest)
1205 {
1206 #if 0
1207  int i;
1208  for(i=0; i<n_outer_joints; i++)
1209  {
1210  double* buf = outer_joints[i]->f_final.data();
1211  MPI_Send(buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
1212  }
1213  double* buf;
1214  buf = last_pjoints[0]->f_final.data();
1215  MPI_Send(buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
1216  buf = last_pjoints[1]->f_final.data();
1217  MPI_Send(buf, 6, MPI_DOUBLE, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
1218 #else
1219  MPI_Send(MPI_BOTTOM, 1, parent_force_type, dest, PSIM_TAG_FORCE, MPI_COMM_WORLD);
1220 #endif
1221 }
1222 #endif
1223 
1224 void pSubChain::disassembly_leaf()
1225 {
1226  Joint* target_joint = links[0]->joint;
1227 // if(!target_joint->parent) return; // skip space
1228  // convert all forces/moments to joint frame
1229  cerr << "---- " << target_joint->name << ": disassembly_leaf" << endl;
1230  int i;
1231  static fVec3 total_f, total_n;
1232  static fVec3 pos;
1233  static fMat33 att, t_att;
1234  static fVec acc(6);
1235  static fVec allf(6);
1236  total_f.zero();
1237  total_n.zero();
1238  acc.zero();
1239  pos.set(target_joint->abs_pos);
1240  att.set(target_joint->abs_att);
1241  t_att.tran(att);
1242  for(i=0; i<n_outer_joints; i++)
1243  {
1244  static fVec3 loc_f, loc_n, f, n, fn;
1245  static fVec3 jpos, rel_pos, pp;
1246  static fMat33 jatt, rel_att;
1247  cerr << "outer[" << i << "]: " << outer_joints[i]->joint->name << endl;
1248  cerr << "f_final = " << tran(outer_joints[i]->f_final) << endl;
1249  loc_f(0) = outer_joints[i]->f_final(0);
1250  loc_f(1) = outer_joints[i]->f_final(1);
1251  loc_f(2) = outer_joints[i]->f_final(2);
1252  loc_n(0) = outer_joints[i]->f_final(3);
1253  loc_n(1) = outer_joints[i]->f_final(4);
1254  loc_n(2) = outer_joints[i]->f_final(5);
1255  jpos.set(outer_joints[i]->joint->abs_pos);
1256  jatt.set(outer_joints[i]->joint->abs_att);
1257  pp.sub(jpos, pos);
1258  rel_pos.mul(t_att, pp);
1259  rel_att.mul(t_att, jatt);
1260  f.mul(rel_att, loc_f); // force
1261  n.mul(rel_att, loc_n);
1262  fn.cross(rel_pos, f);
1263  n += fn;
1264  cerr << "(f n) = " << f << n << endl;
1265  total_f += f;
1266  total_n += n;
1267  }
1268  allf(0) = total_f(0);
1269  allf(1) = total_f(1);
1270  allf(2) = total_f(2);
1271  allf(3) = total_n(0);
1272  allf(4) = total_n(1);
1273  allf(5) = total_n(2);
1274  cerr << "total_f = " << total_f << endl;
1275  cerr << "total_n = " << total_n << endl;
1276  acc.lineq_posv(links[0]->M, allf);
1277  acc += links[0]->acc;
1278  cerr << "Minv = " << links[0]->Minv << endl;
1279  cerr << "acc = " << tran(links[0]->acc) << endl;
1280  cerr << "link acc = " << tran(acc) << endl;
1281 }
1282 
1283 void pSubChain::disassembly_body()
1284 {
1285  // for space
1286  if(!children[1]) return;
1287 #ifdef VERBOSE
1288  update_log << "disassembly_body" << endl;
1289 #endif
1290 #ifdef USE_MPI
1291  if(sim->rank != rank) return;
1292  if(parent && sim->rank != parent->rank)
1293  {
1294 #ifdef TIMING_CHECK
1295  cerr << "[" << sim->rank << "] " << last_joint->name << ": recv force from " << parent->last_joint->name << " [" << parent->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
1296 #endif
1297  parent->recv_force();
1298  }
1299 #endif
1300  int i;
1301 #ifdef TIMING_CHECK
1302  if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
1303  cerr << "[" << sim->rank << "] " << last_joint->name << " enter disassembly t = " << MPI_Wtime()-update_start_time << endl;
1304 #endif
1305  // compute final constraint force
1306  static fVec KLf, Lf(6), Lf_temp[2], v(6), f, f_final(6);
1307  KLf.resize(n_const);
1308  Lf_temp[0].resize(6);
1309  Lf_temp[1].resize(6);
1310  f.resize(n_const);
1311  Lf_temp[0].zero();
1312  Lf_temp[1].zero();
1313  for(i=0; i<n_outer_joints; i++)
1314  {
1315  int org = outer_joints_origin[i];
1316  int index = outer_joints_index[i];
1317  fMat& Lambda_i = children[org]->Lambda[last_index[org]][index];
1318  // first multiply and increment
1319 // v.mul(Lambda_i, children[org]->outer_joints[index]->f_final);
1320  v.mul(Lambda_i, outer_joints[i]->f_final);
1321 #ifdef VERBOSE
1322 // update_log << "children[" << org << "]->Lambda[" << last_index[org] << "][" << index << "] = " << Lambda_i << endl;
1323  update_log << outer_joints[i]->joint->name << ": f_final[" << i << "] = " << tran(outer_joints[i]->f_final) << endl;
1324 #endif
1325  Lf_temp[org] += v;
1326  }
1327  Lf.sub(Lf_temp[0], Lf_temp[1]);
1328 // update_log << "Lf_temp[0] = " << tran(Lf_temp[0]) << endl;
1329 // update_log << "Lf_temp[1] = " << tran(Lf_temp[1]) << endl;
1330  // all test codes removed on 02/09/2007
1331  static fVec pp(6);
1332 #ifndef USE_DCA
1333  // new formulation
1334  pp.add(da6, Lf);
1335  f_final.mul(W, pp);
1336  for(i=0; i<n_dof; i++)
1337  f_final(joint_index[i]) += tau(i);
1338  last_pjoints[0]->f_final.set(f_final);
1339  last_pjoints[1]->f_final.neg(f_final);
1340  v.mul(IW, pp);
1341  for(i=0; i<n_dof; i++)
1342  {
1343  acc_final(i) = v(joint_index[i]);
1344  }
1345  last_joint->joint_f.set(fVec3(f_final(0), f_final(1), f_final(2)));
1346  last_joint->joint_n.set(fVec3(f_final(3), f_final(4), f_final(5)));
1347 #else // #ifndef USE_DCA (DCA test)
1348  static fVec vp(6), svp;
1349  svp.resize(n_dof);
1350  pp.set(da6);
1351  pp += Lf;
1352  vp.mul(Vhat, pp);
1353  for(i=0; i<n_dof; i++)
1354  {
1355  svp(i) = tau(i) + vp(joint_index[i]);
1356  }
1357  acc_final.lineq_posv(SVS, svp);
1358 // cerr << "SVS = " << SVS << endl;
1359 // cerr << "svp = " << tran(svp) << endl;
1360 // cerr << "acc_final = " << tran(acc_final) << endl;
1361  v.zero();
1362  switch(last_joint->j_type)
1363  {
1364  case JROTATE:
1365  case JSLIDE:
1366  v(axis) = acc_final(0);
1367  break;
1368  case JSPHERE:
1369  v(3) = acc_final(0);
1370  v(4) = acc_final(1);
1371  v(5) = acc_final(2);
1372  break;
1373  case JFREE:
1374  v.set(acc_final);
1375  break;
1376  }
1377  pp -= v;
1378  f_final.mul(Vhat, pp);
1379  last_pjoints[0]->f_final.neg(f_final);
1380  last_pjoints[1]->f_final.set(f_final);
1381 #endif
1382 #ifndef USE_MPI
1383  if(last_joint->t_given) {
1384  switch(last_joint->j_type) {
1385  case JROTATE:
1386  case JSLIDE:
1387  last_joint->SetJointAcc(v(axis));
1388  // cerr << last_joint->name << ": " << v(axis) << endl;
1389  break;
1390  case JSPHERE:
1391  last_joint->SetJointAcc(v(3), v(4), v(5));
1392  break;
1393  case JFREE:
1394  last_joint->SetJointAcc(v(0), v(1), v(2), v(3), v(4), v(5));
1395 #ifdef VERBOSE
1396  update_log << last_joint->name << ": " << tran(v) << endl;
1397 #endif
1398  break;
1399  default:
1400  break;
1401  }
1402  }
1403 #endif
1404 #ifdef USE_MPI
1405  if(children[0] && sim->rank != children[0]->rank)
1406  {
1407 #ifdef TIMING_CHECK
1408  cerr << "[" << sim->rank << "] " << last_joint->name << ": send force to " << children[0]->last_joint->name << " [" << children[0]->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
1409 #endif
1410  send_force(children[0]->rank);
1411  }
1412  if(children[1] && children[0] != children[1] && sim->rank != children[1]->rank)
1413  {
1414 #ifdef TIMING_CHECK
1415  cerr << "[" << sim->rank << "] " << last_joint->name << ": send force to " << children[1]->last_joint->name << " [" << children[1]->rank << "] t = " << MPI_Wtime()-update_start_time << endl;
1416 #endif
1417  send_force(children[1]->rank);
1418  }
1419 #endif
1420 }
pJoint::calc_jacobian
void calc_jacobian()
Definition: update.cpp:164
fMat33
3x3 matrix class.
Definition: fMatrix3.h:29
autoplay.c
int c
Definition: autoplay.py:16
pJoint::calc_dvel
void calc_dvel()
Definition: update.cpp:585
fMat::resize
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
pSubChain::col_disassembly
void col_disassembly()
Definition: update.cpp:681
pSubChain::calc_acc_leaf
void calc_acc_leaf()
Definition: update.cpp:952
s1
png_voidp s1
Definition: png.h:2110
fVec3::zero
void zero()
Creates a zero vector.
Definition: fMatrix3.h:283
JointInfo::pjoints
pJoint * pjoints[2]
Definition: psim.h:438
fMat33::set
void set(const fMat33 &mat)
Copies a matrix.
Definition: fMatrix3.cpp:623
fMat33::mul
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
pSubChain::calc_inertia
void calc_inertia()
Definition: update.cpp:199
pSim::update_velocity
void update_velocity()
Definition: update.cpp:768
fMat33::tran
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
Definition: fMatrix3.cpp:607
fVec3::cross
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
size
png_uint_32 size
Definition: png.h:1521
psim.h
Forward dynamics computation based on Assembly-Disassembly Algorithm.
pSubChain::disassembly_body
void disassembly_body()
Definition: update.cpp:1283
pSim::col_disassembly
void col_disassembly()
Definition: update.cpp:676
Chain::CalcVelocity
void CalcVelocity()
Definition: fk.cpp:41
pSubChain::calc_dvel_leaf
void calc_dvel_leaf()
Definition: update.cpp:625
pSubChain::calc_dvel_body
void calc_dvel_body()
Definition: update.cpp:634
Chain::n_joint
int n_joint
Definition: chain.h:469
pSim::Update
int Update()
Compute joint accelerations.
Definition: update.cpp:40
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
pSim::update_collision
void update_collision()
Definition: update.cpp:568
JSPHERE
spherical (3DOF)
Definition: chain.h:43
fMat::mul
void mul(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1217
fMat::lineq
friend fMat lineq(const fMat &A, const fMat &b)
solve linear equation Ax = b
Definition: fMatrix.cpp:137
fVec::set
void set(double *_d)
Sets all elements.
Definition: fMatrix.h:533
fVec::mul
void mul(const fVec &vec, double d)
Definition: fMatrix.cpp:1620
pSim::disassembly
void disassembly()
Definition: update.cpp:1155
pSim::calc_dvel
void calc_dvel()
Definition: update.cpp:574
Chain::n_dof
int n_dof
Definition: chain.h:468
fMat::inv_svd
int inv_svd(const fMat &, int lwork=-1)
inverse
j
def j(str, encoding="cp932")
index
list index
pSim::subchains
pSubChain * subchains
Definition: psim.h:603
fVec::sub
void sub(const fVec &vec1, const fVec &vec2)
Definition: fMatrix.cpp:1583
Joint::abs_att
fMat33 abs_att
absolute orientation
Definition: chain.h:742
JointInfo::plink
pLink * plink
Definition: psim.h:439
a
string a
fMat33::cross
void cross(const fVec3 &p)
Sets spectial matrices.
Definition: fMatrix3.cpp:217
Chain::do_connect
int do_connect
true after Connect() was called; application (or subclass) must reset the flag
Definition: chain.h:476
Joint::name
char * name
joint name (including the character name)
Definition: chain.h:691
Chain::Root
Joint * Root()
Definition: chain.h:151
pJoint::calc_jdot
void calc_jdot()
Definition: update.cpp:802
fVec::lineq_posv
int lineq_posv(const fMat &A, const fVec &b)
Definition: fMatrix.cpp:765
pSubChain::calc_inertia_leaf
void calc_inertia_leaf()
Definition: update.cpp:268
fVec::resize
void resize(int i)
Change the size.
Definition: fMatrix.h:511
pSubChain::disassembly_leaf
void disassembly_leaf()
Definition: update.cpp:1224
fVec
Vector of generic size.
Definition: fMatrix.h:491
buf
png_bytep buf
Definition: png.h:2729
pSubChain::calc_inertia_body
void calc_inertia_body()
Definition: update.cpp:313
JSLIDE
prismatic (1DOF)
Definition: chain.h:42
s2
png_voidp png_voidp s2
Definition: png.h:2110
tran
fMat tran(const fMat &mat)
Definition: fMatrix.cpp:1013
pSubChain::col_disassembly_body
void col_disassembly_body()
Definition: update.cpp:693
fVec::zero
void zero()
Creates a zero vector.
Definition: fMatrix.h:575
sim
static pSim * sim
Definition: schedule.cpp:23
fVec3::sub
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
fMat::tran
friend fMat tran(const fMat &mat)
Returns the transpose of a matrix.
Definition: fMatrix.cpp:1013
fMat::set
void set(double *_d)
Sets the values from an array.
Definition: fMatrix.h:187
fVec3
3-element vector class.
Definition: fMatrix3.h:206
fMat::symmetric
void symmetric(char t= 'U')
Change to symmetric matrix.
Definition: fMatrix.cpp:88
fVec::length
friend double length(const fVec &v)
Length of the vector.
Definition: fMatrix.h:549
JROTATE
rotational (1DOF)
Definition: chain.h:41
Joint::abs_pos
fVec3 abs_pos
absolute position
Definition: chain.h:741
org
org
fVec3::mul
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
pSubChain::calc_dvel
void calc_dvel()
Definition: update.cpp:609
pSubChain::calc_acc_body
void calc_acc_body()
Definition: update.cpp:982
Joint
The class for representing a joint.
Definition: chain.h:538
pSim::joint_info
JointInfo * joint_info
Definition: psim.h:602
fVec::add
void add(const fVec &vec1, const fVec &vec2)
Definition: fMatrix.cpp:1562
pSubChain::calc_acc
void calc_acc()
Definition: update.cpp:887
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
pSim::update_position
void update_position()
Definition: update.cpp:146
pSubChain::disassembly
void disassembly()
Definition: update.cpp:1160

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