update_lcp.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_lcp.cpp
11  * Create: Katsu Yamane, 04.02.25
12  */
13 
14 #include "psim.h"
15 #include <limits>
16 
17 #define USE_INITIAL_GUESS
18 
19 #include <sdcontact.h>
20 #include <lcp.h>
21 //#include <QuadProg.h>
22 
23 #ifdef VERBOSE
24 #include <fstream>
25 extern std::ofstream update_log;
26 #endif
27 
28 int pSim::Update(double timestep, std::vector<SDContactPair*>& sdContactPairs)
29 {
30  ClearExtForce();
31  int n_pairs = sdContactPairs.size();
32  if(n_pairs == 0)
33  {
34  return pSim::Update();
35  }
36 #ifdef VERBOSE
37  update_log << "Update(" << timestep << ")" << endl;
38 #endif
39  int n_total_points = 0;
40 #ifdef VERBOSE
41  update_log << "num pairs = " << n_pairs << endl;
42 #endif
43  in_create_chain = true;
44  clear_contact();
45  // add spherical joints to the links in contact
46  for(int i=0; i<n_pairs; i++)
47  {
48  SDContactPair* sd_pair = sdContactPairs[i];
49  Joint* pjoint = sd_pair->GetJoint(0);
50  Joint* rjoint = sd_pair->GetJoint(1);
51  int n_points = sd_pair->NumPoints();
52  if(n_points == 0) continue;
53 #ifdef VERBOSE
54  update_log << "== [" << pjoint->name << ", " << rjoint->name << "]: contact points = " << n_points << endl;
55 #endif
56  // velocity of rjoint in pjoint frame
57  static fVec3 rjoint_lin_vel, rjoint_ang_vel, rjoint_pos, temp;
58  static fMat33 t_p_att, rjoint_att, t_rjoint_att;
59  t_p_att.tran(pjoint->abs_att);
60  temp.sub(rjoint->abs_pos, pjoint->abs_pos);
61  rjoint_pos.mul(t_p_att, temp);
62  rjoint_att.mul(t_p_att, rjoint->abs_att);
63  rjoint_lin_vel.mul(rjoint_att, rjoint->loc_lin_vel);
64  rjoint_ang_vel.mul(rjoint_att, rjoint->loc_ang_vel);
65  t_rjoint_att.tran(rjoint_att);
66 #ifdef VERBOSE
67  update_log << rjoint->name << ": rel_lin_vel = " << rjoint->rel_lin_vel << endl;
68  update_log << rjoint->name << ": rel_ang_vel = " << rjoint->rel_ang_vel << endl;
69 // update_log << "rjoint_ang_vel = " << rjoint_ang_vel << endl;
70 #endif
71  std::vector<fVec3> pair_positions;
72  std::vector<fVec3> pair_normals;
73  std::vector<fVec3> pair_relvels;
74  std::vector<fVec3> pair_positions_real;
75  std::vector<fMat33> pair_orientations;
76  std::vector<fMat33> pair_orientations_real;
77  std::vector<fVec3> pair_jdot;
78  std::vector<fVec3> pair_jdot_real;
79  for(int m=0; m<n_points; m++)
80  {
81 #ifdef VERBOSE
82  update_log << "- contact point " << m << endl;
83 #endif
84  // location: contact point 2
85  // z: normal
86  // x: relative slip velocity (arbitrary if no slip)
87 
88  static fVec3 temp;
89  static fVec3 relpos1, relpos2, relnorm;
90  double depth;
91  relpos1(0) = sd_pair->Coord(m)(0);
92  relpos1(1) = sd_pair->Coord(m)(1);
93  relpos1(2) = sd_pair->Coord(m)(2);
94  relpos2.set(relpos1);
95  relnorm(0) = sd_pair->Normal(m)(0);
96  relnorm(1) = sd_pair->Normal(m)(1);
97  relnorm(2) = sd_pair->Normal(m)(2);
98  relnorm.unit();
99  depth = sd_pair->Depth(m);
100 #ifdef VERBOSE
101  update_log << "p1 = " << relpos1 << ", p2 = " << relpos2 << ", norm = " << relnorm << ", depth = " << depth << endl;
102 #endif
103  // relative velocity at contact point 2
104  static fVec3 vel1, vel2, relvel, slipvel;
105  vel1.cross(pjoint->loc_ang_vel, relpos2);
106  vel1 += pjoint->loc_lin_vel;
107  vel2.cross(rjoint_ang_vel, relpos2-rjoint_pos);
108  vel2 += rjoint_lin_vel;
109 #ifdef VERBOSE
110  update_log << "vel1 = " << vel1 << endl;
111  update_log << "vel2 = " << vel2 << endl;
112 #endif
113  relvel.sub(vel2, vel1);
114  slipvel.set(relvel);
115  slipvel -= (relnorm*relvel)*relnorm;
116 #ifdef VERBOSE
117  update_log << "relvel = " << relvel << endl;
118 #endif
119 // if(relvel*relnorm < 0.0)
120  {
121  relvel -= (0.005*depth/timestep)*relnorm;
122 // relvel -= (depth/timestep)*relnorm;
123  n_total_points++;
124  // relative orientation in joint 0 frame
125  double abs_slipvel = slipvel.length();
126  static fVec3 x, y, z;
127  static fMat33 ratt;
128  z.set(relnorm);
129  if(abs_slipvel > 1e-6)
130  {
131  slipvel.unit();
132  x.set(slipvel);
133  }
134  else
135  {
136  fVec3 tmp(1,0,0);
137  slipvel.set(tmp);
138  slipvel -= (relnorm*tmp)*relnorm;
139  abs_slipvel = slipvel.length();
140  if(abs_slipvel > 1e-6)
141  {
142  slipvel.unit();
143  x.set(slipvel);
144  }
145  else
146  {
147  tmp(0) = 0.0; tmp(1) = 1.0; tmp(2) = 0.0;
148  slipvel.set(tmp);
149  slipvel -= (relnorm*tmp)*relnorm;
150  abs_slipvel = slipvel.length();
151  slipvel.unit();
152  x.set(slipvel);
153  }
154  }
155  y.cross(z, x);
156  ratt(0,0) = x(0); ratt(0,1) = y(0); ratt(0,2) = z(0);
157  ratt(1,0) = x(1); ratt(1,1) = y(1); ratt(1,2) = z(1);
158  ratt(2,0) = x(2); ratt(2,1) = y(2); ratt(2,2) = z(2);
159  // position/orientation in rjoint frame
160  static fVec3 relpos2_r;
161  static fMat33 ratt_r;
162  temp.sub(relpos2, rjoint_pos);
163  relpos2_r.mul(t_rjoint_att, temp);
164  ratt_r.mul(t_rjoint_att, ratt);
165  pair_positions.push_back(relpos2);
166  pair_normals.push_back(relnorm);
167  pair_relvels.push_back(relvel);
168 #ifdef VERBOSE
169 // update_log << "ratt = " << ratt << endl;
170 #endif
171  pair_orientations.push_back(ratt);
172  pair_positions_real.push_back(relpos2_r);
173  pair_orientations_real.push_back(ratt_r);
174  static fVec3 omega_x_p, omega_x_omega_x_p, jdot;
175  omega_x_p.cross(pjoint->rel_ang_vel, relpos2);
176  omega_x_omega_x_p.cross(pjoint->rel_ang_vel, omega_x_p);
177  jdot.mul(omega_x_omega_x_p, ratt);
178  pair_jdot.push_back(jdot);
179  omega_x_p.cross(rjoint_ang_vel, relpos2);
180  omega_x_omega_x_p.cross(rjoint_ang_vel, omega_x_p);
181  jdot.mul(omega_x_omega_x_p, ratt);
182  pair_jdot_real.push_back(jdot);
183  }
184  }
185  // leave only the outmost points and remove others
186  int n_valid_points = pair_relvels.size();
187  if(n_valid_points == 0) continue;
188  static char jname[256];
189  sprintf(jname, "%s_%s", pjoint->name, rjoint->name);
190  Joint* main_vjoint = new Joint(jname, JFREE);
191  main_vjoint->real = rjoint;
192  // pos/ori of pjoint in rjoint frame
193  static fVec3 pjoint_pos;
194  static fMat33 pjoint_att;
195  pjoint_att.tran(rjoint_att);
196  pjoint_pos.mul(pjoint_att, rjoint_pos);
197  pjoint_pos *= -1.0;
198  main_vjoint->rpos_real.set(pjoint_pos);
199  main_vjoint->ratt_real.set(pjoint_att);
200  AddJoint(main_vjoint, pjoint);
201  contact_vjoints.push_back(main_vjoint);
202  std::vector<int> add;
203  add.resize(n_valid_points);
204  double min_distance = 1.0e-3;
205  for(int m=0; m<n_valid_points; m++)
206  {
207  add[m] = true;
208  fVec3& posm = pair_positions[m];
209  for(int n=0; n<m; n++)
210  {
211  if(!add[n]) continue;
212  fVec3& posn = pair_positions[n];
213  static fVec3 pp;
214  pp.sub(posn, posm);
215  double len = pp.length();
216  // very close: leave the first one
217  if(len < min_distance)
218  {
219 #ifdef VERBOSE
220  update_log << " " << m << ": too close: " << len << endl;
221 #endif
222  add[m] = false;
223  break;
224  }
225  }
226  }
227  for(int m=0; m<n_valid_points; m++)
228  {
229  if(!add[m]) continue;
230  fVec3& posm = pair_positions[m];
231  static fVec3 first_vec;
232  double min_angle=0.0, max_angle=0.0;
233  int have_first_vec = false;
234  for(int n=0; n<n_valid_points; n++)
235  {
236  if(n == m || !add[n]) continue;
237  fVec3& posn = pair_positions[n];
238  fVec3& norm = pair_normals[n];
239  static fVec3 pp;
240  pp.sub(posn, posm);
241  double len = pp.length();
242  pp /= len;
243  if(!have_first_vec)
244  {
245  first_vec.set(pp);
246  have_first_vec = true;
247  min_angle = 0.0;
248  max_angle = 0.0;
249  continue;
250  }
251  // compute angle
252  static fVec3 fcp;
253  double cs = first_vec * pp;
254  fcp.cross(first_vec, pp);
255  double ss = fcp.length();
256  if(fcp * norm < 0.0) ss *= -1.0;
257  double angle = atan2(ss, cs);
258  if(angle < min_angle) min_angle = angle;
259  if(angle > max_angle) max_angle = angle;
260  if(max_angle - min_angle >= PI)
261  {
262 #ifdef VERBOSE
263  update_log << " inside: " << max_angle << ", " << min_angle << endl;
264 #endif
265  add[m] = false;
266  break;
267  }
268  }
269  }
270  for(int m=0; m<n_valid_points; m++)
271  {
272 #ifdef VERBOSE
273  update_log << "check[" << m << "]: pos = " << pair_positions[m] << ", add = " << add[m] << endl;
274 #endif
275  if(add[m])
276  {
277  static fVec3 loc_relvel;
278  all_vjoints.push_back(main_vjoint);
279  loc_relvel.mul(pair_relvels[m], pair_orientations[m]);
280  contact_relvels.push_back(loc_relvel);
281  fric_coefs.push_back(sd_pair->SlipFric());
282  // compute Jacobian
283  static fMat Jv(3,6), Jr(3,6);
284  static fMat33 Pcross, RtPx;
285  fVec3& rpos = pair_positions[m];
286  static fMat33 t_ratt;
287  t_ratt.tran(pair_orientations[m]);
288  Pcross.cross(rpos);
289  RtPx.mul(t_ratt, Pcross);
290  Jv(0,0) = t_ratt(0,0);
291  Jv(0,1) = t_ratt(0,1);
292  Jv(0,2) = t_ratt(0,2);
293  Jv(1,0) = t_ratt(1,0);
294  Jv(1,1) = t_ratt(1,1);
295  Jv(1,2) = t_ratt(1,2);
296  Jv(2,0) = t_ratt(2,0);
297  Jv(2,1) = t_ratt(2,1);
298  Jv(2,2) = t_ratt(2,2);
299  Jv(0,3) = -RtPx(0,0);
300  Jv(0,4) = -RtPx(0,1);
301  Jv(0,5) = -RtPx(0,2);
302  Jv(1,3) = -RtPx(1,0);
303  Jv(1,4) = -RtPx(1,1);
304  Jv(1,5) = -RtPx(1,2);
305  Jv(2,3) = -RtPx(2,0);
306  Jv(2,4) = -RtPx(2,1);
307  Jv(2,5) = -RtPx(2,2);
308 #ifdef VERBOSE
309 // update_log << "rpos_v = " << rpos << endl;
310 // update_log << "t_ratt_v = " << t_ratt << endl;
311 // update_log << "Jv = " << Jv << endl;
312 #endif
313  rpos = pair_positions_real[m];
314  t_ratt.tran(pair_orientations_real[m]);
315  Pcross.cross(rpos);
316  RtPx.mul(t_ratt, Pcross);
317  Jr(0,0) = t_ratt(0,0);
318  Jr(0,1) = t_ratt(0,1);
319  Jr(0,2) = t_ratt(0,2);
320  Jr(1,0) = t_ratt(1,0);
321  Jr(1,1) = t_ratt(1,1);
322  Jr(1,2) = t_ratt(1,2);
323  Jr(2,0) = t_ratt(2,0);
324  Jr(2,1) = t_ratt(2,1);
325  Jr(2,2) = t_ratt(2,2);
326  Jr(0,3) = -RtPx(0,0);
327  Jr(0,4) = -RtPx(0,1);
328  Jr(0,5) = -RtPx(0,2);
329  Jr(1,3) = -RtPx(1,0);
330  Jr(1,4) = -RtPx(1,1);
331  Jr(1,5) = -RtPx(1,2);
332  Jr(2,3) = -RtPx(2,0);
333  Jr(2,4) = -RtPx(2,1);
334  Jr(2,5) = -RtPx(2,2);
335 #ifdef VERBOSE
336 // update_log << "rpos_r = " << rpos << endl;
337 // update_log << "t_ratt_r = " << t_ratt << endl;
338 // update_log << "Jr = " << Jr << endl;
339 #endif
340  all_Jv.push_back(Jv);
341  all_Jr.push_back(Jr);
342  all_jdot_v.push_back(pair_jdot[m]);
343  all_jdot_r.push_back(pair_jdot_real[m]);
344  }
345  }
346  }
347  init_contact();
348  in_create_chain = false;
349 // Schedule();
351  if(n_total_points == 0)
352  {
353  pSim::Update();
354  return 0;
355  }
356  //
357 // DumpSchedule(update_log);
358  CalcPosition();
359  CalcVelocity();
360  update_position();
361  update_velocity();
362 
363  // compute contact force
364  subchains->calc_contact_force(timestep);
365 
366  //
367  disassembly();
368 #ifdef USE_MPI
369  // scatter results
370  scatter_acc();
371 #endif
372  // clear f_final; use ext_force and ext_moment for the next steps
373  // (e.g. in Runge-Kutta)
374  subchains->clear_f_final();
375  return 0;
376 }
377 
378 void pSubChain::clear_f_final()
379 {
380  for(int i=0; i<n_outer_joints; i++)
381  {
382  outer_joints[i]->f_final.zero();
383  }
384 }
385 
386 int pSubChain::calc_contact_force(double timestep)
387 {
388  assert(n_outer_joints/2 == sim->contact_vjoints.size());
389  int n_contacts = sim->all_vjoints.size();
390  int n_contacts_3 = 3*n_contacts;
391  int n_coefs = N_FRIC_CONE_DIV+1;
392  fMat Phi_hat(n_contacts_3, n_contacts_3);
393  fVec bias_hat(n_contacts_3);
394  Phi_hat.zero();
395  bias_hat.zero();
396  std::vector<fVec3>& rvels = sim->contact_relvels;
397  for(int i=0; i<n_contacts; i++)
398  {
399  int index = 3*i;
400  static fVec3 v;
401  v.set(rvels[i]);
402  bias_hat(index) = v(0);
403  bias_hat(index+1) = v(1);
404  bias_hat(index+2) = v(2);
405  }
406 // update_log << "bias_hat(0) = " << bias_hat << endl;
407  std::vector<int> r_index;
408  std::vector<int> v_index;
409  r_index.resize(n_contacts);
410  v_index.resize(n_contacts);
411  for(int i=0; i<n_contacts; i++)
412  {
413  Joint* vjoint = sim->all_vjoints[i];
414  for(int j=0; j<n_outer_joints; j++)
415  {
416  if(outer_joints[j] == sim->joint_info[vjoint->i_joint].pjoints[0])
417  {
418  r_index[i] = j;
419  }
420  if(outer_joints[j] == sim->joint_info[vjoint->i_joint].pjoints[1])
421  {
422  v_index[i] = j;
423  }
424  }
425 // update_log << "contact[" << i << "]: r_index = " << r_index[i] << ", v_index = " << v_index[i] << endl;
426  }
427  for(int i=0; i<n_contacts; i++)
428  {
429  int i3 = 3*i;
430  // bias_hat
431  static fVec dacc_r(3), dacc_v(3);
432 // dacc_r.mul(sim->all_Jr[i], acc_temp[r_index[i]]);
433  dacc_r.mul(sim->all_Jv[i], acc_temp[r_index[i]]);
434  dacc_v.mul(sim->all_Jv[i], acc_temp[v_index[i]]);
435 #if 1
436  dacc_r(0) += sim->all_jdot_r[i](0);
437  dacc_r(1) += sim->all_jdot_r[i](1);
438  dacc_r(2) += sim->all_jdot_r[i](2);
439  dacc_v(0) += sim->all_jdot_v[i](0);
440  dacc_v(1) += sim->all_jdot_v[i](1);
441  dacc_v(2) += sim->all_jdot_v[i](2);
442 #endif
443  dacc_r -= dacc_v;
444  dacc_r *= timestep;
445  bias_hat(i3) += dacc_r(0);
446  bias_hat(i3+1) += dacc_r(1);
447  bias_hat(i3+2) += dacc_r(2);
448 #ifdef VERBOSE
449 // update_log << "contact[" << i << "]: bias_hat = [" << bias_hat(i3) << " " << bias_hat(i3+1) << " " << bias_hat(i3+2) << "]" << endl;
450 #endif
451  // Phi_hat
452  for(int j=0; j<n_contacts; j++)
453  {
454  int j3 = 3*j;
455  static fMat PJ(6,3), JPJ(3,3), L(3,3);
456  L.zero();
457  PJ.mul(Lambda[r_index[i]][r_index[j]], tran(sim->all_Jv[j]));
458  JPJ.mul(sim->all_Jv[i], PJ);
459 // update_log << "Lambda[" << r_index[i] << "][" << r_index[j] << "] = " << Lambda[r_index[i]][r_index[j]] << endl;
460 // update_log << "Lambda[" << r_index[i] << "][" << v_index[j] << "] = " << Lambda[r_index[i]][v_index[j]] << endl;
461 // update_log << "Lambda[" << v_index[i] << "][" << r_index[j] << "] = " << Lambda[v_index[i]][r_index[j]] << endl;
462 // update_log << "Lambda[" << v_index[i] << "][" << v_index[j] << "] = " << Lambda[v_index[i]][v_index[j]] << endl;
463 // update_log << "JPJ = " << JPJ << endl;
464  L += JPJ;
465  PJ.mul(Lambda[v_index[i]][r_index[j]], tran(sim->all_Jv[j]));
466  JPJ.mul(sim->all_Jv[i], PJ);
467  L -= JPJ;
468  PJ.mul(Lambda[r_index[i]][v_index[j]], tran(sim->all_Jv[j]));
469  JPJ.mul(sim->all_Jv[i], PJ);
470  L -= JPJ;
471  PJ.mul(Lambda[v_index[i]][v_index[j]], tran(sim->all_Jv[j]));
472  JPJ.mul(sim->all_Jv[i], PJ);
473  L += JPJ;
474 #ifdef VERBOSE
475 // update_log << "Phi_hat[" << i << "][" << j << "] = " << L << endl;
476 #endif
477  Phi_hat.set_submat(i3, j3, L);
478 #if 0
479  static fMat Lsub(3,3);
480  Lsub.get_submat(i3, j3, Phi_hat);
481  update_log << "Phi_hat[" << i << "][" << j << "] = " << Lsub << endl;
482 #if 1
483  Lsub.get_submat(0, 0, Lambda[r_index[i]][r_index[j]]);
484  update_log << "L[" << r_index[i] << "][" << r_index[j] << "] = " << Lsub << endl;
485  Lsub.get_submat(0, 0, Lambda[v_index[i]][r_index[j]]);
486  update_log << "L[" << v_index[i] << "][" << r_index[j] << "] = " << Lsub << endl;
487  Lsub.get_submat(0, 0, Lambda[r_index[i]][v_index[j]]);
488  update_log << "L[" << r_index[i] << "][" << v_index[j] << "] = " << Lsub << endl;
489  Lsub.get_submat(0, 0, Lambda[v_index[i]][v_index[j]]);
490  update_log << "L[" << v_index[i] << "][" << v_index[j] << "] = " << Lsub << endl;
491 #endif
492 #endif
493  }
494  }
495 #ifdef VERBOSE
496 // update_log << "Phi_hat = " << Phi_hat << endl;
497 // update_log << "bias_hat = " << bias_hat << endl;
498 #endif
499  Phi_hat *= timestep;
500  // LCP formulation
501  int n_total_coef = n_coefs*n_contacts + n_contacts;
502  fMat N(n_total_coef, n_total_coef);
503  fMat Ck(3, n_coefs);
504  fMat E(n_coefs, 1);
505  fMat* Ehat_t = new fMat [n_contacts];
506  fVec r(n_total_coef);
507  // Ck, E
508  Ck.zero();
509  Ck(2, 0) = 1.0;
510  E.zero();
511  for(int m=0; m<N_FRIC_CONE_DIV; m++)
512  {
513  Ck(0, m+1) = sim->cone_dir[m](0);
514  Ck(1, m+1) = sim->cone_dir[m](1);
515  E(m+1, 0) = 1.0;
516  }
517  for(int i=0; i<n_contacts; i++)
518  {
519  double fric_coef = sim->fric_coefs[i];
520  Ehat_t[i].resize(1, n_coefs);
521  Ehat_t[i](0,0) = fric_coef;
522  for(int m=0; m<N_FRIC_CONE_DIV; m++)
523  {
524  Ehat_t[i](0, m+1) = -1.0;
525  }
526  }
527  N.zero();
528  r.zero();
529  for(int i=0; i<n_contacts; i++)
530  {
531  static fMat N_ij(n_coefs,n_coefs);
532  static fVec r_i(n_coefs);
533  int i3 = i*3, iN = i*n_coefs;
534  r_i.zero();
535  for(int j=0; j<n_contacts; j++)
536  {
537  static fMat Phi_ij(3,3), CP(n_coefs,3);
538  static fMat B_ij(n_coefs,n_coefs), CinvP(n_coefs, 3);
539  // N (CPhiC)
540  int j3 = j*3, jN = j*(n_coefs);
541  Phi_ij.get_submat(i3, j3, Phi_hat);
542  CP.mul(tran(Ck), Phi_ij);
543  N_ij.mul(CP, Ck);
544  N.set_submat(iN, jN, N_ij);
545  }
546  // r
547  static fVec bias_i(3);
548  bias_i.get_subvec(i3, bias_hat);
549  r_i.mul(bias_i, Ck);
550  r.set_subvec(iN, r_i);
551  // E
552  N.set_submat(iN, n_coefs*n_contacts+i, E);
553  N.set_submat(n_coefs*n_contacts+i, iN, Ehat_t[i]);
554  }
555  // LCP parameters
556  double max_error = 1.0e-3;
557 // int max_iteration = n_total_coef * 10;
558  int max_iteration = 10000;
559  // presolve
560  std::vector<int> presolve_g2w;
561  int presolve_failed = false;
562  fVec presolve_g2;
563  presolve_g2w.resize(n_contacts);
564  for(int i=0; i<n_contacts; i++)
565  {
566  presolve_g2w[i] = -1;
567  }
568  {
569  fMat Ns(n_contacts, n_contacts);
570  fVec rs(n_contacts), as;
571  int n_iteration = 0;
572  for(int i=0; i<n_contacts; i++)
573  {
574  rs(i) = bias_hat(i*3+2);
575  for(int j=0; j<n_contacts; j++)
576  {
577  Ns(i, j) = Phi_hat(i*3+2, j*3+2);
578  }
579  }
580  LCP lcp_s(Ns, rs);
581  presolve_failed = lcp_s.SolvePivot2(presolve_g2, as, max_error, max_iteration, &n_iteration, presolve_g2w);
582  if(presolve_failed)
583  {
584  cerr << "presolve_failed (" << n_iteration << "/" << max_iteration << ")" << endl;
585  cerr << "Phi_hat = " << Phi_hat << endl;
586  }
587 #ifdef VERBOSE
588  for(int i=0; i<n_contacts; i++)
589  {
590  update_log << "presolve_g2w[" << i << "] = " << presolve_g2w[i] << endl;
591  }
592 #endif
593 #ifdef MEASURE_COLLISION_TIME
594  sim->n_total_nodes += n_iteration;
595 #endif
596  }
597 #ifdef USE_INITIAL_GUESS
598  std::vector<int> w2a, w2g, z2a, z2g;
599 #endif
600  // search for appropriate set of constraints
601  std::vector<int> active2all;
602  fVec* fk = new fVec [n_contacts];
603  fVec* vk = new fVec [n_contacts];
604  fMat N_active;
605  fVec r_active;
606 #if 1
607  if(!presolve_failed)
608  {
609  for(int i=0; i<n_contacts; i++)
610  {
611  if(presolve_g2w[i] >= 0)
612  {
613  active2all.push_back(i);
614  }
615  }
616  }
617  else
618 #endif
619  {
620  for(int i=0; i<n_contacts; i++)
621  {
622  active2all.push_back(i);
623  }
624  }
625 #ifdef VERBOSE
626  update_log << "*** start searching constraint set" << endl;
627 #endif
628  int max_global_iteration = 1;
629  int count = 0;
630  while(count < max_global_iteration)
631  {
632  std::vector<int> all2active;
633  std::vector<int> g2w;
634  all2active.resize(n_contacts);
635  for(int i=0; i<n_contacts; i++)
636  {
637  all2active[i] = -1;
638  fk[i].resize(3);
639  fk[i].zero();
640  }
641  int n_active_contacts = active2all.size();
642  if(n_active_contacts > 0)
643  {
644  int n_active_coef = n_active_contacts*n_coefs + n_active_contacts;
645  N_active.resize(n_active_coef, n_active_coef);
646  r_active.resize(n_active_coef);
647  N_active.zero();
648  r_active.zero();
649  for(int i=0; i<n_active_contacts; i++)
650  {
651  all2active[active2all[i]] = i;
652  }
653 #ifdef VERBOSE
654  update_log << "n_active_contacts = " << n_active_contacts << endl;
655  update_log << "n_active_coef = " << n_active_coef << endl;
656 #endif
657  for(int i=0; i<n_active_contacts; i++)
658  {
659  static fMat N_ij(n_coefs,n_coefs);
660  static fVec r_i(n_coefs);
661  int i_active_N = i*n_coefs;
662  int i_all = active2all[i];
663  int i_all_N = i_all*n_coefs;
664  for(int j=0; j<n_active_contacts; j++)
665  {
666  int j_all = active2all[j];
667  int j_all_N = j_all*n_coefs;
668  int j_active_N = j*n_coefs;
669  N_ij.get_submat(i_all_N, j_all_N, N);
670  N_active.set_submat(i_active_N, j_active_N, N_ij);
671  }
672  r_i.get_subvec(i_all_N, r);
673  r_active.set_subvec(i_active_N, r_i);
674  N_active.set_submat(i_active_N, n_coefs*n_active_contacts+i, E);
675  N_active.set_submat(n_coefs*n_active_contacts+i, i_active_N, Ehat_t[i]);
676  }
677 #ifdef USE_INITIAL_GUESS
678  static fMat N_init;
679  static fVec r_init;
680  N_init.resize(n_active_coef, n_active_coef);
681  r_init.resize(n_active_coef);
682  w2a.resize(n_active_coef);
683  w2g.resize(n_active_coef);
684  z2a.resize(n_active_coef);
685  z2g.resize(n_active_coef);
686  for(int i=0; i<n_active_coef; i++)
687  {
688  w2a[i] = i;
689  w2g[i] = -1;
690  z2a[i] = -1;
691  z2g[i] = i;
692  }
693  for(int i=0; i<n_active_contacts; i++)
694  {
695  int idx = i*n_coefs;
696  w2a[idx] = -1;
697  w2g[idx] = idx;
698  z2a[idx] = idx;
699  z2g[idx] = -1;
700  }
701  LCP::Pivot(w2a, w2g, z2a, z2g, N_active, r_active, N_init, r_init);
702  N_active.set(N_init);
703  r_active.set(r_init);
704 #endif
705  LCP lcp(N_active, r_active);
706  // solution
707  static fVec g2, a;
708  int n_iteration = 0;
709  g2w.resize(n_active_coef);
710 #ifdef MEASURE_COLLISION_TIME
711  sim->n_total_contacts += n_contacts;
712  sim->n_active_contacts += n_active_contacts;
713  LongInteger t1 = GetTick();
714 #endif
715 #ifdef USE_NORMAL_LEMKE
716  int failed = lcp.SolvePivot(g2, a, max_error, max_iteration, &n_iteration, g2w);
717 #else
718  int failed = lcp.SolvePivot2(g2, a, max_error, max_iteration, &n_iteration, g2w);
719 #endif
720  count++;
721 #ifdef MEASURE_COLLISION_TIME
722  LongInteger t2 = GetTick();
723  sim->lcp_solve_time += ExpiredTime(t1, t2);
724  sim->n_total_nodes += n_iteration;
725  sim->n_loops += lcp.NumLoops();
726  sim->n_errors += lcp.NumErrors();
727 #endif
728 #ifdef VERBOSE
729  update_log << "n_iteration = " << n_iteration << endl;
730 #endif
731  fVec error(n_active_coef);
732  error.mul(N_active, g2);
733  error += r_active;
734  error -= a;
735 #ifdef VERBOSE
736  update_log << "error = " << error.length() << endl;
737 #endif
738  if(error.length() > 1e-4) failed = true;
739  if(failed)
740  {
741  cerr << "failed (" << n_iteration << "/" << max_iteration << "): n_contacts = " << n_contacts << endl;
742 #ifdef MEASURE_COLLISION_TIME
743  sim->n_failure_frames++;
744 #endif
745 #ifdef VERBOSE
746  update_log << "failed" << endl;
747 #endif
748  if(!presolve_failed)
749  {
750  cerr << "using presolve result" << endl;
751  for(int i=0; i<n_contacts; i++)
752  {
753  fk[i](2) = presolve_g2(i);
754  }
755  }
756  break;
757  }
758  else
759  {
760 #ifdef VERBOSE
761  update_log << "success" << endl;
762  update_log << "g2 = " << tran(g2) << endl;
763  update_log << "a = " << tran(a) << endl;
764  update_log << "g2w = " << endl;
765  for(int i=0; i<n_active_contacts; i++)
766  {
767  update_log << "active contact[" << i << "] a = " << flush;
768  for(int j=0; j<n_coefs; j++)
769  {
770  update_log << g2w[i*n_coefs+j] << " " << flush;
771  }
772  update_log << endl;
773  }
774  for(int i=0; i<n_active_contacts; i++)
775  {
776  update_log << "active contact[" << i << "] lambda = " << g2w[n_active_contacts*n_coefs+i] << endl;
777  }
778 #endif
779 #ifdef USE_INITIAL_GUESS
780  for(int i=0; i<n_active_coef; i++)
781  {
782  if(w2g[i] >= 0)
783  {
784  g2(i) = a(w2g[i]);
785  }
786  }
787 #endif
788  for(int i=0; i<n_active_contacts; i++)
789  {
790  static fVec g2_i(n_coefs);
791  int iN = i*n_coefs;
792  g2_i.get_subvec(iN, g2);
793  fk[active2all[i]].mul(Ck, g2_i);
794 #ifdef VERBOSE
795  update_log << "fk[" << active2all[i] << "] = " << tran(fk[active2all[i]]) << endl;
796 #endif
797  }
798  }
799  active2all.clear();
800  }
801  // check velocity
802  for(int i=0; i<n_contacts; i++)
803  {
804  vk[i].resize(3);
805  vk[i].zero();
806  }
807  int constraint_modified = false;
808  for(int i=0; i<n_contacts; i++)
809  {
810  static fVec bias_i(3);
811  bias_i.get_subvec(3*i, bias_hat);
812  for(int j=0; j<n_contacts; j++)
813  {
814  static fMat Phi_ij(3,3);
815  Phi_ij.get_submat(3*i, 3*j, Phi_hat);
816  vk[i] += Phi_ij * fk[j];
817  }
818  vk[i] += bias_i;
819 #ifdef VERBOSE
820  update_log << "vk[" << i << "] = " << tran(vk[i]) << endl;
821  if(vk[i](2) < -max_error)
822  {
823  update_log << "--- too small vertical velocity" << endl;
824  }
825 #endif
826 #ifdef USE_INITIAL_GUESS
827  if((all2active[i] >= 0 && w2g[all2active[i]*n_coefs] < 0 && g2w[all2active[i]*n_coefs] >= 0) ||
828  (all2active[i] >= 0 && w2g[all2active[i]*n_coefs] >= 0 && g2w[all2active[i]*n_coefs] < 0) ||
829 #else
830  if((all2active[i] >= 0 && g2w[all2active[i]*n_coefs] >= 0) ||
831 #endif
832  vk[i](2) < -max_error)
833  {
834  active2all.push_back(i);
835 #ifdef VERBOSE
836  update_log << "added to active" << endl;
837 #endif
838  if(vk[i](2) < -max_error)
839  {
840  constraint_modified = true;
841 #ifdef VERBOSE
842  update_log << "constraint modified" << endl;
843 #endif
844  }
845  }
846 #ifdef VERBOSE
847  else
848 #ifdef USE_INITIAL_GUESS
849  if((all2active[i] >= 0 && w2g[all2active[i]*n_coefs] < 0 && g2w[all2active[i]*n_coefs] >= 0) ||
850  (all2active[i] >= 0 && w2g[all2active[i]*n_coefs] >= 0 && g2w[all2active[i]*n_coefs] < 0))
851 #else
852  if(all2active[i] >= 0 && g2w[all2active[i]*n_coefs] >= 0)
853 #endif
854  {
855  update_log << "removed from active" << endl;
856  }
857 #endif
858  }
859  if(!constraint_modified) break;
860  }
861 #ifdef VERBOSE
862  update_log << "end of search" << endl;
863 #endif
864 // cerr << "trials = " << count << endl;
865  // apply the force
866  for(int i=0; i<n_outer_joints; i++)
867  {
868  outer_joints[i]->f_final.zero();
869  }
870  for(int i=0; i<n_contacts; i++)
871  {
872  static fVec3 f, f_virt, f_real, moment;
873  f(0) = fk[i](0);
874  f(1) = fk[i](1);
875  f(2) = fk[i](2);
876  Joint* r_joint = outer_joints[r_index[i]]->joint;
877  Joint* v_joint = outer_joints[v_index[i]]->joint;
878  static fVec f0(6);
879  f0.mul(fk[i], sim->all_Jr[i]);
880  r_joint->real->ext_force(0) += f0(0);
881  r_joint->real->ext_force(1) += f0(1);
882  r_joint->real->ext_force(2) += f0(2);
883  r_joint->real->ext_moment(0) += f0(3);
884  r_joint->real->ext_moment(1) += f0(4);
885  r_joint->real->ext_moment(2) += f0(5);
886  f0.mul(fk[i], sim->all_Jv[i]);
887  v_joint->parent->ext_force(0) -= f0(0);
888  v_joint->parent->ext_force(1) -= f0(1);
889  v_joint->parent->ext_force(2) -= f0(2);
890  v_joint->parent->ext_moment(0) -= f0(3);
891  v_joint->parent->ext_moment(1) -= f0(4);
892  v_joint->parent->ext_moment(2) -= f0(5);
893  outer_joints[r_index[i]]->f_final += f0;
894  outer_joints[v_index[i]]->f_final -= f0;
895 #ifdef VERBOSE
896  update_log << r_joint->real->name << " force/moment: " << r_joint->real->ext_force << r_joint->real->ext_moment << endl;
897  update_log << v_joint->parent->name << " force/moment: " << v_joint->parent->ext_force << v_joint->parent->ext_moment << endl;
898 #endif
899  }
900  delete[] vk;
901  delete[] fk;
902  delete[] Ehat_t;
903  return 0;
904 }
905 
906 
pSim::clear_contact
virtual int clear_contact()
Definition: psim.cpp:123
Joint::loc_lin_vel
fVec3 loc_lin_vel
linear velocity in local frame
Definition: chain.h:743
fMat33
3x3 matrix class.
Definition: fMatrix3.h:29
pSim::cone_dir
fVec3 cone_dir[N_FRIC_CONE_DIV]
Definition: psim.h:587
pSim::contact_vjoints
joint_list contact_vjoints
Definition: psim.h:577
fVec::set_subvec
void set_subvec(int start, const fVec &subV)
Copy a smaller vector as a sub-vector.
Definition: fMatrix.cpp:76
fMat::resize
int resize(int i, int j)
Changes the matrix size.
Definition: fMatrix.h:133
SDContactPair::Coord
const fVec3 & Coord(int index)
Definition: sdcontact.h:83
fVec3::unit
friend fVec3 unit(const fVec3 &v)
Returns the unit vector with the same direction (with length check)
Definition: fMatrix3.h:300
LCP::NumErrors
int NumErrors()
Definition: lcp_pivot.cpp:558
x
* x
Definition: IceUtils.h:98
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
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
Chain::in_create_chain
int in_create_chain
true if between BeginCreateChain() and EndCreateChain().
Definition: chain.h:473
fVec3::cross
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
Joint::rpos_real
fVec3 rpos_real
relative position in the real joint frame
Definition: chain.h:688
psim.h
Forward dynamics computation based on Assembly-Disassembly Algorithm.
Joint::loc_ang_vel
fVec3 loc_ang_vel
angular velocity in local frame
Definition: chain.h:744
LCP::Pivot
static void Pivot(int idx1, int idx2, const fMat &M, const fVec &q, fMat &M_new, fVec &q_new)
Definition: lcp_pivot.cpp:1321
Chain::CalcVelocity
void CalcVelocity()
Definition: fk.cpp:41
error
void error(char *msg) const
Definition: minigzip.c:87
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
SDContactPair::SlipFric
double SlipFric()
Definition: sdcontact.h:60
SDContactPair::Normal
const fVec3 & Normal(int index)
Definition: sdcontact.h:86
fMat::mul
void mul(const fMat &mat1, const fMat &mat2)
Definition: fMatrix.cpp:1217
fMat::zero
void zero()
Creates a zero matrix.
Definition: fMatrix.h:249
Chain::CalcPosition
void CalcPosition()
Forward kinematics.
Definition: fk.cpp:16
fVec::set
void set(double *_d)
Sets all elements.
Definition: fMatrix.h:533
SDContactPair::NumPoints
int NumPoints()
Definition: sdcontact.h:80
LCP::SolvePivot
int SolvePivot(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve using Lemke&#39;s method.
Definition: lcp_pivot.cpp:698
fVec::mul
void mul(const fVec &vec, double d)
Definition: fMatrix.cpp:1620
pSim::disassembly
void disassembly()
Definition: update.cpp:1155
pSim::all_jdot_r
std::vector< fVec3 > all_jdot_r
Definition: psim.h:585
pSim::init_contact
int init_contact()
Definition: psim.cpp:173
LCP::NumLoops
int NumLoops()
Definition: lcp_pivot.cpp:555
lcp.h
Solves a Linear Complementarity Problem (LCP)
index
list index
pSim::subchains
pSubChain * subchains
Definition: psim.h:603
Joint::rel_ang_vel
fVec3 rel_ang_vel
Definition: chain.h:729
Joint::ext_force
fVec3 ext_force
external force
Definition: chain.h:736
Joint::abs_att
fMat33 abs_att
absolute orientation
Definition: chain.h:742
pSim::contact_relvels
std::vector< fVec3 > contact_relvels
Definition: psim.h:578
fMat::get_submat
void get_submat(int row_start, int col_start, const fMat &allM)
Extract a sub-matrix and set to myself.
Definition: fMatrix.cpp:32
a
string a
fVec::get_subvec
void get_subvec(int start, const fVec &allV)
Copy a sub-vector of a larger vector.
Definition: fMatrix.cpp:65
fMat33::cross
void cross(const fVec3 &p)
Sets spectial matrices.
Definition: fMatrix3.cpp:217
Joint::name
char * name
joint name (including the character name)
Definition: chain.h:691
pSim::all_jdot_v
std::vector< fVec3 > all_jdot_v
Definition: psim.h:584
PI
#define PI
PI.
Definition: IceTypes.h:32
fVec::resize
void resize(int i)
Change the size.
Definition: fMatrix.h:511
fVec
Vector of generic size.
Definition: fMatrix.h:491
Chain::AddJoint
int AddJoint(Joint *target, Joint *p)
Add a new joint target as a child of joint p.
Definition: edit.cpp:117
LCP
Definition: lcp.h:32
test_modelloader.norm
def norm(a)
Definition: test_modelloader.py:56
Joint::ratt_real
fMat33 ratt_real
relative orientation in the real joint frame
Definition: chain.h:689
tran
fMat tran(const fMat &mat)
Definition: fMatrix.cpp:1013
Joint::rel_lin_vel
fVec3 rel_lin_vel
Definition: chain.h:728
pSubChain::calc_contact_force
int calc_contact_force(double timestep)
Definition: update_lcp.cpp:386
fVec::zero
void zero()
Creates a zero vector.
Definition: fMatrix.h:575
sprintf
std::string sprintf(char const *__restrict fmt,...)
sim
static pSim * sim
Definition: schedule.cpp:23
fMat::set_submat
void set_submat(int row_start, int col_start, const fMat &subM)
Sets a sub-matrix of myself.
Definition: fMatrix.cpp:48
pSubChain::clear_f_final
void clear_f_final()
Definition: update_lcp.cpp:378
fVec3::sub
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
Chain::Joint
friend class Joint
Definition: chain.h:146
fVec3::length
friend double length(const fVec3 &v)
Returns the length of a vector.
Definition: fMatrix3.h:293
pSim::fric_coefs
std::vector< double > fric_coefs
Definition: psim.h:579
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
SDContactPair::GetJoint
Joint * GetJoint(int index)
Definition: sdcontact.h:64
pSim::all_vjoints
joint_list all_vjoints
Definition: psim.h:581
LCP::SolvePivot2
int SolvePivot2(fVec &g, fVec &a, double _max_error, int _max_iteration, int *n_iteration, std::vector< int > &_g2w)
Solve by pivot using path planning algorithm.
Definition: lcp_pivot.cpp:562
Joint::ext_moment
fVec3 ext_moment
external moment around the local frame
Definition: chain.h:737
fVec::length
friend double length(const fVec &v)
Length of the vector.
Definition: fMatrix.h:549
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
N_FRIC_CONE_DIV
#define N_FRIC_CONE_DIV
Number of vertices of the friction cone approximation.
Definition: psim.h:33
Chain::ClearExtForce
int ClearExtForce()
Clear the external forces/torques.
Definition: joint.cpp:928
Joint
The class for representing a joint.
Definition: chain.h:538
Joint::real
Joint * real
pointer to the real (connected) joint; for closed chains.
Definition: chain.h:687
Joint::i_joint
int i_joint
index of the joint
Definition: chain.h:722
SDContactPair::Depth
double Depth(int index)
Definition: sdcontact.h:89
y
* y
Definition: IceUtils.h:97
pSim::joint_info
JointInfo * joint_info
Definition: psim.h:602
pSim::all_Jr
std::vector< fMat > all_Jr
Definition: psim.h:583
pSim::all_Jv
std::vector< fMat > all_Jv
Definition: psim.h:582
Joint::parent
Joint * parent
pointer to the parent joint
Definition: chain.h:683
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

openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Sep 8 2022 02:24:05