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 compliant contact/bristle friction 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 // Control whether we use a dwell-time model. Otherwise we allow instantaneous
44 // sliding/stiction transition.
45 #define ENABLE_DWELL_TIME
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 // MY HYBRID VERTEX CONTACT ELEMENT
56 //==============================================================================
57 /* Given a contact material (with compliant material properties), this
58 represents a compliant point-and-rigid-halfspace contact element. We track
59 the height h of a vertex V on body B over a halfspace H on body P. The
60 halfspace frame H serves as the contact frame, with Hz in the contact
61 normal direction and Hxy spanning the tangent plane. The pose X_PH is
62 a given constant.
63
64 Normal force
65 ------------
66 If the vertex height h is above the halfspace surface (h>=0), no forces are
67 generated. If below the surface (h<0) we generate a normal force of magnitude
68 N=max(0, -k*h(1-d*hdot)), with N >= 0
69 applied to both bodies at the contact point C, which is the point at h=0 just
70 up the halfspace normal direction from the vertex (because we're considering
71 the halfspace to be rigid). Denote by Cb the station (material point) of B
72 that is coincident with C, and Cp the station of P that is coincident with C.
73
74 Bristle friction force
75 ----------------------
76 Ref: Gonthier, et al. 2004 Multibody Sys. Dyn. 11:209-33.
77
78 k_br bristle stiffness (= alpha0 from Gonthier)
79 t_br bristle time constant (= alpha1/alpha0 from Gonthier)
80 t_dw dwell time constant
81 v_trans Stribeck transition velocity
82 v_tol numerical velocity noise limit (< v_trans/10)
83
84 f tangential friction force vector (2d)
85
86 z bristle deformation vector (2d state)
87 v tangential slip velocity vector (2d) [v=0 when not in contact]
88 r rolling condition [0,1] (0=slipping, 1=rolling) (=s from Gonthier)
89 r_dw r delayed by time constants t_dw,t_br (state) [0,1]
90
91 f = { sat(f_br,f_max) - N*mu_v*v, if in contact
92 { 0, otherwise
93
94 f_br = -k_br*(z + t_br*zdot) [TODO: need area term here?]
95 zdot = r * zdot_s + (1-r) * zdot_d
96 zdot_s = v
97 zdot_d = -(z + f_d/k_br)/t_br
98 f_d = -mu_d*N*dir(v, v_tol)
99
100 f_max = mu_eff*N
101 mu_eff = mu_d + (mu_s-mu_d)*r_dw
102
103 r = exp(-|v|^2/v_trans^2)
104
105 r_dw_dot ={ (r - r_dw)/t_dw, r >= r_dw
106 { (r - r_dw)/t_br, r < r_dw
107
108 sat(f_br, f_max) = { f_br, |f_br| <= f_max
109 { f_max*f_br/|f_br|, |f_br| > f_max
110
111 dir(v, v_tol) = { v/|v|, |v| >= v_tol
112 { v/v_tol, |v| < v_tol [Gonthier has *stepUp(|v|/v_tol)]
113
114 */
115
116 // Return f, but with magnitude limited by fmax>=0.
sat(const Vec2 & f,Real fmax)117 static inline Vec2 sat(const Vec2& f, Real fmax) {
118 assert(fmax >= 0);
119 const Real fsq = f.normSqr();
120 if (fsq <= fmax*fmax)
121 return f;
122
123 return (fmax/std::sqrt(fsq))*f;
124 }
125
126 // Implement relaxed unit vector in direction of v, where the direction vector
127 // has magnitude less than 1 if |v|<vtol, dropping to 0 when |v|=0.
dir(const Vec2 & v,Real vnorm,Real oovtol)128 static inline Vec2 dir(const Vec2& v, Real vnorm, Real oovtol) {
129 assert(oovtol > 0);
130 const Real scale = vnorm*oovtol;
131 if (scale >= 1)
132 return v/vnorm;
133 //const Real smooth = 1;
134 //const Real smooth = stepUp(scale);
135 const Real smooth = 1.5*scale-0.5*cube(scale);// Gonthier
136 return v*(smooth*oovtol);
137 }
138
139 // This is a velocity-stage cache entry.
140 struct MyBristleContactInfo {
MyBristleContactInfoMyBristleContactInfo141 MyBristleContactInfo()
142 : h(NaN), Cp(NaN), Cb(NaN), v_HCb(NaN), vSlipMag(NaN), f_HCb(NaN)
143 {
144 }
145 // Position info.
146 Real h; // signed distance, h<0 means contact
147 Vec3 Cp; // station on P coincident with the contact point
148 Vec3 Cb; // station on B coincident with the contact point
149 Rotation R_GH;
150
151 // Velocity info. H frame has z along contact normal, x,y in tangent plane.
152 Vec3 v_HCb; // velocity of Cb in the contact frame H
153 Real vSlipMag; // |(v_HCb.x, v_HCb.y)|
154 Vec3 f_HCb; // contact force on Cb in contact frame H
155 };
156
157 class MyBristleVertexContactElementImpl : public Force::Custom::Implementation {
158 public:
MyBristleVertexContactElementImpl(const GeneralForceSubsystem & forces,MobilizedBody & hsmobod,const UnitVec3 & hsn,Real hsh,MobilizedBody & vmobod,const Vec3 & vertex,const ContactMaterial & material,Real vTrans)159 MyBristleVertexContactElementImpl(const GeneralForceSubsystem& forces,
160 MobilizedBody& hsmobod, const UnitVec3& hsn, Real hsh,
161 MobilizedBody& vmobod, const Vec3& vertex,
162 const ContactMaterial& material, Real vTrans)
163 : m_matter(hsmobod.getMatterSubsystem()), m_forces(forces),
164 m_hsmobodx(hsmobod), m_X_PH(Rotation(hsn, ZAxis), hsh*hsn),
165 m_vmobodx(vmobod), m_vertex_B(vertex),
166 m_material(material),
167 m_kBristle(NaN), m_tBristle(NaN), m_tDwell(NaN),
168 m_vtrans(vTrans), m_vtol(NaN), // assign later
169 m_index(-1)
170 {
171 m_kBristle = m_material.getStiffness()/10;
172 m_tBristle = .01; // 10 ms
173 m_tDwell = .1; // 100 ms
174 m_vtol = m_vtrans/10;
175 }
176
setContactIndex(int index)177 void setContactIndex(int index) {m_index=index;}
178
initialize(State & s)179 void initialize(State& s) {
180 updZ(s) = Vec2(0);
181 updRDwell(s) = 0;
182 setPrevSlipDir(s, Vec2(NaN));
183 }
184
initializeForStiction(State & s)185 void initializeForStiction(State& s) {
186 updZ(s) = Vec2(0);
187 updRDwell(s) = 1; // rolling
188 setPrevSlipDir(s, Vec2(NaN));
189 }
190
isInContact(const State & s) const191 bool isInContact(const State& s) const
192 { return findH(s) < 0; }
193
isSticking(const State & s) const194 bool isSticking(const State& s) const
195 {
196 return getActualSlipSpeed(s) <= m_vtrans;
197 }
198
199 // Return a point in Ground coincident with the vertex.
whereToDisplay(const State & s) const200 Vec3 whereToDisplay(const State& s) const {
201 return getBodyB().findStationLocationInGround(s, m_vertex_B);
202 }
203
getActualSlipSpeed(const State & s) const204 Real getActualSlipSpeed(const State& s) const {
205 const MyBristleContactInfo& info = getContactInfo(s);
206 return info.vSlipMag;
207 }
208
209 // Return the normal force N >= 0 currently being generated by this
210 // contact. State must be realized to Stage::Velocity.
getNormalForce(const State & s) const211 Real getNormalForce(const State& s) const {
212 const MyBristleContactInfo& info = getContactInfo(s);
213 if (info.h >= 0) return 0;
214 const Real N = info.f_HCb[2]; // z component is normal
215 return N;
216 }
217
getHeight(const State & s) const218 Real getHeight(const State& s) const {
219 const MyBristleContactInfo& info = getContactInfo(s);
220 return info.h;
221 }
222
getHeightDot(const State & s) const223 Real getHeightDot(const State& s) const {
224 const MyBristleContactInfo& info = getContactInfo(s);
225 return info.v_HCb[2]; // velocity in plane normal direction
226 }
227
228 // Return the sliding force 2-vector in the halfspace plane that is being
229 // applied to the contact point station on body B. If we are sticking
230 // then this force is zero; call getStickingForce() to get the real value.
getSlidingForce(const State & s) const231 const Vec2& getSlidingForce(const State& s) const {
232 const MyBristleContactInfo& info = getContactInfo(s);
233 const Vec2& f = info.f_HCb.getSubVec<2>(0);
234 return f;
235 }
236
237 // Return the sliding velocity 2-vector in the halfspace plane of the
238 // contact point station on body B relative to the contact point station
239 // on body P.
getSlipVelocity(const State & s) const240 const Vec2& getSlipVelocity(const State& s) const {
241 const MyBristleContactInfo& info = getContactInfo(s);
242 const Vec2& v = info.v_HCb.getSubVec<2>(0);
243 return v;
244 }
245
246 // Return height of vertex over plane; negative if penetrated. You can
247 // call this at Stage::Position.
findH(const State & state) const248 Real findH(const State& state) const {
249 const Vec3 V_P = findVInP(state); // location of V in P
250 const Real h = dot(V_P-m_X_PH.p(), m_X_PH.z());
251 return h;
252 }
253
254 // Returns height of vertex over plane same as findH(); also returns
255 // contact point (projection of V onto plane along Hz).
findContactPointInP(const State & state,Vec3 & contactPointInP) const256 Real findContactPointInP(const State& state, Vec3& contactPointInP) const {
257 const Vec3 V_P = findVInP(state); // location of V in P
258 const Real h = dot(V_P-m_X_PH.p(), m_X_PH.z());
259 contactPointInP = V_P - h*m_X_PH.z();
260 return h;
261 }
262
263 // Calculate v_eff, the direction to be opposed by the sliding force.
getEffectiveSlipDir(const State & s,const Vec2 & vSlip,Real vSlipMag) const264 Vec2 getEffectiveSlipDir(const State& s, const Vec2& vSlip, Real vSlipMag) const {
265 const Vec2 prevVslipDir = getPrevSlipDir(s);
266 if (shouldUpdate(vSlip, vSlipMag, prevVslipDir, m_vtol))
267 return vSlip/vSlipMag;
268 else return prevVslipDir;
269 }
270
getZ(const State & s) const271 const Vec2& getZ(const State& s) const
272 { return Vec2::getAs(&m_forces.getZ(s)[this->m_bristleDeformationIx]); }
updZ(State & s) const273 Vec2& updZ(State& s) const
274 { return Vec2::updAs(&m_forces.updZ(s)[this->m_bristleDeformationIx]); }
updZDot(const State & s) const275 Vec2& updZDot(const State& s) const
276 { return Vec2::updAs(&m_forces.updZDot(s)[m_bristleDeformationIx]); }
277
getRDwell(const State & s) const278 const Real& getRDwell(const State& s) const
279 { return m_forces.getZ(s)[this->m_dwellIx]; }
updRDwell(State & s)280 Real& updRDwell(State& s)
281 { return m_forces.updZ(s)[this->m_dwellIx]; }
updRDwellDot(const State & s) const282 Real& updRDwellDot(const State& s) const
283 { return m_forces.updZDot(s)[m_dwellIx]; }
284
285
286 // Return the slip velocity as recorded at the end of the last time step.
getPrevSlipDir(const State & state) const287 const Vec2& getPrevSlipDir(const State& state) const {
288 const Vec2& prevSlipDir = Value<Vec2>::downcast
289 (m_forces.getDiscreteVariable(state, m_prevSlipDirIx));
290 return prevSlipDir;
291 }
292 // Modify the discrete state variable directly.
setPrevSlipDir(State & state,const Vec2 & slipDir) const293 void setPrevSlipDir(State& state, const Vec2& slipDir) const {
294 Vec2& prevSlipDir = Value<Vec2>::updDowncast
295 (m_forces.updDiscreteVariable(state, m_prevSlipDirIx));
296 prevSlipDir = slipDir;
297 SimTK_DEBUG3("STATE CHG %d: prevDir to %g %g\n",
298 m_index, slipDir[0], slipDir[1]);
299 }
300
301 // Get access to the auto-update cache variable that will update the
302 // previous slip direction state variable if the step is accepted.
updPrevSlipDirAutoUpdateValue(const State & state) const303 Vec2& updPrevSlipDirAutoUpdateValue(const State& state) const {
304 Vec2& prevSlipUpdate = Value<Vec2>::updDowncast
305 (m_forces.updDiscreteVarUpdateValue(state, m_prevSlipDirIx));
306 return prevSlipUpdate;
307 }
308 // Mark the auto-update cache entry valid.
markPrevSlipDirAutoUpdateValueValid(const State & state) const309 void markPrevSlipDirAutoUpdateValueValid(const State& state) const {
310 m_forces.markDiscreteVarUpdateValueRealized(state, m_prevSlipDirIx);
311 }
312
313
getContactInfo(const State & state) const314 const MyBristleContactInfo& getContactInfo(const State& state) const {
315 const MyBristleContactInfo& info =
316 Value<MyBristleContactInfo>::downcast
317 (m_forces.getCacheEntry(state, m_contactInfoIx));
318 return info;
319 }
320
updContactInfo(const State & state) const321 MyBristleContactInfo& updContactInfo(const State& state) const {
322 MyBristleContactInfo& info =
323 Value<MyBristleContactInfo>::updDowncast
324 (m_forces.updCacheEntry(state, m_contactInfoIx));
325 return info;
326 }
327 //--------------------------------------------------------------------------
328 // Custom force virtuals
329 // Apply the normal force, and the sliding friction force if it is enabled.
330 // This is called during realize(Dynamics).
calcForce(const State & state,Vector_<SpatialVec> & bodyForces,Vector_<Vec3> & particleForces,Vector & mobilityForces) const331 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
332 Vector_<Vec3>& particleForces, Vector& mobilityForces) const
333 override
334 {
335 const MyBristleContactInfo& info = getContactInfo(state);
336 if (info.h >= 0)
337 return; // no contact
338
339 const Vec3 f_GCb = info.R_GH * info.f_HCb; // re-express in Ground
340 getBodyB().applyForceToBodyPoint(state, info.Cb, f_GCb, bodyForces);
341 getBodyP().applyForceToBodyPoint(state, info.Cp, -f_GCb, bodyForces);
342 }
343
344 // The normal force stores energy as 2/5 k h^(5/2) when h<0.
calcPotentialEnergy(const State & state) const345 Real calcPotentialEnergy(const State& state) const override {
346 const MyBristleContactInfo& info = getContactInfo(state);
347 if (info.h >= 0)
348 return 0; // no contact
349 const Real h52 = square(info.h)*std::sqrt(-info.h);
350 const Real k = m_material.getStiffness();
351 return 0.4*k*h52;
352 }
353
354 // Allocate state variable for storing the previous sliding direction.
realizeTopology(State & state) const355 void realizeTopology(State& state) const override {
356 m_bristleDeformationIx = m_forces.allocateZ(state, Vector(2, Real(0)));
357 m_dwellIx = m_forces.allocateZ(state, Vector(1, Real(0)));
358 m_prevSlipDirIx = m_forces.allocateAutoUpdateDiscreteVariable
359 (state, Stage::Velocity, new Value<Vec2>(Vec2(NaN)),
360 Stage::Velocity);
361
362 m_contactInfoIx = m_forces.allocateCacheEntry
363 (state, Stage::Velocity, new Value<MyBristleContactInfo>());
364 }
365
366
367 // Calculate everything here and save in contact info cache entry where
368 // it can be retrieved for generating forces, reporting, etc.
realizeVelocity(const State & state) const369 void realizeVelocity(const State& state) const override {
370 MyBristleContactInfo& info = updContactInfo(state);
371
372 // Forces generated only if h<0. Cp always be the projection of the
373 // vertex onto the halfspace surface.
374 info.h = findContactPointInP(state, info.Cp);
375
376 const Rotation& R_PH = m_X_PH.R();
377 const Rotation& R_GP = getBodyP().getBodyRotation(state);
378 info.R_GH = R_GP*R_PH;
379
380 // Station of B coincident with the contact point.
381 info.Cb = info.h < 0
382 ? findPointOfBCoincidentWithPointOfP(state, info.Cp)
383 : getVertexOnB();
384
385 // Velocity of B's contact station in P.
386 const Vec3 v_PCb = getBodyB().findStationVelocityInAnotherBody
387 (state, info.Cb, getBodyP());
388 info.v_HCb = ~R_PH*v_PCb; // re-express in H
389 const Real hdot = info.v_HCb[2]; // z component
390 const Vec2 vSlip_HC(info.v_HCb[0], info.v_HCb[1]);
391 info.vSlipMag = vSlip_HC.norm();
392
393 const Real k = m_material.getStiffness(),
394 c = m_material.getDissipation(),
395 mu_d = m_material.getDynamicFriction(),
396 mu_s = m_material.getStaticFriction(),
397 mu_v = m_material.getViscousFriction();
398
399 const Vec2& z = getZ(state);
400 const Real& rdw = getRDwell(state);
401 Vec2& zdot = updZDot(state); // to be computed
402 Real& rdwdot = updRDwellDot(state);
403
404 info.f_HCb = Vec3(0); // Assume no contact force.
405 Real N = 0;
406
407 if (info.h < 0) { // contact
408 const Real h32 = -info.h*sqrt(-info.h); // |h| ^ (3/2)
409 N = std::max(Real(0), k*h32*(1-c*hdot));
410 }
411
412 if (N==0) {
413 zdot = -z / m_tBristle; // decay fast
414 rdwdot = -rdw / m_tBristle;
415 return; // no contact force
416 }
417
418 // N is the Hz component of the force on Cb.
419
420 Vec2 fT_HCb(0); // This will be the (Hx,Hy) components of force on Cb.
421
422 // Make v unitless slip ratio.
423 const Real v = info.vSlipMag / m_vtrans;
424 Real r = std::exp(-square(v)); // 1==rolling, 0==slipping
425
426 const Vec2& dir_prev = getPrevSlipDir(state);
427 Vec2 dir_eff = dir(vSlip_HC, info.vSlipMag, 1/m_vtol);
428 if (~dir_eff*dir_prev < 0) {
429 //cout << "REVERSED from " << dir_prev << " to " << dir_eff << endl;
430 //dir_eff = dir_prev;
431 }
432 updPrevSlipDirAutoUpdateValue(state) = dir_eff;
433 markPrevSlipDirAutoUpdateValueValid(state);
434
435 // This is what the force would be if we were fully in sliding.
436 //const Vec2 f_d = -mu_d*N*dir(vSlip_HC, info.vSlipMag, 1/m_vtol);
437 const Vec2 f_d = -mu_d*N*dir_eff;
438 // This is the bristle deformation that would generate f_d
439 const Vec2 z_d = -f_d/m_kBristle;
440
441 const Vec2 zdot_s = vSlip_HC;
442 const Vec2 zdot_d = (z_d - z)/m_tBristle;
443
444 #ifdef ENABLE_DWELL_TIME
445 const Real r_dwell = clamp(0,rdw,1);
446 // Update dwell state derivative.
447 const Real tconst = r >= rdw ? m_tDwell : m_tBristle;
448 rdwdot = (r - rdw) / tconst;
449 #else
450 const Real r_dwell = r;
451 rdwdot = 0;
452 #endif
453
454 zdot = r*zdot_s + (1-r)*zdot_d; // Gonthier
455 //zdot = r_dwell*zdot_s + (1-r_dwell)*zdot_d; // is this better?
456 const Vec2 f_br = -m_kBristle*(z + m_tBristle*zdot);
457
458 const Real mu_eff = mu_d + (mu_s-mu_d)*r_dwell;
459 const Real f_max = mu_eff*N;
460 fT_HCb = sat(f_br, f_max) - N * mu_v*vSlip_HC;
461
462 info.f_HCb = Vec3(fT_HCb[0], fT_HCb[1], N); // force on Cb, in H
463 }
464
writeFrictionInfo(const State & s,const String & indent,std::ostream & o) const465 std::ostream& writeFrictionInfo(const State& s, const String& indent,
466 std::ostream& o) const
467 {
468 o << indent;
469 if (!isInContact(s)) o << "OFF";
470 else if (isSticking(s)) o << "STICK";
471 else o << "SLIP";
472
473 const Vec2 v = getSlipVelocity(s);
474 const Vec2 f = getSlidingForce(s);
475 o << " V=" << ~v << " |v|=" << v.norm() << endl;
476 o << indent << " F=" << ~f << " |f|=" << f.norm() << endl;
477 o << indent << " z=" << ~getZ(s) << " |z|=" << getZ(s).norm() << endl;
478 o << indent << " rdw=" << getRDwell(s)
479 << " prevDir=" << ~getPrevSlipDir(s) << endl;
480 return o;
481 }
482
483
showContactForces(const State & s,Array_<DecorativeGeometry> & geometry) const484 void showContactForces(const State& s, Array_<DecorativeGeometry>& geometry)
485 const
486 {
487 const Real Scale = 0.01;
488 const Real NH = getNormalForce(s);
489
490 const bool isInStiction = getActualSlipSpeed(s) <= m_vtrans;
491 const Vec2 fH = getSlidingForce(s);
492
493 if (fH.normSqr() < square(SignificantReal) && NH < SignificantReal)
494 return;
495
496 const MyBristleContactInfo& info = getContactInfo(s);
497 const Vec3 fG = info.R_GH * Vec3(fH[0],fH[1],0); // friction
498 const Vec3 NG = info.R_GH * Vec3(0, 0, NH); // normal
499
500 const MobilizedBody& bodyB = getBodyB();
501 const Vec3& stationB = getVertexOnB();
502 const Vec3 stationG = bodyB.getBodyTransform(s)*stationB;
503 const Vec3 endfG = stationG - Scale*fG;
504 const Vec3 endNG = stationG + Scale*NG;
505 geometry.push_back(DecorativeLine(endfG + Vec3(0,.05,0),
506 stationG + Vec3(0,.05,0))
507 .setColor(isInStiction?Green:Orange));
508 geometry.push_back(DecorativeLine(endNG + Vec3(0,.05,0),
509 stationG + Vec3(0,.05,0))
510 .setColor(Red));
511 }
512
getIndex() const513 int getIndex() const {return m_index;}
getBodyB() const514 const MobilizedBody& getBodyB() const
515 { return m_matter.getMobilizedBody(m_vmobodx); }
getVertexOnB() const516 const Vec3& getVertexOnB() const {return m_vertex_B;}
getBodyP() const517 const MobilizedBody& getBodyP() const
518 { return m_matter.getMobilizedBody(m_hsmobodx); }
getX_PH() const519 const Transform& getX_PH() const {return m_X_PH;}
getMaterial() const520 const ContactMaterial& getMaterial() const {return m_material;}
521
522 //--------------------------------------------------------------------------
523 private:
524 // Determine whether the current slip velocity is reliable enough that
525 // we should use it to replace the previous slip velocity.
shouldUpdate(const Vec2 & vSlip,Real vSlipMag,const Vec2 & prevSlipDir,Real velTol)526 static bool shouldUpdate(const Vec2& vSlip, Real vSlipMag,
527 const Vec2& prevSlipDir, Real velTol) {
528 if (prevSlipDir.isNaN())
529 return vSlipMag > 0; // we'll take anything
530
531 // Check for reversal.
532 bool reversed = (~vSlip*prevSlipDir < 0);
533 return !reversed && (vSlipMag > velTol);
534 }
535
536 // Find the location of the vertex on B, measured and expressed in P.
findVInP(const State & s) const537 Vec3 findVInP(const State& s) const {
538 return getBodyB().findStationLocationInAnotherBody
539 (s, m_vertex_B, getBodyP());
540 }
541
542 // Find the location and velocity of the vertex on B, measured from and
543 // expressed in P.
findVDotInP(const State & s) const544 Vec3 findVDotInP(const State& s) const {
545 return getBodyB().findStationVelocityInAnotherBody
546 (s, m_vertex_B, getBodyP());
547 }
548
findPointOfBCoincidentWithPointOfP(const State & s,const Vec3 & r_P) const549 Vec3 findPointOfBCoincidentWithPointOfP
550 (const State& s, const Vec3& r_P) const
551 {
552 return getBodyP().findStationLocationInAnotherBody(s, r_P, getBodyB());
553 }
554
555
556 private:
557 const GeneralForceSubsystem& m_forces;
558 const SimbodyMatterSubsystem& m_matter;
559 const MobilizedBodyIndex m_hsmobodx; // body P with halfspace
560 Transform m_X_PH; // halfspace frame in P
561 const MobilizedBodyIndex m_vmobodx; // body B with vertex
562 Vec3 m_vertex_B; // vertex location in B
563 ContactMaterial m_material; // composite material props
564 Real m_kBristle; // bristle stiffness
565 Real m_tBristle; // bristle time constant
566 Real m_tDwell; // dwell time constant
567
568 Real m_vtrans; // transition velocity for
569 // Stribeck stiction
570 Real m_vtol; // max meaningful slip velocity
571
572 int m_index; // unique id for this contact
573
574 // Set during realizeTopology().
575 mutable ZIndex m_bristleDeformationIx; // zx,zy
576 mutable ZIndex m_dwellIx; // r_dw
577 mutable DiscreteVariableIndex m_prevSlipDirIx; // previous slip direction
578
579 mutable CacheEntryIndex m_contactInfoIx;
580 };
581
582
583 //==============================================================================
584 // MY UNILATERAL CONSTRAINT SET
585 //==============================================================================
586
587 class MyUnilateralConstraintSet {
588 public:
589 // Transition velocity: if a slip velocity is smaller than this the
590 // contact is a candidate for stiction.
MyUnilateralConstraintSet(const MultibodySystem & mbs)591 MyUnilateralConstraintSet(const MultibodySystem& mbs)
592 : m_mbs(mbs) {}
593
594 // Ownership of this force element belongs to the System; we're just keeping
595 // a reference to it here.
addBristleElement(MyBristleVertexContactElementImpl * vertex)596 int addBristleElement(MyBristleVertexContactElementImpl* vertex) {
597 const int index = (int)m_bristle.size();
598 m_bristle.push_back(vertex);
599 vertex->setContactIndex(index);
600 return index;
601 }
602
getNumContactElements() const603 int getNumContactElements() const {return (int)m_bristle.size();}
getContactElement(int ix) const604 const MyBristleVertexContactElementImpl& getContactElement(int ix) const
605 { return *m_bristle[ix]; }
updContactElement(int ix)606 MyBristleVertexContactElementImpl& updContactElement(int ix)
607 { return *m_bristle[ix]; }
608
609 // Initialize all states and runtime fields in the contact elements.
initialize(State & state)610 void initialize(State& state)
611 {
612 for (unsigned i=0; i < m_bristle.size(); ++i)
613 m_bristle[i]->initialize(state);
614 }
615
616 // In Debug mode, produce a useful summary of the current state of the
617 // contact and friction elements.
618 void showContactStatus(const State& state, const String& place) const;
619
~MyUnilateralConstraintSet()620 ~MyUnilateralConstraintSet() {
621 // Nothing to delete since we are only holding references.
622 }
623
getMultibodySystem() const624 const MultibodySystem& getMultibodySystem() const {return m_mbs;}
625 private:
626 const MultibodySystem& m_mbs;
627 Array_<MyBristleVertexContactElementImpl*> m_bristle; // unowned ref
628 };
629
630
631
632 //==============================================================================
633 // STATE SAVER
634 //==============================================================================
635 // This reporter is called now and again to save the current state so we can
636 // play back a movie at the end.
637 class StateSaver : public PeriodicEventReporter {
638 public:
StateSaver(const MultibodySystem & mbs,const MyUnilateralConstraintSet & unis,const Integrator & integ,Real reportInterval)639 StateSaver(const MultibodySystem& mbs,
640 const MyUnilateralConstraintSet& unis,
641 const Integrator& integ,
642 Real reportInterval)
643 : PeriodicEventReporter(reportInterval),
644 m_mbs(mbs), m_unis(unis), m_integ(integ)
645 { m_states.reserve(2000); }
646
~StateSaver()647 ~StateSaver() {}
648
clear()649 void clear() {m_states.clear();}
getNumSavedStates() const650 int getNumSavedStates() const {return (int)m_states.size();}
getState(int n) const651 const State& getState(int n) const {return m_states[n];}
652
handleEvent(const State & s) const653 void handleEvent(const State& s) const override {
654 const SimbodyMatterSubsystem& matter=m_mbs.getMatterSubsystem();
655 const SpatialVec PG = matter.calcSystemMomentumAboutGroundOrigin(s);
656 m_mbs.realize(s, Stage::Acceleration);
657
658 #ifndef NDEBUG
659 printf("%3d: %5g mom=%g,%g E=%g", m_integ.getNumStepsTaken(),
660 s.getTime(),
661 PG[0].norm(), PG[1].norm(), m_mbs.calcEnergy(s));
662 m_unis.showContactStatus(s, "STATE SAVER");
663 #endif
664
665 m_states.push_back(s);
666 }
667 private:
668 const MultibodySystem& m_mbs;
669 const MyUnilateralConstraintSet& m_unis;
670 const Integrator& m_integ;
671 mutable Array_<State> m_states;
672 };
673
674 // A periodic event reporter that does nothing; useful for exploring the
675 // effects of interrupting the simulation.
676 class Nada : public PeriodicEventReporter {
677 public:
Nada(Real reportInterval)678 explicit Nada(Real reportInterval)
679 : PeriodicEventReporter(reportInterval) {}
680
handleEvent(const State & s) const681 void handleEvent(const State& s) const override {
682 #ifndef NDEBUG
683 printf("%7g NADA\n", s.getTime());
684 #endif
685 }
686 };
687
688
689 //==============================================================================
690 // SHOW CONTACT
691 //==============================================================================
692 // For each visualization frame, generate ephemeral geometry to show just
693 // during this frame, based on the current State.
694 class ShowContact : public DecorationGenerator {
695 public:
ShowContact(const MyUnilateralConstraintSet & unis)696 ShowContact(const MyUnilateralConstraintSet& unis)
697 : m_unis(unis) {}
698
generateDecorations(const State & state,Array_<DecorativeGeometry> & geometry)699 void generateDecorations(const State& state,
700 Array_<DecorativeGeometry>& geometry) override
701 {
702 for (int i=0; i < m_unis.getNumContactElements(); ++i) {
703 const MyBristleVertexContactElementImpl& contact =
704 m_unis.getContactElement(i);
705 const Vec3 loc = contact.whereToDisplay(state);
706 if (contact.isInContact(state)) {
707 geometry.push_back(DecorativeSphere(.1)
708 .setTransform(loc)
709 .setColor(Red).setOpacity(.25));
710 String text = "LOCKED";
711 text = contact.isSticking(state) ? "STICKING"
712 : "CONTACT";
713 m_unis.getMultibodySystem().realize(state, Stage::Acceleration);
714 contact.showContactForces(state, geometry);
715 geometry.push_back(DecorativeText(String(i)+"-"+text)
716 .setColor(White).setScale(.1)
717 .setTransform(loc+Vec3(0,.04,0)));
718 } else {
719 geometry.push_back(DecorativeText(String(i))
720 .setColor(White).setScale(.1)
721 .setTransform(loc+Vec3(0,.02,0)));
722 }
723 }
724 }
725 private:
726 const MyUnilateralConstraintSet& m_unis;
727 };
728
729
730 //==============================================================================
731 // MY PUSH FORCE
732 //==============================================================================
733 // This is a force element that generates a constant force on a body for a
734 // specified time interval. It is useful to perturb the system to force it
735 // to transition from sticking to sliding, for example.
736 class MyPushForceImpl : public Force::Custom::Implementation {
737 public:
MyPushForceImpl(const MobilizedBody & bodyB,const Vec3 & stationB,const Vec3 & forceG,Real onTime,Real offTime)738 MyPushForceImpl(const MobilizedBody& bodyB,
739 const Vec3& stationB,
740 const Vec3& forceG, // force direction in Ground!
741 Real onTime,
742 Real offTime)
743 : m_bodyB(bodyB), m_stationB(stationB), m_forceG(forceG),
744 m_on(onTime), m_off(offTime)
745 { }
746
747 //--------------------------------------------------------------------------
748 // Custom force virtuals
calcForce(const State & state,Vector_<SpatialVec> & bodyForces,Vector_<Vec3> & particleForces,Vector & mobilityForces) const749 void calcForce(const State& state, Vector_<SpatialVec>& bodyForces,
750 Vector_<Vec3>& particleForces, Vector& mobilityForces) const
751 override
752 {
753 if (!(m_on <= state.getTime() && state.getTime() <= m_off))
754 return;
755
756 m_bodyB.applyForceToBodyPoint(state, m_stationB, m_forceG, bodyForces);
757
758 //SimTK_DEBUG4("PUSHING @t=%g (%g,%g,%g)\n", state.getTime(),
759 // m_forceG[0], m_forceG[1], m_forceG[2]);
760 }
761
762 // No potential energy.
calcPotentialEnergy(const State & state) const763 Real calcPotentialEnergy(const State& state) const override {return 0;}
764
calcDecorativeGeometryAndAppend(const State & state,Stage stage,Array_<DecorativeGeometry> & geometry) const765 void calcDecorativeGeometryAndAppend
766 (const State& state, Stage stage,
767 Array_<DecorativeGeometry>& geometry) const override
768 {
769 const Real ScaleFactor = 0.1;
770 if (stage != Stage::Time) return;
771 if (!(m_on <= state.getTime() && state.getTime() <= m_off))
772 return;
773 const Vec3 stationG = m_bodyB.findStationLocationInGround(state, m_stationB);
774 geometry.push_back(DecorativeLine(stationG-ScaleFactor*m_forceG, stationG)
775 .setColor(Yellow)
776 .setLineThickness(3));
777 }
778 private:
779 const MobilizedBody& m_bodyB;
780 const Vec3 m_stationB;
781 const Vec3 m_forceG;
782 Real m_on;
783 Real m_off;
784 };
785
786
787
788 //==============================================================================
789 // MAIN
790 //==============================================================================
main(int argc,char ** argv)791 int main(int argc, char** argv) {
792 try { // If anything goes wrong, an exception will be thrown.
793 const Real ReportInterval=1./30;
794 const Vec3 BrickHalfDims(.1, .25, .5);
795 const Real BrickMass = 10;
796 #ifdef USE_TIMS_PARAMS
797 const Real RunTime=16; // Tim's time
798 const Real Stiffness = 2e7;
799 const Real Dissipation = 1;
800 const Real CoefRest = 0;
801 const Real mu_d = .5; /* compliant: .7*/
802 const Real mu_s = .8; /* compliant: .7*/
803 const Real mu_v = /*0.05*/0; //TODO: fails with mu_v=1, vtrans=.01
804 const Real TransitionVelocity = 0.01;
805 const Inertia brickInertia(.1,.1,.1);
806 //const Real Radius = .02;
807 const Real Radius = 1;
808 #else
809 const Real RunTime=20;
810 const Real Stiffness = 1e6;
811 const Real CoefRest = 0;
812 const Real TargetVelocity = 3; // speed at which to match coef rest
813 // const Real Dissipation = (1-CoefRest)/TargetVelocity;
814 const Real Dissipation = .1;
815 const Real mu_d = .5;
816 const Real mu_s = .8;
817 const Real mu_v = 0*0.05;
818 const Real TransitionVelocity = 0.01;
819 const Inertia brickInertia(BrickMass*UnitInertia::brick(BrickHalfDims));
820 const Real Radius = BrickHalfDims[0]/3;
821 #endif
822
823 printf("\n******************** Tim's Box Bristle ********************\n");
824 printf("USING BRISTLE MODEL: Compliant material/integrated stiction\n");
825 #ifdef USE_TIMS_PARAMS
826 printf("Using Tim's parameters:\n");
827 #else
828 printf("Using Sherm's parameters:\n");
829 #endif
830 printf(" stiffness=%g dissipation=%g\n", Stiffness, Dissipation);
831 printf(" mu_d=%g mu_s=%g mu_v=%g\n", mu_d, mu_s, mu_v);
832 printf(" transition velocity=%g\n", TransitionVelocity);
833 printf(" brick inertia=%g %g %g\n",
834 brickInertia.getMoments()[0], brickInertia.getMoments()[1],
835 brickInertia.getMoments()[2]);
836 printf("******************** Tim's Box Bristle ********************\n\n");
837
838 // CREATE MULTIBODY SYSTEM AND ITS SUBSYSTEMS
839 MultibodySystem mbs;
840 SimbodyMatterSubsystem matter(mbs);
841 GeneralForceSubsystem forces(mbs);
842 Force::Gravity gravity(forces, matter, -YAxis, 9.81);
843 //Force::Gravity gravity(forces, matter, -UnitVec3(.3,1,0), 3*9.81);
844 MobilizedBody& Ground = matter.updGround();
845
846 // Define a material to use for contact. This is not very stiff.
847 ContactMaterial material(std::sqrt(Radius)*Stiffness,
848 Dissipation,
849 mu_s, // mu_static
850 mu_d, // mu_dynamic
851 mu_v); // mu_viscous
852
853 // ADD MOBILIZED BODIES AND CONTACT CONSTRAINTS
854
855 MyUnilateralConstraintSet unis(mbs);
856
857 Body::Rigid brickBody =
858 Body::Rigid(MassProperties(BrickMass, Vec3(0), brickInertia));
859
860 MobilizedBody::Free brick(Ground, Vec3(0),
861 brickBody, Vec3(0));
862 brick.addBodyDecoration(Transform(), DecorativeBrick(BrickHalfDims)
863 .setColor(Red).setOpacity(.3));
864 /*
865 1) t= 0.5, dt = 2 sec, pt = (0.05, 0.2, 0.4), fdir = (1,0,0), mag = 50N
866 2) t= 4.0, dt = 0.5 sec, pt = (0.03, 0.06, 0.09), fdir = (0.2,0.8,0), mag = 300N
867 3) t= 0.9, dt = 2 sec, pt = (0,0,0), fdir = (0,1,0), mag = 49.333N (half the weight of the block)
868 4) t= 13.0, dt = 1 sec, pt = (0 0 0), fdir = (-1,0,0), mag = 200N
869 */
870 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.05,0.2,0.4),
871 50 * Vec3(1,0,0),
872 0.5, 0.5+2));
873 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0.03, 0.06, 0.09),
874 300 * UnitVec3(0.2,0.8,0),
875 //300 * Vec3(0.2,0.8,0),
876 4, 4+0.5));
877 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
878 49.033 * Vec3(0,1,0),
879 9, 9+2));
880 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(0),
881 200 * Vec3(-1,0,0),
882 13, 13+1));
883
884 #ifndef USE_TIMS_PARAMS
885 // Extra late force.
886 Force::Custom(forces, new MyPushForceImpl(brick, Vec3(.1, 0, .45),
887 20 * Vec3(-1,-1,.5),
888 15, Infinity));
889 #endif
890
891 for (int i=-1; i<=1; i+=2)
892 for (int j=-1; j<=1; j+=2)
893 for (int k=-1; k<=1; k+=2) {
894 const Vec3 pt = Vec3(i,j,k).elementwiseMultiply(BrickHalfDims);
895 MyBristleVertexContactElementImpl* vertex =
896 new MyBristleVertexContactElementImpl(forces,
897 Ground, YAxis, 0, // halfplane
898 brick, pt, material, TransitionVelocity);
899 Force::Custom(forces, vertex); // add force element to system
900 unis.addBristleElement(vertex); // assign index, transition velocity
901 }
902
903 matter.setShowDefaultGeometry(false);
904 Visualizer viz(mbs);
905 viz.setShowSimTime(true);
906 viz.setShowFrameNumber(true);
907 viz.setShowFrameRate(true);
908 viz.addDecorationGenerator(new ShowContact(unis));
909
910 #ifdef ANIMATE
911 mbs.addEventReporter(new Visualizer::Reporter(viz, ReportInterval));
912 #else
913 // This does nothing but interrupt the simulation so that exact step
914 // sequence will be maintained with animation off.
915 mbs.addEventReporter(new Nada(ReportInterval));
916 #endif
917
918 viz.addFrameController(new Visualizer::BodyFollower(brick, Vec3(0),
919 Vec3(0, 1, 5)));
920
921 Vec3 cameraPos(0, 1, 2);
922 UnitVec3 cameraZ(0,0,1);
923 viz.setCameraTransform(Transform(Rotation(cameraZ, ZAxis,
924 UnitVec3(YAxis), YAxis),
925 cameraPos));
926 viz.pointCameraAt(Vec3(0,0,0), Vec3(0,1,0));
927
928 #ifdef USE_TIMS_PARAMS
929 Real accuracy = 1e-4;
930 RungeKuttaMersonIntegrator integ(mbs);
931 #else
932 //Real accuracy = 1e-4;
933 Real accuracy = 1e-3;
934 //Real accuracy = 1e-5;
935 //ExplicitEulerIntegrator integ(mbs);
936 //RungeKutta2Integrator integ(mbs);
937 //RungeKutta3Integrator integ(mbs);
938 //SemiExplicitEulerIntegrator integ(mbs, .005);
939 SemiExplicitEuler2Integrator integ(mbs);
940 //RungeKuttaFeldbergIntegrator integ(mbs);
941 //RungeKuttaMersonIntegrator integ(mbs);
942 //VerletIntegrator integ(mbs);
943 //CPodesIntegrator integ(mbs);
944 #endif
945
946 integ.setAccuracy(accuracy);
947 //integ.setMaximumStepSize(0.25);
948 integ.setMaximumStepSize(0.05);
949 //integ.setMaximumStepSize(0.002);
950 //integ.setAllowInterpolation(false);
951
952 StateSaver* stateSaver = new StateSaver(mbs,unis,integ,ReportInterval);
953 mbs.addEventReporter(stateSaver);
954
955 State s = mbs.realizeTopology(); // returns a reference to the the default state
956
957 //matter.setUseEulerAngles(s, true);
958
959 mbs.realizeModel(s); // define appropriate states for this System
960 mbs.realize(s, Stage::Instance); // instantiate constraints if any
961
962
963 /*
964 rX_q = 0.7 rad
965 rX_u = 1.0 rad/sec
966
967 rY_q = 0.6 rad
968 rY_u = 0.0 rad/sec
969
970 rZ_q = 0.5 rad
971 rZ_u = 0.2 rad/sec
972
973 tX_q = 0.0 m
974 tX_u = 10 m/s
975
976 tY_q = 1.4 m
977 tY_u = 0.0 m/s
978
979 tZ_q = 0.0 m
980 tZ_u = 0.0 m/s
981 */
982
983 #ifdef USE_TIMS_PARAMS
984 brick.setQToFitTranslation(s, Vec3(0,10,0));
985 brick.setUToFitLinearVelocity(s, Vec3(0,0,0));
986 #else
987 brick.setQToFitTranslation(s, Vec3(0,1.4,0));
988 brick.setUToFitLinearVelocity(s, Vec3(10,0,0));
989 const Rotation R_BC(SimTK::BodyRotationSequence,
990 0.7, XAxis, 0.6, YAxis, 0.5, ZAxis);
991 brick.setQToFitRotation(s, R_BC);
992 brick.setUToFitAngularVelocity(s, Vec3(1,0,.2));
993 #endif
994
995 mbs.realize(s, Stage::Velocity);
996 viz.report(s);
997
998 cout << "Initial configuration shown. Next? ";
999 getchar();
1000
1001 Assembler(mbs).setErrorTolerance(1e-6).assemble(s);
1002 viz.report(s);
1003 cout << "Assembled configuration shown. Ready? ";
1004 getchar();
1005
1006 // Now look for enabled contacts that aren't sliding; turn on stiction
1007 // for those.
1008 mbs.realize(s, Stage::Velocity);
1009 for (int i=0; i < unis.getNumContactElements(); ++i) {
1010 MyBristleVertexContactElementImpl& fric = unis.updContactElement(i);
1011 if (!fric.isInContact(s)) continue;
1012 const Real vSlip = fric.getActualSlipSpeed(s);
1013 fric.initializeForStiction(s); // just in case
1014 printf("friction element %d has v_slip=%g%s\n", i, vSlip,
1015 vSlip==0?" (ENABLING STICTION)":"");
1016 }
1017
1018 // Make sure Lapack gets initialized.
1019 Matrix M(1,1); M(0,0)=1.23;
1020 FactorLU Mlu(M);
1021
1022
1023 // Simulate it.
1024
1025 integ.setReturnEveryInternalStep(true);
1026 TimeStepper ts(mbs, integ);
1027 ts.setReportAllSignificantStates(true);
1028
1029 #ifdef TEST_REPEATABILITY
1030 const int tries = NTries;
1031 #else
1032 const int tries = 1;
1033 #endif
1034
1035 Array_< Array_<State> > states(tries);
1036 Array_< Array_<Real> > timeDiff(tries-1);
1037 Array_< Array_<Vector> > yDiff(tries-1);
1038 cout.precision(18);
1039 for (int ntry=0; ntry < tries; ++ntry) {
1040 mbs.resetAllCountersToZero();
1041 unis.initialize(s); // reinitialize
1042 ts.updIntegrator().resetAllStatistics();
1043 ts.initialize(s);
1044 int nStepsWithEvent = 0;
1045
1046 const double startReal = realTime();
1047 const double startCPU = cpuTime();
1048
1049 Integrator::SuccessfulStepStatus status;
1050 do {
1051 status=ts.stepTo(RunTime);
1052 #ifdef TEST_REPEATABILITY
1053 states[ntry].push_back(ts.getState());
1054 #endif
1055 const int j = states[ntry].size()-1;
1056 if (ntry>0) {
1057 int prev = ntry-1;
1058 //prev=0;
1059 Real dt = states[ntry][j].getTime()
1060 - states[prev][j].getTime();
1061 volatile double vd;
1062 const int ny = states[ntry][j].getNY();
1063 Vector d(ny);
1064 for (int i=0; i<ny; ++i) {
1065 vd = states[ntry][j].getY()[i]
1066 - states[prev][j].getY()[i];
1067 d[i] = vd;
1068 }
1069 timeDiff[prev].push_back(dt);
1070 yDiff[prev].push_back(d);
1071 cout << "\n" << states[prev][j].getTime()
1072 << "+" << dt << ":" << d << endl;
1073 }
1074
1075 const bool handledEvent = status==Integrator::ReachedEventTrigger;
1076 if (handledEvent) {
1077 ++nStepsWithEvent;
1078 SimTK_DEBUG3("EVENT %3d @%.17g status=%s\n\n",
1079 nStepsWithEvent, ts.getTime(),
1080 Integrator::getSuccessfulStepStatusString(status).c_str());
1081 } else {
1082 SimTK_DEBUG3("step %3d @%.17g status=%s\n",
1083 integ.getNumStepsTaken(), ts.getTime(),
1084 Integrator::getSuccessfulStepStatusString(status).c_str());
1085 }
1086 #ifndef NDEBUG
1087 viz.report(ts.getState());
1088 #endif
1089 } while (ts.getTime() < RunTime);
1090
1091
1092 const double timeInSec = realTime()-startReal;
1093 const double cpuInSec = cpuTime()-startCPU;
1094 const int evals = integ.getNumRealizations();
1095 cout << "Done -- took " << integ.getNumStepsTaken() << " steps in " <<
1096 timeInSec << "s for " << ts.getTime() << "s sim (avg step="
1097 << (1000*ts.getTime())/integ.getNumStepsTaken() << "ms) "
1098 << (1000*ts.getTime())/evals << "ms/eval\n";
1099 cout << "CPUtime " << cpuInSec << endl;
1100
1101 printf("Used Integrator %s at accuracy %g:\n",
1102 integ.getMethodName(), integ.getAccuracyInUse());
1103 printf("# STEPS/ATTEMPTS = %d/%d\n", integ.getNumStepsTaken(), integ.getNumStepsAttempted());
1104 printf("# ERR TEST FAILS = %d\n", integ.getNumErrorTestFailures());
1105 printf("# REALIZE/PROJECT = %d/%d\n", integ.getNumRealizations(), integ.getNumProjections());
1106 printf("# EVENT STEPS/HANDLER CALLS = %d/%d\n",
1107 nStepsWithEvent, mbs.getNumHandleEventCalls());
1108 }
1109
1110 for (int i=0; i<tries; ++i)
1111 cout << "nstates " << i << " " << states[i].size() << endl;
1112
1113 // Instant replay.
1114 while(true) {
1115 printf("Hit ENTER for replay (%d states) ...",
1116 stateSaver->getNumSavedStates());
1117 getchar();
1118 for (int i=0; i < stateSaver->getNumSavedStates(); ++i) {
1119 mbs.realize(stateSaver->getState(i), Stage::Velocity);
1120 viz.report(stateSaver->getState(i));
1121 }
1122 }
1123
1124 }
1125 catch (const std::exception& e) {
1126 printf("EXCEPTION THROWN: %s\n", e.what());
1127 exit(1);
1128 }
1129 catch (...) {
1130 printf("UNKNOWN EXCEPTION THROWN\n");
1131 exit(1);
1132 }
1133
1134 }
1135
1136 //==============================================================================
1137 // MY UNILATERAL CONSTRAINT SET
1138 //==============================================================================
1139
1140
1141 //--------------------------- SHOW CONTACT STATUS ------------------------------
1142 void MyUnilateralConstraintSet::
showContactStatus(const State & s,const String & place) const1143 showContactStatus(const State& s, const String& place) const
1144 {
1145 #ifndef NDEBUG
1146 printf("\n%s: uni status @t=%.15g\n", place.c_str(), s.getTime());
1147 m_mbs.realize(s, Stage::Acceleration);
1148 for (int i=0; i < getNumContactElements(); ++i) {
1149 const MyBristleVertexContactElementImpl& contact = getContactElement(i);
1150 const bool isActive = contact.isInContact(s);
1151 printf(" %6s %2d %s, h=%g dh=%g f=%g\n",
1152 isActive?"ACTIVE":"off", i, "bristle",
1153 contact.getHeight(s),contact.getHeightDot(s),
1154 isActive?contact.getNormalForce(s):Zero);
1155 if (!isActive) continue;
1156
1157 const bool isSticking = contact.isSticking(s);
1158 printf(" %8s friction %2d, |v|=%g\n",
1159 isSticking?"STICKING":"sliding", i,
1160 contact.getActualSlipSpeed(s));
1161 contact.writeFrictionInfo(s, " ", std::cout);
1162 }
1163 printf("\n");
1164 #endif
1165 }
1166