Go to the documentation of this file.
24 sd_log <<
"Update(" << n_contact <<
")" << endl;
30 for(
i=0;
i<n_contact;
i++)
38 sd_log <<
"end Update" << endl;
59 return (1.0 - exp(-
c*v));
66 static fVec3 relpos1, relpos2;
68 relpos1(0) = coord[0];
69 relpos1(1) = coord[1];
70 relpos1(2) = coord[2];
71 relpos2(0) = coord[0];
72 relpos2(1) = coord[1];
73 relpos2(2) = coord[2];
74 relnorm(0) = normal[0];
75 relnorm(1) = normal[1];
76 relnorm(2) = normal[2];
77 sd_log <<
"relpos1 = " << relpos1 << endl;
78 sd_log <<
"relnorm = " << relnorm << endl;
80 static fVec3 vel, vel1, vel2, force, tmp;
86 static fVec3 rpos2, pp, lin2, ang2;
99 vel2.
cross(ang2, relpos2);
105 sd_log <<
"depth = " << depth << endl;
106 sd_log <<
"vel1 = " << vel1 << endl;
108 sd_log <<
"vel2 = " << vel2 << endl;
109 sd_log <<
"vel = " << vel << endl;
118 if(
f < 0.0)
return 0;
119 force.
mul(
f, relnorm);
124 static fVec3 slip_vel;
125 double abs_slip, slip_func_coef, w;
126 slip_vel.
mul(v, relnorm);
128 abs_slip = slip_vel.
length();
137 static fVec3 tmp_fric;
144 double _t = tmp_fric * relnorm;
145 tmp.
mul(_t, relnorm);
149 double abs_fric = tmp_fric.
length();
156 double tiny = 1e-8, _c;
161 tmp_fric.
mul(-_c, slip_vel);
165 double tiny = 1e-8, _c;
170 tmp_fric.
mul(-_c, slip_vel);
177 fVec3 fric_save(tmp_fric);
178 tmp_fric.
mul(_c, fric_save);
187 tmp_fric.
mul(-_c, slip_vel);
194 fVec3 fric_save(tmp_fric);
195 tmp_fric.
mul(_c, fric_save);
203 force /= (double)n_contact;
207 m.
cross(relpos1, force);
215 tmp.
mul(force, ratt2);
216 sd_log <<
"ext_force = " << tmp << endl;
218 m.
cross(relpos2, force);
221 sd_log <<
"ext_moment = " << tmp << endl;
fVec3 ext_moment
external moment around the local frame
fVec3 abs_pos
absolute position
void add(const fVec3 &vec1, const fVec3 &vec2)
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
The class for representing a joint.
void mul(const fMat33 &mat1, const fMat33 &mat2)
fVec3 ext_force
external force
fMat33 abs_att
absolute orientation
void sub(const fVec3 &vec1, const fVec3 &vec2)
friend double length(const fVec3 &v)
Returns the length of a vector.
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
fVec3 loc_lin_vel
linear velocity in local frame
fVec3 loc_ang_vel
angular velocity in local frame
void mul(const fVec3 &vec, double d)
openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Wed Sep 7 2022 02:51:04