1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) - Tim's Box (hybrid contact model) *
3 * -------------------------------------------------------------------------- *
4 * This is part of the SimTK biosimulation toolkit originating from *
5 * Simbios, the NIH National Center for Physics-Based Simulation of *
6 * Biological Structures at Stanford, funded under the NIH Roadmap for *
7 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
8 * *
9 * Portions copyright (c) 2013 Stanford University and the Authors. *
10 * Authors: Michael Sherman *
11 * Contributors: *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 /* Investigate a hybrid compliant contact/rigid stiction model, using Tim's
25 box as a test case. */
26
27 //#define NDEBUG 1
28
29 #include "Simbody.h"
30
31 #include <string>
32 #include <iostream>
33 #include <exception>
34
35 using std::cout;
36 using std::endl;
37
38 using namespace SimTK;
39
40 //#define USE_TIMS_PARAMS
41 #define ANIMATE // off to get more accurate CPU time (you can still playback)
42
43 // Set to revert to the no-constraint stiction model for performance comparison
44 // with the new constraint-based one.
45 //#define USE_CONTINUOUS_STICTION
46
47 // Define this to run the simulation NTries times, saving the states and
48 // comparing them bitwise to see if the simulations are perfectly repeatable
49 // as they should be. You should see nothing but exact zeroes print out for
50 // second and subsequent runs.
51 //#define TEST_REPEATABILITY
52 static const int NTries=3;
53
54
55 // This is the continuous stiction model used in Simbody's compliant contact
56 // system for comparison with the new hybrid model. See end of this file for
57 // implementation.
58 static Real stribeck(Real us, Real ud, Real uv, Real v);
59
60 //==============================================================================
61 // MY HYBRID VERTEX CONTACT ELEMENT
62 //==============================================================================
63 // Given a contact material (with compliant material properties), this
64 // represents a compliant point-and-rigid-halfspace contact element. We track
65 // the height h of a vertex V on body B over a halfspace H on body P. The
66 // halfspace frame H serves as the contact frame, with Hz in the contact
67 // normal direction and Hxy spanning the tangent plane. The pose X_PH is
68 // a given constant.
69 //
70 // Normal force
71 // ------------
72 // If the vertex height h is above the halfspace surface (h>=0), no forces are
73 // generated. If below the surface (h<0) we generate a normal force of magnitude
74 // N=max(0, -k*h(1-d*hdot)), with N >= 0
75 // applied to both bodies at the contact point C, which is the point at h=0 just
76 // up the halfspace normal direction from the vertex (because we're considering
77 // the halfspace to be rigid). Denote by Cb the station (material point) of B
78 // that is coincident with C, and Cp the station of P that is coincident with C.
79 //
80 // Sliding force
81 // -------------
82 // Then if we are in sliding mode, we also generate a tangential force of
83 // magnitude T=(mu_d+mu_v*|v|)*N, where mu_d, mu_v are the dynamic and viscous
84 // coefficients of friction, and v is the velocity of Cb relative to Cp,
85 // projected into the tangent plane (a 2d vector). If |v|>tol then the
86 // tangential slip direction is s=v/|v|, and we record this as the previous slip
87 // direction s_prev. Otherwise the slip direction remains s=s_prev. In any case
88 // the tangential force applied is -T*s.
89 //
90 // Stiction force
91 // --------------
92 // If we are instead in stiction mode, then no sliding force is generated.
93 // Instead a pair of no-slip constraints is active, generating constraint
94 // multipliers (tx,ty), and we record s_prev=-[tx,ty]/|[tx,ty]| as the
95 // impending slip direction. Note that this will not be available until
96 // Acceleration stage, while the normal force and tangential sliding force can
97 // be calculated at Velocity stage.
98 //
99 // Witness function
100 // ----------------
101 // (trigger on + -> -):
102 // sliding_to_stiction = dot(v, s_prev)
103 // stiction_to_sliding = mu_s*N - |[tx,ty]|
104
105 // This is a velocity-stage cache entry.
106 struct MyHybridContactInfo {
MyHybridContactInfoMyHybridContactInfo107 MyHybridContactInfo()
108 : h(NaN), Cp(NaN), Cb(NaN), v_HCb(NaN), vSlipMag(NaN), f_HCb(NaN)
109 {
110 }
111 // Position info.
112 Real h; // signed distance, h<0 means contact
113 Vec3 Cp; // station on P coincident with the contact point
114 Vec3 Cb; // station on B coincident with the contact point
115 Rotation R_GH;
116
117 // Velocity info. H frame has z along contact normal, x,y in tangent plane.
118 Vec3 v_HCb; // velocity of Cb in the contact frame H
119 Real vSlipMag; // |(v_HCb.x, v_HCb.y)|
120 Vec3 f_HCb; // contact force on Cb in contact frame H
121 };
122
123 class MyHybridVertexContactElementImpl : public Force::Custom::Implementation {
124 public:
MyHybridVertexContactElementImpl(const GeneralForceSubsystem & forces,MobilizedBody & hsmobod,const UnitVec3 & hsn,Real hsh,MobilizedBody & vmobod,const Vec3 & vertex,const ContactMaterial & material)125 MyHybridVertexContactElementImpl(const GeneralForceSubsystem& forces,
126 MobilizedBody& hsmobod, const UnitVec3& hsn, Real hsh,
127 MobilizedBody& vmobod, const Vec3& vertex,
128 const ContactMaterial& material)
129 : m_matter(hsmobod.getMatterSubsystem()), m_forces(forces),
130 m_hsmobodx(hsmobod), m_X_PH(Rotation(hsn, ZAxis), hsh*hsn),
131 m_vmobodx(vmobod), m_vertex_B(vertex),
132 m_material(material),
133 m_noslipX(hsmobod, Vec3(NaN), m_X_PH.x(), hsmobod, vmobod),
134 m_noslipY(hsmobod, Vec3(NaN), m_X_PH.y(), hsmobod, vmobod),
135 m_index(-1), m_vtrans(NaN), // assign later
136 m_contactPointInP(NaN), m_recordedSlipDir(NaN)
137 {
138 m_noslipX.setDisabledByDefault(true);
139 m_noslipY.setDisabledByDefault(true);
140 }
141
setContactIndex(int index)142 void setContactIndex(int index) {m_index=index;}
setTransitionVelocity(Real vtrans)143 void setTransitionVelocity(Real vtrans) {m_vtrans=vtrans;}
144
initialize()145 void initialize() { // TODO
146 m_contactPointInP = NaN;
147 }
148
149 // Set the friction application point to be the projection of the contact
150 // point onto the contact plane. This will be used the next time stiction
151 // is enabled. Requires state realized to Position stage.
initializeForStiction(const State & s)152 void initializeForStiction(const State& s) {
153 const Real h = findContactPointInP(s, m_contactPointInP);
154 }
155
isInContact(const State & s) const156 bool isInContact(const State& s) const
157 { return findH(s) < 0; }
158
isSticking(const State & s) const159 bool isSticking(const State& s) const
160 {
161 #ifdef USE_CONTINUOUS_STICTION
162 return getActualSlipSpeed(s) <= m_vtrans;
163 #else
164 return !m_noslipX.isDisabled(s); // X,Y always on or off together
165 #endif
166 }
167
168 // Return a point in Ground coincident with the vertex.
whereToDisplay(const State & s) const169 Vec3 whereToDisplay(const State& s) const {
170 return getBodyB().findStationLocationInGround(s, m_vertex_B);
171 }
172
getActualSlipSpeed(const State & s) const173 Real getActualSlipSpeed(const State& s) const {
174 const MyHybridContactInfo& info = getContactInfo(s);
175 return info.vSlipMag;
176 }
177
178 // Return the normal force N >= 0 currently being generated by this
179 // contact. State must be realized to Stage::Velocity.
getNormalForce(const State & s) const180 Real getNormalForce(const State& s) const {
181 const MyHybridContactInfo& info = getContactInfo(s);
182 if (info.h >= 0) return 0;
183 const Real N = info.f_HCb[2]; // z component is normal
184 return N;
185 }
186
getHeight(const State & s) const187 Real getHeight(const State& s) const {
188 const MyHybridContactInfo& info = getContactInfo(s);
189 return info.h;
190 }
191
getHeightDot(const State & s) const192 Real getHeightDot(const State& s) const {
193 const MyHybridContactInfo& info = getContactInfo(s);
194 return info.v_HCb[2]; // velocity in plane normal direction
195 }
196
197 // Return the sliding force 2-vector in the halfspace plane that is being
198 // applied to the contact point station on body B. If we are sticking
199 // then this force is zero; call getStickingForce() to get the real value.
getSlidingForce(const State & s) const200 const Vec2& getSlidingForce(const State& s) const {
201 const MyHybridContactInfo& info = getContactInfo(s);
202 const Vec2& f = info.f_HCb.getSubVec<2>(0);
203 return f;
204 }
205
206 // Return the sliding velocity 2-vector in the halfspace plane of the
207 // contact point station on body B relative to the contact point station
208 // on body P.
getSlipVelocity(const State & s) const209 const Vec2& getSlipVelocity(const State& s) const {
210 const MyHybridContactInfo& info = getContactInfo(s);
211 const Vec2& v = info.v_HCb.getSubVec<2>(0);
212 return v;
213 }
214
215 // The way we constructed the NoSlip1D constraints makes the multipliers be
216 // the force on the half space on body P; we negate here so we'll get the
217 // force on the vertex body B instead.
getStictionForce(const State & s) const218 Vec2 getStictionForce(const State& s) const {
219 assert(isSticking(s));
220 #ifndef USE_CONTINUOUS_STICTION
221 return Vec2(-m_noslipX.getMultiplier(s), -m_noslipY.getMultiplier(s));
222 #else
223 return getSlidingForce(s);
224 #endif
225 }
226
recordImpendingSlipDir(const State & s) const227 void recordImpendingSlipDir(const State& s) const {
228 const Vec2 f = getStictionForce(s);
229 const Real fMag = f.norm();
230 const Vec2 dir = fMag==0 ? Vec2(1,0) : Vec2(-f/fMag);
231 m_recordedSlipDir = dir;
232 }
233
recordActualSlipDir(const State & s) const234 void recordActualSlipDir(const State& s) const {
235 const Vec2 v = getSlipVelocity(s);
236 const Real vMag = v.norm();
237 const Vec2 dir = vMag==0 ? Vec2(1,0) : Vec2(v/vMag);
238 m_recordedSlipDir = dir;
239 }
240
updatePrevSlipDirFromRecorded(State & s) const241 void updatePrevSlipDirFromRecorded(State& s) const {
242 setPrevSlipDir(s, m_recordedSlipDir);
243 }
244
calcSlipSpeedWitness(const State & s) const245 Real calcSlipSpeedWitness(const State& s) const {
246 if (getHeight(s) >= 0 || isSticking(s)) return 0;
247 const Vec2& slipDirPrev = getPrevSlipDir(s);
248 const Vec2& vNow = getSlipVelocity(s);
249 if (slipDirPrev.isNaN()) return vNow.norm();
250 return dot(vNow, slipDirPrev);
251 }
252
calcStictionForceWitness(const State & s) const253 Real calcStictionForceWitness(const State& s) const {
254 if (getHeight(s) >= 0 || !isSticking(s)) return 0;
255 const Real mu_s = m_material.getStaticFriction();
256 const Real N = getNormalForce(s);
257 const Vec2 f = getStictionForce(s);
258 const Real fmag = f.norm();
259 return mu_s*N - fmag;
260 }
261
262
263
264 // Note that initializeForStiction() must have been called first.
enableStiction(State & s) const265 void enableStiction(State& s) const
266 { m_noslipX.setContactPoint(s, m_contactPointInP);
267 m_noslipY.setContactPoint(s, m_contactPointInP);
268 m_noslipX.enable(s); m_noslipY.enable(s); }
269
disableStiction(State & s) const270 void disableStiction(State& s) const
271 { m_noslipX.disable(s); m_noslipY.disable(s); }
272
273 // Return height of vertex over plane; negative if penetrated. You can
274 // call this at Stage::Position.
findH(const State & state) const275 Real findH(const State& state) const {
276 const Vec3 V_P = findVInP(state); // location of V in P
277 const Real h = dot(V_P-m_X_PH.p(), m_X_PH.z());
278 return h;
279 }
280
281 // Returns height of vertex over plane same as findH(); also returns
282 // contact point (projection of V onto plane along Hz).
findContactPointInP(const State & state,Vec3 & contactPointInP) const283 Real findContactPointInP(const State& state, Vec3& contactPointInP) const {
284 const Vec3 V_P = findVInP(state); // location of V in P
285 const Real h = dot(V_P-m_X_PH.p(), m_X_PH.z());
286 contactPointInP = V_P - h*m_X_PH.z();
287 return h;
288 }
289
290 // Calculate v_eff, the direction to be opposed by the sliding force.
getEffectiveSlipDir(const State & s,const Vec2 & vSlip,Real vSlipMag) const291 Vec2 getEffectiveSlipDir(const State& s, const Vec2& vSlip, Real vSlipMag) const {
292 const Vec2 prevVslipDir = getPrevSlipDir(s);
293 if (shouldUpdate(vSlip, vSlipMag, prevVslipDir, m_vtrans))
294 return vSlip/vSlipMag;
295 else return prevVslipDir;
296 }
297
298 // Return the slip velocity as recorded at the end of the last time step.
getPrevSlipDir(const State & state) const299 const Vec2& getPrevSlipDir(const State& state) const {
300 const Vec2& prevSlipDir = Value<Vec2>::downcast
301 (m_forces.getDiscreteVariable(state, m_prevSlipDirIx));
302 return prevSlipDir;
303 }
304 // Modify the discrete state variable directly.
setPrevSlipDir(State & state,const Vec2 & slipDir) const305 void setPrevSlipDir(State& state, const Vec2& slipDir) const {
306 Vec2& prevSlipDir = Value<Vec2>::updDowncast
307 (m_forces.updDiscreteVariable(state, m_prevSlipDirIx));
308 prevSlipDir = slipDir;
309 SimTK_DEBUG3("STATE CHG %d: prevDir to %g %g\n",
310 m_index, slipDir[0], slipDir[1]);
311 }
312
313 // Get access to the auto-update cache variable that will update the
314 // previous slip direction state variable if the step is accepted.
updPrevSlipDirAutoUpdateValue(const State & state) const315 Vec2& updPrevSlipDirAutoUpdateValue(const State& state) const {
316 Vec2& prevSlipUpdate = Value<Vec2>::updDowncast
317 (m_forces.updDiscreteVarUpdateValue(state, m_prevSlipDirIx));
318 return prevSlipUpdate;
319 }
320 // Mark the auto-update cache entry valid.
markPrevSlipDirAutoUpdateValueValid(const State & state) const321 void markPrevSlipDirAutoUpdateValueValid(const State& state) const {
322 m_forces.markDiscreteVarUpdateValueRealized(state, m_prevSlipDirIx);
323 }
324
getContactInfo(const State & state) const325 const MyHybridContactInfo& getContactInfo(const State& state) const {
326 const MyHybridContactInfo& info =
327 Value<MyHybridContactInfo>::downcast
328 (m_forces.getCacheEntry(state, m_contactInfoIx));
329 return info;
330 }
331
updContactInfo(const State & state) const332 MyHybridContactInfo& updContactInfo(const State& state) const {
333 MyHybridContactInfo& info =
334 Value<MyHybridContactInfo>::updDowncast
335 (m_forces.updCacheEntry(state, m_contactInfoIx));
336 return info;
337 }
338 //--------------------------------------------------------------------------
339 // Custom force virtuals
340 // Apply the normal force, and the sliding friction force if it is enabled.
341 // This is called during realize(Dynamics).
calcForce(const State & state,Vector_<SpatialVec> & bodyForces,Vector_<Vec3> & particleForces,Vector & mobilityForces) const342 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
343 Vector_<Vec3>& particleForces, Vector& mobilityForces) const
344 override
345 {
346 const MyHybridContactInfo& info = getContactInfo(state);
347 if (info.h >= 0)
348 return; // no contact
349
350 const Vec3 f_GCb = info.R_GH * info.f_HCb; // re-express in Ground
351 getBodyB().applyForceToBodyPoint(state, info.Cb, f_GCb, bodyForces);
352 getBodyP().applyForceToBodyPoint(state, info.Cp, -f_GCb, bodyForces);
353 }
354
355 // The normal force stores energy as 2/5 k h^(5/2) when h<0.
calcPotentialEnergy(const State & state) const356 Real calcPotentialEnergy(const State& state) const override {
357 const MyHybridContactInfo& info = getContactInfo(state);
358 if (info.h >= 0)
359 return 0; // no contact
360 const Real h52 = square(info.h)*std::sqrt(-info.h);
361 const Real k = m_material.getStiffness();
362 return 0.4*k*h52;
363 }
364
365 // Allocate state variable for storing the previous sliding direction.
realizeTopology(State & state) const366 void realizeTopology(State& state) const override {
367 // The previous sliding direction is used in an event witness that
368 // is evaluated at Velocity stage.
369 m_prevSlipDirIx = m_forces.allocateAutoUpdateDiscreteVariable
370 (state, Stage::Velocity, new Value<Vec2>(Vec2(NaN)),
371 Stage::Velocity);
372
373 m_contactInfoIx = m_forces.allocateCacheEntry
374 (state, Stage::Velocity, new Value<MyHybridContactInfo>());
375 }
376
377
378 // Calculate everything here and save in contact info cache entry where
379 // it can be retrieved for generating forces, reporting, etc.
realizeVelocity(const State & state) const380 void realizeVelocity(const State& state) const override {
381 MyHybridContactInfo& info = updContactInfo(state);
382
383 // Forces generated only if h<0. Cp always be the projection of the
384 // vertex onto the halfspace surface.
385 info.h = findContactPointInP(state, info.Cp);
386
387 const Rotation& R_PH = m_X_PH.R();
388 const Rotation& R_GP = getBodyP().getBodyRotation(state);
389 info.R_GH = R_GP*R_PH;
390
391 // Station of B coincident with the contact point.
392 info.Cb = info.h < 0
393 ? findPointOfBCoincidentWithPointOfP(state, info.Cp)
394 : getVertexOnB();
395
396 // Velocity of B's contact station in P.
397 const Vec3 v_PCb = getBodyB().findStationVelocityInAnotherBody
398 (state, info.Cb, getBodyP());
399 info.v_HCb = ~R_PH*v_PCb; // re-express in H
400 const Real hdot = info.v_HCb[2]; // z component
401 const Vec2 vSlip_HC(info.v_HCb[0], info.v_HCb[1]);
402 info.vSlipMag = vSlip_HC.norm();
403
404 const Real k = m_material.getStiffness(),
405 c = m_material.getDissipation(),
406 mu_d = m_material.getDynamicFriction(),
407 mu_s = m_material.getStaticFriction(),
408 mu_v = m_material.getViscousFriction();
409
410 info.f_HCb = Vec3(0);
411 if (info.h >= 0)
412 return; // no contact
413
414 const Real h32 = -info.h*sqrt(-info.h); // |h| ^ (3/2)
415 const Real N = std::max(Real(0), k*h32*(1-c*hdot));
416 if (N==0)
417 return; // no contact force
418
419 // N is the Hz component of the force on Cb.
420 Vec2 fT_HCb(0); // This will be the (Hx,Hy) components of force on Cb.
421 #ifdef USE_CONTINUOUS_STICTION
422 {
423 // Make v unitless velocity and scale viscous coefficient to match.
424 const Real v = info.vSlipMag / m_vtrans;
425 const Real mu=stribeck(mu_s,mu_d,mu_v*m_vtrans,v);
426 const Real T = mu*N;
427 fT_HCb = (-T/info.vSlipMag)*vSlip_HC;
428 }
429 #else
430 if (!isSticking(state)) {
431 // Apply sliding force
432 const Real T = (mu_d + mu_v*info.vSlipMag)*N;
433 fT_HCb = -T*getEffectiveSlipDir(state, vSlip_HC, // in Hxy
434 info.vSlipMag);
435 }
436 #endif
437
438 info.f_HCb = Vec3(fT_HCb[0], fT_HCb[1], N); // force on Cb, in H
439 }
440
441 // If we're sticking, set the update value for the previous slip direction
442 // to the opposite of the stiction force direction.
443 // If we're sliding, set the update value for the previous slip direction
444 // if the current slip velocity is usable.
445 #ifndef USE_CONTINUOUS_STICTION
realizeAcceleration(const State & state) const446 void realizeAcceleration(const State& state) const override {
447 const MyHybridContactInfo& info = getContactInfo(state);
448 const Vec2& prevSlipDir = getPrevSlipDir(state);
449
450 if (info.h >= 0) {
451 // No contact. Forget previous slip direction.
452 if (!prevSlipDir.isNaN()) {
453 SimTK_DEBUG1("%d LIFTOFF UPDATE, forget prevSlipDir\n", m_index);
454 updPrevSlipDirAutoUpdateValue(state).setToNaN();
455 markPrevSlipDirAutoUpdateValueValid(state);
456 }
457 return;
458 }
459
460 // Sticking.
461 if (isSticking(state)) {
462 const Vec2 f_HCb = getStictionForce(state);
463 const Real fMag = f_HCb.norm();
464 if (fMag > 0) {
465 Vec2& prevSlipUpdate = updPrevSlipDirAutoUpdateValue(state);
466 prevSlipUpdate = -f_HCb / fMag;
467 #ifndef NDEBUG
468 printf("%d STICKING UPDATE: prevSlipDir=%g %g; now=%g %g\n",
469 m_index, getPrevSlipDir(state)[0],getPrevSlipDir(state)[1],
470 prevSlipUpdate[0], prevSlipUpdate[1]);
471 #endif
472 markPrevSlipDirAutoUpdateValueValid(state);
473 }
474 return;
475 }
476
477 // Sliding.
478 const Vec2& vSlip_HCb = info.v_HCb.getSubVec<2>(0); // x,y
479
480 if (shouldUpdate(vSlip_HCb, info.vSlipMag, prevSlipDir, m_vtrans)) {
481 Vec2& prevSlipUpdate = updPrevSlipDirAutoUpdateValue(state);
482 prevSlipUpdate = vSlip_HCb / info.vSlipMag;
483 markPrevSlipDirAutoUpdateValueValid(state);
484
485 #ifndef NDEBUG
486 printf("%d SLIDING UPDATE: prevSlipDir=%g %g; now=%g %g; |v|=%g dot=%g vdot=%g\n",
487 m_index, prevSlipDir[0],prevSlipDir[1],
488 prevSlipUpdate[0],prevSlipUpdate[1], info.vSlipMag,
489 ~prevSlipUpdate*prevSlipDir, ~vSlip_HCb*prevSlipDir);
490 #endif
491 } else {
492 #ifndef NDEBUG
493 printf("%d SLIDING; NO UPDATE: prevSlipDir=%g %g; Vnow=%g %g; |v|=%g vdot=%g\n",
494 m_index,
495 prevSlipDir[0],prevSlipDir[1],vSlip_HCb[0],vSlip_HCb[1],
496 info.vSlipMag, ~vSlip_HCb*prevSlipDir);
497 #endif
498 }
499 }
500 #endif
501
writeFrictionInfo(const State & s,const String & indent,std::ostream & o) const502 std::ostream& writeFrictionInfo(const State& s, const String& indent,
503 std::ostream& o) const
504 {
505 o << indent;
506 if (!isInContact(s)) o << "OFF";
507 else if (isSticking(s)) o << "STICK";
508 else o << "SLIP";
509
510 const Vec2 v = getSlipVelocity(s);
511 const Vec2 pd = getPrevSlipDir(s);
512 const Vec2 f = isSticking(s) ? getStictionForce(s)
513 : getSlidingForce(s);
514 o << " prevDir=" << ~pd << " V=" << ~v << " Vdot=" << ~v*pd
515 << " F=" << ~f << endl;
516 return o;
517 }
518
519
showContactForces(const State & s,Array_<DecorativeGeometry> & geometry) const520 void showContactForces(const State& s, Array_<DecorativeGeometry>& geometry)
521 const
522 {
523 const Real Scale = 0.01;
524 const Real NH = getNormalForce(s);
525
526 #ifdef USE_CONTINUOUS_STICTION
527 const bool isInStiction = getActualSlipSpeed(s) <= m_vtrans;
528 const Vec2 fH = getSlidingForce(s);
529 #else
530 const bool isInStiction = isSticking(s);
531 const Vec2 fH = isSticking(s) ? getStictionForce(s) : getSlidingForce(s);
532 #endif
533
534 if (fH.normSqr() < square(SignificantReal) && NH < SignificantReal)
535 return;
536
537 const MyHybridContactInfo& info = getContactInfo(s);
538 const Vec3 fG = info.R_GH * Vec3(fH[0],fH[1],0); // friction
539 const Vec3 NG = info.R_GH * Vec3(0, 0, NH); // normal
540
541 const MobilizedBody& bodyB = getBodyB();
542 const Vec3& stationB = getVertexOnB();
543 const Vec3 stationG = bodyB.getBodyTransform(s)*stationB;
544 const Vec3 endfG = stationG - Scale*fG;
545 const Vec3 endNG = stationG + Scale*NG;
546 geometry.push_back(DecorativeLine(endfG + Vec3(0,.05,0),
547 stationG + Vec3(0,.05,0))
548 .setColor(isInStiction?Green:Orange));
549 geometry.push_back(DecorativeLine(endNG + Vec3(0,.05,0),
550 stationG + Vec3(0,.05,0))
551 .setColor(Red));
552 }
553
getIndex() const554 int getIndex() const {return m_index;}
getBodyB() const555 const MobilizedBody& getBodyB() const
556 { return m_matter.getMobilizedBody(m_vmobodx); }
getVertexOnB() const557 const Vec3& getVertexOnB() const {return m_vertex_B;}
getBodyP() const558 const MobilizedBody& getBodyP() const
559 { return m_matter.getMobilizedBody(m_hsmobodx); }
getX_PH() const560 const Transform& getX_PH() const {return m_X_PH;}
getMaterial() const561 const ContactMaterial& getMaterial() const {return m_material;}
562
563 //--------------------------------------------------------------------------
564 private:
565 // Determine whether the current slip velocity is reliable enough that
566 // we should use it to replace the previous slip velocity.
shouldUpdate(const Vec2 & vSlip,Real vSlipMag,const Vec2 & prevSlipDir,Real velTol)567 static bool shouldUpdate(const Vec2& vSlip, Real vSlipMag,
568 const Vec2& prevSlipDir, Real velTol) {
569 if (prevSlipDir.isNaN())
570 return vSlipMag > 0; // we'll take anything
571
572 // Check for reversal.
573 bool reversed = (~vSlip*prevSlipDir < 0);
574 return !reversed && (vSlipMag > velTol);
575 }
576
577 // Find the location of the vertex on B, measured and expressed in P.
findVInP(const State & s) const578 Vec3 findVInP(const State& s) const {
579 return getBodyB().findStationLocationInAnotherBody
580 (s, m_vertex_B, getBodyP());
581 }
582
583 // Find the location and velocity of the vertex on B, measured from and
584 // expressed in P.
findVDotInP(const State & s) const585 Vec3 findVDotInP(const State& s) const {
586 return getBodyB().findStationVelocityInAnotherBody
587 (s, m_vertex_B, getBodyP());
588 }
589
590
findPointOfBCoincidentWithPointOfP(const State & s,const Vec3 & r_P) const591 Vec3 findPointOfBCoincidentWithPointOfP
592 (const State& s, const Vec3& r_P) const
593 {
594 return getBodyP().findStationLocationInAnotherBody(s, r_P, getBodyB());
595 }
596
597
598 private:
599 const GeneralForceSubsystem& m_forces;
600 const SimbodyMatterSubsystem& m_matter;
601 const MobilizedBodyIndex m_hsmobodx; // body P with halfspace
602 Transform m_X_PH; // halfspace frame in P
603 const MobilizedBodyIndex m_vmobodx; // body B with vertex
604 Vec3 m_vertex_B; // vertex location in B
605 ContactMaterial m_material; // composite material props
606
607 Real m_vtrans; // transition velocity for
608 // Stribeck stiction
609
610 Constraint::NoSlip1D m_noslipX;
611 Constraint::NoSlip1D m_noslipY;
612
613 int m_index; // unique id for this contact
614
615 // This is recorded at Position stage prior to turning on stiction
616 // constraints; then we use it to set the contact point in noslipX,Y.
617 Vec3 m_contactPointInP;
618
619 // This is recorded during event handling and then used to set the
620 // remembered previous slip direction at the end of the handler to make
621 // sure all witness functions are positive then.
622 mutable Vec2 m_recordedSlipDir;
623
624 // Set during realizeTopology().
625 mutable DiscreteVariableIndex m_prevSlipDirIx; // previous slip direction
626 mutable CacheEntryIndex m_contactInfoIx;
627 };
628
629
630 //==============================================================================
631 // MY UNILATERAL CONSTRAINT SET
632 //==============================================================================
633
634 // These are indices into the unilateral constraint set arrays.
635 struct MyElementSubset {
clearMyElementSubset636 void clear()
637 { m_contact.clear();m_sliding.clear();m_stiction.clear();
638 m_ineligible.clear(); }
639 Array_<int> m_contact;
640 Array_<int> m_sliding; // subset of above that can only be sliding
641 Array_<int> m_stiction; // subset of above that might be in stiction
642 Array_<int> m_ineligible; // stiction on, but not in contact any more
643 };
644
645 class MyUnilateralConstraintSet {
646 public:
647 // Transition velocity: if a slip velocity is smaller than this the
648 // contact is a candidate for stiction.
MyUnilateralConstraintSet(const MultibodySystem & mbs,Real transitionVelocity)649 MyUnilateralConstraintSet(const MultibodySystem& mbs,
650 Real transitionVelocity)
651 : m_mbs(mbs), m_transitionVelocity(transitionVelocity) {}
652
653 // Ownership of this force element belongs to the System; we're just keeping
654 // a reference to it here.
addHybridElement(MyHybridVertexContactElementImpl * vertex)655 int addHybridElement(MyHybridVertexContactElementImpl* vertex) {
656 const int index = (int)m_hybrid.size();
657 m_hybrid.push_back(vertex);
658 vertex->setContactIndex(index);
659 vertex->setTransitionVelocity(m_transitionVelocity);
660 return index;
661 }
662
getTransitionVelocity() const663 Real getTransitionVelocity() const {return m_transitionVelocity;}
setTransitionVelocity(Real v)664 void setTransitionVelocity(Real v) {m_transitionVelocity=v;}
665
getNumContactElements() const666 int getNumContactElements() const {return (int)m_hybrid.size();}
getContactElement(int ix) const667 const MyHybridVertexContactElementImpl& getContactElement(int ix) const
668 { return *m_hybrid[ix]; }
updContactElement(int ix)669 MyHybridVertexContactElementImpl& updContactElement(int ix)
670 { return *m_hybrid[ix]; }
671
672 // Initialize all runtime fields in the contact & friction elements.
initialize()673 void initialize()
674 {
675 for (unsigned i=0; i < m_hybrid.size(); ++i)
676 m_hybrid[i]->initialize();
677 }
678
679 // Return the contact and friction elements that might be involved in
680 // generating contact forces at the current state. Candidate contact
681 // elements are those that are (a) already enabled, or (b) for which
682 // perr <= posTol and verr <= velTol. Candidate friction elements are those
683 // whose normal force master is unconditional or a candidate and (a) which
684 // are already sticking, or (b) for which vslip <= velTol, or (c) for which
685 // vslip opposes the previous slip direction, meaning it has reversed and
686 // must have passed through zero during the last step. These are the elements
687 // that can be activated without making any changes to the configuration or
688 // velocity state variables, except slightly for constraint projection.
689 //
690 // We also record the friction elements that, if their masters are active,
691 // can only slide because they have a significant slip velocity. State must
692 // be realized through Velocity stage.
findCandidateElements(const State & s,Real velTol,MyElementSubset & candidates) const693 void findCandidateElements(const State& s,
694 Real velTol,
695 MyElementSubset& candidates) const
696 {
697 candidates.clear();
698 for (unsigned i=0; i < m_hybrid.size(); ++i) {
699 if (!m_hybrid[i]->isInContact(s)) {
700 if (m_hybrid[i]->isSticking(s)) {
701 candidates.m_ineligible.push_back(i); // must disable
702 SimTK_DEBUG2("%d NOW INELIGIBLE because h=%g.\n",
703 i, m_hybrid[i]->findH(s));
704 }
705 continue;
706 }
707
708 candidates.m_contact.push_back(i);
709
710 if (m_hybrid[i]->isSticking(s)
711 || m_hybrid[i]->getActualSlipSpeed(s) <= velTol
712 || m_hybrid[i]->calcSlipSpeedWitness(s) <= 0)
713 {
714 m_hybrid[i]->initializeForStiction(s);
715 candidates.m_stiction.push_back(i); // could stick or slide
716 } else
717 candidates.m_sliding.push_back(i); // can only slide
718 }
719 }
720
721 // Look through the given constraint subset and enable any constraints
722 // that are currently disabled, and disable any constraints that are still
723 // on after liftoff. Returns true if any change was made.
enableConstraintSubset(const MyElementSubset & subset,State & state) const724 bool enableConstraintSubset(const MyElementSubset& subset,
725 State& state) const
726 {
727 bool changedSomething = false;
728
729 // Disable all ineligible constraints.
730 for (unsigned i=0; i < subset.m_ineligible.size(); ++i) {
731 const int which = subset.m_ineligible[i];
732 const MyHybridVertexContactElementImpl& fric =
733 getContactElement(which);
734 SimTK_DEBUG1("%d DISABLING INELIGIBLE STICTION.\n", i);
735 fric.disableStiction(state);
736 changedSomething = true;
737 }
738
739 // Enable all stiction constraints.
740 for (unsigned i=0; i < subset.m_stiction.size(); ++i) {
741 const int which = subset.m_stiction[i];
742 const MyHybridVertexContactElementImpl& fric =
743 getContactElement(which);
744 if (!fric.isSticking(state)) {
745 SimTK_DEBUG1("%d ENABLING CANDIDATE STICTION.\n", i);
746 fric.enableStiction(state);
747 changedSomething = true;
748 }
749 }
750
751 m_mbs.realize(state, Stage::Instance);
752 return changedSomething;
753 }
754
755 // All event handlers call this method before returning. Given a state for
756 // which no (further) impulse is required, here we decide which contact and
757 // stiction constraints are active, and ensure that they satisfy the
758 // required constraint tolerances to the given accuracy. For sliding
759 // contacts, we will have recorded the slip or impending slip direction and
760 // converged the normal forces.
761 // TODO: in future this may return indicating that an impulse is required
762 // after all, as in Painleve's paradox.
763 void selectActiveConstraints(State& state, Real accuracy) const;
764
765 // This is the inner loop of selectActiveConstraints(). Given a set of
766 // candidates to consider, it finds an active subset and enables those
767 // constraints.
768 void findActiveCandidates(State& state,
769 const MyElementSubset& candidates) const;
770
771 // In Debug mode, produce a useful summary of the current state of the
772 // contact and friction elements.
773 void showConstraintStatus(const State& state, const String& place) const;
774
~MyUnilateralConstraintSet()775 ~MyUnilateralConstraintSet() {
776 // Nothing to delete since we are only holding references.
777 }
778
getMultibodySystem() const779 const MultibodySystem& getMultibodySystem() const {return m_mbs;}
780 private:
781 const MultibodySystem& m_mbs;
782 Real m_transitionVelocity;
783 Array_<MyHybridVertexContactElementImpl*> m_hybrid; // unowned ref
784 };
785
786
787
788 //==============================================================================
789 // STATE SAVER
790 //==============================================================================
791 // This reporter is called now and again to save the current state so we can
792 // play back a movie at the end.
793 class StateSaver : public PeriodicEventReporter {
794 public:
StateSaver(const MultibodySystem & mbs,const MyUnilateralConstraintSet & unis,const Integrator & integ,Real reportInterval)795 StateSaver(const MultibodySystem& mbs,
796 const MyUnilateralConstraintSet& unis,
797 const Integrator& integ,
798 Real reportInterval)
799 : PeriodicEventReporter(reportInterval),
800 m_mbs(mbs), m_unis(unis), m_integ(integ)
801 { m_states.reserve(2000); }
802
~StateSaver()803 ~StateSaver() {}
804
clear()805 void clear() {m_states.clear();}
getNumSavedStates() const806 int getNumSavedStates() const {return (int)m_states.size();}
getState(int n) const807 const State& getState(int n) const {return m_states[n];}
808
handleEvent(const State & s) const809 void handleEvent(const State& s) const override {
810 const SimbodyMatterSubsystem& matter=m_mbs.getMatterSubsystem();
811 const SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
812 m_mbs.realize(s, Stage::Acceleration);
813
814 #ifndef NDEBUG
815 printf("%3d: %5g mom=%g,%g E=%g", m_integ.getNumStepsTaken(),
816 s.getTime(),
817 PG[0].norm(), PG[1].norm(), m_mbs.calcEnergy(s));
818 cout << " Triggers=" << s.getEventTriggers() << endl;
819 m_unis.showConstraintStatus(s, "STATE SAVER");
820 #endif
821
822 m_states.push_back(s);
823 }
824 private:
825 const MultibodySystem& m_mbs;
826 const MyUnilateralConstraintSet& m_unis;
827 const Integrator& m_integ;
828 mutable Array_<State> m_states;
829 };
830
831 // A periodic event reporter that does nothing; useful for exploring the
832 // effects of interrupting the simulation.
833 class Nada : public PeriodicEventReporter {
834 public:
Nada(Real reportInterval)835 explicit Nada(Real reportInterval)
836 : PeriodicEventReporter(reportInterval) {}
837
handleEvent(const State & s) const838 void handleEvent(const State& s) const override {
839 #ifndef NDEBUG
840 printf("%7g NADA\n", s.getTime());
841 #endif
842 }
843 };
844
845
846 //==============================================================================
847 // SHOW CONTACT
848 //==============================================================================
849 // For each visualization frame, generate ephemeral geometry to show just
850 // during this frame, based on the current State.
851 class ShowContact : public DecorationGenerator {
852 public:
ShowContact(const MyUnilateralConstraintSet & unis)853 ShowContact(const MyUnilateralConstraintSet& unis)
854 : m_unis(unis) {}
855
generateDecorations(const State & state,Array_<DecorativeGeometry> & geometry)856 void generateDecorations(const State& state,
857 Array_<DecorativeGeometry>& geometry) override
858 {
859 for (int i=0; i < m_unis.getNumContactElements(); ++i) {
860 const MyHybridVertexContactElementImpl& contact =
861 m_unis.getContactElement(i);
862 const Vec3 loc = contact.whereToDisplay(state);
863 if (contact.isInContact(state)) {
864 geometry.push_back(DecorativeSphere(.1)
865 .setTransform(loc)
866 .setColor(Red).setOpacity(.25));
867 String text = "LOCKED";
868 text = contact.isSticking(state) ? "STICKING"
869 : "CONTACT";
870 m_unis.getMultibodySystem().realize(state, Stage::Acceleration);
871 contact.showContactForces(state, geometry);
872 geometry.push_back(DecorativeText(String(i)+"-"+text)
873 .setColor(White).setScale(.1)
874 .setTransform(loc+Vec3(0,.04,0)));
875 } else {
876 geometry.push_back(DecorativeText(String(i))
877 .setColor(White).setScale(.1)
878 .setTransform(loc+Vec3(0,.02,0)));
879 }
880 }
881 }
882 private:
883 const MyUnilateralConstraintSet& m_unis;
884 };
885
886
887 //==============================================================================
888 // STICTION ON HANDLER
889 //==============================================================================
890 // Allocate one of these for each contact constraint that has friction. This
891 // handler takes care of turning on the stiction constraints when the sliding
892 // velocity drops to zero.
893 class StictionOn: public TriggeredEventHandler {
894 public:
StictionOn(const MultibodySystem & system,const MyUnilateralConstraintSet & unis,unsigned which)895 StictionOn(const MultibodySystem& system,
896 const MyUnilateralConstraintSet& unis,
897 unsigned which)
898 : TriggeredEventHandler(Stage::Velocity),
899 m_mbs(system), m_unis(unis), m_which(which)
900 {
901 getTriggerInfo().setTriggerOnRisingSignTransition(false);
902 }
903
904 // This is the witness function. It is positive as long as we continue
905 // to slide in the same direction; negative means reversal.
getValue(const State & state) const906 Real getValue(const State& state) const override {
907 const MyHybridVertexContactElementImpl& contact =
908 m_unis.getContactElement(m_which);
909 if (!contact.isInContact(state)) return 0;
910 const Real signedSlipSpeed = contact.calcSlipSpeedWitness(state);
911 return signedSlipSpeed;
912 }
913
handleEvent(State & s,Real accuracy,bool & shouldTerminate) const914 void handleEvent
915 (State& s, Real accuracy, bool& shouldTerminate) const override
916 {
917 ++m_counter;
918 //printf("StictionOn #%d\n", m_counter);
919 SimTK_DEBUG2("\nhandle %d slide->stick@%.17g\n", m_which, s.getTime());
920 SimTK_DEBUG("\n----------------------------------------------------\n");
921 SimTK_DEBUG2("STICTION ON triggered by friction element %d @t=%.15g\n",
922 m_which, s.getTime());
923 m_mbs.realize(s, Stage::Acceleration);
924
925 #ifndef NDEBUG
926 m_unis.showConstraintStatus(s, "ENTER STICTION ON");
927 cout << " entry triggers=" << s.getEventTriggers() << "\n";
928 #endif
929
930 m_unis.selectActiveConstraints(s, accuracy);
931
932 #ifndef NDEBUG
933 m_mbs.realize(s, Stage::Acceleration);
934 cout << " exit triggers=" << s.getEventTriggers() << "\n";
935 #endif
936
937 SimTK_DEBUG("STICTION ON done.\n");
938 SimTK_DEBUG("----------------------------------------------------\n");
939 }
940
941 private:
942 const MultibodySystem& m_mbs;
943 const MyUnilateralConstraintSet& m_unis;
944 const int m_which; // one of the contact elements
945 static int m_counter;
946 };
947 int StictionOn::m_counter = 0;
948
949
950
951 //==============================================================================
952 // STICTION OFF HANDLER
953 //==============================================================================
954 // Allocate one of these for each contact constraint. This handler takes
955 // care of turning off stiction constraints when the stiction force magnitude
956 // exceeds mu*N.
957 class StictionOff: public TriggeredEventHandler {
958 public:
StictionOff(const MultibodySystem & system,const MyUnilateralConstraintSet & unis,unsigned which)959 StictionOff(const MultibodySystem& system,
960 const MyUnilateralConstraintSet& unis,
961 unsigned which)
962 : TriggeredEventHandler(Stage::Acceleration),
963 m_mbs(system), m_unis(unis), m_which(which)
964 {
965 getTriggerInfo().setTriggerOnRisingSignTransition(false);
966 }
967
968 // This is the witness function. It is positive as long as mu_s*N is greater
969 // than the friction force magnitude.
getValue(const State & state) const970 Real getValue(const State& state) const override {
971 const MyHybridVertexContactElementImpl& contact =
972 m_unis.getContactElement(m_which);
973 if (!contact.isInContact(state)) return 0;
974 const Real capacity = contact.calcStictionForceWitness(state);
975 return capacity; // how much stiction capacity is left
976 }
977
handleEvent(State & s,Real accuracy,bool & shouldTerminate) const978 void handleEvent
979 (State& s, Real accuracy, bool& shouldTerminate) const override
980 {
981 ++m_counter;
982 //printf("StictionOff #%d\n", m_counter);
983 SimTK_DEBUG2("\nhandle %d stick->slide@%.17g\n", m_which, s.getTime());
984 SimTK_DEBUG("\n----------------------------------------------------\n");
985 SimTK_DEBUG2("STICTION OFF triggered by friction element %d @t=%.15g\n",
986 m_which, s.getTime());
987 m_mbs.realize(s, Stage::Acceleration);
988
989 #ifndef NDEBUG
990 m_unis.showConstraintStatus(s, "ENTER STICTION OFF");
991 cout << " triggers=" << s.getEventTriggers() << "\n";
992 #endif
993
994 m_unis.selectActiveConstraints(s, accuracy);
995
996 #ifndef NDEBUG
997 m_mbs.realize(s, Stage::Acceleration);
998 cout << " exit triggers=" << s.getEventTriggers() << "\n";
999 #endif
1000
1001 SimTK_DEBUG("STICTION OFF done.\n");
1002 SimTK_DEBUG("----------------------------------------------------\n");
1003 }
1004
1005 private:
1006 const MultibodySystem& m_mbs;
1007 const MyUnilateralConstraintSet& m_unis;
1008 const int m_which; // one of the friction elements
1009 static int m_counter;
1010 };
1011 int StictionOff::m_counter = 0;
1012
1013
1014
1015 //==============================================================================
1016 // MY PUSH FORCE
1017 //==============================================================================
1018 // This is a force element that generates a constant force on a body for a
1019 // specified time interval. It is useful to perturb the system to force it
1020 // to transition from sticking to sliding, for example.
1021 class MyPushForceImpl : public Force::Custom::Implementation {
1022 public:
MyPushForceImpl(const MobilizedBody & bodyB,const Vec3 & stationB,const Vec3 & forceG,Real onTime,Real offTime)1023 MyPushForceImpl(const MobilizedBody& bodyB,
1024 const Vec3& stationB,
1025 const Vec3& forceG, // force direction in Ground!
1026 Real onTime,
1027 Real offTime)
1028 : m_bodyB(bodyB), m_stationB(stationB), m_forceG(forceG),
1029 m_on(onTime), m_off(offTime)
1030 { }
1031
1032 //--------------------------------------------------------------------------
1033 // Custom force virtuals
calcForce(const State & state,Vector_<SpatialVec> & bodyForces,Vector_<Vec3> & particleForces,Vector & mobilityForces) const1034 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
1035 Vector_<Vec3>& particleForces, Vector& mobilityForces) const
1036 override
1037 {
1038 if (!(m_on <= state.getTime() && state.getTime() <= m_off))
1039 return;
1040
1041 m_bodyB.applyForceToBodyPoint(state, m_stationB, m_forceG, bodyForces);
1042
1043 //SimTK_DEBUG4("PUSHING @t=%g (%g,%g,%g)\n", state.getTime(),
1044 // m_forceG[0], m_forceG[1], m_forceG[2]);
1045 }
1046
1047 // No potential energy.
calcPotentialEnergy(const State & state) const1048 Real calcPotentialEnergy(const State& state) const override {return 0;}
1049
calcDecorativeGeometryAndAppend(const State & state,Stage stage,Array_<DecorativeGeometry> & geometry) const1050 void calcDecorativeGeometryAndAppend
1051 (const State& state, Stage stage,
1052 Array_<DecorativeGeometry>& geometry) const override
1053 {
1054 const Real ScaleFactor = 0.1;
1055 if (stage != Stage::Time) return;
1056 if (!(m_on <= state.getTime() && state.getTime() <= m_off))
1057 return;
1058 const Vec3 stationG = m_bodyB.findStationLocationInGround(state, m_stationB);
1059 geometry.push_back(DecorativeLine(stationG-ScaleFactor*m_forceG, stationG)
1060 .setColor(Yellow)
1061 .setLineThickness(3));
1062 }
1063 private:
1064 const MobilizedBody& m_bodyB;
1065 const Vec3 m_stationB;
1066 const Vec3 m_forceG;
1067 Real m_on;
1068 Real m_off;
1069 };
1070
1071
1072
1073 //==============================================================================
1074 // MAIN
1075 //==============================================================================
main(int argc,char ** argv)1076 int main(int argc, char** argv) {
1077 try { // If anything goes wrong, an exception will be thrown.
1078 const Real ReportInterval=1./30;
1079 const Vec3 BrickHalfDims(.1, .25, .5);
1080 const Real BrickMass = 10;
1081 #ifdef USE_TIMS_PARAMS
1082 const Real RunTime=16; // Tim's time
1083 const Real Stiffness = 2e7;
1084 const Real Dissipation = 1;
1085 const Real CoefRest = 0;
1086 const Real mu_d = .5; /* compliant: .7*/
1087 const Real mu_s = .8; /* compliant: .7*/
1088 const Real mu_v = /*0.05*/0; //TODO: fails with mu_v=1, vtrans=.01
1089 const Real TransitionVelocity = 0.01;
1090 const Inertia brickInertia(.1,.1,.1);
1091 //const Real Radius = .02;
1092 const Real Radius = 1;
1093 #else
1094 const Real RunTime=20;
1095 const Real Stiffness = 1e6;
1096 const Real CoefRest = 0;
1097 const Real TargetVelocity = 3; // speed at which to match coef rest
1098 // const Real Dissipation = (1-CoefRest)/TargetVelocity;
1099 const Real Dissipation = .1;
1100 const Real mu_d = .5;
1101 const Real mu_s = .8;
1102 const Real mu_v = 0*0.05;
1103 const Real TransitionVelocity = 0.01;
1104 const Inertia brickInertia(BrickMass*UnitInertia::brick(BrickHalfDims));
1105 const Real Radius = BrickHalfDims[0]/3;
1106 #endif
1107
1108 printf("\n******************** Tim's Box Hybrid ********************\n");
1109 #ifdef USE_CONTINUOUS_STICTION
1110 printf("USING OLD MODEL: Continuous Stiction (Stribeck)\n");
1111 #else
1112 printf("USING NEW MODEL: Hybrid Compliant material/rigid stiction\n");
1113 #endif
1114 #ifdef USE_TIMS_PARAMS
1115 printf("Using Tim's parameters:\n");
1116 #else
1117 printf("Using Sherm's parameters:\n");
1118 #endif
1119 printf(" stiffness=%g dissipation=%g\n", Stiffness, Dissipation);
1120 printf(" mu_d=%g mu_s=%g mu_v=%g\n", mu_d, mu_s, mu_v);
1121 printf(" transition velocity=%g\n", TransitionVelocity);
1122 printf(" brick inertia=%g %g %g\n",
1123 brickInertia.getMoments()[0], brickInertia.getMoments()[1],
1124 brickInertia.getMoments()[2]);
1125 printf("******************** Tim's Box Hybrid ********************\n\n");
1126
1127 // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
1128 MultibodySystem mbs;
1129 SimbodyMatterSubsystem matter(mbs);
1130 GeneralForceSubsystem forces(mbs);
1131 Force::Gravity gravity(forces, matter, -YAxis, 9.81);
1132 //Force::Gravity gravity(forces, matter, -UnitVec3(.3,1,0), 3*9.81);
1133 MobilizedBody& Ground = matter.updGround();
1134
1135 // Define a material to use for contact. This is not very stiff.
1136 ContactMaterial material(std::sqrt(Radius)*Stiffness,
1137 Dissipation,
1138 mu_s, // mu_static
1139 mu_d, // mu_dynamic
1140 mu_v); // mu_viscous
1141
1142 // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
1143
1144 MyUnilateralConstraintSet unis(mbs, TransitionVelocity);
1145
1146 Body::Rigid brickBody =
1147 Body::Rigid(MassProperties(BrickMass, Vec3(0), brickInertia));
1148
1149 MobilizedBody::Free brick(Ground, Vec3(0),
1150 brickBody, Vec3(0));
1151 brick.addBodyDecoration(Transform(), DecorativeBrick(BrickHalfDims)
1152 .setColor(Red).setOpacity(.3));
1153 /*
1154 1) t= 0.5, dt = 2 sec, pt = (0.05, 0.2, 0.4), fdir = (1,0,0), mag = 50N
1155 2) t= 4.0, dt = 0.5 sec, pt = (0.03, 0.06, 0.09), fdir = (0.2,0.8,0), mag = 300N
1156 3) t= 0.9, dt = 2 sec, pt = (0,0,0), fdir = (0,1,0), mag = 49.333N (half the weight of the block)
1157 4) t= 13.0, dt = 1 sec, pt = (0 0 0), fdir = (-1,0,0), mag = 200N
1158 */
1159 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.05,0.2,0.4),
1160 50 * Vec3(1,0,0),
1161 0.5, 0.5+2));
1162 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.03, 0.06, 0.09),
1163 300 * UnitVec3(0.2,0.8,0),
1164 //300 * Vec3(0.2,0.8,0),
1165 4, 4+0.5));
1166 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
1167 49.033 * Vec3(0,1,0),
1168 9, 9+2));
1169 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
1170 200 * Vec3(-1,0,0),
1171 13, 13+1));
1172
1173 #ifndef USE_TIMS_PARAMS
1174 // Extra late force.
1175 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(.1, 0, .45),
1176 20 * Vec3(-1,-1,.5),
1177 15, Infinity));
1178 #endif
1179
1180 for (int i=-1; i<=1; i+=2)
1181 for (int j=-1; j<=1; j+=2)
1182 for (int k=-1; k<=1; k+=2) {
1183 const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(BrickHalfDims);
1184 MyHybridVertexContactElementImpl* vertex =
1185 new MyHybridVertexContactElementImpl(forces,
1186 Ground, YAxis, 0, // halfplane
1187 brick, pt, material);
1188 Force::Custom(forces, vertex); // add force element to system
1189 unis.addHybridElement(vertex); // assign index, transition velocity
1190 #ifndef USE_CONTINUOUS_STICTION
1191 mbs.addEventHandler(new StictionOn(mbs, unis, vertex->getIndex()));
1192 mbs.addEventHandler(new StictionOff(mbs, unis, vertex->getIndex()));
1193 #endif
1194 }
1195
1196 matter.setShowDefaultGeometry(false);
1197 Visualizer viz(mbs);
1198 viz.setShowSimTime(true);
1199 viz.setShowFrameNumber(true);
1200 viz.setShowFrameRate(true);
1201 viz.addDecorationGenerator(new ShowContact(unis));
1202
1203 #ifdef ANIMATE
1204 mbs.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
1205 #else
1206 // This does nothing but interrupt the simulation so that exact step
1207 // sequence will be maintained with animation off.
1208 mbs.addEventReporter(new Nada(ReportInterval));
1209 #endif
1210
1211 viz.addFrameController(
1212 new Visualizer::BodyFollower(brick, Vec3(0), Vec3(0, 1, 5)));
1213
1214 Vec3 cameraPos(0, 1, 2);
1215 UnitVec3 cameraZ(0,0,1);
1216 viz.setCameraTransform(Transform(Rotation(cameraZ, ZAxis,
1217 UnitVec3(YAxis), YAxis),
1218 cameraPos));
1219 viz.pointCameraAt(Vec3(0,0,0), Vec3(0,1,0));
1220
1221 #ifdef USE_TIMS_PARAMS
1222 Real accuracy = 1e-4;
1223 RungeKuttaMersonIntegrator integ(mbs);
1224 #else
1225 //Real accuracy = 1e-1;
1226 Real accuracy = 1e-3;
1227 //Real accuracy = 1e-5;
1228 //ExplicitEulerIntegrator integ(mbs);
1229 //RungeKutta2Integrator integ(mbs);
1230 //RungeKutta3Integrator integ(mbs);
1231 //SemiExplicitEulerIntegrator integ(mbs, .005);
1232 SemiExplicitEuler2Integrator integ(mbs);
1233 //RungeKuttaFeldbergIntegrator integ(mbs);
1234 //RungeKuttaMersonIntegrator integ(mbs);
1235 //VerletIntegrator integ(mbs);
1236 //CPodesIntegrator integ(mbs);
1237 #endif
1238
1239 integ.setAccuracy(accuracy);
1240 //integ.setMaximumStepSize(0.25);
1241 integ.setMaximumStepSize(0.05);
1242 //integ.setMaximumStepSize(0.002);
1243 //integ.setAllowInterpolation(false);
1244
1245 StateSaver* stateSaver = new StateSaver(mbs,unis,integ,ReportInterval);
1246 mbs.addEventReporter(stateSaver);
1247
1248 State s = mbs.realizeTopology(); // returns a reference to the the default state
1249
1250 //matter.setUseEulerAngles(s, true);
1251
1252 mbs.realizeModel(s); // define appropriate states for this System
1253 mbs.realize(s, Stage::Instance); // instantiate constraints if any
1254
1255
1256 /*
1257 rX_q = 0.7 rad
1258 rX_u = 1.0 rad/sec
1259
1260 rY_q = 0.6 rad
1261 rY_u = 0.0 rad/sec
1262
1263 rZ_q = 0.5 rad
1264 rZ_u = 0.2 rad/sec
1265
1266 tX_q = 0.0 m
1267 tX_u = 10 m/s
1268
1269 tY_q = 1.4 m
1270 tY_u = 0.0 m/s
1271
1272 tZ_q = 0.0 m
1273 tZ_u = 0.0 m/s
1274 */
1275
1276 #ifdef USE_TIMS_PARAMS
1277 brick.setQToFitTranslation(s, Vec3(0,10,0));
1278 brick.setUToFitLinearVelocity(s, Vec3(0,0,0));
1279 #else
1280 brick.setQToFitTranslation(s, Vec3(0,1.4,0));
1281 brick.setUToFitLinearVelocity(s, Vec3(10,0,0));
1282 const Rotation R_BC(SimTK::BodyRotationSequence,
1283 0.7, XAxis, 0.6, YAxis, 0.5, ZAxis);
1284 brick.setQToFitRotation(s, R_BC);
1285 brick.setUToFitAngularVelocity(s, Vec3(1,0,.2));
1286 #endif
1287
1288 mbs.realize(s, Stage::Velocity);
1289 viz.report(s);
1290
1291 cout << "Initial configuration shown. Next? ";
1292 getchar();
1293
1294 Assembler(mbs).setErrorTolerance(1e-6).assemble(s);
1295 viz.report(s);
1296 cout << "Assembled configuration shown. Ready? ";
1297 getchar();
1298
1299 // Now look for enabled contacts that aren't sliding; turn on stiction
1300 // for those.
1301 mbs.realize(s, Stage::Velocity);
1302 Array_<int> enableTheseStictions;
1303 for (int i=0; i < unis.getNumContactElements(); ++i) {
1304 MyHybridVertexContactElementImpl& fric = unis.updContactElement(i);
1305 if (!fric.isInContact(s)) continue;
1306 const Real vSlip = fric.getActualSlipSpeed(s);
1307 fric.initializeForStiction(s); // just in case
1308 printf("friction element %d has v_slip=%g%s\n", i, vSlip,
1309 vSlip==0?" (ENABLING STICTION)":"");
1310 if (vSlip == 0)
1311 enableTheseStictions.push_back(i);
1312 }
1313 for (unsigned i=0; i < enableTheseStictions.size(); ++i)
1314 unis.getContactElement(enableTheseStictions[i]).enableStiction(s);
1315
1316 // Make sure Lapack gets initialized.
1317 Matrix M(1,1); M(0,0)=1.23;
1318 FactorLU Mlu(M);
1319
1320
1321 // Simulate it.
1322
1323 integ.setReturnEveryInternalStep(true);
1324 TimeStepper ts(mbs, integ);
1325 ts.setReportAllSignificantStates(true);
1326
1327 #ifdef TEST_REPEATABILITY
1328 const int tries = NTries;
1329 #else
1330 const int tries = 1;
1331 #endif
1332
1333 Array_< Array_<State> > states(tries);
1334 Array_< Array_<Real> > timeDiff(tries-1);
1335 Array_< Array_<Vector> > yDiff(tries-1);
1336 cout.precision(18);
1337 for (int ntry=0; ntry < tries; ++ntry) {
1338 mbs.resetAllCountersToZero();
1339 unis.initialize(); // reinitialize
1340 ts.updIntegrator().resetAllStatistics();
1341 ts.initialize(s);
1342 int nStepsWithEvent = 0;
1343
1344 const double startReal = realTime();
1345 const double startCPU = cpuTime();
1346
1347 Integrator::SuccessfulStepStatus status;
1348 do {
1349 status=ts.stepTo(RunTime);
1350 #ifdef TEST_REPEATABILITY
1351 states[ntry].push_back(ts.getState());
1352 #endif
1353 const int j = states[ntry].size()-1;
1354 if (ntry>0) {
1355 int prev = ntry-1;
1356 //prev=0;
1357 Real dt = states[ntry][j].getTime()
1358 - states[prev][j].getTime();
1359 volatile double vd;
1360 const int ny = states[ntry][j].getNY();
1361 Vector d(ny);
1362 for (int i=0; i<ny; ++i) {
1363 vd = states[ntry][j].getY()[i]
1364 - states[prev][j].getY()[i];
1365 d[i] = vd;
1366 }
1367 timeDiff[prev].push_back(dt);
1368 yDiff[prev].push_back(d);
1369 cout << "\n" << states[prev][j].getTime()
1370 << "+" << dt << ":" << d << endl;
1371 }
1372
1373 const bool handledEvent = status==Integrator::ReachedEventTrigger;
1374 if (handledEvent) {
1375 ++nStepsWithEvent;
1376 SimTK_DEBUG3("EVENT %3d @%.17g status=%s\n\n",
1377 nStepsWithEvent, ts.getTime(),
1378 Integrator::getSuccessfulStepStatusString(status).c_str());
1379 } else {
1380 SimTK_DEBUG3("step %3d @%.17g status=%s\n",
1381 integ.getNumStepsTaken(), ts.getTime(),
1382 Integrator::getSuccessfulStepStatusString(status).c_str());
1383 }
1384 #ifndef NDEBUG
1385 viz.report(ts.getState());
1386 #endif
1387 } while (ts.getTime() < RunTime);
1388
1389
1390 const double timeInSec = realTime()-startReal;
1391 const double cpuInSec = cpuTime()-startCPU;
1392 const int evals = integ.getNumRealizations();
1393 cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
1394 timeInSec << "s for " << ts.getTime() << "s sim (avg step="
1395 << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
1396 << (1000*ts.getTime())/evals << "ms/eval\n";
1397 cout << "CPUtime " << cpuInSec << endl;
1398
1399 printf("Used Integrator %s at accuracy %g:\n",
1400 integ.getMethodName(), integ.getAccuracyInUse());
1401 printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
1402 printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
1403 printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
1404 printf("# EVENT STEPS/HANDLER CALLS = %d/%d\n",
1405 nStepsWithEvent, mbs.getNumHandleEventCalls());
1406 }
1407
1408 for (int i=0; i<tries; ++i)
1409 cout << "nstates " << i << " " << states[i].size() << endl;
1410
1411 // Instant replay.
1412 while(true) {
1413 printf("Hit ENTER for replay (%d states) ...",
1414 stateSaver->getNumSavedStates());
1415 getchar();
1416 for (int i=0; i < stateSaver->getNumSavedStates(); ++i) {
1417 mbs.realize(stateSaver->getState(i), Stage::Velocity);
1418 viz.report(stateSaver->getState(i));
1419 }
1420 }
1421
1422 }
1423 catch (const std::exception& e) {
1424 printf("EXCEPTION THROWN: %s\n", e.what());
1425 exit(1);
1426 }
1427 catch (...) {
1428 printf("UNKNOWN EXCEPTION THROWN\n");
1429 exit(1);
1430 }
1431
1432 }
1433
1434 //==============================================================================
1435 // MY UNILATERAL CONSTRAINT SET
1436 //==============================================================================
1437
1438 //------------------------ SELECT ACTIVE CONSTRAINTS ---------------------------
1439 void MyUnilateralConstraintSet::
selectActiveConstraints(State & state,Real vtol) const1440 selectActiveConstraints(State& state, Real vtol) const {
1441
1442 // Find all the contacts and stiction elements that might be active based
1443 // on kinematics.
1444 MyElementSubset candidates;
1445
1446 bool needRestart;
1447 do {
1448 //TODO: this (mis)use of accuracy needs to be revisited.
1449 findCandidateElements(state, vtol, candidates);
1450
1451 // Evaluate accelerations and reaction forces and check if
1452 // any of the active contacts are generating negative ("pulling")
1453 // forces; if so, inactivate them.
1454 findActiveCandidates(state, candidates);
1455
1456 // Project active constraints to the constraint manifolds.
1457 const Real tol = vtol/1000;
1458 SimTK_DEBUG1("projecting active constraints to tol=%g\n", tol);
1459 m_mbs.project(state, tol);
1460
1461 // It is possible that the projection above took some marginally-sliding
1462 // friction and slowed it down enough to make it a stiction candidate.
1463 needRestart = false;
1464 for (unsigned i=0; i < candidates.m_sliding.size(); ++i) {
1465 const int which = candidates.m_sliding[i];
1466 const MyHybridVertexContactElementImpl& contact =
1467 getContactElement(which);
1468 assert(!contact.isSticking(state));
1469 if ( contact.getActualSlipSpeed(state) <= vtol
1470 || contact.calcSlipSpeedWitness(state) <= 0)
1471 {
1472 SimTK_DEBUG3("***RESTART** selectActiveConstraints() because "
1473 "sliding velocity %d is now |v|=%g or witness=%g\n",
1474 which, contact.getActualSlipSpeed(state),
1475 contact.calcSlipSpeedWitness(state));
1476 needRestart = true;
1477 break;
1478 }
1479 }
1480 } while (needRestart);
1481 }
1482
1483
1484
1485
1486 //-------------------------- FIND ACTIVE CANDIDATES ---------------------------
1487 // Given a list of candidate stiction constraints,
1488 // determine which ones are active in the least squares solution for the
1489 // constraint multipliers. Candidates are those constraints that meet all
1490 // kinematic conditions -- for stiction, sliding velocity less than tolerance,
1491 // or sliding direction reversed in the last step. Also, any
1492 // constraint that is currently active is a candidate, regardless of its
1493 // kinematics (might have drifted but that can't disable it).
1494 //
1495 // This method should be called only from within an event handler. For sliding
1496 // friction it will have reset the "previous slip direction" to the current
1497 // slip or impending slip direction.
1498 //
1499 // Algorithm
1500 // ---------
1501 // We're given a set of stiction candidates. If any are inactive, activate them.
1502 // -- at this point all aerr==0, some ferr might be < 0
1503 //
1504 // loop
1505 // - Realize(Acceleration) with the current active set
1506 // - Calculate ferr for active constraints, aerr for inactive
1507 // - If all ferr>=0, aerr>=0 -> break loop
1508 // - Check for aerr < 0 [tol?]. Shouldn't happen but if it does must turn on the
1509 // associated constraint for the worst violation, then -> continue loop
1510 // - Find worst (most negative) offender:
1511 // stiction offense = mu_s*max(0, fc) - |fs|
1512 // - Inactivate chosen constraint
1513 // record impending slip direction stick->slide
1514 // end loop
1515 //
1516 void MyUnilateralConstraintSet::
findActiveCandidates(State & s,const MyElementSubset & candidates) const1517 findActiveCandidates(State& s, const MyElementSubset& candidates) const
1518 {
1519 const int ReviseNormalNIters = 6;
1520 showConstraintStatus(s, "ENTER findActiveCandidates()");
1521
1522 SimTK_DEBUG3(
1523 "findActiveCandidates() %d/%d/%d contact/stick/ineligible ...\n",
1524 candidates.m_contact.size(), candidates.m_stiction.size(),
1525 candidates.m_ineligible.size());
1526
1527 // Disable any sticking constraints that are now ineligible due to
1528 // liftoff, and enable all other candidate stiction constraints if any
1529 // are currently disabled.
1530 enableConstraintSubset(candidates, s);
1531
1532 if (candidates.m_contact.empty()) {
1533 // Can't be any friction either, if there are no contacts.
1534 SimTK_DEBUG("EXIT findActiveCandidates: no candidates.\n");
1535 m_mbs.realize(s, Stage::Acceleration);
1536 return;
1537 }
1538
1539 int pass=0, nStictionDisabled=0;
1540 while (true) {
1541 ++pass;
1542 SimTK_DEBUG1("\nfindActiveCandidates(): pass %d\n", pass);
1543
1544 // Given an active set, evaluate multipliers and accelerations.
1545 m_mbs.realize(s, Stage::Acceleration);
1546 if (pass==1) {
1547 // First time through record all the slip directions.
1548 for (unsigned i=0; i < candidates.m_contact.size(); ++i) {
1549 const int which = candidates.m_contact[i];
1550 const MyHybridVertexContactElementImpl& fric =
1551 getContactElement(which);
1552 if (fric.isSticking(s))
1553 fric.recordImpendingSlipDir(s);
1554 else fric.recordActualSlipDir(s);
1555 }
1556 }
1557
1558 // Scan all candidate stictions to find the active one that has the
1559 // most negative capacity.
1560
1561 int worstActiveStiction=-1; Real mostNegativeStictionCapacity=0;
1562 SimTK_DEBUG("analyzing stiction constraints ...\n");
1563 for (unsigned i=0; i < candidates.m_stiction.size(); ++i) {
1564 const int which = candidates.m_stiction[i];
1565 SimTK_DEBUG1(" %d: ", which);
1566 const MyHybridVertexContactElementImpl& fric =
1567 getContactElement(which);
1568
1569 if (!fric.isSticking(s)) {
1570 SimTK_DEBUG("off\n");
1571 continue;
1572 }
1573
1574 const Real capacity = fric.calcStictionForceWitness(s);
1575 SimTK_DEBUG2("on capacity=%g (N=%g)\n",
1576 capacity, fric.getNormalForce(s));
1577
1578 if (capacity < mostNegativeStictionCapacity) {
1579 worstActiveStiction = which;
1580 mostNegativeStictionCapacity = capacity;
1581 }
1582 }
1583
1584 #ifndef NDEBUG
1585 if (mostNegativeStictionCapacity == 0)
1586 printf(" All active stiction constraints are satisfied.\n");
1587 else
1588 printf(" Active stiction %d was worst violator with capacity=%g\n",
1589 worstActiveStiction, mostNegativeStictionCapacity);
1590 #endif
1591
1592 if (mostNegativeStictionCapacity==0) {
1593 SimTK_DEBUG("DONE. Current active set is a winner.\n");
1594 break;
1595 }
1596
1597 SimTK_DEBUG1(" Disable stiction %d\n", worstActiveStiction);
1598 const MyHybridVertexContactElementImpl& fric =
1599 getContactElement(worstActiveStiction);
1600
1601 ++nStictionDisabled;
1602 fric.disableStiction(s);
1603
1604 // Go back for another pass.
1605 }
1606
1607 // Reset all the slip directions so that all slip->stick event witnesses
1608 // will be positive when integration resumes.
1609 for (unsigned i=0; i < candidates.m_contact.size(); ++i) {
1610 const int which = candidates.m_contact[i];
1611 const MyHybridVertexContactElementImpl& fric =
1612 getContactElement(which);
1613 fric.updatePrevSlipDirFromRecorded(s);
1614 }
1615
1616 // Always leave at acceleration stage.
1617 m_mbs.realize(s, Stage::Acceleration);
1618
1619 SimTK_DEBUG1("... Done; %d stictions broken.\n", nStictionDisabled);
1620
1621 showConstraintStatus(s, "EXIT findActiveCandidates()");
1622 }
1623
1624
1625
1626 //-------------------------- SHOW CONSTRAINT STATUS ----------------------------
1627 void MyUnilateralConstraintSet::
showConstraintStatus(const State & s,const String & place) const1628 showConstraintStatus(const State& s, const String& place) const
1629 {
1630 #ifndef NDEBUG
1631 printf("\n%s: uni status @t=%.15g\n", place.c_str(), s.getTime());
1632 m_mbs.realize(s, Stage::Acceleration);
1633 for (int i=0; i < getNumContactElements(); ++i) {
1634 const MyHybridVertexContactElementImpl& contact = getContactElement(i);
1635 const bool isActive = contact.isInContact(s);
1636 printf(" %6s %2d %s, h=%g dh=%g f=%g\n",
1637 isActive?"ACTIVE":"off", i, "hybrid",
1638 contact.getHeight(s),contact.getHeightDot(s),
1639 isActive?contact.getNormalForce(s):Zero);
1640 if (!isActive) continue;
1641
1642 const bool isSticking = contact.isSticking(s);
1643 printf(" %8s friction %2d, |v|=%g witness=%g\n",
1644 isSticking?"STICKING":"sliding", i,
1645 contact.getActualSlipSpeed(s),
1646 isSticking?contact.calcStictionForceWitness(s)
1647 :contact.calcSlipSpeedWitness(s));
1648 contact.writeFrictionInfo(s, " ", std::cout);
1649 }
1650 printf("\n");
1651 #endif
1652 }
1653
1654 //------------------------ STRIBECK FRICTION STATICS ---------------------------
1655 // This is extracted from Simbody's continuous friction model so that we can
1656 // compare it with the new implementation.
1657
1658 // Input x goes from 0 to 1; output goes 0 to 1 but smoothed with an S-shaped
1659 // quintic with two zero derivatives at either end. Cost is 7 flops.
step5(Real x)1660 inline static Real step5(Real x) {
1661 assert(0 <= x && x <= 1);
1662 const Real x3=x*x*x;
1663 return x3*(10+x*(6*x-15)); //10x^3-15x^4+6x^5
1664 }
1665
1666 // This is the sum of two curves:
1667 // (1) a wet friction term mu_wet which is a linear function of velocity:
1668 // mu_wet = uv*v
1669 // (2) a dry friction term mu_dry which is a quintic spline with 4 segments:
1670 // mu_dry =
1671 // (a) v=0..1: smooth interpolation from 0 to us
1672 // (b) v=1..3: smooth interp from us down to ud (Stribeck)
1673 // (c) v=3..Inf ud
1674 // CAUTION: uv and v must be dimensionless in multiples of transition velocity.
1675 // The function mu = mu_wet + mu_dry is zero at v=0 with 1st deriv (slope) uv
1676 // and 2nd deriv (curvature) 0. At large velocities v>>0 the value is
1677 // ud+uv*v, again with slope uv and zero curvature. We want mu(v) to be c2
1678 // continuous, so mu_wet(v) must have zero slope and curvature at v==0 and
1679 // at v==3 where it takes on a constant value ud.
1680 //
1681 // Cost: stiction 12 flops
1682 // stribeck 14 flops
1683 // sliding 3 flops
1684 // Curve looks like this:
1685 //
1686 // us+uv ***
1687 // * * *
1688 // * * *____| slope = uv at Inf
1689 // * * *
1690 // ud+3uv * * *
1691 // *
1692 // *
1693 // *
1694 // 0 *____| slope = uv at 0
1695 //
1696 // | | |
1697 // v=0 1 3
1698 //
1699 // This calculates a composite coefficient of friction that you should use
1700 // to scale the normal force to produce the friction force.
stribeck(Real us,Real ud,Real uv,Real v)1701 static Real stribeck(Real us, Real ud, Real uv, Real v) {
1702 const Real mu_wet = uv*v;
1703 Real mu_dry;
1704 if (v >= 3) mu_dry = ud; // sliding
1705 else if (v >= 1) mu_dry = us - (us-ud)*step5((v-1)/2); // Stribeck
1706 else mu_dry = us*step5(v); // 0 <= v < 1 (stiction)
1707 return mu_dry + mu_wet;
1708 }
1709