sdcontact.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  */
00014 //#define USE_SLIP_FUNC
00015 
00016 #include "sdcontact.h"
00017 
00018 #include <fstream>
00019 //static ofstream sd_log("sd.log");
00020 static ofstream sd_log;
00021 
00022 int SDContactPair::Update(double timestep, int n_contact, double** coords, double** normals, double* depths)
00023 {
00024         sd_log << "Update(" << n_contact << ")" << endl;
00025         in_slip = false;
00026         if(n_contact > 0)
00027         {
00028                 int i;
00029                 if(!init_set) set_init();
00030                 for(i=0; i<n_contact; i++)
00031                         update(timestep, n_contact, coords[i], normals[i], depths[i]);
00032                 if(in_slip) init_set = false;  // to reset initial pos/ori
00033         }
00034         else
00035         {
00036                 init_set = false;
00037         }
00038         sd_log << "end Update" << endl;
00039         return 0;
00040 }
00041 
00042 int SDContactPair::set_init()
00043 {
00044         init_set = true;
00045         Joint* joint1 = joints[0];
00046         Joint* joint2 = joints[1];
00047         static fVec3 pp;
00048         static fMat33 tr;
00049 
00050         pp.sub(joint2->abs_pos, joint1->abs_pos);
00051         tr.tran(joint1->abs_att);
00052         init_pos.mul(tr, pp);
00053         init_att.mul(tr, joint2->abs_att);
00054         return 0;
00055 }
00056 
00057 static double slip_func(double c, double v)
00058 {
00059         return (1.0 - exp(-c*v));
00060 }
00061 
00062 int SDContactPair::update(double timestep, int n_contact, double* coord, double* normal, double depth)
00063 {
00064         Joint* joint1 = joints[0];
00065         Joint* joint2 = joints[1];
00066         static fVec3 relpos1, relpos2;
00067         static fVec3 relnorm;
00068         relpos1(0) = coord[0];
00069         relpos1(1) = coord[1];
00070         relpos1(2) = coord[2];
00071         relpos2(0) = coord[0];
00072         relpos2(1) = coord[1];
00073         relpos2(2) = coord[2];
00074         relnorm(0) = normal[0];
00075         relnorm(1) = normal[1];
00076         relnorm(2) = normal[2];
00077         sd_log << "relpos1 = " << relpos1 << endl;
00078         sd_log << "relnorm = " << relnorm << endl;
00079         // velocity
00080         static fVec3 vel, vel1, vel2, force, tmp;
00081         double d, v, f;
00082         // joint1
00083         vel1.cross(joint1->loc_ang_vel, relpos1);
00084         vel1 += joint1->loc_lin_vel;
00085         // joint2
00086         static fVec3 rpos2, pp, lin2, ang2;
00087         static fMat33 ratt2, tr;
00088         pp.sub(joint2->abs_pos, joint1->abs_pos);
00089         tr.tran(joint1->abs_att);
00090         // pos/ori of joint2 in joint1 frame
00091         rpos2.mul(tr, pp);
00092         ratt2.mul(tr, joint2->abs_att);
00093         // lin/ang velocity of joint2 in joint1 frame
00094         lin2.mul(ratt2, joint2->loc_lin_vel);
00095         ang2.mul(ratt2, joint2->loc_ang_vel);
00096         // vector from joint2 frame to contact point 2, in joint1 frame
00097         relpos2 -= rpos2;
00098         // velocity of contact point 2 in joint1 frame
00099         vel2.cross(ang2, relpos2);
00100         vel2 += lin2;
00101         // relative velocity
00102         vel.sub(vel1, vel2);
00103         v = relnorm * vel;
00104         d = depth;
00105         sd_log << "depth = " << depth << endl;
00106         sd_log << "vel1 = " << vel1 << endl;
00107         sd_log << "joint2->loc_lin_vel = " << joint2->loc_lin_vel << endl;
00108         sd_log << "vel2 = " << vel2 << endl;
00109         sd_log << "vel = " << vel << endl;
00110         // average depth during timestep
00111 //      d += 0.5 * v * timestep;
00112         // normal force applied to joint2, in joint1 frame
00113 #ifdef SD_NONLINEAR
00114         f = (spring + damper * v) * d;
00115 #else
00116         f = (spring * d + damper * v);
00117 #endif
00118         if(f < 0.0) return 0;
00119         force.mul(f, relnorm);
00120         //
00121         // static friction
00122         //
00123         // slip info
00124         static fVec3 slip_vel;
00125         double abs_slip, slip_func_coef, w;
00126         slip_vel.mul(v, relnorm);
00127         slip_vel -= vel;  // slip_vel = vel - v*relnorm
00128         abs_slip = slip_vel.length();
00129         slip_func_coef = slip_func_coef_base / timestep;
00130         w = slip_func(slip_func_coef, abs_slip);
00131         // initial position (in joint1 frame)
00132         static fVec3 p_init;
00133 //      p_init.mul(init_att, relpos2);
00134 //      p_init += init_pos;
00135         p_init.add(init_pos, relpos2);
00136         // static friction
00137         static fVec3 tmp_fric;
00138         tmp_fric.sub(init_pos, relpos2);
00139         tmp_fric -= rpos2;
00140         tmp_fric *= slip_p;
00141         tmp.mul(-slip_d, slip_vel);
00142         tmp_fric += tmp;
00143         // tangential force
00144         double _t = tmp_fric * relnorm;
00145         tmp.mul(_t, relnorm);
00146         tmp_fric -= tmp;
00147 //      cerr << "static fric = " << tmp_fric << endl;
00148         // check
00149         double abs_fric = tmp_fric.length();
00150         // slip friction
00151 #ifdef USE_SLIP_FUNC
00152         if(abs_fric > f*static_fric)
00153         {
00154 //              cerr << "slip" << endl;
00155 //              cerr << abs_fric << ", " << abs_slip << endl;
00156                 double tiny = 1e-8, _c;
00157                 if(abs_slip > tiny)
00158                         _c = slip_fric * f * w / abs_slip;
00159                 else
00160                         _c = slip_fric * f * slip_func_coef;
00161                 tmp_fric.mul(-_c, slip_vel);
00162                 in_slip = true;  // need? (05.09.14)
00163         }
00164 #else
00165         double tiny = 1e-8, _c;
00166 #if 1
00167         if(abs_slip > tiny)  // slip friction
00168         {
00169                 _c = slip_fric * f / abs_slip;
00170                 tmp_fric.mul(-_c, slip_vel);
00171                 in_slip = true;  // need? (05.09.14)
00172         }
00173         else if(abs_fric > f*static_fric)  // start slipping
00174         {
00175                 // parallel to the potential static friction
00176                 _c = slip_fric * f / abs_fric;
00177                 fVec3 fric_save(tmp_fric);
00178                 tmp_fric.mul(_c, fric_save);
00179                 in_slip = true;  // need? (05.09.14)
00180         }
00181 #else
00182         if(abs_fric > f*static_fric)
00183         {
00184                 if(abs_slip > tiny)  // slip friction
00185                 {
00186                         _c = slip_fric * f / abs_slip;
00187                         tmp_fric.mul(-_c, slip_vel);
00188                         in_slip = true;  // need? (05.09.14)
00189                 }
00190                 else
00191                 {
00192                         // parallel to the potential static friction
00193                         _c = slip_fric * f / abs_fric;
00194                         fVec3 fric_save(tmp_fric);
00195                         tmp_fric.mul(_c, fric_save);
00196                         in_slip = true;  // need? (05.09.14)
00197                 }
00198         }
00199 #endif
00200 #endif
00201         force += tmp_fric;
00202         // average
00203         force /= (double)n_contact;
00204         // apply to joint1
00205         static fVec3 m;
00206         joint1->ext_force -= force;
00207         m.cross(relpos1, force);
00208         joint1->ext_moment -= m;
00209         // apply to joint2
00210 //      cerr << "---" << endl;
00211 //      cerr << "depth = " << d << endl;
00212 //      cerr << "force = " << force << endl;
00213 //      cerr << "relpos = " << relpos1 << endl;
00214 //      cerr << "norm = " << relnorm << endl;
00215         tmp.mul(force, ratt2);
00216         sd_log << "ext_force = " << tmp << endl;
00217         joint2->ext_force += tmp;
00218         m.cross(relpos2, force);
00219         tmp.mul(m, ratt2);
00220         joint2->ext_moment += tmp;
00221         sd_log << "ext_moment = " << tmp << endl;
00222         return 0;
00223 }
00224 


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:56