update_lcp.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00009 /*
00010  * update_lcp.cpp
00011  * Create: Katsu Yamane, 04.02.25
00012  */
00013 
00014 #include "psim.h"
00015 #include <limits>
00016 
00017 #define USE_INITIAL_GUESS
00018 
00019 #include <sdcontact.h>
00020 #include <lcp.h>
00021 //#include <QuadProg.h>
00022 
00023 #ifdef VERBOSE
00024 #include <fstream>
00025 extern std::ofstream update_log;
00026 #endif
00027 
00028 int pSim::Update(double timestep, std::vector<SDContactPair*>& sdContactPairs)
00029 {
00030         ClearExtForce();
00031         int n_pairs = sdContactPairs.size();
00032         if(n_pairs == 0)
00033         {
00034                 return pSim::Update();
00035         }
00036 #ifdef VERBOSE
00037         update_log << "Update(" << timestep << ")" << endl;
00038 #endif
00039         int n_total_points = 0;
00040 #ifdef VERBOSE
00041         update_log << "num pairs = " << n_pairs << endl;
00042 #endif
00043         in_create_chain = true;
00044         clear_contact();
00045         // add spherical joints to the links in contact
00046         for(int i=0; i<n_pairs; i++)
00047         {
00048                 SDContactPair* sd_pair = sdContactPairs[i];
00049                 Joint* pjoint = sd_pair->GetJoint(0);
00050                 Joint* rjoint = sd_pair->GetJoint(1);
00051                 int n_points = sd_pair->NumPoints();
00052                 if(n_points == 0) continue;
00053 #ifdef VERBOSE
00054                 update_log << "== [" << pjoint->name << ", " << rjoint->name << "]: contact points = " << n_points << endl;
00055 #endif
00056                 // velocity of rjoint in pjoint frame
00057                 static fVec3 rjoint_lin_vel, rjoint_ang_vel, rjoint_pos, temp;
00058                 static fMat33 t_p_att, rjoint_att, t_rjoint_att;
00059                 t_p_att.tran(pjoint->abs_att);
00060                 temp.sub(rjoint->abs_pos, pjoint->abs_pos);
00061                 rjoint_pos.mul(t_p_att, temp);
00062                 rjoint_att.mul(t_p_att, rjoint->abs_att);
00063                 rjoint_lin_vel.mul(rjoint_att, rjoint->loc_lin_vel);
00064                 rjoint_ang_vel.mul(rjoint_att, rjoint->loc_ang_vel);
00065                 t_rjoint_att.tran(rjoint_att);
00066 #ifdef VERBOSE
00067                 update_log << rjoint->name << ": rel_lin_vel = " << rjoint->rel_lin_vel << endl;
00068                 update_log << rjoint->name << ": rel_ang_vel = " << rjoint->rel_ang_vel << endl;
00069 //              update_log << "rjoint_ang_vel = " << rjoint_ang_vel << endl;
00070 #endif
00071                 std::vector<fVec3> pair_positions;
00072                 std::vector<fVec3> pair_normals;
00073                 std::vector<fVec3> pair_relvels;
00074                 std::vector<fVec3> pair_positions_real;
00075                 std::vector<fMat33> pair_orientations;
00076                 std::vector<fMat33> pair_orientations_real;
00077                 std::vector<fVec3> pair_jdot;
00078                 std::vector<fVec3> pair_jdot_real;
00079                 for(int m=0; m<n_points; m++)
00080                 {
00081 #ifdef VERBOSE
00082                         update_log << "- contact point " << m << endl;
00083 #endif
00084                         // location: contact point 2
00085                         // z: normal
00086                         // x: relative slip velocity (arbitrary if no slip)
00087 
00088                         static fVec3 temp;
00089                         static fVec3 relpos1, relpos2, relnorm;
00090                         double depth;
00091                         relpos1(0) = sd_pair->Coord(m)(0);
00092                         relpos1(1) = sd_pair->Coord(m)(1);
00093                         relpos1(2) = sd_pair->Coord(m)(2);
00094                         relpos2.set(relpos1);
00095                         relnorm(0) = sd_pair->Normal(m)(0);
00096                         relnorm(1) = sd_pair->Normal(m)(1);
00097                         relnorm(2) = sd_pair->Normal(m)(2);
00098                         relnorm.unit();
00099                         depth = sd_pair->Depth(m);
00100 #ifdef VERBOSE
00101                         update_log << "p1 = " << relpos1 << ", p2 = " << relpos2 << ", norm = " << relnorm << ", depth = " << depth << endl;
00102 #endif
00103                         // relative velocity at contact point 2
00104                         static fVec3 vel1, vel2, relvel, slipvel;
00105                         vel1.cross(pjoint->loc_ang_vel, relpos2);
00106                         vel1 += pjoint->loc_lin_vel;
00107                         vel2.cross(rjoint_ang_vel, relpos2-rjoint_pos);
00108                         vel2 += rjoint_lin_vel;
00109 #ifdef VERBOSE
00110                         update_log << "vel1 = " << vel1 << endl;
00111                         update_log << "vel2 = " << vel2 << endl;
00112 #endif
00113                         relvel.sub(vel2, vel1);
00114                         slipvel.set(relvel);
00115                         slipvel -= (relnorm*relvel)*relnorm;
00116 #ifdef VERBOSE
00117                         update_log << "relvel = " << relvel << endl;
00118 #endif
00119 //                      if(relvel*relnorm < 0.0)
00120                         {
00121                                 relvel -= (0.005*depth/timestep)*relnorm;
00122 //                              relvel -= (depth/timestep)*relnorm;
00123                                 n_total_points++;
00124                                 // relative orientation in joint 0 frame
00125                                 double abs_slipvel = slipvel.length();
00126                                 static fVec3 x, y, z;
00127                                 static fMat33 ratt;
00128                                 z.set(relnorm);
00129                                 if(abs_slipvel > 1e-6)
00130                                 {
00131                                         slipvel.unit();
00132                                         x.set(slipvel);
00133                                 }
00134                                 else
00135                                 {
00136                                         fVec3 tmp(1,0,0);
00137                                         slipvel.set(tmp);
00138                                         slipvel -= (relnorm*tmp)*relnorm;
00139                                         abs_slipvel = slipvel.length();
00140                                         if(abs_slipvel > 1e-6)
00141                                         {
00142                                                 slipvel.unit();
00143                                                 x.set(slipvel);
00144                                         }
00145                                         else
00146                                         {
00147                                                 tmp(0) = 0.0; tmp(1) = 1.0; tmp(2) = 0.0;
00148                                                 slipvel.set(tmp);
00149                                                 slipvel -= (relnorm*tmp)*relnorm;
00150                                                 abs_slipvel = slipvel.length();
00151                                                 slipvel.unit();
00152                                                 x.set(slipvel);
00153                                         }
00154                                 }
00155                                 y.cross(z, x);
00156                                 ratt(0,0) = x(0); ratt(0,1) = y(0); ratt(0,2) = z(0);
00157                                 ratt(1,0) = x(1); ratt(1,1) = y(1); ratt(1,2) = z(1);
00158                                 ratt(2,0) = x(2); ratt(2,1) = y(2); ratt(2,2) = z(2);
00159                                 // position/orientation in rjoint frame
00160                                 static fVec3 relpos2_r;
00161                                 static fMat33 ratt_r;
00162                                 temp.sub(relpos2, rjoint_pos);
00163                                 relpos2_r.mul(t_rjoint_att, temp);
00164                                 ratt_r.mul(t_rjoint_att, ratt);
00165                                 pair_positions.push_back(relpos2);
00166                                 pair_normals.push_back(relnorm);
00167                                 pair_relvels.push_back(relvel);
00168 #ifdef VERBOSE
00169 //                              update_log << "ratt = " << ratt << endl;
00170 #endif
00171                                 pair_orientations.push_back(ratt);
00172                                 pair_positions_real.push_back(relpos2_r);
00173                                 pair_orientations_real.push_back(ratt_r);
00174                                 static fVec3 omega_x_p, omega_x_omega_x_p, jdot;
00175                                 omega_x_p.cross(pjoint->rel_ang_vel, relpos2);
00176                                 omega_x_omega_x_p.cross(pjoint->rel_ang_vel, omega_x_p);
00177                                 jdot.mul(omega_x_omega_x_p, ratt);
00178                                 pair_jdot.push_back(jdot);
00179                                 omega_x_p.cross(rjoint_ang_vel, relpos2);
00180                                 omega_x_omega_x_p.cross(rjoint_ang_vel, omega_x_p);
00181                                 jdot.mul(omega_x_omega_x_p, ratt);
00182                                 pair_jdot_real.push_back(jdot);
00183                         }
00184                 }
00185                 // leave only the outmost points and remove others
00186                 int n_valid_points = pair_relvels.size();
00187                 if(n_valid_points == 0) continue;
00188                 static char jname[256];
00189                 sprintf(jname, "%s_%s", pjoint->name, rjoint->name);
00190                 Joint* main_vjoint = new Joint(jname, JFREE);
00191                 main_vjoint->real = rjoint;
00192                 // pos/ori of pjoint in rjoint frame
00193                 static fVec3 pjoint_pos;
00194                 static fMat33 pjoint_att;
00195                 pjoint_att.tran(rjoint_att);
00196                 pjoint_pos.mul(pjoint_att, rjoint_pos);
00197                 pjoint_pos *= -1.0;
00198                 main_vjoint->rpos_real.set(pjoint_pos);
00199                 main_vjoint->ratt_real.set(pjoint_att);
00200                 AddJoint(main_vjoint, pjoint);
00201                 contact_vjoints.push_back(main_vjoint);
00202                 std::vector<int> add;
00203                 add.resize(n_valid_points);
00204                 double min_distance = 1.0e-3;
00205                 for(int m=0; m<n_valid_points; m++)
00206                 {
00207                         add[m] = true;
00208                         fVec3& posm = pair_positions[m];
00209                         for(int n=0; n<m; n++)
00210                         {
00211                                 if(!add[n]) continue;
00212                                 fVec3& posn = pair_positions[n];
00213                                 static fVec3 pp;
00214                                 pp.sub(posn, posm);
00215                                 double len = pp.length();
00216                                 // very close: leave the first one
00217                                 if(len < min_distance)
00218                                 {
00219 #ifdef VERBOSE
00220                                         update_log << " " << m << ": too close: " << len << endl;
00221 #endif
00222                                         add[m] = false;
00223                                         break;
00224                                 }
00225                         }
00226                 }
00227                 for(int m=0; m<n_valid_points; m++)
00228                 {
00229                         if(!add[m]) continue;
00230                         fVec3& posm = pair_positions[m];
00231                         static fVec3 first_vec;
00232                         double min_angle=0.0, max_angle=0.0;
00233                         int have_first_vec = false;
00234                         for(int n=0; n<n_valid_points; n++)
00235                         {
00236                                 if(n == m || !add[n]) continue;
00237                                 fVec3& posn = pair_positions[n];
00238                                 fVec3& norm = pair_normals[n];
00239                                 static fVec3 pp;
00240                                 pp.sub(posn, posm);
00241                                 double len = pp.length();
00242                                 pp /= len;
00243                                 if(!have_first_vec)
00244                                 {
00245                                         first_vec.set(pp);
00246                                         have_first_vec = true;
00247                                         min_angle = 0.0;
00248                                         max_angle = 0.0;
00249                                         continue;
00250                                 }
00251                                 // compute angle
00252                                 static fVec3 fcp;
00253                                 double cs = first_vec * pp;
00254                                 fcp.cross(first_vec, pp);
00255                                 double ss = fcp.length();
00256                                 if(fcp * norm < 0.0) ss *= -1.0;
00257                                 double angle = atan2(ss, cs);
00258                                 if(angle < min_angle) min_angle = angle;
00259                                 if(angle > max_angle) max_angle = angle;
00260                                 if(max_angle - min_angle >= PI)
00261                                 {
00262 #ifdef VERBOSE
00263                                         update_log << " inside: " << max_angle << ", " << min_angle << endl;
00264 #endif
00265                                         add[m] = false;
00266                                         break;
00267                                 }
00268                         }
00269                 }
00270                 for(int m=0; m<n_valid_points; m++)
00271                 {
00272 #ifdef VERBOSE
00273                         update_log << "check[" << m << "]: pos = " << pair_positions[m] << ", add = " << add[m] << endl;
00274 #endif
00275                         if(add[m])
00276                         {
00277                                 static fVec3 loc_relvel;
00278                                 all_vjoints.push_back(main_vjoint);
00279                                 loc_relvel.mul(pair_relvels[m], pair_orientations[m]);
00280                                 contact_relvels.push_back(loc_relvel);
00281                                 fric_coefs.push_back(sd_pair->SlipFric());
00282                                 // compute Jacobian
00283                                 static fMat Jv(3,6), Jr(3,6);
00284                                 static fMat33 Pcross, RtPx;
00285                                 fVec3& rpos = pair_positions[m];
00286                                 static fMat33 t_ratt;
00287                                 t_ratt.tran(pair_orientations[m]);
00288                                 Pcross.cross(rpos);
00289                                 RtPx.mul(t_ratt, Pcross);
00290                                 Jv(0,0) = t_ratt(0,0);
00291                                 Jv(0,1) = t_ratt(0,1);
00292                                 Jv(0,2) = t_ratt(0,2);
00293                                 Jv(1,0) = t_ratt(1,0);
00294                                 Jv(1,1) = t_ratt(1,1);
00295                                 Jv(1,2) = t_ratt(1,2);
00296                                 Jv(2,0) = t_ratt(2,0);
00297                                 Jv(2,1) = t_ratt(2,1);
00298                                 Jv(2,2) = t_ratt(2,2);
00299                                 Jv(0,3) = -RtPx(0,0);
00300                                 Jv(0,4) = -RtPx(0,1);
00301                                 Jv(0,5) = -RtPx(0,2);
00302                                 Jv(1,3) = -RtPx(1,0);
00303                                 Jv(1,4) = -RtPx(1,1);
00304                                 Jv(1,5) = -RtPx(1,2);
00305                                 Jv(2,3) = -RtPx(2,0);
00306                                 Jv(2,4) = -RtPx(2,1);
00307                                 Jv(2,5) = -RtPx(2,2);
00308 #ifdef VERBOSE
00309 //                              update_log << "rpos_v = " << rpos << endl;
00310 //                              update_log << "t_ratt_v = " << t_ratt << endl;
00311 //                              update_log << "Jv = " << Jv << endl;
00312 #endif
00313                                 rpos = pair_positions_real[m];
00314                                 t_ratt.tran(pair_orientations_real[m]);
00315                                 Pcross.cross(rpos);
00316                                 RtPx.mul(t_ratt, Pcross);
00317                                 Jr(0,0) = t_ratt(0,0);
00318                                 Jr(0,1) = t_ratt(0,1);
00319                                 Jr(0,2) = t_ratt(0,2);
00320                                 Jr(1,0) = t_ratt(1,0);
00321                                 Jr(1,1) = t_ratt(1,1);
00322                                 Jr(1,2) = t_ratt(1,2);
00323                                 Jr(2,0) = t_ratt(2,0);
00324                                 Jr(2,1) = t_ratt(2,1);
00325                                 Jr(2,2) = t_ratt(2,2);
00326                                 Jr(0,3) = -RtPx(0,0);
00327                                 Jr(0,4) = -RtPx(0,1);
00328                                 Jr(0,5) = -RtPx(0,2);
00329                                 Jr(1,3) = -RtPx(1,0);
00330                                 Jr(1,4) = -RtPx(1,1);
00331                                 Jr(1,5) = -RtPx(1,2);
00332                                 Jr(2,3) = -RtPx(2,0);
00333                                 Jr(2,4) = -RtPx(2,1);
00334                                 Jr(2,5) = -RtPx(2,2);
00335 #ifdef VERBOSE
00336 //                              update_log << "rpos_r = " << rpos << endl;
00337 //                              update_log << "t_ratt_r = " << t_ratt << endl;
00338 //                              update_log << "Jr = " << Jr << endl;
00339 #endif
00340                                 all_Jv.push_back(Jv);
00341                                 all_Jr.push_back(Jr);
00342                                 all_jdot_v.push_back(pair_jdot[m]);
00343                                 all_jdot_r.push_back(pair_jdot_real[m]);
00344                         }
00345                 }
00346         }
00347         init_contact();
00348         in_create_chain = false;
00349 //      Schedule();
00351         if(n_total_points == 0)
00352         {
00353                 pSim::Update();
00354                 return 0;
00355         }
00356         //
00357 //      DumpSchedule(update_log);
00358         CalcPosition();
00359         CalcVelocity();
00360         update_position();
00361         update_velocity();
00362 
00363         // compute contact force
00364         subchains->calc_contact_force(timestep);
00365 
00366         //
00367         disassembly();
00368 #ifdef USE_MPI
00369         // scatter results
00370         scatter_acc();
00371 #endif
00372         // clear f_final; use ext_force and ext_moment for the next steps
00373         // (e.g. in Runge-Kutta)
00374         subchains->clear_f_final();
00375         return 0;
00376 }
00377 
00378 void pSubChain::clear_f_final()
00379 {
00380         for(int i=0; i<n_outer_joints; i++)
00381         {
00382                 outer_joints[i]->f_final.zero();
00383         }
00384 }
00385 
00386 int pSubChain::calc_contact_force(double timestep)
00387 {
00388         assert(n_outer_joints/2 == sim->contact_vjoints.size());
00389         int n_contacts = sim->all_vjoints.size();
00390         int n_contacts_3 = 3*n_contacts;
00391         int n_coefs = N_FRIC_CONE_DIV+1;
00392         fMat Phi_hat(n_contacts_3, n_contacts_3);
00393         fVec bias_hat(n_contacts_3);
00394         Phi_hat.zero();
00395         bias_hat.zero();
00396         std::vector<fVec3>& rvels = sim->contact_relvels;
00397         for(int i=0; i<n_contacts; i++)
00398         {
00399                 int index = 3*i;
00400                 static fVec3 v;
00401                 v.set(rvels[i]);
00402                 bias_hat(index) = v(0);
00403                 bias_hat(index+1) = v(1);
00404                 bias_hat(index+2) = v(2);
00405         }
00406 //      update_log << "bias_hat(0) = " << bias_hat << endl;
00407         std::vector<int> r_index;
00408         std::vector<int> v_index;
00409         r_index.resize(n_contacts);
00410         v_index.resize(n_contacts);
00411         for(int i=0; i<n_contacts; i++)
00412         {
00413                 Joint* vjoint = sim->all_vjoints[i];
00414                 for(int j=0; j<n_outer_joints; j++)
00415                 {
00416                         if(outer_joints[j] == sim->joint_info[vjoint->i_joint].pjoints[0])
00417                         {
00418                                 r_index[i] = j;
00419                         }
00420                         if(outer_joints[j] == sim->joint_info[vjoint->i_joint].pjoints[1])
00421                         {
00422                                 v_index[i] = j;
00423                         }
00424                 }
00425 //              update_log << "contact[" << i << "]: r_index = " << r_index[i] << ", v_index = " << v_index[i] << endl;
00426         }
00427         for(int i=0; i<n_contacts; i++)
00428         {
00429                 int i3 = 3*i;
00430                 // bias_hat
00431                 static fVec dacc_r(3), dacc_v(3);
00432 //              dacc_r.mul(sim->all_Jr[i], acc_temp[r_index[i]]);
00433                 dacc_r.mul(sim->all_Jv[i], acc_temp[r_index[i]]);
00434                 dacc_v.mul(sim->all_Jv[i], acc_temp[v_index[i]]);
00435 #if 1
00436                 dacc_r(0) += sim->all_jdot_r[i](0);
00437                 dacc_r(1) += sim->all_jdot_r[i](1);
00438                 dacc_r(2) += sim->all_jdot_r[i](2);
00439                 dacc_v(0) += sim->all_jdot_v[i](0);
00440                 dacc_v(1) += sim->all_jdot_v[i](1);
00441                 dacc_v(2) += sim->all_jdot_v[i](2);
00442 #endif
00443                 dacc_r -= dacc_v;
00444                 dacc_r *= timestep;
00445                 bias_hat(i3) += dacc_r(0);
00446                 bias_hat(i3+1) += dacc_r(1);
00447                 bias_hat(i3+2) += dacc_r(2);
00448 #ifdef VERBOSE
00449 //              update_log << "contact[" << i << "]: bias_hat = [" << bias_hat(i3) << " " << bias_hat(i3+1) << " " << bias_hat(i3+2) << "]" << endl;
00450 #endif
00451                 // Phi_hat
00452                 for(int j=0; j<n_contacts; j++)
00453                 {
00454                         int j3 = 3*j;
00455                         static fMat PJ(6,3), JPJ(3,3), L(3,3);
00456                         L.zero();
00457                         PJ.mul(Lambda[r_index[i]][r_index[j]], tran(sim->all_Jv[j]));
00458                         JPJ.mul(sim->all_Jv[i], PJ);
00459 //                      update_log << "Lambda[" << r_index[i] << "][" << r_index[j] << "] = " << Lambda[r_index[i]][r_index[j]] << endl;
00460 //                      update_log << "Lambda[" << r_index[i] << "][" << v_index[j] << "] = " << Lambda[r_index[i]][v_index[j]] << endl;
00461 //                      update_log << "Lambda[" << v_index[i] << "][" << r_index[j] << "] = " << Lambda[v_index[i]][r_index[j]] << endl;
00462 //                      update_log << "Lambda[" << v_index[i] << "][" << v_index[j] << "] = " << Lambda[v_index[i]][v_index[j]] << endl;
00463 //                      update_log << "JPJ = " << JPJ << endl;
00464                         L += JPJ;
00465                         PJ.mul(Lambda[v_index[i]][r_index[j]], tran(sim->all_Jv[j]));
00466                         JPJ.mul(sim->all_Jv[i], PJ);
00467                         L -= JPJ;
00468                         PJ.mul(Lambda[r_index[i]][v_index[j]], tran(sim->all_Jv[j]));
00469                         JPJ.mul(sim->all_Jv[i], PJ);
00470                         L -= JPJ;
00471                         PJ.mul(Lambda[v_index[i]][v_index[j]], tran(sim->all_Jv[j]));
00472                         JPJ.mul(sim->all_Jv[i], PJ);
00473                         L += JPJ;
00474 #ifdef VERBOSE
00475 //                      update_log << "Phi_hat[" << i << "][" << j << "] = " << L << endl;
00476 #endif
00477                         Phi_hat.set_submat(i3, j3, L);
00478 #if 0
00479                         static fMat Lsub(3,3);
00480                         Lsub.get_submat(i3, j3, Phi_hat);
00481                         update_log << "Phi_hat[" << i << "][" << j << "] = " << Lsub << endl;
00482 #if 1
00483                         Lsub.get_submat(0, 0, Lambda[r_index[i]][r_index[j]]);
00484                         update_log << "L[" << r_index[i] << "][" << r_index[j] << "] = " << Lsub << endl;
00485                         Lsub.get_submat(0, 0, Lambda[v_index[i]][r_index[j]]);
00486                         update_log << "L[" << v_index[i] << "][" << r_index[j] << "] = " << Lsub << endl;
00487                         Lsub.get_submat(0, 0, Lambda[r_index[i]][v_index[j]]);
00488                         update_log << "L[" << r_index[i] << "][" << v_index[j] << "] = " << Lsub << endl;
00489                         Lsub.get_submat(0, 0, Lambda[v_index[i]][v_index[j]]);
00490                         update_log << "L[" << v_index[i] << "][" << v_index[j] << "] = " << Lsub << endl;
00491 #endif
00492 #endif
00493                 }
00494         }
00495 #ifdef VERBOSE
00496 //      update_log << "Phi_hat = " << Phi_hat << endl;
00497 //      update_log << "bias_hat = " << bias_hat << endl;
00498 #endif
00499         Phi_hat *= timestep;
00500         // LCP formulation
00501         int n_total_coef = n_coefs*n_contacts + n_contacts;
00502         fMat N(n_total_coef, n_total_coef);
00503         fMat Ck(3, n_coefs);
00504         fMat E(n_coefs, 1);
00505         fMat* Ehat_t = new fMat [n_contacts];
00506         fVec r(n_total_coef);
00507         // Ck, E
00508         Ck.zero();
00509         Ck(2, 0) = 1.0;
00510         E.zero();
00511         for(int m=0; m<N_FRIC_CONE_DIV; m++)
00512         {
00513                 Ck(0, m+1) = sim->cone_dir[m](0);
00514                 Ck(1, m+1) = sim->cone_dir[m](1);
00515                 E(m+1, 0) = 1.0;
00516         }
00517         for(int i=0; i<n_contacts; i++)
00518         {
00519                 double fric_coef = sim->fric_coefs[i];
00520                 Ehat_t[i].resize(1, n_coefs);
00521                 Ehat_t[i](0,0) = fric_coef;
00522                 for(int m=0; m<N_FRIC_CONE_DIV; m++)
00523                 {
00524                         Ehat_t[i](0, m+1) = -1.0;
00525                 }
00526         }
00527         N.zero();
00528         r.zero();
00529         for(int i=0; i<n_contacts; i++)
00530         {
00531                 static fMat N_ij(n_coefs,n_coefs);
00532                 static fVec r_i(n_coefs);
00533                 int i3 = i*3, iN = i*n_coefs;
00534                 r_i.zero();
00535                 for(int j=0; j<n_contacts; j++)
00536                 {
00537                         static fMat Phi_ij(3,3), CP(n_coefs,3);
00538                         static fMat B_ij(n_coefs,n_coefs), CinvP(n_coefs, 3);
00539                         // N (CPhiC)
00540                         int j3 = j*3, jN = j*(n_coefs);
00541                         Phi_ij.get_submat(i3, j3, Phi_hat);
00542                         CP.mul(tran(Ck), Phi_ij);
00543                         N_ij.mul(CP, Ck);
00544                         N.set_submat(iN, jN, N_ij);
00545                 }
00546                 // r
00547                 static fVec bias_i(3);
00548                 bias_i.get_subvec(i3, bias_hat);
00549                 r_i.mul(bias_i, Ck);
00550                 r.set_subvec(iN, r_i);
00551                 // E
00552                 N.set_submat(iN, n_coefs*n_contacts+i, E);
00553                 N.set_submat(n_coefs*n_contacts+i, iN, Ehat_t[i]);
00554         }
00555         // LCP parameters
00556         double max_error = 1.0e-3;
00557 //      int max_iteration = n_total_coef * 10;
00558         int max_iteration = 10000;
00559         // presolve
00560         std::vector<int> presolve_g2w;
00561         int presolve_failed = false;
00562         fVec presolve_g2;
00563         presolve_g2w.resize(n_contacts);
00564         for(int i=0; i<n_contacts; i++)
00565         {
00566                 presolve_g2w[i] = -1;
00567         }
00568         {
00569                 fMat Ns(n_contacts, n_contacts);
00570                 fVec rs(n_contacts), as;
00571                 int n_iteration = 0;
00572                 for(int i=0; i<n_contacts; i++)
00573                 {
00574                         rs(i) = bias_hat(i*3+2);
00575                         for(int j=0; j<n_contacts; j++)
00576                         {
00577                                 Ns(i, j) = Phi_hat(i*3+2, j*3+2);
00578                         }
00579                 }
00580                 LCP lcp_s(Ns, rs);
00581                 presolve_failed = lcp_s.SolvePivot2(presolve_g2, as, max_error, max_iteration, &n_iteration, presolve_g2w);
00582                 if(presolve_failed)
00583                 {
00584                         cerr << "presolve_failed (" << n_iteration << "/" << max_iteration << ")" << endl;
00585                         cerr << "Phi_hat = " << Phi_hat << endl;
00586                 }
00587 #ifdef VERBOSE
00588                 for(int i=0; i<n_contacts; i++)
00589                 {
00590                         update_log << "presolve_g2w[" << i << "] = " << presolve_g2w[i] << endl;
00591                 }
00592 #endif
00593 #ifdef MEASURE_COLLISION_TIME
00594                 sim->n_total_nodes += n_iteration;
00595 #endif
00596         }
00597 #ifdef USE_INITIAL_GUESS
00598         std::vector<int> w2a, w2g, z2a, z2g;
00599 #endif
00600         // search for appropriate set of constraints
00601         std::vector<int> active2all;
00602         fVec* fk = new fVec [n_contacts];
00603         fVec* vk = new fVec [n_contacts];
00604         fMat N_active;
00605         fVec r_active;
00606 #if 1
00607         if(!presolve_failed)
00608         {
00609                 for(int i=0; i<n_contacts; i++)
00610                 {
00611                         if(presolve_g2w[i] >= 0)
00612                         {
00613                                 active2all.push_back(i);
00614                         }
00615                 }
00616         }
00617         else
00618 #endif
00619         {
00620                 for(int i=0; i<n_contacts; i++)
00621                 {
00622                         active2all.push_back(i);
00623                 }
00624         }
00625 #ifdef VERBOSE
00626         update_log << "*** start searching constraint set" << endl;
00627 #endif
00628         int max_global_iteration = 1;
00629         int count = 0;
00630         while(count < max_global_iteration)
00631         {
00632                 std::vector<int> all2active;
00633                 std::vector<int> g2w;
00634                 all2active.resize(n_contacts);
00635                 for(int i=0; i<n_contacts; i++)
00636                 {
00637                         all2active[i] = -1;
00638                         fk[i].resize(3);
00639                         fk[i].zero();
00640                 }
00641                 int n_active_contacts = active2all.size();
00642                 if(n_active_contacts > 0)
00643                 {
00644                         int n_active_coef = n_active_contacts*n_coefs + n_active_contacts;
00645                         N_active.resize(n_active_coef, n_active_coef);
00646                         r_active.resize(n_active_coef);
00647                         N_active.zero();
00648                         r_active.zero();
00649                         for(int i=0; i<n_active_contacts; i++)
00650                         {
00651                                 all2active[active2all[i]] = i;
00652                         }
00653 #ifdef VERBOSE
00654                         update_log << "n_active_contacts = " << n_active_contacts << endl;
00655                         update_log << "n_active_coef = " << n_active_coef << endl;
00656 #endif
00657                         for(int i=0; i<n_active_contacts; i++)
00658                         {
00659                                 static fMat N_ij(n_coefs,n_coefs);
00660                                 static fVec r_i(n_coefs);
00661                                 int i_active_N = i*n_coefs;
00662                                 int i_all = active2all[i];
00663                                 int i_all_N = i_all*n_coefs;
00664                                 for(int j=0; j<n_active_contacts; j++)
00665                                 {
00666                                         int j_all = active2all[j];
00667                                         int j_all_N = j_all*n_coefs;
00668                                         int j_active_N = j*n_coefs;
00669                                         N_ij.get_submat(i_all_N, j_all_N, N);
00670                                         N_active.set_submat(i_active_N, j_active_N, N_ij);
00671                                 }
00672                                 r_i.get_subvec(i_all_N, r);
00673                                 r_active.set_subvec(i_active_N, r_i);
00674                                 N_active.set_submat(i_active_N, n_coefs*n_active_contacts+i, E);
00675                                 N_active.set_submat(n_coefs*n_active_contacts+i, i_active_N, Ehat_t[i]);
00676                         }
00677 #ifdef USE_INITIAL_GUESS
00678                         static fMat N_init;
00679                         static fVec r_init;
00680                         N_init.resize(n_active_coef, n_active_coef);
00681                         r_init.resize(n_active_coef);
00682                         w2a.resize(n_active_coef);
00683                         w2g.resize(n_active_coef);
00684                         z2a.resize(n_active_coef);
00685                         z2g.resize(n_active_coef);
00686                         for(int i=0; i<n_active_coef; i++)
00687                         {
00688                                 w2a[i] = i;
00689                                 w2g[i] = -1;
00690                                 z2a[i] = -1;
00691                                 z2g[i] = i;
00692                         }
00693                         for(int i=0; i<n_active_contacts; i++)
00694                         {
00695                                 int idx = i*n_coefs;
00696                                 w2a[idx] = -1;
00697                                 w2g[idx] = idx;
00698                                 z2a[idx] = idx;
00699                                 z2g[idx] = -1;
00700                         }
00701                         LCP::Pivot(w2a, w2g, z2a, z2g, N_active, r_active, N_init, r_init);
00702                         N_active.set(N_init);
00703                         r_active.set(r_init);
00704 #endif
00705                         LCP lcp(N_active, r_active);
00706                         // solution
00707                         static fVec g2, a;
00708                         int n_iteration = 0;
00709                         g2w.resize(n_active_coef);
00710 #ifdef MEASURE_COLLISION_TIME
00711                         sim->n_total_contacts += n_contacts;
00712                         sim->n_active_contacts += n_active_contacts;
00713                         LongInteger t1 = GetTick();
00714 #endif
00715 #ifdef USE_NORMAL_LEMKE
00716                         int failed = lcp.SolvePivot(g2, a, max_error, max_iteration, &n_iteration, g2w);
00717 #else
00718                         int failed = lcp.SolvePivot2(g2, a, max_error, max_iteration, &n_iteration, g2w);
00719 #endif
00720                         count++;
00721 #ifdef MEASURE_COLLISION_TIME
00722                         LongInteger t2 = GetTick();
00723                         sim->lcp_solve_time += ExpiredTime(t1, t2);
00724                         sim->n_total_nodes += n_iteration;
00725                         sim->n_loops += lcp.NumLoops();
00726                         sim->n_errors += lcp.NumErrors();
00727 #endif
00728 #ifdef VERBOSE
00729                         update_log << "n_iteration = " << n_iteration << endl;
00730 #endif
00731                         fVec error(n_active_coef);
00732                         error.mul(N_active, g2);
00733                         error += r_active;
00734                         error -= a;
00735 #ifdef VERBOSE
00736                         update_log << "error = " << error.length() << endl;
00737 #endif
00738                         if(error.length() > 1e-4) failed = true;
00739                         if(failed)
00740                         {
00741                                 cerr << "failed (" << n_iteration << "/" << max_iteration << "): n_contacts = " << n_contacts << endl;
00742 #ifdef MEASURE_COLLISION_TIME
00743                                 sim->n_failure_frames++;
00744 #endif
00745 #ifdef VERBOSE
00746                                 update_log << "failed" << endl;
00747 #endif
00748                                 if(!presolve_failed)
00749                                 {
00750                                         cerr << "using presolve result" << endl;
00751                                         for(int i=0; i<n_contacts; i++)
00752                                         {
00753                                                 fk[i](2) = presolve_g2(i);
00754                                         }
00755                                 }
00756                                 break;
00757                         }
00758                         else
00759                         {
00760 #ifdef VERBOSE
00761                                 update_log << "success" << endl;
00762                                 update_log << "g2 = " << tran(g2) << endl;
00763                                 update_log << "a = " << tran(a) << endl;
00764                                 update_log << "g2w = " << endl;
00765                                 for(int i=0; i<n_active_contacts; i++)
00766                                 {
00767                                         update_log << "active contact[" << i << "] a = " << flush;
00768                                         for(int j=0; j<n_coefs; j++)
00769                                         {
00770                                                 update_log << g2w[i*n_coefs+j] << " " << flush;
00771                                         }
00772                                         update_log << endl;
00773                                 }
00774                                 for(int i=0; i<n_active_contacts; i++)
00775                                 {
00776                                         update_log << "active contact[" << i << "] lambda = " << g2w[n_active_contacts*n_coefs+i] << endl;
00777                                 }
00778 #endif
00779 #ifdef USE_INITIAL_GUESS
00780                                 for(int i=0; i<n_active_coef; i++)
00781                                 {
00782                                         if(w2g[i] >= 0)
00783                                         {
00784                                                 g2(i) = a(w2g[i]);
00785                                         }
00786                                 }
00787 #endif
00788                                 for(int i=0; i<n_active_contacts; i++)
00789                                 {
00790                                         static fVec g2_i(n_coefs);
00791                                         int iN = i*n_coefs;
00792                                         g2_i.get_subvec(iN, g2);
00793                                         fk[active2all[i]].mul(Ck, g2_i);
00794 #ifdef VERBOSE
00795                                         update_log << "fk[" << active2all[i] << "] = " << tran(fk[active2all[i]]) << endl;
00796 #endif
00797                                 }
00798                         }
00799                         active2all.clear();
00800                 }
00801                 // check velocity
00802                 for(int i=0; i<n_contacts; i++)
00803                 {
00804                         vk[i].resize(3);
00805                         vk[i].zero();
00806                 }
00807                 int constraint_modified = false;
00808                 for(int i=0; i<n_contacts; i++)
00809                 {
00810                         static fVec bias_i(3);
00811                         bias_i.get_subvec(3*i, bias_hat);
00812                         for(int j=0; j<n_contacts; j++)
00813                         {
00814                                 static fMat Phi_ij(3,3);
00815                                 Phi_ij.get_submat(3*i, 3*j, Phi_hat);
00816                                 vk[i] += Phi_ij * fk[j];
00817                         }
00818                         vk[i] += bias_i;
00819 #ifdef VERBOSE
00820                         update_log << "vk[" << i << "] = " << tran(vk[i]) << endl;
00821                         if(vk[i](2) < -max_error)
00822                         {
00823                                 update_log << "--- too small vertical velocity" << endl;
00824                         }
00825 #endif
00826 #ifdef USE_INITIAL_GUESS
00827                         if((all2active[i] >= 0 && w2g[all2active[i]*n_coefs] < 0 && g2w[all2active[i]*n_coefs] >= 0) ||
00828                            (all2active[i] >= 0 && w2g[all2active[i]*n_coefs] >= 0 && g2w[all2active[i]*n_coefs] < 0) ||
00829 #else
00830                         if((all2active[i] >= 0 && g2w[all2active[i]*n_coefs] >= 0) ||
00831 #endif
00832                            vk[i](2) < -max_error)
00833                         {
00834                                 active2all.push_back(i);
00835 #ifdef VERBOSE
00836                                 update_log << "added to active" << endl;
00837 #endif
00838                                 if(vk[i](2) < -max_error)
00839                                 {
00840                                         constraint_modified = true;
00841 #ifdef VERBOSE
00842                                         update_log << "constraint modified" << endl;
00843 #endif
00844                                 }
00845                         }
00846 #ifdef VERBOSE
00847                         else
00848 #ifdef USE_INITIAL_GUESS
00849                         if((all2active[i] >= 0 && w2g[all2active[i]*n_coefs] < 0 && g2w[all2active[i]*n_coefs] >= 0) ||
00850                            (all2active[i] >= 0 && w2g[all2active[i]*n_coefs] >= 0 && g2w[all2active[i]*n_coefs] < 0))
00851 #else
00852                         if(all2active[i] >= 0 && g2w[all2active[i]*n_coefs] >= 0)
00853 #endif
00854                         {
00855                                 update_log << "removed from active" << endl;
00856                         }
00857 #endif
00858                 }
00859                 if(!constraint_modified) break;
00860         }
00861 #ifdef VERBOSE
00862         update_log << "end of search" << endl;
00863 #endif
00864 //      cerr << "trials = " << count << endl;
00865         // apply the force
00866         for(int i=0; i<n_outer_joints; i++)
00867         {
00868                 outer_joints[i]->f_final.zero();
00869         }
00870         for(int i=0; i<n_contacts; i++)
00871         {
00872                 static fVec3 f, f_virt, f_real, moment;
00873                 f(0) = fk[i](0);
00874                 f(1) = fk[i](1);
00875                 f(2) = fk[i](2);
00876                 Joint* r_joint = outer_joints[r_index[i]]->joint;
00877                 Joint* v_joint = outer_joints[v_index[i]]->joint;
00878                 static fVec f0(6);
00879                 f0.mul(fk[i], sim->all_Jr[i]);
00880                 r_joint->real->ext_force(0) += f0(0);
00881                 r_joint->real->ext_force(1) += f0(1);
00882                 r_joint->real->ext_force(2) += f0(2);
00883                 r_joint->real->ext_moment(0) += f0(3);
00884                 r_joint->real->ext_moment(1) += f0(4);
00885                 r_joint->real->ext_moment(2) += f0(5);
00886                 f0.mul(fk[i], sim->all_Jv[i]);
00887                 v_joint->parent->ext_force(0) -= f0(0);
00888                 v_joint->parent->ext_force(1) -= f0(1);
00889                 v_joint->parent->ext_force(2) -= f0(2);
00890                 v_joint->parent->ext_moment(0) -= f0(3);
00891                 v_joint->parent->ext_moment(1) -= f0(4);
00892                 v_joint->parent->ext_moment(2) -= f0(5);
00893                 outer_joints[r_index[i]]->f_final += f0;
00894                 outer_joints[v_index[i]]->f_final -= f0;
00895 #ifdef VERBOSE
00896                 update_log << r_joint->real->name << " force/moment: " << r_joint->real->ext_force << r_joint->real->ext_moment << endl;
00897                 update_log << v_joint->parent->name << " force/moment: " << v_joint->parent->ext_force << v_joint->parent->ext_moment << endl;
00898 #endif
00899         }
00900         delete[] vk;
00901         delete[] fk;
00902         delete[] Ehat_t;
00903         return 0;
00904 }
00905 
00906 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Thu Apr 11 2019 03:30:19