sdcontact.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  */
14 //#define USE_SLIP_FUNC
15 
16 #include "sdcontact.h"
17 
18 #include <fstream>
19 //static ofstream sd_log("sd.log");
20 static ofstream sd_log;
21 
22 int SDContactPair::Update(double timestep, int n_contact, double** coords, double** normals, double* depths)
23 {
24  sd_log << "Update(" << n_contact << ")" << endl;
25  in_slip = false;
26  if(n_contact > 0)
27  {
28  int i;
29  if(!init_set) set_init();
30  for(i=0; i<n_contact; i++)
31  update(timestep, n_contact, coords[i], normals[i], depths[i]);
32  if(in_slip) init_set = false; // to reset initial pos/ori
33  }
34  else
35  {
36  init_set = false;
37  }
38  sd_log << "end Update" << endl;
39  return 0;
40 }
41 
43 {
44  init_set = true;
45  Joint* joint1 = joints[0];
46  Joint* joint2 = joints[1];
47  static fVec3 pp;
48  static fMat33 tr;
49 
50  pp.sub(joint2->abs_pos, joint1->abs_pos);
51  tr.tran(joint1->abs_att);
52  init_pos.mul(tr, pp);
53  init_att.mul(tr, joint2->abs_att);
54  return 0;
55 }
56 
57 static double slip_func(double c, double v)
58 {
59  return (1.0 - exp(-c*v));
60 }
61 
62 int SDContactPair::update(double timestep, int n_contact, double* coord, double* normal, double depth)
63 {
64  Joint* joint1 = joints[0];
65  Joint* joint2 = joints[1];
66  static fVec3 relpos1, relpos2;
67  static fVec3 relnorm;
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;
79  // velocity
80  static fVec3 vel, vel1, vel2, force, tmp;
81  double d, v, f;
82  // joint1
83  vel1.cross(joint1->loc_ang_vel, relpos1);
84  vel1 += joint1->loc_lin_vel;
85  // joint2
86  static fVec3 rpos2, pp, lin2, ang2;
87  static fMat33 ratt2, tr;
88  pp.sub(joint2->abs_pos, joint1->abs_pos);
89  tr.tran(joint1->abs_att);
90  // pos/ori of joint2 in joint1 frame
91  rpos2.mul(tr, pp);
92  ratt2.mul(tr, joint2->abs_att);
93  // lin/ang velocity of joint2 in joint1 frame
94  lin2.mul(ratt2, joint2->loc_lin_vel);
95  ang2.mul(ratt2, joint2->loc_ang_vel);
96  // vector from joint2 frame to contact point 2, in joint1 frame
97  relpos2 -= rpos2;
98  // velocity of contact point 2 in joint1 frame
99  vel2.cross(ang2, relpos2);
100  vel2 += lin2;
101  // relative velocity
102  vel.sub(vel1, vel2);
103  v = relnorm * vel;
104  d = depth;
105  sd_log << "depth = " << depth << endl;
106  sd_log << "vel1 = " << vel1 << endl;
107  sd_log << "joint2->loc_lin_vel = " << joint2->loc_lin_vel << endl;
108  sd_log << "vel2 = " << vel2 << endl;
109  sd_log << "vel = " << vel << endl;
110  // average depth during timestep
111 // d += 0.5 * v * timestep;
112  // normal force applied to joint2, in joint1 frame
113 #ifdef SD_NONLINEAR
114  f = (spring + damper * v) * d;
115 #else
116  f = (spring * d + damper * v);
117 #endif
118  if(f < 0.0) return 0;
119  force.mul(f, relnorm);
120  //
121  // static friction
122  //
123  // slip info
124  static fVec3 slip_vel;
125  double abs_slip, slip_func_coef, w;
126  slip_vel.mul(v, relnorm);
127  slip_vel -= vel; // slip_vel = vel - v*relnorm
128  abs_slip = slip_vel.length();
129  slip_func_coef = slip_func_coef_base / timestep;
130  w = slip_func(slip_func_coef, abs_slip);
131  // initial position (in joint1 frame)
132  static fVec3 p_init;
133 // p_init.mul(init_att, relpos2);
134 // p_init += init_pos;
135  p_init.add(init_pos, relpos2);
136  // static friction
137  static fVec3 tmp_fric;
138  tmp_fric.sub(init_pos, relpos2);
139  tmp_fric -= rpos2;
140  tmp_fric *= slip_p;
141  tmp.mul(-slip_d, slip_vel);
142  tmp_fric += tmp;
143  // tangential force
144  double _t = tmp_fric * relnorm;
145  tmp.mul(_t, relnorm);
146  tmp_fric -= tmp;
147 // cerr << "static fric = " << tmp_fric << endl;
148  // check
149  double abs_fric = tmp_fric.length();
150  // slip friction
151 #ifdef USE_SLIP_FUNC
152  if(abs_fric > f*static_fric)
153  {
154 // cerr << "slip" << endl;
155 // cerr << abs_fric << ", " << abs_slip << endl;
156  double tiny = 1e-8, _c;
157  if(abs_slip > tiny)
158  _c = slip_fric * f * w / abs_slip;
159  else
160  _c = slip_fric * f * slip_func_coef;
161  tmp_fric.mul(-_c, slip_vel);
162  in_slip = true; // need? (05.09.14)
163  }
164 #else
165  double tiny = 1e-8, _c;
166 #if 1
167  if(abs_slip > tiny) // slip friction
168  {
169  _c = slip_fric * f / abs_slip;
170  tmp_fric.mul(-_c, slip_vel);
171  in_slip = true; // need? (05.09.14)
172  }
173  else if(abs_fric > f*static_fric) // start slipping
174  {
175  // parallel to the potential static friction
176  _c = slip_fric * f / abs_fric;
177  fVec3 fric_save(tmp_fric);
178  tmp_fric.mul(_c, fric_save);
179  in_slip = true; // need? (05.09.14)
180  }
181 #else
182  if(abs_fric > f*static_fric)
183  {
184  if(abs_slip > tiny) // slip friction
185  {
186  _c = slip_fric * f / abs_slip;
187  tmp_fric.mul(-_c, slip_vel);
188  in_slip = true; // need? (05.09.14)
189  }
190  else
191  {
192  // parallel to the potential static friction
193  _c = slip_fric * f / abs_fric;
194  fVec3 fric_save(tmp_fric);
195  tmp_fric.mul(_c, fric_save);
196  in_slip = true; // need? (05.09.14)
197  }
198  }
199 #endif
200 #endif
201  force += tmp_fric;
202  // average
203  force /= (double)n_contact;
204  // apply to joint1
205  static fVec3 m;
206  joint1->ext_force -= force;
207  m.cross(relpos1, force);
208  joint1->ext_moment -= m;
209  // apply to joint2
210 // cerr << "---" << endl;
211 // cerr << "depth = " << d << endl;
212 // cerr << "force = " << force << endl;
213 // cerr << "relpos = " << relpos1 << endl;
214 // cerr << "norm = " << relnorm << endl;
215  tmp.mul(force, ratt2);
216  sd_log << "ext_force = " << tmp << endl;
217  joint2->ext_force += tmp;
218  m.cross(relpos2, force);
219  tmp.mul(m, ratt2);
220  joint2->ext_moment += tmp;
221  sd_log << "ext_moment = " << tmp << endl;
222  return 0;
223 }
224 
int update(double timestep, int n_contact, double *coord, double *normal, double depth)
Definition: sdcontact.cpp:62
fVec3 loc_lin_vel
linear velocity in local frame
Definition: chain.h:743
void add(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:888
3x3 matrix class.
Definition: fMatrix3.h:29
int c
Definition: autoplay.py:16
double slip_fric
Definition: sdcontact.h:112
void mul(const fMat33 &mat1, const fMat33 &mat2)
Definition: fMatrix3.cpp:694
friend fMat33 tran(const fMat33 &m)
Returns the transpose.
Definition: fMatrix3.cpp:607
void cross(const fVec3 &vec1, const fVec3 &vec2)
Cross product.
Definition: fMatrix3.cpp:944
double damper
Definition: sdcontact.h:109
fVec3 loc_ang_vel
angular velocity in local frame
Definition: chain.h:744
#define tiny
Definition: fEulerPara.cpp:16
w
png_uint_32 i
Definition: png.h:2735
double spring
Definition: sdcontact.h:108
double slip_func_coef_base
Definition: sdcontact.h:115
fVec3 ext_force
external force
Definition: chain.h:736
fMat33 abs_att
absolute orientation
Definition: chain.h:742
Joint * joints[2]
Definition: sdcontact.h:101
double slip_d
Definition: sdcontact.h:114
double static_fric
Definition: sdcontact.h:111
int set_init()
Definition: sdcontact.cpp:42
fVec3 init_pos
Definition: sdcontact.h:118
fMat33 init_att
Definition: sdcontact.h:119
void sub(const fVec3 &vec1, const fVec3 &vec2)
Definition: fMatrix3.cpp:902
friend double length(const fVec3 &v)
Returns the length of a vector.
Definition: fMatrix3.h:293
3-element vector class.
Definition: fMatrix3.h:206
static double slip_func(double c, double v)
Definition: sdcontact.cpp:57
fVec3 ext_moment
external moment around the local frame
Definition: chain.h:737
fVec3 abs_pos
absolute position
Definition: chain.h:741
void mul(const fVec3 &vec, double d)
Definition: fMatrix3.cpp:916
The class for representing a joint.
Definition: chain.h:538
static ofstream sd_log
Definition: sdcontact.cpp:20
int Update(double timestep, int n_contact, double **coords, double **normals, double *depths)
Definition: sdcontact.cpp:22
double slip_p
Definition: sdcontact.h:113


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