1 /* -------------------------------------------------------------------------- *
2 * Simbody(tm) *
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) 2008-12 Stanford University and the Authors. *
10 * Authors: Michael Sherman *
11 * Contributors: Peter Eastman *
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 /**@file
25 Private implementation of CompliantContactSubsystem and the built in contact
26 patch analysis methods.
27 **/
28
29 #include "SimTKcommon.h"
30
31 #include "simbody/internal/common.h"
32 #include "simbody/internal/ForceSubsystem.h"
33 #include "simbody/internal/ForceSubsystemGuts.h"
34 #include "simbody/internal/CompliantContactSubsystem.h"
35 #include "simbody/internal/ContactTrackerSubsystem.h"
36 #include "simbody/internal/SimbodyMatterSubsystem.h"
37 #include "simbody/internal/MultibodySystem.h"
38
39 namespace SimTK {
40
41 //==============================================================================
42 // COMPLIANT CONTACT SUBSYSTEM IMPL
43 //==============================================================================
44
45 class CompliantContactSubsystemImpl : public ForceSubsystemRep {
46 typedef std::map<ContactTypeId, const ContactForceGenerator*> GeneratorMap;
47 public:
48
CompliantContactSubsystemImpl(const ContactTrackerSubsystem & tracker)49 CompliantContactSubsystemImpl(const ContactTrackerSubsystem& tracker)
50 : ForceSubsystemRep("CompliantContactSubsystem", "0.0.1"),
51 m_tracker(tracker), m_transitionVelocity(Real(0.01)),
52 m_ooTransitionVelocity(1/m_transitionVelocity),
53 m_trackDissipatedEnergy(false), m_defaultGenerator(0)
54 {
55 }
56
getTransitionVelocity() const57 Real getTransitionVelocity() const {return m_transitionVelocity;}
getOOTransitionVelocity() const58 Real getOOTransitionVelocity() const {return m_ooTransitionVelocity;}
setTransitionVelocity(Real vt)59 void setTransitionVelocity(Real vt)
60 { m_transitionVelocity=vt; m_ooTransitionVelocity=1/vt;}
61
setTrackDissipatedEnergy(bool shouldTrack)62 void setTrackDissipatedEnergy(bool shouldTrack) {
63 if (m_trackDissipatedEnergy != shouldTrack) {
64 m_trackDissipatedEnergy = shouldTrack;
65 invalidateSubsystemTopologyCache();
66 }
67 }
getTrackDissipatedEnergy() const68 bool getTrackDissipatedEnergy() const {return m_trackDissipatedEnergy;}
69
getNumContactForces(const State & s) const70 int getNumContactForces(const State& s) const {
71 ensureForceCacheValid(s);
72 const Array_<ContactForce>& forces = getForceCache(s);
73 return (int)forces.size();
74 }
75
getContactForce(const State & s,int n) const76 const ContactForce& getContactForce(const State& s, int n) const {
77 const int numContactForces = getNumContactForces(s); // realize if needed
78 SimTK_ERRCHK2_ALWAYS(n < numContactForces,
79 "CompliantContactSubsystem::getContactForce()",
80 "There are currently only %d contact forces but force %d"
81 " was requested (they are numbered from 0). Use getNumContactForces()"
82 " first to determine how many are available.", numContactForces, n);
83 const Array_<ContactForce>& forces = getForceCache(s);
84 return forces[n];
85 }
86
87
getContactForceById(const State & s,ContactId id) const88 const ContactForce& getContactForceById(const State& s, ContactId id) const {
89 static const ContactForce invalidForce;
90 const int numContactForces = getNumContactForces(s); // realize if needed
91 const Array_<ContactForce>& forces = getForceCache(s);
92 // TODO: use a map
93 for (int i=0; i < numContactForces; ++i)
94 if (forces[i].getContactId()==id)
95 return forces[i];
96
97 return invalidForce;
98 }
99
calcContactPatchDetailsById(const State & state,ContactId id,ContactPatch & patch_G) const100 bool calcContactPatchDetailsById(const State& state,
101 ContactId id,
102 ContactPatch& patch_G) const
103 {
104 SimTK_STAGECHECK_GE_ALWAYS(getStage(state), Stage::Velocity,
105 "CompliantContactSubystemImpl::ensureForceCacheValid()");
106
107 const ContactSnapshot& active = m_tracker.getActiveContacts(state);
108 const Contact& contact = active.getContactById(id);
109
110 if (contact.isEmpty() || contact.getCondition() == Contact::Broken) {
111 patch_G.clear();
112 return false;
113 }
114
115 const Transform& X_S1S2 = contact.getTransform();
116 const ContactSurfaceIndex surf1(contact.getSurface1());
117 const ContactSurfaceIndex surf2(contact.getSurface2());
118 const MobilizedBody& mobod1 = m_tracker.getMobilizedBody(surf1);
119 const MobilizedBody& mobod2 = m_tracker.getMobilizedBody(surf2);
120
121 const Transform X_GS1 = mobod1.findFrameTransformInGround
122 (state, m_tracker.getContactSurfaceTransform(surf1));
123 const Transform X_GS2 = mobod2.findFrameTransformInGround
124 (state, m_tracker.getContactSurfaceTransform(surf2));
125
126 const SpatialVec V_GS1 = mobod1.findFrameVelocityInGround
127 (state, m_tracker.getContactSurfaceTransform(surf1));
128 const SpatialVec V_GS2 = mobod2.findFrameVelocityInGround
129 (state, m_tracker.getContactSurfaceTransform(surf2));
130
131 // Calculate the relative velocity of S2 in S1, expressed in S1.
132 const SpatialVec V_S1S2 =
133 findRelativeVelocity(X_GS1, V_GS1, X_GS2, V_GS2);
134
135 // Get the right force generator to use for this kind of contact.
136 const ContactForceGenerator& generator =
137 getForceGenerator(contact.getTypeId());
138
139 // Calculate the contact patch measured and expressed in S1.
140 generator.calcContactPatch(state, contact, V_S1S2,
141 patch_G); // it is actually in S1 at this point
142
143 // Re-express the contact patch in Ground for later use.
144 if (patch_G.isValid()) {
145 patch_G.changeFrameInPlace(X_GS1); // switch from S1 to Ground
146 return true;
147 }
148
149 return false;
150 }
151
getContactTrackerSubsystem() const152 const ContactTrackerSubsystem& getContactTrackerSubsystem() const
153 { return m_tracker; }
154
~CompliantContactSubsystemImpl()155 ~CompliantContactSubsystemImpl() {
156 delete m_defaultGenerator;
157 for (GeneratorMap::iterator p = m_generators.begin();
158 p != m_generators.end(); ++p)
159 delete p->second; // free the generator
160 // The map itself gets freed by its destructor here.
161 }
162
163 // We're going to take over ownership of this object. If the generator map
164 // already contains a generator for this type of contact, the new one replaces
165 // it.
adoptForceGenerator(CompliantContactSubsystem * subsys,ContactForceGenerator * generator)166 void adoptForceGenerator(CompliantContactSubsystem* subsys,
167 ContactForceGenerator* generator) {
168 assert(generator);
169 generator->setCompliantContactSubsystem(subsys);
170 invalidateSubsystemTopologyCache();
171 GeneratorMap::iterator p=m_generators.find(generator->getContactTypeId());
172 if (p != m_generators.end()) {
173 // We're replacing an existing generator.
174 delete p->second;
175 p->second = generator; // just copying the pointer
176 } else {
177 // Insert the new generator.
178 m_generators.insert(std::make_pair(generator->getContactTypeId(),
179 generator));
180 }
181 }
182
183 // Set the default force generator, deleting the previous one. Null is OK.
adoptDefaultForceGenerator(CompliantContactSubsystem * subsys,ContactForceGenerator * generator)184 void adoptDefaultForceGenerator(CompliantContactSubsystem* subsys,
185 ContactForceGenerator* generator) {
186 invalidateSubsystemTopologyCache();
187 delete m_defaultGenerator;
188 m_defaultGenerator = generator; // just copying the pointer
189 if (m_defaultGenerator)
190 m_defaultGenerator->setCompliantContactSubsystem(subsys);
191 }
192
193 // Do we have a generator registered for this type? If not, we'll invoke our
194 // default generator.
hasForceGenerator(ContactTypeId type) const195 bool hasForceGenerator(ContactTypeId type) const
196 { return m_generators.find(type) != m_generators.end(); }
197
hasDefaultForceGenerator() const198 bool hasDefaultForceGenerator() const
199 { return m_defaultGenerator != 0; }
200
201 // Get the generator registered for this type of contact or the default
202 // generator if nothing is registered.
getForceGenerator(ContactTypeId type) const203 const ContactForceGenerator& getForceGenerator(ContactTypeId type) const
204 { GeneratorMap::const_iterator p=m_generators.find(type);
205 return p != m_generators.end() ? *p->second : getDefaultForceGenerator(); }
206
207 // Get the default generator.
getDefaultForceGenerator() const208 const ContactForceGenerator& getDefaultForceGenerator() const
209 { assert(m_defaultGenerator); return *m_defaultGenerator; }
210
211
212 // These override default implementations of virtual methods in the
213 // Subsystem::Guts class.
214
cloneImpl() const215 CompliantContactSubsystemImpl* cloneImpl() const override
216 { return new CompliantContactSubsystemImpl(*this); }
217
218 // Allocate lazy evaluation cache entries to hold potential energy (calculable
219 // after position stage) and forces and power (calculable after velocity stage).
realizeSubsystemTopologyImpl(State & s) const220 int realizeSubsystemTopologyImpl(State& s) const override {
221 // Get writability briefly to fill in the Topology cache.
222 CompliantContactSubsystemImpl* wThis =
223 const_cast<CompliantContactSubsystemImpl*>(this);
224
225 // Calculating forces includes calculating PE for each force.
226 wThis->m_forceCacheIx = allocateLazyCacheEntry(s,
227 Stage::Velocity, new Value<Array_<ContactForce> >());
228 // This usually just requires summing up the forceCache PE values, but
229 // may also have to be calculated at Position stage in which case we can't
230 // calculate the forces.
231 wThis->m_potEnergyCacheIx = allocateLazyCacheEntry(s,
232 Stage::Position, new Value<Real>(NaN));
233
234 // This state variable is used to integrate power to get dissipated
235 // energy. Allocate only if requested.
236 if (m_trackDissipatedEnergy) {
237 Vector einit(1, Real(0));
238 wThis->m_dissipatedEnergyIx = allocateZ(s,einit);
239 }
240
241 return 0;
242 }
243
realizeSubsystemModelImpl(State & s) const244 int realizeSubsystemModelImpl(State& s) const override {
245 return 0;
246 }
247
realizeSubsystemInstanceImpl(const State & s) const248 int realizeSubsystemInstanceImpl(const State& s) const override {
249 return 0;
250 }
251
realizeSubsystemTimeImpl(const State & s) const252 int realizeSubsystemTimeImpl(const State& s) const override {
253 return 0;
254 }
255
realizeSubsystemPositionImpl(const State & s) const256 int realizeSubsystemPositionImpl(const State& s) const override {
257 return 0;
258 }
259
realizeSubsystemVelocityImpl(const State & s) const260 int realizeSubsystemVelocityImpl(const State& s) const override {
261 return 0;
262 }
263
realizeSubsystemDynamicsImpl(const State & s) const264 int realizeSubsystemDynamicsImpl(const State& s) const override {
265 ensureForceCacheValid(s);
266
267 const MultibodySystem& mbs = getMultibodySystem(); // my owner
268 const SimbodyMatterSubsystem& matter = mbs.getMatterSubsystem();
269
270 // Get access to System-global force cache array.
271 Vector_<SpatialVec>& rigidBodyForces =
272 mbs.updRigidBodyForces(s, Stage::Dynamics);
273
274 // Accumulate the values from the cache into the global arrays.
275 const ContactSnapshot& contacts = m_tracker.getActiveContacts(s);
276 const Array_<ContactForce>& forces = getForceCache(s);
277 for (unsigned i=0; i < forces.size(); ++i) {
278 const ContactForce& force = forces[i];
279 const Contact& contact = contacts.getContactById(force.getContactId());
280 const MobilizedBody& mobod1 = m_tracker.getMobilizedBody
281 (contact.getSurface1());
282 const MobilizedBody& mobod2 = m_tracker.getMobilizedBody
283 (contact.getSurface2());
284 const Vec3 r1 = force.getContactPoint() - mobod1.getBodyOriginLocation(s);
285 const Vec3 r2 = force.getContactPoint() - mobod2.getBodyOriginLocation(s);
286 const SpatialVec& F2cpt = force.getForceOnSurface2(); // at contact pt
287 // Shift applied force to body origins.
288 const SpatialVec F2( F2cpt[0] + r2 % F2cpt[1], F2cpt[1]);
289 const SpatialVec F1(-F2cpt[0] + r1 % -F2cpt[1], -F2cpt[1]);
290 mobod1.applyBodyForce(s, F1, rigidBodyForces);
291 mobod2.applyBodyForce(s, F2, rigidBodyForces);
292 }
293
294 return 0;
295 }
296
297 // Potential energy is normally a side effect of force calculation done after
298 // Velocity stage. But if only positions are available, we
299 // have to calculate forces at zero velocity and then throw away everything
300 // but the PE.
calcPotentialEnergy(const State & state) const301 Real calcPotentialEnergy(const State& state) const override {
302 ensurePotentialEnergyCacheValid(state);
303 return getPotentialEnergyCache(state);
304 }
305
realizeSubsystemAccelerationImpl(const State & state) const306 int realizeSubsystemAccelerationImpl(const State& state) const override {
307 if (!m_trackDissipatedEnergy)
308 return 0; // nothing to do here in that case
309
310 ensureForceCacheValid(state);
311 Real powerLoss = 0;
312 const Array_<ContactForce>& forces = getForceCache(state);
313 for (unsigned i=0; i < forces.size(); ++i)
314 powerLoss += forces[i].getPowerDissipation();
315 updDissipatedEnergyDeriv(state) = powerLoss;
316 return 0;
317 }
318
realizeSubsystemReportImpl(const State &) const319 int realizeSubsystemReportImpl(const State&) const override {
320 return 0;
321 }
322
getDissipatedEnergyVar(const State & s) const323 const Real& getDissipatedEnergyVar(const State& s) const
324 { return getZ(s)[m_dissipatedEnergyIx]; }
updDissipatedEnergyVar(State & s) const325 Real& updDissipatedEnergyVar(State& s) const
326 { return updZ(s)[m_dissipatedEnergyIx]; }
327
328 //--------------------------------------------------------------------------
329 private:
330
updDissipatedEnergyDeriv(const State & s) const331 Real& updDissipatedEnergyDeriv(const State& s) const
332 { return updZDot(s)[m_dissipatedEnergyIx]; }
333
getPotentialEnergyCache(const State & s) const334 const Real& getPotentialEnergyCache(const State& s) const
335 { return Value<Real>::downcast(getCacheEntry(s,m_potEnergyCacheIx)); }
getForceCache(const State & s) const336 const Array_<ContactForce>& getForceCache(const State& s) const
337 { return Value<Array_<ContactForce> >::downcast
338 (getCacheEntry(s,m_forceCacheIx)); }
339
updPotentialEnergyCache(const State & s) const340 Real& updPotentialEnergyCache(const State& s) const
341 { return Value<Real>::updDowncast(updCacheEntry(s,m_potEnergyCacheIx)); }
updForceCache(const State & s) const342 Array_<ContactForce>& updForceCache(const State& s) const
343 { return Value<Array_<ContactForce> >::updDowncast
344 (updCacheEntry(s,m_forceCacheIx)); }
345
isPotentialEnergyCacheValid(const State & s) const346 bool isPotentialEnergyCacheValid(const State& s) const
347 { return isCacheValueRealized(s,m_potEnergyCacheIx); }
isForceCacheValid(const State & s) const348 bool isForceCacheValid(const State& s) const
349 { return isCacheValueRealized(s,m_forceCacheIx); }
350
351
markPotentialEnergyCacheValid(const State & s) const352 void markPotentialEnergyCacheValid(const State& s) const
353 { markCacheValueRealized(s,m_potEnergyCacheIx); }
markForceCacheValid(const State & s) const354 void markForceCacheValid(const State& s) const
355 { markCacheValueRealized(s,m_forceCacheIx); }
356
357 void ensurePotentialEnergyCacheValid(const State&) const;
358 void ensureForceCacheValid(const State&) const;
359
360
361
362 // TOPOLOGY "STATE"
363
364 // This is the ContactTracker that we will consult to obtain Contact objects
365 // to use as raw material for contact force generation.
366 const ContactTrackerSubsystem& m_tracker;
367
368 // This is the value that should be used by generators for the friction
369 // transition velocity if they are using a Hollars-like friction model.
370 // Also precalculate the inverse to avoid expensive runtime division.
371 Real m_transitionVelocity;
372 Real m_ooTransitionVelocity; // 1/vt
373
374 // This flag determines whether we allocate a Z variable to integrate
375 // dissipated power to track dissipated energy.
376 bool m_trackDissipatedEnergy;
377
378 // This map owns the generator objects; be sure to clean up on destruction or
379 // when a generator is replaced.
380 GeneratorMap m_generators;
381
382 // This is the generator to use for an unrecognized ContactTypeId. Typically
383 // this will either do nothing silently or throw an error.
384 ContactForceGenerator* m_defaultGenerator;
385
386 // TOPOLOGY "CACHE"
387
388 // These must be set during realizeTopology and treated as const thereafter.
389 // Note that we don't allocate the dissipated energy variable unless the
390 // flag above is set true (prior to realizeTopology()).
391 ZIndex m_dissipatedEnergyIx;
392 CacheEntryIndex m_potEnergyCacheIx;
393 CacheEntryIndex m_forceCacheIx;
394 };
395
396 void CompliantContactSubsystemImpl::
ensurePotentialEnergyCacheValid(const State & state) const397 ensurePotentialEnergyCacheValid(const State& state) const {
398 if (isPotentialEnergyCacheValid(state)) return;
399
400 SimTK_STAGECHECK_GE_ALWAYS(getStage(state), Stage::Position,
401 "CompliantContactSubystemImpl::ensurePotentialEnergyCacheValid()");
402
403 Real& pe = updPotentialEnergyCache(state);
404 pe = 0;
405
406 // If the State has been realized to Velocity stage, we can just sum up
407 // the PEs from the ContactForce objects.
408 if (getStage(state) >= Stage::Velocity) {
409 ensureForceCacheValid(state);
410 const Array_<ContactForce>& forces = getForceCache(state);
411 for (unsigned i=0; i < forces.size(); ++i)
412 pe += forces[i].getPotentialEnergy();
413 markPotentialEnergyCacheValid(state);
414 return;
415 }
416
417 // The State has been realized to Position stage, so we're going to have
418 // to calculate forces at zero velocity and then throw away all the
419 // results except for the PE.
420 const ContactSnapshot& active = m_tracker.getActiveContacts(state);
421 const int nContacts = active.getNumContacts();
422 for (int i=0; i<nContacts; ++i) {
423 const Contact& contact = active.getContact(i);
424 const ContactForceGenerator& generator =
425 getForceGenerator(contact.getTypeId());
426 ContactForce force;
427 generator.calcContactForce(state,contact,SpatialVec(Vec3(0)), force);
428 pe += force.getPotentialEnergy();
429 }
430
431 markPotentialEnergyCacheValid(state);
432 }
433
434
435 void CompliantContactSubsystemImpl::
ensureForceCacheValid(const State & state) const436 ensureForceCacheValid(const State& state) const {
437 if (isForceCacheValid(state)) return;
438
439 SimTK_STAGECHECK_GE_ALWAYS(getStage(state), Stage::Velocity,
440 "CompliantContactSubystemImpl::ensureForceCacheValid()");
441
442 Array_<ContactForce>& forces = updForceCache(state);
443 forces.clear();
444
445
446 const ContactSnapshot& active = m_tracker.getActiveContacts(state);
447 const int nContacts = active.getNumContacts();
448 for (int i=0; i<nContacts; ++i) {
449 const Contact& contact = active.getContact(i);
450 if (contact.getCondition() == Contact::Broken) {
451 // No need to generate forces; this will be gone next time.
452 continue;
453 }
454 const Transform& X_S1S2 = contact.getTransform();
455 const ContactSurfaceIndex surf1(contact.getSurface1());
456 const ContactSurfaceIndex surf2(contact.getSurface2());
457 const MobilizedBody& mobod1 = m_tracker.getMobilizedBody(surf1);
458 const MobilizedBody& mobod2 = m_tracker.getMobilizedBody(surf2);
459
460 // TODO: These two are expensive (63 flops each) and shouldn't have
461 // to be recalculated here since we must have used them in creating
462 // the Contact and X_S1S2.
463 const Transform X_GS1 = mobod1.findFrameTransformInGround
464 (state, m_tracker.getContactSurfaceTransform(surf1));
465 const Transform X_GS2 = mobod2.findFrameTransformInGround
466 (state, m_tracker.getContactSurfaceTransform(surf2));
467
468 const SpatialVec V_GS1 = mobod1.findFrameVelocityInGround
469 (state, m_tracker.getContactSurfaceTransform(surf1));
470 const SpatialVec V_GS2 = mobod2.findFrameVelocityInGround
471 (state, m_tracker.getContactSurfaceTransform(surf2));
472
473 // Calculate the relative velocity of S2 in S1, expressed in S1.
474 const SpatialVec V_S1S2 =
475 findRelativeVelocity(X_GS1, V_GS1, X_GS2, V_GS2); // 51 flops
476
477 const ContactForceGenerator& generator =
478 getForceGenerator(contact.getTypeId());
479 forces.push_back(); // allocate a new garbage ContactForce
480 // Calculate the contact force measured and expressed in S1.
481 generator.calcContactForce(state, contact, V_S1S2, forces.back());
482 // Re-express the contact force in Ground for later use.
483 if (forces.back().isValid())
484 forces.back().changeFrameInPlace(X_GS1); // switch to Ground
485 else
486 forces.pop_back(); // never mind ...
487 }
488
489 markForceCacheValid(state);
490 }
491
492
493 //==============================================================================
494 // COMPLIANT CONTACT SUBSYSTEM
495 //==============================================================================
496
497 /*static*/ bool
isInstanceOf(const ForceSubsystem & s)498 CompliantContactSubsystem::isInstanceOf(const ForceSubsystem& s)
499 { return CompliantContactSubsystemImpl::isA(s.getRep()); }
500 /*static*/ const CompliantContactSubsystem&
downcast(const ForceSubsystem & s)501 CompliantContactSubsystem::downcast(const ForceSubsystem& s)
502 { assert(isInstanceOf(s));
503 return static_cast<const CompliantContactSubsystem&>(s); }
504 /*static*/ CompliantContactSubsystem&
updDowncast(ForceSubsystem & s)505 CompliantContactSubsystem::updDowncast(ForceSubsystem& s)
506 { assert(isInstanceOf(s));
507 return static_cast<CompliantContactSubsystem&>(s); }
508
509 const CompliantContactSubsystemImpl&
getImpl() const510 CompliantContactSubsystem::getImpl() const
511 { return SimTK_DYNAMIC_CAST_DEBUG<const CompliantContactSubsystemImpl&>
512 (ForceSubsystem::getRep()); }
513 CompliantContactSubsystemImpl&
updImpl()514 CompliantContactSubsystem::updImpl()
515 { return SimTK_DYNAMIC_CAST_DEBUG<CompliantContactSubsystemImpl&>
516 (ForceSubsystem::updRep()); }
517
CompliantContactSubsystem(MultibodySystem & mbs,const ContactTrackerSubsystem & tracker)518 CompliantContactSubsystem::CompliantContactSubsystem
519 (MultibodySystem& mbs, const ContactTrackerSubsystem& tracker)
520 : ForceSubsystem()
521 { adoptSubsystemGuts(new CompliantContactSubsystemImpl(tracker));
522 mbs.addForceSubsystem(*this); // mbs steals ownership
523
524 // Register default contact force generators.
525 adoptForceGenerator(new ContactForceGenerator::HertzCircular());
526 adoptForceGenerator(new ContactForceGenerator::HertzElliptical());
527 adoptForceGenerator(new ContactForceGenerator::BrickHalfSpacePenalty());
528 adoptForceGenerator(new ContactForceGenerator::ElasticFoundation());
529 adoptDefaultForceGenerator(new ContactForceGenerator::DoNothing());
530 }
531
getTransitionVelocity() const532 Real CompliantContactSubsystem::getTransitionVelocity() const
533 { return getImpl().getTransitionVelocity(); }
getOOTransitionVelocity() const534 Real CompliantContactSubsystem::getOOTransitionVelocity() const
535 { return getImpl().getOOTransitionVelocity(); }
setTransitionVelocity(Real vt)536 void CompliantContactSubsystem::setTransitionVelocity(Real vt) {
537 SimTK_ERRCHK1_ALWAYS(vt > 0,
538 "CompliantContactSubsystem::setTransitionVelocity()",
539 "Friction transition velocity %g is illegal.", vt);
540 updImpl().setTransitionVelocity(vt);
541 }
542
setTrackDissipatedEnergy(bool shouldTrack)543 void CompliantContactSubsystem::setTrackDissipatedEnergy(bool shouldTrack)
544 { updImpl().setTrackDissipatedEnergy(shouldTrack); }
getTrackDissipatedEnergy() const545 bool CompliantContactSubsystem::getTrackDissipatedEnergy() const
546 { return getImpl().getTrackDissipatedEnergy(); }
547
getNumContactForces(const State & s) const548 int CompliantContactSubsystem::getNumContactForces(const State& s) const
549 { return getImpl().getNumContactForces(s); }
550
551 const ContactForce& CompliantContactSubsystem::
getContactForce(const State & s,int n) const552 getContactForce(const State& s, int n) const
553 { return getImpl().getContactForce(s,n); }
554
555
556 const ContactForce& CompliantContactSubsystem::
getContactForceById(const State & s,ContactId id) const557 getContactForceById(const State& s, ContactId id) const
558 { return getImpl().getContactForceById(s,id); }
559
560 bool CompliantContactSubsystem::
calcContactPatchDetailsById(const State & state,ContactId id,ContactPatch & patch_G) const561 calcContactPatchDetailsById(const State& state,
562 ContactId id,
563 ContactPatch& patch_G) const
564 { return getImpl().calcContactPatchDetailsById(state,id,patch_G); }
565
566 Real CompliantContactSubsystem::
getDissipatedEnergy(const State & s) const567 getDissipatedEnergy(const State& s) const {
568 SimTK_ERRCHK_ALWAYS(getTrackDissipatedEnergy(),
569 "CompliantContactSubsystem::getDissipatedEnergy()",
570 "You can't call getDissipatedEnergy() unless you have previously "
571 "enabled tracking of dissipated energy with a call to "
572 "setTrackDissipatedEnergy().");
573
574 return getImpl().getDissipatedEnergyVar(s); }
575
576 void CompliantContactSubsystem::
setDissipatedEnergy(State & s,Real energy) const577 setDissipatedEnergy(State& s, Real energy) const {
578 SimTK_ERRCHK_ALWAYS(getTrackDissipatedEnergy(),
579 "CompliantContactSubsystem::setDissipatedEnergy()",
580 "You can't call setDissipatedEnergy() unless you have previously "
581 "enabled tracking of dissipated energy with a call to "
582 "setTrackDissipatedEnergy().");
583 SimTK_ERRCHK1_ALWAYS(energy >= 0,
584 "CompliantContactSubsystem::setDissipatedEnergy()",
585 "The initial value for the dissipated energy must be nonnegative"
586 " but an attempt was made to set it to %g.", energy);
587
588 getImpl().updDissipatedEnergyVar(s) = energy;
589 }
590
591
adoptForceGenerator(ContactForceGenerator * generator)592 void CompliantContactSubsystem::adoptForceGenerator
593 (ContactForceGenerator* generator)
594 { return updImpl().adoptForceGenerator(this, generator); }
595
adoptDefaultForceGenerator(ContactForceGenerator * generator)596 void CompliantContactSubsystem::adoptDefaultForceGenerator
597 (ContactForceGenerator* generator)
598 { return updImpl().adoptDefaultForceGenerator(this, generator); }
599
hasForceGenerator(ContactTypeId type) const600 bool CompliantContactSubsystem::hasForceGenerator(ContactTypeId type) const
601 { return getImpl().hasForceGenerator(type); }
hasDefaultForceGenerator() const602 bool CompliantContactSubsystem::hasDefaultForceGenerator() const
603 { return getImpl().hasDefaultForceGenerator(); }
604
605 const ContactForceGenerator& CompliantContactSubsystem::
getContactForceGenerator(ContactTypeId contactType) const606 getContactForceGenerator(ContactTypeId contactType) const
607 { return getImpl().getForceGenerator(contactType); }
608
609 const ContactForceGenerator& CompliantContactSubsystem::
getDefaultForceGenerator() const610 getDefaultForceGenerator() const
611 { return getImpl().getDefaultForceGenerator(); }
612
613 const ContactTrackerSubsystem& CompliantContactSubsystem::
getContactTrackerSubsystem() const614 getContactTrackerSubsystem() const
615 { return getImpl().getContactTrackerSubsystem(); }
616
getMultibodySystem() const617 const MultibodySystem& CompliantContactSubsystem::getMultibodySystem() const
618 { return MultibodySystem::downcast(getSystem()); }
619
620
621 // Input x goes from 0 to 1; output goes 0 to 1 but smoothed with an S-shaped
622 // quintic with two zero derivatives at either end. Cost is 7 flops.
step5(Real x)623 inline static Real step5(Real x) {
624 assert(0 <= x && x <= 1);
625 const Real x3=x*x*x;
626 return x3*(10+x*(6*x-15)); //10x^3-15x^4+6x^5
627 }
628
629 // Input x goes from 0 to 1; output goes from 0 to y. First derivative
630 // is 0 at the beginning and yd at the end. Second derivatives are zero
631 // at both ends:
632 //
633 // y - *
634 // * | slope=yd
635 // *------
636 // *
637 // 0 - * *
638 // x=0 1
639 //
640 // Cost is 16 flops.
step5d(Real y,Real yd,Real x)641 inline static Real step5d(Real y, Real yd, Real x) {
642 assert(0 <= x && x <= 1);
643 const Real a=6*y-3*yd, b=-15*y+7*yd, c=10*y-4*yd, x3=x*x*x;
644 return x3*(c + x*(b + x*a));
645 }
646
647
648 // This is the sum of two curves:
649 // (1) a wet friction term mu_wet which is a linear function of velocity:
650 // mu_wet = uv*v
651 // (2) a dry friction term mu_dry which is a quintic spline with 4 segments:
652 // mu_dry =
653 // (a) v=0..1: smooth interpolation from 0 to us
654 // (b) v=1..3: smooth interp from us down to ud (Stribeck)
655 // (c) v=3..Inf ud
656 // CAUTION: uv and v must be dimensionless in multiples of transition velocity.
657 // The function mu = mu_wet + mu_dry is zero at v=0 with 1st deriv (slope) uv
658 // and 2nd deriv (curvature) 0. At large velocities v>>0 the value is
659 // ud+uv*v, again with slope uv and zero curvature. We want mu(v) to be c2
660 // continuous, so mu_wet(v) must have zero slope and curvature at v==0 and
661 // at v==3 where it takes on a constant value ud.
662 //
663 // Cost: stiction 12 flops
664 // stribeck 14 flops
665 // sliding 3 flops
666 // Curve looks like this:
667 //
668 // us+uv ***
669 // * * *
670 // * * *____| slope = uv at Inf
671 // * * *
672 // ud+3uv * * *
673 // *
674 // *
675 // *
676 // 0 *____| slope = uv at 0
677 //
678 // | | |
679 // v=0 1 3
680 //
681 // This calculates a composite coefficient of friction that you should use
682 // to scale the normal force to produce the friction force.
stribeck(Real us,Real ud,Real uv,Real v)683 inline static Real stribeck(Real us, Real ud, Real uv, Real v) {
684 const Real mu_wet = uv*v;
685 Real mu_dry;
686 if (v >= 3) mu_dry = ud; // sliding
687 else if (v >= 1) mu_dry = us - (us-ud)*step5((v-1)/2); // Stribeck
688 else mu_dry = us*step5(v); // 0 <= v < 1 (stiction)
689 return mu_dry + mu_wet;
690 }
691
692 // CAUTION: uv and v must be dimensionless in multiples of transition velocity.
693 // Const 9 flops + 1 divide = approx 25 flops.
694 // This calculates a composite coefficient of friction that you should use
695 // to scale the normal force to produce the friction force.
696 // Curve is similar to Stribeck above but more violent with a discontinuous
697 // derivative at v==1.
hollars(Real us,Real ud,Real uv,Real v)698 inline static Real hollars(Real us, Real ud, Real uv, Real v) {
699 const Real mu = std::min(v, Real(1))*(ud + 2*(us-ud)/(1+v*v))
700 + uv*v;
701 return mu;
702 }
703
704 // This function is shared between the Hertz circular and Hertz elliptical
705 // contacts. We only need to pass in the effective relative radius R and the
706 // elliptical correction factor e(=1 for circular); everything else is the
707 // same.
708 // This is around 250 flops.
calcHertzContactForce(const CompliantContactSubsystem & subsys,const ContactTrackerSubsystem & tracker,const State & state,const Contact & contact,const UnitVec3 & normal_S1,const Vec3 & origin_S1,Real depth,const SpatialVec & V_S1S2,Real R,Real e,ContactForce & contactForce_S1,Array_<ContactDetail> * details)709 static void calcHertzContactForce
710 (const CompliantContactSubsystem& subsys,
711 const ContactTrackerSubsystem& tracker,
712 const State& state,
713 const Contact& contact, // contains X_S1S2
714 const UnitVec3& normal_S1, // away from surf1, exp. in S1
715 const Vec3& origin_S1, // half way btw undeformed surfaces
716 Real depth,
717 const SpatialVec& V_S1S2, // relative surface velocity, S2 in S1
718 Real R, // effective relative radius
719 Real e, // elliptical correction factor
720 ContactForce& contactForce_S1,
721 Array_<ContactDetail>* details) // pass as null if you don't care
722 {
723 if (details) details->clear();
724 if (depth <= 0) {
725 contactForce_S1.clear(); // no contact; invalidate return result
726 return;
727 }
728
729 const ContactSurfaceIndex surf1x = contact.getSurface1();
730 const ContactSurfaceIndex surf2x = contact.getSurface2();
731 const Transform& X_S1S2 = contact.getTransform();
732
733 // Abbreviations.
734 const Rotation& R12 = X_S1S2.R(); // orientation of S2 in S1
735 const Vec3& p12 = X_S1S2.p(); // position of O2 in S1
736 const Vec3& w12 = V_S1S2[0]; // ang. vel. of S2 in S1
737 const Vec3& v12 = V_S1S2[1]; // vel. of O2 in S1
738
739 const ContactSurface& surf1 = tracker.getContactSurface(surf1x);
740 const ContactSurface& surf2 = tracker.getContactSurface(surf2x);
741 // Note that Hertz doesn't care about the "thickness" property of a
742 // ContactSurface; it is able to estimate %strain from the relationship
743 // between the deformation and local radii of curvature.
744
745 const ContactMaterial& mat1 = surf1.getMaterial();
746 const ContactMaterial& mat2 = surf2.getMaterial();
747
748 // Use 2/3 power of stiffness for combining here. (We'll raise the combined
749 // result k to the 3/2 power below.)
750 const Real k1=mat1.getStiffness23(), k2=mat2.getStiffness23();
751 const Real c1=mat1.getDissipation(), c2=mat2.getDissipation();
752
753 // Adjust the contact location based on the relative stiffness of the
754 // two materials. s1 is the fraction of the "squishing" (deformation) that
755 // is done by surface1 -- if surface2 is very stiff then surface1 does most.
756
757 const Real s1 = k2/(k1+k2); // 0..1
758 const Real s2 = 1-s1; // 1..0
759 const Real x = depth;
760
761 // Actual contact point moves closer to stiffer surface.
762 const Vec3 contactPt_S1 = origin_S1 + (x*(Real(0.5)-s1))*normal_S1;
763
764 // Calculate the Hertz force fH, which is conservative.
765 const Real k = k1*s1; // (==k2*s2) == E^(2/3)
766 const Real c = c1*s1 + c2*s2;
767 // fH = 4/3 e R^1/2 k^3/2 x^3/2
768 const Real fH = e*Real(4./3.)*k*x*std::sqrt(R*k*x); // always >= 0
769
770 // Calculate the relative velocity of the two bodies at the contact point.
771 // We're considering S1 fixed, so we just need the velocity in S1 of
772 // the station of S2 that is coincident with the contact point.
773 const Vec3 contactPt2_S1 = contactPt_S1 - p12; // S2 station, exp. in S1
774
775 // All vectors are in S1; dropping the "_S1" notation now.
776
777 // Velocity of surf2 at contact point is opposite direction of normal
778 // when penetration is increasing.
779 const Vec3 vel = v12 + w12 % contactPt2_S1;
780 // Want xdot > 0 when x is increasing; normal points the other way
781 const Real xdot = -dot(vel, normal_S1); // penetration rate (signed)
782 const Vec3 velNormal = -xdot*normal_S1;
783 const Vec3 velTangent = vel-velNormal;
784
785 // Calculate the Hunt-Crossley force, which is dissipative, and the
786 // total normal force including elasticity and dissipation.
787 const Real fHC = fH*Real(1.5)*c*xdot; // same sign as xdot
788 const Real fNormal = fH + fHC; // < 0 means "sticking"; see below
789
790 // Start filling out the contact force.
791 contactForce_S1.setContactId(contact.getContactId());
792 contactForce_S1.setContactPoint(contactPt_S1);
793
794 // Total force can be negative under unusual circumstances ("yanking");
795 // that means no force is generated and no stored PE will be recovered.
796 // This will most often occur in to-be-rejected trial steps but can
797 // occasionally be real.
798 if (fNormal <= 0) {
799 //std::cout << "YANKING!!!\n";
800 contactForce_S1.setForceOnSurface2(SpatialVec(Vec3(0)));
801 contactForce_S1.setPotentialEnergy(0);
802 contactForce_S1.setPowerDissipation(0);
803 return; // there is contact, but no force
804 }
805
806 const Vec3 forceH = fH *normal_S1; // as applied to surf2
807 const Vec3 forceHC = fHC*normal_S1;
808 const Real potentialEnergy = Real(2./5.)*fH*x;
809 const Real powerHC = fHC*xdot; // rate of energy loss, >= 0
810
811 // Calculate the friction force.
812 Vec3 forceFriction(0);
813 Real powerFriction = 0;
814 const Real vslipSq = velTangent.normSqr();
815 if (vslipSq > square(SignificantReal)) {
816 const Real vslip = std::sqrt(vslipSq); // expensive
817 const Real us1=mat1.getStaticFriction(), us2=mat2.getStaticFriction();
818 const Real ud1=mat1.getDynamicFriction(), ud2=mat2.getDynamicFriction();
819 const Real uv1=mat1.getViscousFriction(), uv2=mat2.getViscousFriction();
820 const Real vtrans = subsys.getTransitionVelocity();
821 const Real ooVtrans = subsys.getOOTransitionVelocity(); // 1/vtrans
822
823 // Calculate effective coefficients of friction, being careful not
824 // to divide 0/0 if both are frictionless.
825 Real us = 2*us1*us2; if (us!=0) us /= (us1+us2);
826 Real ud = 2*ud1*ud2; if (ud!=0) ud /= (ud1+ud2);
827 Real uv = 2*uv1*uv2; if (uv!=0) uv /= (uv1+uv2);
828 assert(us >= ud);
829
830 // Express slip velocity as unitless multiple of transition velocity.
831 const Real v = vslip * ooVtrans;
832 // Must scale viscous coefficient to match unitless velocity.
833 const Real mu=stribeck(us,ud,uv*vtrans,v);
834 //const Real mu=hollars(us,ud,uv*vtrans,v);
835 const Real fFriction = fNormal * mu;
836 // Force direction on S2 opposes S2's velocity.
837 forceFriction = (-fFriction/vslip)*velTangent; // in S1
838 powerFriction = fFriction * vslip; // >= 0
839 }
840
841 const Vec3 forceLoss = forceHC + forceFriction;
842 const Vec3 forceTotal = forceH + forceLoss;
843
844 // Report the force (as applied at contactPt).
845 contactForce_S1.setForceOnSurface2(SpatialVec(Vec3(0),forceTotal));
846 contactForce_S1.setPotentialEnergy(potentialEnergy);
847 // Don't include dot(forceH,velNormal) power due to conservative force
848 // here. This way we don't double-count the energy on the way in as
849 // integrated power and potential energy. Although the books would balance
850 // again when the contact is broken, it makes continuous contact look as
851 // though some energy has been lost. In the "yanking" case above, without
852 // including the conservative power term we will actually lose energy
853 // because the deformed material isn't allowed to push back on us so the
854 // energy is lost to surface vibrations or some other unmodeled effect.
855 const Real powerLoss = powerHC + powerFriction;
856 contactForce_S1.setPowerDissipation(powerLoss);
857
858 // Fill in contact details if requested (in S1 frame).
859 if (details) {
860 details->push_back(); // default constructed
861 ContactDetail& detail = details->back();
862 detail.m_contactPt = contactPt_S1;
863 detail.m_patchNormal = normal_S1;
864 detail.m_slipVelocity = velTangent;
865 detail.m_forceOnSurface2 = forceTotal;
866 detail.m_deformation = x;
867 detail.m_deformationRate = xdot;
868 detail.m_patchArea = NaN;
869 detail.m_peakPressure = NaN;
870 detail.m_potentialEnergy = potentialEnergy;
871 detail.m_powerLoss = powerLoss;
872 }
873 }
874
875
876 //==============================================================================
877 // HERTZ CIRCULAR GENERATOR
878 //==============================================================================
calcContactForce(const State & state,const Contact & overlap,const SpatialVec & V_S1S2,ContactForce & contactForce_S1) const879 void ContactForceGenerator::HertzCircular::calcContactForce
880 (const State& state,
881 const Contact& overlap, // contains X_S1S2
882 const SpatialVec& V_S1S2, // relative surface velocity, S2 in S1
883 ContactForce& contactForce_S1) const
884 {
885 SimTK_ASSERT(CircularPointContact::isInstance(overlap),
886 "ContactForceGenerator::HertzCircular::calcContactForce(): expected"
887 " CircularPointContact.");
888
889 const CircularPointContact& contact = CircularPointContact::getAs(overlap);
890 const Real depth = contact.getDepth();
891
892 const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
893 const ContactTrackerSubsystem& tracker = subsys.getContactTrackerSubsystem();
894
895 // normal points away from surf1, expressed in surf1's frame
896 const UnitVec3& normal_S1 = contact.getNormal();
897 // origin is half way between the two surfaces as though they had
898 // equal stiffness
899 const Vec3& origin_S1 = contact.getOrigin();
900
901 const Real R = contact.getEffectiveRadius();
902
903 calcHertzContactForce(subsys, tracker, state, overlap,
904 normal_S1, origin_S1, depth,
905 V_S1S2, R, 1, contactForce_S1, 0);
906 }
907
calcContactPatch(const State & state,const Contact & overlap,const SpatialVec & V_S1S2,ContactPatch & patch_S1) const908 void ContactForceGenerator::HertzCircular::calcContactPatch
909 (const State& state,
910 const Contact& overlap,
911 const SpatialVec& V_S1S2,
912 ContactPatch& patch_S1) const
913 {
914 SimTK_ASSERT(CircularPointContact::isInstance(overlap),
915 "ContactForceGenerator::HertzCircular::calcContactPatch(): expected"
916 " CircularPointContact.");
917
918 const CircularPointContact& contact = CircularPointContact::getAs(overlap);
919 const Real depth = contact.getDepth();
920
921 const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
922 const ContactTrackerSubsystem& tracker = subsys.getContactTrackerSubsystem();
923
924 // normal points away from surf1, expressed in surf1's frame
925 const UnitVec3& normal_S1 = contact.getNormal();
926 // origin is half way between the two surfaces as though they had
927 // equal stiffness
928 const Vec3& origin_S1 = contact.getOrigin();
929
930 const Real R = contact.getEffectiveRadius();
931
932 calcHertzContactForce(subsys, tracker, state, overlap,
933 normal_S1, origin_S1, depth,
934 V_S1S2, R, 1, patch_S1.m_resultant,
935 &patch_S1.m_elements);
936 }
937
938
939
940 //==============================================================================
941 // HERTZ ELLIPTICAL GENERATOR
942 //==============================================================================
943
944
945 // Given the relative principal curvatures characterizing the undeformed
946 // contact geometry, calculate the ratio k=a/b >= 1 of the semi axes of the
947 // deformed contact ellipse (the actual size depends on the penetration but
948 // the ratio depends only on the relative curvatures).
949 //
950 // Hertz theory relates them in this implicit equation for k:
951 // B kmax k^2 E(m)-K(m)
952 // - = ---- = -------------, m = 1 - (1/k)^2, B >= A.
953 // A kmin K(m)-E(m)
954 //
955 // (B=kmax/2 and A=kmin/2 are semi-curvatures but their ratio
956 // is the same as the ratio of curvatures.)
957 //
958 // This equation can be solved numerically for k (see the numerical
959 // version of this routine below). Here we instead use an approximation
960 // k = (B/A)^p where exponent p=p(B/A) as follows:
961 // 1 + u0 q + u1 q^2 + u2 q^3 + u3 q^4
962 // p = 2/3 -----------------------------------, q = log10^2(B/A)
963 // 1 + v0 q + v1 q^2 + v2 q^3 + v3 q^4
964 // This gives 5 digits or better of accuracy across the full range
965 // of possible ratios kmax:kmin at a cost of about 160 flops.
966 //
967 // Antoine, Visa, Sauvey, Abba. "Approximate analytical model for
968 // Hertzian Elliptical Contact Problems", ASME J. Tribology 128:660, 2006.
969
approxContactEllipseRatio(Real kmax,Real kmin)970 static Real approxContactEllipseRatio(Real kmax, Real kmin) {
971 static const Real u[] =
972 { (Real).40227436, (Real)3.7491752e-2, (Real)7.4855761e-4,
973 (Real)2.1667028e-6 };
974 static const Real v[] =
975 { (Real).42678878, (Real)4.2605401e-2, (Real)9.0786922e-4,
976 (Real)2.7868927e-6 };
977
978 Real BoA = kmax/kmin; // ~20 flops
979 Real q = square(std::log10(BoA)), q2=q*q, q3=q2*q, q4=q3*q; // ~50 flops?
980 Real n = 1 + u[0]*q + u[1]*q2 + u[2]*q3 + u[3]*q4; // 8 flops
981 Real d = 1 + v[0]*q + v[1]*q2 + v[2]*q3 + v[3]*q4; // 8 flops
982 Real p = (2*n)/(3*d); // ~20 flops
983 Real k = std::pow(BoA, p); // ellipse ratio k=a/b, >50 flops?
984 return k;
985 }
986
987 // Given the relative principal curvatures characterizing the undeformed
988 // contact geometry, calculate the ratio k=a/b >= 1 of the semiaxes of the
989 // deformed contact ellipse (the actual size depends on the penetration but
990 // the ratio depends only on the relative curvatures).
991 //
992 // Hertz theory relates them in this implicit equation for k:
993 // B kmax k^2 E(m)-K(m)
994 // - = ---- = -------------, m = 1 - (1/k)^2, B >= A.
995 // A kmin K(m)-E(m)
996 //
997 // (B=kmax/2 and A=kmin/2 are semi-curvatures but their ratio
998 // is the same as the ratio of curvatures.)
999 //
1000 // Here we numerically solve for k using a Newton iteration, and
1001 // obtain K(m) and E(m) as a side effect useful later.
1002 // The cost is roughly ~100 + (300+70)*n, typically about 1600 flops
1003 // at n=4.
1004 //
1005 // This method is adapted from
1006 // Dyson, Evans, Snidle. "A simple accurate method for calculation of
1007 // stresses and deformations in elliptical Hertzian contacts",
1008 // J. Mech. Eng. Sci. Proc. IMechE part C 206:139-141, 1992.
numContactEllipseRatio(Real kmax,Real kmin,std::pair<Real,Real> & KE)1009 static Real numContactEllipseRatio
1010 (Real kmax, Real kmin, std::pair<Real,Real>& KE)
1011 {
1012 // See Dyson.
1013 Real lambda = kmin/kmax; // A/B
1014 Real kp = std::pow(lambda, Real(2./3.)); // note inverted k'=b/a
1015 Real k2 = kp*kp;
1016 Real lambda1;
1017 for(;;) {
1018 if (k2 > 1) k2 = 1;
1019 Real m = 1-k2;
1020 KE = completeEllipticIntegralsKE(m); // K,E
1021 if (k2==1) return 1;
1022 lambda1 = (KE.first - KE.second) / (KE.second/k2 - KE.first);
1023 if (std::abs(lambda-lambda1) < 10*lambda*SignificantReal)
1024 break;
1025 Real den = lambda1*lambda1+2*m*lambda1-k2;
1026 k2 += (2*m*k2*(lambda-lambda1))/den; // might make k2 slightly >1
1027 };
1028 kp = std::sqrt(k2);
1029 return 1/kp; // return k
1030 }
1031
1032 // Given the undeformed relative geometry semi-curvatures A and B (A<=B),
1033 // the deformed contact ellipse ratio k=a/b >= 1 and associated complete
1034 // elliptic integrals K(m) and E(m) where m = 1 - (1/k)^2, and the
1035 // penetration depth d, return the ellipse semimajor and semiminor
1036 // axes a, b resp. (a>=b), the contact force P, and the peak contact
1037 // pressure p0.
1038 // d Em
1039 // b = sqrt( -------- ), a = b*k
1040 // (A+B) Km
1041 //
1042 // 4 (Pi E*)^2 Em k^2 2 b^3 E* k Pi (A+B)
1043 // P = sqrt( d^3 ------------------ ) = -------------------
1044 // 9 (A+B) Km^3 3 Em
1045 //
1046 // p0 = 3/2 avg. pressure = 3/2 P/(Pi*a*b)
1047 //
1048 // Cost is about 100 flops.
calcContactInfo(Real A,Real B,Real k,const std::pair<Real,Real> & KE,Real d,Real Estar,Vec2 & ab,Real & P,Real & p0)1049 static void calcContactInfo
1050 (Real A, Real B, Real k, const std::pair<Real,Real>& KE, Real d,
1051 Real Estar, Vec2& ab, Real& P, Real& p0)
1052 {
1053 Real Km = KE.first, Em = KE.second;
1054 Real b = std::sqrt((d*Em)/((A+B)*Km)); // ~ 50 flops
1055 Real a = b*k;
1056 ab = Vec2(a,b);
1057 // The rest is about 50 more flops
1058 P = 2*cube(b)*Estar*k*Pi*(A+B)/(3*Em);
1059 p0 = (3*P)/(2*Pi*a*b);
1060 }
1061
1062 // Given just the undeformed relative geometry curvatures kmin (=2*A)
1063 // and kmax (=2*B) (kmax>=kmin>0), calculate the unitless eccentricity
1064 // correction factor e due to the fact that kmax != kmin. The correction
1065 // factor is 1 for a circular contact (kmax==kmin).
1066 //
1067 // The force due to a displacement d is given by
1068 //
1069 // P = e*[4/3 E* sqrt(R)] d^(3/2), R=1/((kmax+kmin)/2)=1/(A+B)
1070 //
1071 // Pi E(m)^(1/2) k
1072 // e = ---------------, k=a/b, m = 1 - (b/a)^2
1073 // 2 K(m)^(3/2)
1074 //
1075 // Derivation:
1076 // For a circular contact
1077 // P = 4/3 E* sqrt(R) d^(3/2)
1078 // For an elliptical contact
1079 // 2 Pi E* E(m)^(1/2) k
1080 // P = ----------------------- d^(3/2)
1081 // 3 (A+B)^(1/2) K(m)^(3/2)
1082 //
1083 // Pi E(m)^(1/2) k
1084 // = 2/3 E* sqrt(R) --------------- d^(3/2)
1085 // K(m)^(3/2)
1086 // For b/a=1 (circular) we have k=1, m=0, K(0)=E(0)=Pi/2. Substituting in
1087 // the equation for e gives e = Pi (Pi/2)^(1/2) / (2 (Pi/2)^(3/2)) = 1 as
1088 // expected.
1089 //
1090 // We calculate k and then K(m),E(m) using a fast approximate method that
1091 // gives us P as a smooth function that is accurate to about 7 digits.
1092 // More accurate methods are available but seem unnecessary and this is
1093 // expensive enough: about 320 flops unless kmax==kmin in which case it's free.
calcHertzForceEccentricityCorrection(Real kmax,Real kmin)1094 static Real calcHertzForceEccentricityCorrection(Real kmax, Real kmin) {
1095 SimTK_ASSERT(kmax >= kmin && kmin > 0,
1096 "calcHertzForceEccentricityCorrection: kmax,kmin illegal.");
1097 if (kmax==kmin) return 1;
1098
1099 Real k = approxContactEllipseRatio(kmax,kmin); // ~160 flops
1100 Real m = 1 - square(1/k); // ~20 flops
1101 std::pair<Real,Real> KE = approxCompleteEllipticIntegralsKE(m); // ~90 flops
1102
1103 Real Km = KE.first, Em = KE.second;
1104 // Slightly twisted computation here to save one square root.
1105 Real e = Pi * k * std::sqrt(Em*Km) / (2 * Km*Km); // ~50 flops
1106
1107 return e;
1108 }
1109
1110 // This is about 340 flops plus the usual 250 for the Hertz contact force
1111 // calculation.
calcContactForce(const State & state,const Contact & overlap,const SpatialVec & V_S1S2,ContactForce & contactForce_S1) const1112 void ContactForceGenerator::HertzElliptical::calcContactForce
1113 (const State& state,
1114 const Contact& overlap, // contains X_S1S2
1115 const SpatialVec& V_S1S2, // relative surface velocity, S2 in S1
1116 ContactForce& contactForce_S1) const
1117 {
1118 SimTK_ASSERT(EllipticalPointContact::isInstance(overlap),
1119 "ContactForceGenerator::HertzElliptical::calcContactForce(): expected"
1120 " EllipticalPointContact.");
1121
1122 const EllipticalPointContact& contact =
1123 EllipticalPointContact::getAs(overlap);
1124 const Real depth = contact.getDepth();
1125
1126 const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
1127 const ContactTrackerSubsystem& tracker = subsys.getContactTrackerSubsystem();
1128
1129 const Transform& X_S1C = contact.getContactFrame();
1130 const Vec2& k = contact.getCurvatures(); // kmax,kmin
1131 const Real e = calcHertzForceEccentricityCorrection(k[0],k[1]);
1132
1133 // normal points away from surf1, expressed in surf1's frame
1134 const UnitVec3& normal_S1 = X_S1C.z();
1135 // origin is half way between the two surfaces as though they had
1136 // equal stiffness
1137 const Vec3& origin_S1 = X_S1C.p();
1138
1139 const Real R = 2/(k[0]+k[1]); // 1/avg curvature (~20 flops)
1140
1141 calcHertzContactForce(subsys, tracker, state, overlap,
1142 normal_S1, origin_S1, depth,
1143 V_S1S2, R, e, contactForce_S1, 0);
1144 }
1145
1146
calcContactPatch(const State & state,const Contact & overlap,const SpatialVec & V_S1S2,ContactPatch & patch_S1) const1147 void ContactForceGenerator::HertzElliptical::calcContactPatch
1148 (const State& state,
1149 const Contact& overlap,
1150 const SpatialVec& V_S1S2,
1151 ContactPatch& patch_S1) const
1152 {
1153 SimTK_ASSERT(EllipticalPointContact::isInstance(overlap),
1154 "ContactForceGenerator::HertzElliptical::calcContactPatch(): expected"
1155 " EllipticalPointContact.");
1156
1157 const EllipticalPointContact& contact =
1158 EllipticalPointContact::getAs(overlap);
1159 const Real depth = contact.getDepth();
1160
1161 const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
1162 const ContactTrackerSubsystem& tracker = subsys.getContactTrackerSubsystem();
1163
1164 const Transform& X_S1C = contact.getContactFrame();
1165 const Vec2& k = contact.getCurvatures(); // kmax,kmin
1166 const Real e = calcHertzForceEccentricityCorrection(k[0],k[1]);
1167
1168 // normal points away from surf1, expressed in surf1's frame
1169 const UnitVec3& normal_S1 = X_S1C.z();
1170 // origin is half way between the two surfaces as though they had
1171 // equal stiffness
1172 const Vec3& origin_S1 = X_S1C.p();
1173
1174 const Real R = 2/(k[0]+k[1]); // 1/avg curvature (~20 flops)
1175
1176 calcHertzContactForce(subsys, tracker, state, overlap,
1177 normal_S1, origin_S1, depth,
1178 V_S1S2, R, e, patch_S1.m_resultant,
1179 &patch_S1.m_elements);
1180 }
1181
1182 // Given a set of vertices of surface B penetrating a halfspace H with given
1183 // surface normal n, generate at each vertex a normal force resisting
1184 // its penetration and penetration rate, and a tangential force resisting slip.
1185 // The model applied at each vertex is just a kx force; that really only makes
1186 // sense for face-face contact where the contact area doesn't change with
1187 // the penetration depth. A single-vertex contact displaces a volume
1188 // proportional to x^3, a single-edge contact displaces x^2 but we're
1189 // igoring that here.
calcPointHalfSpacePenaltyForce(const CompliantContactSubsystem & subsys,const ContactTrackerSubsystem & tracker,const State & state,const BrickHalfSpaceContact & contact,const SpatialVec & V_HB,ContactForce & contactForce_H,Array_<ContactDetail> * details)1190 static void calcPointHalfSpacePenaltyForce
1191 (const CompliantContactSubsystem& subsys,
1192 const ContactTrackerSubsystem& tracker,
1193 const State& state,
1194 const BrickHalfSpaceContact& contact,
1195 const SpatialVec& V_HB, // relative surface velocity, B in H
1196 ContactForce& contactForce_H,
1197 Array_<ContactDetail>* details) // pass as null if you don't care
1198 {
1199 contactForce_H.clear(); // no contact; invalidate return result
1200 if (details) details->clear();
1201
1202 const int lowestVertex = contact.getLowestVertex();
1203 const Real lowestDepth = contact.getDepth();
1204
1205 if (lowestDepth <= 0) {
1206 return; // not contacting
1207 }
1208
1209 const ContactSurfaceIndex surfHx = contact.getSurface1();
1210 const ContactSurfaceIndex surfBx = contact.getSurface2();
1211 const Transform& X_HB = contact.getTransform();
1212
1213 // Abbreviations.
1214 const Rotation& R_HB = X_HB.R(); // orientation of B in H
1215 const Vec3& p_HB = X_HB.p(); // position of Bo in H
1216 const Vec3& w_HB = V_HB[0]; // ang. vel. of B in H
1217 const Vec3& v_HB = V_HB[1]; // vel. of Bo in H
1218
1219 const ContactSurface& surfH = tracker.getContactSurface(surfHx);
1220 const ContactSurface& surfB = tracker.getContactSurface(surfBx);
1221 const ContactMaterial& matH = surfH.getMaterial();
1222 const ContactMaterial& matB = surfB.getMaterial();
1223
1224 const ContactGeometry::HalfSpace& halfSpace =
1225 ContactGeometry::HalfSpace::getAs(surfH.getShape());
1226 const ContactGeometry::Brick& brick =
1227 ContactGeometry::Brick::getAs(surfB.getShape());
1228
1229 const UnitVec3 normal_H = halfSpace.getNormal();
1230
1231 // The Box represents the Brick surface as a convex mesh with known
1232 // connectivity. We know the vertex that is most penetrated. There are
1233 // three faces connected to that vertex; we want the one whose normal
1234 // is closest to antiparallel to the half-space normal.
1235 const Geo::Box& box = brick.getGeoBox();
1236 int faces[3], which[3];
1237 box.getVertexFaces(lowestVertex, faces, which);
1238 // We want the most negative cosine we can get.
1239 int bestFace = -1, bestWhich = -1; Real bestCos = Infinity;
1240 for (int f=0; f < 3; ++f) {
1241 const int face = faces[f];
1242 const Real cos =
1243 dot(normal_H, R_HB.getAxisUnitVec(box.getFaceCoordinateDirection(face)));
1244 if (cos < bestCos)
1245 bestFace=face, bestWhich=which[f], bestCos=cos;
1246 }
1247
1248 SimTK_ASSERT_ALWAYS(bestCos < 0,
1249 "calcPointHalfSpacePenaltyForce(): lowest vertex should have had a face "
1250 "roughly antiparallel to the half-space. Is something wrong with the box "
1251 "mesh connectivity?");
1252
1253 // Calculate composite material properties.
1254 // TODO: this pairwise material calculation (~60 flops) could be cached.
1255
1256 const Real kH=matH.getStiffness(), kB=matB.getStiffness();
1257 const Real cH=matH.getDissipation(), cB=matB.getDissipation();
1258
1259 // Adjust the contact location based on the relative stiffness of the
1260 // two materials. sH is the fraction of the "squishing" (deformation) that
1261 // is done by surface1 -- if surface2 is very stiff then surface1 does most.
1262
1263 const Real sH = kB/(kH+kB); // 0..1
1264 const Real sB = 1-sH; // 1..0
1265 const Real k = kH*sH; // (==kB*sB) composite stiffness
1266 const Real c = cH*sH + cB*sB; // composite dissipation
1267
1268 const Real usH=matH.getStaticFriction(), usB=matB.getStaticFriction();
1269 const Real udH=matH.getDynamicFriction(), udB=matB.getDynamicFriction();
1270 const Real uvH=matH.getViscousFriction(), uvB=matB.getViscousFriction();
1271 const Real vtrans = subsys.getTransitionVelocity();
1272 const Real ooVtrans = subsys.getOOTransitionVelocity(); // 1/vtrans
1273
1274 // Calculate effective coefficients of friction, being careful not
1275 // to divide 0/0 if both are frictionless.
1276 Real us = 2*usH*usB; if (us!=0) us /= (usH+usB);
1277 Real ud = 2*udH*udB; if (ud!=0) ud /= (udH+udB);
1278 Real uv = 2*uvH*uvB; if (uv!=0) uv /= (uvH+uvB);
1279 assert(us >= ud);
1280
1281 // Accumulate force applied by the half-space to the brick, as though it
1282 // were applied at the half-space origin. We'll move it to the center of
1283 // pressure later (the moment will change in that case). This is expressed
1284 // in H.
1285 SpatialVec FB_H(Vec3(0),Vec3(0));
1286 Real totalPE = 0; // accumulate potential energy
1287 Real totalPower = 0; // accumulate dissipated power
1288 // Accumulate weighted contact points for calculating the
1289 // center of pressure; we'll divide by total moment at the end.
1290 // sum_i (r_i * |r_i X Fn_i|)
1291 // r_c = --------------------------
1292 // sum_i |r_i X Fn_i|
1293 Vec3 centerOfPressure_H(0);
1294 Real totalNormalMoment = 0;
1295
1296
1297 int vertices[4];
1298 box.getFaceVertices(bestFace, vertices);
1299 int nActiveVertices = 0;
1300 for (int i=0; i < 4; ++i) {
1301 const int vx = vertices[i];
1302 const Vec3 v_B = box.getVertexPos(vx);
1303 const Vec3 v_H = X_HB * v_B; // 18 flops
1304 const Real x = -dot(v_H, normal_H); // undeformed pen. depth; 6 flops
1305 if (x <= 0) continue; // not penetrated (1 flop)
1306
1307 // Actual contact point moves closer to stiffer surface. Would be at
1308 // v_H if brick were very stiff and half-space very soft.
1309 const Vec3 contactPt_H = v_H + (x*sB)*normal_H;
1310 const Real fK = k*x; // normal force due to stiffness (>= 0)
1311 const Real pe = k*x*x/2;
1312
1313 // Calculate the relative velocity of the two bodies at the contact
1314 // point. We're considering H fixed, so we just need the velocity in H
1315 // of the station of B that is coincident with the contact point.
1316 const Vec3 contactPtB_H = contactPt_H - p_HB; // B station, exp. in H
1317
1318 // All vectors are in H; dropping the "_H" notation now.
1319
1320 // Velocity of B at contact point is opposite direction of normal
1321 // when penetration is increasing.
1322 const Vec3 vel = v_HB + w_HB % contactPtB_H;
1323 // Want xdot > 0 when x is increasing; normal points the other way
1324 const Real xdot = -dot(vel, normal_H); // penetration rate (signed)
1325 const Vec3 velNormal = -xdot*normal_H;
1326 const Vec3 velTangent = vel-velNormal;
1327
1328 // Calculate the Hunt-Crossley force, which is dissipative, and the
1329 // total normal force including elasticity and dissipation.
1330 const Real fHC = fK*c*xdot; // same sign as xdot
1331 const Real powerHC = fHC*xdot; // dissipation rate due to H&C, >= 0
1332
1333 const Real fNormal = fK + fHC; // < 0 means "sticking"; see below
1334
1335 ++nActiveVertices;
1336
1337 // Normal force can be negative under unusual circumstances ("yanking");
1338 // that means no force is generated and no stored PE will be recovered.
1339 // This will most often occur in to-be-rejected trial steps but can
1340 // occasionally be real. Thanks to Matt Millard for the fix here to
1341 // properly track lost power here so that KE+PE-DE=constant.
1342 // (DE=dissipated energy).
1343 Real powerLoss=0;
1344 Vec3 force(0);
1345 if (fNormal <= 0) {
1346 powerLoss = -fK*xdot; // xdot<0 here
1347 } else {
1348 const Vec3 forceN = fNormal * normal_H; // 3 flops
1349 const Vec3 momentN = contactPt_H % forceN; // about H origin (9 flops)
1350 const Real weight = momentN.norm(); // ~25 flops
1351 totalNormalMoment += weight; // 1 flop
1352 centerOfPressure_H += weight*contactPt_H; // 6 flops
1353
1354 // Calculate the friction force.
1355 Vec3 forceFriction(0);
1356 Real powerFriction = 0;
1357 const Real vslipSq = velTangent.normSqr(); // 5 flops
1358 if (vslipSq > square(SignificantReal)) { // 2
1359 const Real vslip = std::sqrt(vslipSq); // ~20
1360
1361 // Express slip velocity as unitless multiple of transition velocity.
1362 const Real v = vslip * ooVtrans;
1363 // Must scale viscous coefficient to match unitless velocity.
1364 const Real mu=stribeck(us,ud,uv*vtrans,v);
1365 const Real fFriction = fNormal * mu;
1366 // Force direction on B opposes B's velocity.
1367 forceFriction = (-fFriction/vslip)*velTangent; // in H ~14 flops
1368 powerFriction = fFriction * vslip; // >= 0 (1 flop)
1369 }
1370
1371 force = forceN + forceFriction;
1372 // Accumulate moments about H origin; we'll shift to COP later.
1373 FB_H += SpatialVec(contactPt_H % force, force);
1374
1375 // Don't include dot(fK,velNormal) power due to conservative force here.
1376 // This way we don't double-count the energy on the way in as integrated
1377 // power and potential energy. Although the books would balance again
1378 // when the contact is broken, it makes continuous contact look as
1379 // though some energy has been lost.
1380 powerLoss = powerHC + powerFriction;
1381 }
1382
1383 totalPE += pe;
1384 totalPower += powerLoss; // negative
1385
1386 // Fill in contact details if requested (in H frame).
1387 if (details) {
1388 details->push_back(); // default constructed
1389 ContactDetail& detail = details->back();
1390 detail.m_contactPt = contactPt_H;
1391 detail.m_patchNormal = normal_H;
1392 detail.m_slipVelocity = velTangent;
1393 detail.m_forceOnSurface2 = force;
1394 detail.m_deformation = x;
1395 detail.m_deformationRate = xdot;
1396 detail.m_patchArea = NaN;
1397 detail.m_peakPressure = NaN;
1398 detail.m_potentialEnergy = pe;
1399 detail.m_powerLoss = powerLoss;
1400 }
1401 }
1402
1403 assert(nActiveVertices); // at least the deepest vertex should be active
1404
1405 if (totalNormalMoment > 0)
1406 centerOfPressure_H /= totalNormalMoment;
1407
1408 // Shift moment from H origin to COP.
1409 FB_H[0] -= centerOfPressure_H % FB_H[1];
1410
1411 // Fill in the contact force.
1412 // Report the force & moment (as though applied at center of pressure).
1413 contactForce_H.setContactId(contact.getContactId());
1414 contactForce_H.setContactPoint(centerOfPressure_H);
1415 contactForce_H.setForceOnSurface2(FB_H);
1416 contactForce_H.setPotentialEnergy(totalPE);
1417 contactForce_H.setPowerDissipation(totalPower);
1418 }
1419
1420 //==============================================================================
1421 // BRICK HALFSPACE PENALTY GENERATOR
1422 //==============================================================================
1423 // The given Contact object identifies
1424 // - the "contacting face" of the brick
1425 // - which vertex of that face is the most deeply penetrated.
1426 // We will generate a response force at that vertex, and at up to three more
1427 // vertices of the contacting face.
1428 // There are three faces containing the vertex; the contacting face is the one
1429 // that is most parallel to the halfspace surface.
calcContactForce(const State & state,const Contact & overlap,const SpatialVec & V_S1S2,ContactForce & contactForce_S1) const1430 void ContactForceGenerator::BrickHalfSpacePenalty::calcContactForce
1431 (const State& state,
1432 const Contact& overlap, // contains X_S1S2
1433 const SpatialVec& V_S1S2, // relative surface velocity, S2 in S1
1434 ContactForce& contactForce_S1) const
1435 {
1436 SimTK_ASSERT(BrickHalfSpaceContact::isInstance(overlap),
1437 "ContactForceGenerator::BrickHalfSpace::calcContactForce(): expected"
1438 " BrickHalfSpaceContact.");
1439
1440 const BrickHalfSpaceContact& contact =
1441 BrickHalfSpaceContact::getAs(overlap);
1442
1443 const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
1444 const ContactTrackerSubsystem& tracker =
1445 subsys.getContactTrackerSubsystem();
1446
1447 calcPointHalfSpacePenaltyForce(subsys, tracker, state, contact,
1448 V_S1S2, contactForce_S1, 0);
1449 }
1450
calcContactPatch(const State & state,const Contact & overlap,const SpatialVec & V_HB,ContactPatch & patch_H) const1451 void ContactForceGenerator::BrickHalfSpacePenalty::calcContactPatch
1452 (const State& state,
1453 const Contact& overlap,
1454 const SpatialVec& V_HB,
1455 ContactPatch& patch_H) const
1456 {
1457 SimTK_ASSERT(BrickHalfSpaceContact::isInstance(overlap),
1458 "ContactForceGenerator::BrickHalfSpace::calcContactPatch(): expected"
1459 " BrickHalfSpaceContact.");
1460
1461 const BrickHalfSpaceContact& contact =
1462 BrickHalfSpaceContact::getAs(overlap);
1463
1464 const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
1465 const ContactTrackerSubsystem& tracker =
1466 subsys.getContactTrackerSubsystem();
1467
1468 calcPointHalfSpacePenaltyForce(subsys, tracker, state, contact,
1469 V_HB, patch_H.m_resultant,
1470 &patch_H.m_elements);
1471 }
1472
1473
1474
1475 //==============================================================================
1476 // ELASTIC FOUNDATION GENERATOR
1477 //==============================================================================
calcContactForce(const State & state,const Contact & overlap,const SpatialVec & V_S1S2,ContactForce & contactForce_S1) const1478 void ContactForceGenerator::ElasticFoundation::calcContactForce
1479 (const State& state,
1480 const Contact& overlap, // contains X_S1S2
1481 const SpatialVec& V_S1S2, // relative surface velocity
1482 ContactForce& contactForce_S1) const
1483 {
1484 calcContactForceAndDetails(state,overlap,V_S1S2,contactForce_S1,0);
1485 }
1486
1487
1488
calcContactPatch(const State & state,const Contact & overlap,const SpatialVec & V_S1S2,ContactPatch & patch_S1) const1489 void ContactForceGenerator::ElasticFoundation::calcContactPatch
1490 (const State& state,
1491 const Contact& overlap,
1492 const SpatialVec& V_S1S2,
1493 ContactPatch& patch_S1) const
1494 {
1495 calcContactForceAndDetails(state,overlap,V_S1S2,
1496 patch_S1.m_resultant, &patch_S1.m_elements);
1497 }
1498
1499 // How to think about mesh-mesh contact:
1500 // There is only a single contact patch. We would like to mesh the patch
1501 // but we don't have that. Instead, we have two approximations of a meshed
1502 // patch, one on each surface after deformation, but at different resolutions.
1503 // Either one would be sufficient alone, since both cover the whole patch and
1504 // apply equal and opposite forces to the two bodies. So when we evaluate
1505 // forces on each mesh, we are making two approximations of the same force.
1506 // DON'T APPLY BOTH! That would be double-counting. Instead, we use the
1507 // two independent calculations to refine our result. So we scale the
1508 // areas of the two patch approximations so that each contributes 50% to
1509 // the overall patch area, forces, and center of pressure.
1510
calcContactForceAndDetails(const State & state,const Contact & overlap,const SpatialVec & V_S1S2,ContactForce & contactForce_S1,Array_<ContactDetail> * contactDetails_S1) const1511 void ContactForceGenerator::ElasticFoundation::calcContactForceAndDetails
1512 (const State& state,
1513 const Contact& overlap, // contains X_S1S2
1514 const SpatialVec& V_S1S2, // relative surface velocity
1515 ContactForce& contactForce_S1,
1516 Array_<ContactDetail>* contactDetails_S1) const
1517 {
1518 SimTK_ASSERT(TriangleMeshContact::isInstance(overlap),
1519 "ContactForceGenerator::ElasticFoundation::calcContactForce(): expected"
1520 " TriangleMeshContact.");
1521
1522 const bool wantDetails = (contactDetails_S1 != 0);
1523 if (wantDetails)
1524 contactDetails_S1->clear();
1525
1526 const TriangleMeshContact& contact = TriangleMeshContact::getAs(overlap);
1527
1528 const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
1529 const ContactTrackerSubsystem& tracker = subsys.getContactTrackerSubsystem();
1530
1531 const ContactSurfaceIndex surf1x = contact.getSurface1();
1532 const ContactSurfaceIndex surf2x = contact.getSurface2();
1533 const Transform& X_S1S2 = contact.getTransform();
1534
1535 const ContactSurface& surf1 = tracker.getContactSurface(surf1x);
1536 const ContactSurface& surf2 = tracker.getContactSurface(surf2x);
1537
1538 const ContactGeometry& shape1 = surf1.getShape();
1539 const ContactGeometry& shape2 = surf2.getShape();
1540
1541 // One or both of these contact shapes must be a mesh.
1542 const bool isMesh1 =
1543 (shape1.getTypeId() == ContactGeometry::TriangleMesh::classTypeId());
1544 const bool isMesh2 =
1545 (shape2.getTypeId() == ContactGeometry::TriangleMesh::classTypeId());
1546
1547 // This is an assert because we should never have gotten here otherwise.
1548 SimTK_ASSERT_ALWAYS(isMesh1 || isMesh2,
1549 "ContactForceGenerator::ElasticFoundation::calcContactForceAndDetails():"
1550 " At least one of the two surfaces should have been a mesh.");
1551
1552 // We require that any elastic foundation mesh have a non-zero thickness
1553 // specified for the ContactSurface. This is an error rather than an
1554 // assert because we might not find out until first contact that we need
1555 // to know the thickness.
1556 // For a non-mesh surface we'll *allow* a thickness but if there isn't
1557 // one we'll assume it has the same thickness as the mesh.
1558 // TODO: can't we do better than that?
1559 Real h1=surf1.getThickness(), h2=surf2.getThickness();
1560 SimTK_ERRCHK_ALWAYS(!isMesh1 || h1 > 0,
1561 "ContactForceGenerator::ElasticFoundation::calcContactForceAndDetails()",
1562 " Surface 1 was a mesh but no thickness was provided for the"
1563 " ContactSurface.");
1564 SimTK_ERRCHK_ALWAYS(!isMesh2 || h2 > 0,
1565 "ContactForceGenerator::ElasticFoundation::calcContactForceAndDetails()",
1566 " Surface 2 was a mesh but no thickness was provided for the"
1567 " ContactSurface.");
1568
1569 assert(h1>0 || h2>0);
1570
1571 //TODO: for now assign the same thickness to the non-mesh as to the mesh.
1572 //Should be able to do better than this.
1573 if (h1==0) h1=h2;
1574 if (h2==0) h2=h1;
1575
1576 // Compute combined material properties just once -- they are the same
1577 // for all triangles.
1578
1579 const ContactMaterial& mat1 = surf1.getMaterial();
1580 const ContactMaterial& mat2 = surf2.getMaterial();
1581
1582 // Stiffnesses combine linearly here, not as 2/3 power as for Hertz.
1583 const Real k1=mat1.getStiffness(), k2=mat2.getStiffness();
1584 const Real c1=mat1.getDissipation(), c2=mat2.getDissipation();
1585
1586 // Above stiffnesses are in pressure/unit strain; we need to work here
1587 // with pressure/unit deformation using strain=x/h where x is deformation
1588 // and h is thickness (that is, x==h => 100% strain).
1589 const Real kh1 = k1/h1, kh2 = k2/h2;
1590
1591 // Adjust the contact location based on the relative stiffness of the
1592 // two materials. s1 is the fraction of the "squishing" (deformation) that
1593 // is done by surface1 -- if surface2 is very stiff then surface1 does most.
1594 const Real s1 = kh2/(kh1+kh2); // 0..1
1595 const Real s2 = 1-s1; // 1..0
1596
1597 // kh is the effective stiffness, c the effective dissipation
1598 const Real kh = kh1*s1; // (==kh2*s2) == E/x
1599 const Real c = c1*s1 + c2*s2;
1600
1601 // Calculate effective coefficients of friction, being careful not
1602 // to divide 0/0 if both are frictionless.
1603 const Real us1=mat1.getStaticFriction(), us2=mat2.getStaticFriction();
1604 const Real ud1=mat1.getDynamicFriction(), ud2=mat2.getDynamicFriction();
1605 const Real uv1=mat1.getViscousFriction(), uv2=mat2.getViscousFriction();
1606 Real us = 2*us1*us2; if (us!=0) us /= (us1+us2);
1607 Real ud = 2*ud1*ud2; if (ud!=0) ud /= (ud1+ud2);
1608 Real uv = 2*uv1*uv2; if (uv!=0) uv /= (uv1+uv2);
1609
1610 // Now generate forces using the meshed surfaces only (one or two).
1611
1612
1613 // We want both patches to accumulate forces at the same point in
1614 // space. For numerical reasons this should be near the center of the
1615 // patch.
1616 Vec3 weightedPatchCentroid1_S1(0), weightedPatchCentroid2_S1(0);
1617 Real patchArea1 = 0, patchArea2 = 0;
1618 if (shape1.getTypeId() == ContactGeometry::TriangleMesh::classTypeId()) {
1619 const ContactGeometry::TriangleMesh& mesh1 =
1620 ContactGeometry::TriangleMesh::getAs(shape1);
1621
1622 calcWeightedPatchCentroid(mesh1, contact.getSurface1Faces(),
1623 weightedPatchCentroid1_S1, patchArea1);
1624 }
1625 if (shape2.getTypeId() == ContactGeometry::TriangleMesh::classTypeId()) {
1626 const ContactGeometry::TriangleMesh& mesh2 =
1627 ContactGeometry::TriangleMesh::getAs(shape2);
1628 Vec3 weightedPatchCentroid2_S2;
1629
1630 calcWeightedPatchCentroid(mesh2, contact.getSurface2Faces(),
1631 weightedPatchCentroid2_S2, patchArea2);
1632 // Remeasure patch2's weighted centroid from surface1's frame;
1633 // be sure to weight the new offset also.
1634 weightedPatchCentroid2_S1 = X_S1S2.R()*weightedPatchCentroid2_S2
1635 + patchArea2*X_S1S2.p();
1636 }
1637
1638 // At this point one or two area-weighted patch centroids and corresponding
1639 // patch area estimates are known; if one is unused it's (0,0,0) with 0
1640 // area weight. Combine them into a single composite centroid for the patch,
1641 // with each contributing equally, and an averaged patch area estimate.
1642 Real patchArea=0, areaScale1=0, areaScale2=0;
1643 Vec3 patchCentroid_S1(0);
1644 if (patchArea1 != 0) {
1645 if (patchArea2 != 0) {
1646 patchArea = (patchArea1 + patchArea2)/2;
1647 areaScale1 = (patchArea/2) / patchArea1;
1648 areaScale2 = (patchArea/2) / patchArea2;
1649 patchCentroid_S1 =
1650 areaScale1 * weightedPatchCentroid1_S1 / patchArea1
1651 + areaScale2 * weightedPatchCentroid2_S1 / patchArea2;
1652 } else { // only patchArea1
1653 patchArea=patchArea1;
1654 areaScale1 = 1;
1655 areaScale2 = 0;
1656 patchCentroid_S1 = weightedPatchCentroid1_S1 / patchArea1;
1657 }
1658 } else if (patchArea2 != 0) { // only patchArea2
1659 patchArea = patchArea2;
1660 areaScale2 = 1;
1661 areaScale1 = 0;
1662 patchCentroid_S1 = weightedPatchCentroid2_S1 / patchArea2;
1663 }
1664
1665
1666 // The patch centroid as calculated from one or two meshes is now
1667 // in patchCentroid_S1, measured from and expressed in S1. Now we'll
1668 // calculate all the patch forces and accumulate them at the patch
1669 // centroid.
1670
1671
1672 // Forces must be as applied to surface 2 at the patch centroid, but
1673 // expressed in surface 1's frame.
1674 SpatialVec force1_S1(Vec3(0)), force2_S1(Vec3(0));
1675 Real potEnergy1=0, potEnergy2=0;
1676 Real powerLoss1=0, powerLoss2=0;
1677
1678 // Center of pressure r_c is calculated like this:
1679 // sum_i (r_i * |r_i X Fn_i|)
1680 // r_c = --------------------------
1681 // sum_i |r_i X Fn_i|
1682 // where Fn is the normal force applied by element i at location r_i,
1683 // with all locations measured from the patch centroid calculated above.
1684 //
1685 // We're going to calculate the weighted-points numerator for each
1686 // mesh separately in weightedCOP1 and weightedCOP2, and the corresponding
1687 // denominators (sum of all pressure-moment magnitudes) in weightCOP1 and
1688 // weightCOP2. At the end we'll combine and divide to calculate the actual
1689 // center of pressure where we'll ultimately apply the contact force.
1690 Vec3 weightedCOP1_PC_S1(0), weightedCOP2_PC_S1(0); // from patch centroid
1691 Real weightCOP1=0, weightCOP2=0;
1692
1693 if (shape1.getTypeId() == ContactGeometry::TriangleMesh::classTypeId()) {
1694 const ContactGeometry::TriangleMesh& mesh =
1695 ContactGeometry::TriangleMesh::getAs(shape1);
1696
1697 processOneMesh(state,
1698 mesh, contact.getSurface1Faces(),
1699 X_S1S2, V_S1S2, shape2,
1700 s1, areaScale1,
1701 kh, c, us, ud, uv,
1702 patchCentroid_S1,
1703 force1_S1, potEnergy1, powerLoss1,
1704 weightedCOP1_PC_S1, weightCOP1,
1705 contactDetails_S1); // details returned if this is non-null
1706 }
1707
1708 if (shape2.getTypeId() == ContactGeometry::TriangleMesh::classTypeId()) {
1709 const ContactGeometry::TriangleMesh& mesh =
1710 ContactGeometry::TriangleMesh::getAs(shape2);
1711
1712 // Costs 120 flops to flip everything into S2 for processing and
1713 // then put the results back in S1.
1714
1715 const Transform X_S2S1 = ~X_S1S2; // 3 flops
1716 const SpatialVec V_S2S1 =
1717 reverseRelativeVelocity(X_S1S2,V_S1S2); // 51 flops
1718
1719 const Vec3 patchCentroid_S2 = X_S2S1*patchCentroid_S1; // 18 flops
1720
1721 SpatialVec force2_S2;
1722 Vec3 weightedCOP2_PC_S2;
1723
1724 const unsigned nDetailsBefore =
1725 wantDetails ? contactDetails_S1->size() : 0;
1726
1727 processOneMesh(state,
1728 mesh, contact.getSurface2Faces(),
1729 X_S2S1, V_S2S1, shape1,
1730 s2, areaScale2,
1731 kh, c, us, ud, uv,
1732 patchCentroid_S2,
1733 force2_S2, potEnergy2, powerLoss2,
1734 weightedCOP2_PC_S2, weightCOP2,
1735 contactDetails_S1); // details returned *in S2* if this is non-null
1736
1737 // Switch contact details from S1 in S2 to S2 in S1.
1738 if (wantDetails) {
1739 for (unsigned i=nDetailsBefore; i < contactDetails_S1->size(); ++i)
1740 (*contactDetails_S1)[i].changeFrameAndSwitchSurfacesInPlace(X_S1S2);
1741 }
1742
1743 // Returned force is as applied to surface 1 (at the patch centroid);
1744 // negate to make it the force applied to surface 2. Also re-express
1745 // the moment and force vectors in S1.
1746 force2_S1 = -(X_S1S2.R()*force2_S2); // 36 flops
1747 // Returned weighted COP is measured from patch centroid but
1748 // expressed in S2; reexpress in S1.
1749 weightedCOP2_PC_S1 = X_S1S2.R()*weightedCOP2_PC_S2; // 15 flops
1750 }
1751
1752 const Real weightCOP = weightCOP1+weightCOP2;
1753 Vec3 centerOfPressure_PC_S1 = // offset from patch centroid
1754 weightCOP > 0 ? (weightedCOP1_PC_S1+weightedCOP2_PC_S1) / weightCOP
1755 : Vec3(0);
1756
1757 // Calculate total force applied to surface 2 at the patch centroid,
1758 // expressed in S1.
1759 const SpatialVec force_PC_S1 = force1_S1 + force2_S1;
1760
1761 // Shift to center of pressure.
1762 const SpatialVec forceCOP_S1 =
1763 shiftForceBy(force_PC_S1, centerOfPressure_PC_S1);
1764
1765 // Contact point is the center of pressure measured from the S1 origin.
1766 const Vec3 contactPt_S1 = patchCentroid_S1 + centerOfPressure_PC_S1;
1767
1768 // Fill in contact force object.
1769 contactForce_S1.setTo(contact.getContactId(),
1770 contactPt_S1, // contact point in S1
1771 forceCOP_S1, // force on surf2 at contact pt exp. in S1
1772 potEnergy1 + potEnergy2,
1773 powerLoss1 + powerLoss2);
1774 }
1775
1776
1777
1778 // Compute the approximate geometric center of the patch represented by the
1779 // set of "inside faces" of this mesh, weighted by the total patch area.
1780 // We'll use the inside faces' centroids, weighted by face area, to calculate
1781 // a point that will be somewhere near the center of the patch, times the total
1782 // area. Dividing by the area yields the actual patch centroid point which can
1783 // be used as a numerically good nearby point for accumulating all the forces,
1784 // since the moments will all be small and thus
1785 // not suffer huge roundoff errors when combined. Afterwards, the numerically
1786 // good resultant can be shifted to whatever point is convenient, such as the
1787 // body frame. We return the total area represented by this patch so that
1788 // it can be combined with another mesh's patch if needed.
1789 // The weighted centroid is returned in the mesh surface's own frame.
1790 void ContactForceGenerator::ElasticFoundation::
calcWeightedPatchCentroid(const ContactGeometry::TriangleMesh & mesh,const std::set<int> & insideFaces,Vec3 & weightedPatchCentroid,Real & patchArea) const1791 calcWeightedPatchCentroid
1792 (const ContactGeometry::TriangleMesh& mesh,
1793 const std::set<int>& insideFaces,
1794 Vec3& weightedPatchCentroid,
1795 Real& patchArea) const
1796 {
1797 weightedPatchCentroid = Vec3(0); patchArea = 0;
1798 for (std::set<int>::const_iterator iter = insideFaces.begin();
1799 iter != insideFaces.end(); ++iter)
1800 { const int face = *iter;
1801 const Real area = mesh.getFaceArea(face);
1802 weightedPatchCentroid += area*mesh.findCentroid(face);
1803 patchArea += area;
1804 }
1805 }
1806
1807
1808
1809 // Private method that calculates the net contact force produced by a single
1810 // triangle mesh in contact with some other object (which might be another
1811 // mesh; we don't care). We are given the relative spatial pose and velocity of
1812 // the two surface frames, and for the mesh we are given a specific list of
1813 // the faces that are suspected of being at least partially inside the
1814 // other surface (in the undeformed geometry overlap). Normally this just
1815 // computes the resultant force but it can optionally append contact patch
1816 // details (one entry per element) as well, if the contactDetails argument is
1817 // non-null.
1818 // An area-scaling factor is used to scale the area represented by each face.
1819 // If there is only one mesh involved this should be 1. However, if this
1820 // is part of a pair of contact meshes each half of the pair should be scaled
1821 // so that both surfaces see the same overall patch area (so nominally the
1822 // area-scaling factor would be 0.5).
1823 void ContactForceGenerator::ElasticFoundation::
processOneMesh(const State & state,const ContactGeometry::TriangleMesh & mesh,const std::set<int> & insideFaces,const Transform & X_MO,const SpatialVec & V_MO,const ContactGeometry & other,Real meshDeformationFraction,Real areaScaleFactor,Real kh,Real c,Real us,Real ud,Real uv,const Vec3 & resultantPt_M,SpatialVec & resultantForceOnOther_M,Real & potentialEnergy,Real & powerLoss,Vec3 & weightedCenterOfPressure_M,Real & sumOfAllPressureMoments,Array_<ContactDetail> * contactDetails_M) const1824 processOneMesh
1825 (const State& state,
1826 const ContactGeometry::TriangleMesh& mesh,
1827 const std::set<int>& insideFaces,
1828 const Transform& X_MO,
1829 const SpatialVec& V_MO,
1830 const ContactGeometry& other,
1831 Real meshDeformationFraction, // 0..1
1832 Real areaScaleFactor,
1833 Real kh, Real c, Real us, Real ud, Real uv, // composite material props
1834 const Vec3& resultantPt_M, // where to apply forces
1835 SpatialVec& resultantForceOnOther_M, // at resultant pt
1836 Real& potentialEnergy,
1837 Real& powerLoss,
1838 Vec3& weightedCenterOfPressure_M,
1839 Real& sumOfAllPressureMoments, // COP weight
1840 Array_<ContactDetail>* contactDetails_M) const // in/out if present
1841 {
1842 assert(!insideFaces.empty());
1843 const bool wantDetails = (contactDetails_M != 0);
1844
1845 // Initialize all results to zero.
1846 resultantForceOnOther_M = SpatialVec(Vec3(0),Vec3(0));
1847 potentialEnergy = powerLoss = 0;
1848 weightedCenterOfPressure_M = Vec3(0);
1849 sumOfAllPressureMoments = 0;
1850
1851 // Don't initialize contact details; we're going to append them.
1852
1853 // Abbreviations.
1854 const Rotation& RMO = X_MO.R(); // orientation of O in M
1855 const Vec3& pMO = X_MO.p(); // position of OO (origin of O) in M
1856 const Vec3& wMO = V_MO[0]; // ang. vel. of O in M
1857 const Vec3& vMO = V_MO[1]; // vel. of OO in M
1858
1859 // Get friction model transition velocity vt and 1/vt.
1860 const CompliantContactSubsystem& subsys = getCompliantContactSubsystem();
1861 const Real vtrans = subsys.getTransitionVelocity();
1862 const Real ooVtrans = subsys.getOOTransitionVelocity(); // 1/vtrans
1863
1864 // Now loop over all the faces again, evaluate the force from each
1865 // spring, and apply it at the patch centroid.
1866 // This costs roughly 300 flops per contacting face.
1867 for (std::set<int>::const_iterator iter = insideFaces.begin();
1868 iter != insideFaces.end(); ++iter)
1869 { const int face = *iter;
1870 const Vec3 springPos_M = mesh.findCentroid(face);
1871 const Real faceArea = areaScaleFactor*mesh.getFaceArea(face);
1872
1873 bool inside;
1874 UnitVec3 normal_O; // not used
1875 const Vec3 nearestPoint_O = // 18 flops + cost of findNearestPoint
1876 other.findNearestPoint(~X_MO*springPos_M, inside, normal_O);
1877 if (!inside)
1878 continue;
1879
1880 // Although the "spring" is associated with just one surface (the mesh M)
1881 // it is considered here to include the compression of both surfaces
1882 // together, using composite material properties for stiffness and
1883 // dissipation properties of the spring. The total displacement vector
1884 // for both surfaces points from the nearest point on the undeformed other
1885 // surface to the undeformed spring position (face centroid) on the
1886 // mesh. Since these overlap (we checked above) the nearest point is
1887 // *inside* the mesh thus the vector points towards the mesh exterior;
1888 // i.e., in the direction that the force will be applied to the
1889 // "other" body. This is the same convention we use for the patch
1890 // normal for Hertz contact.
1891 const Vec3 nearestPoint_M = X_MO*nearestPoint_O; // 18 flops
1892 const Vec3 overlap_M = springPos_M - nearestPoint_M; // 3 flops
1893 const Real overlap = overlap_M.norm(); // ~40 flops
1894
1895 // If there is no overlap we can't generate forces.
1896 if (overlap == 0)
1897 continue;
1898
1899 // The surfaces are compressed by total amount "overlap".
1900 const UnitVec3 normal_M(overlap_M/overlap, true); // ~15 flops
1901
1902 // Calculate the contact point location based on the relative
1903 // squishiness of the two surfaces. The mesh deformation fraction (0-1)
1904 // gives the fraction of the material squishing that is done by the
1905 // mesh; the rest is done by the other surface. At 0 (rigid mesh) the
1906 // contact point will be at the undeformed mesh face centroid; at 1
1907 // (other body rigid) it will be at the (undeformed) nearest point on
1908 // the other body.
1909 const Real meshSquish = meshDeformationFraction*overlap; // mesh displacement
1910 // Remember that the normal points towards the exterior of this mesh.
1911 const Vec3 contactPt_M = springPos_M - meshSquish*normal_M; // 6 flops
1912
1913 // Calculate the relative velocity of the two bodies at the contact
1914 // point. We're considering the mesh M fixed, so we just need the
1915 // velocity in M of the station of O that is coincident with the
1916 // contact point.
1917
1918 // O station, exp. in M
1919 const Vec3 contactPtO_M = contactPt_M - pMO; // 3 flops
1920
1921 // All vectors are in M; dropping the "_M" notation now.
1922
1923 // Velocity of other at contact point is opposite direction of normal
1924 // when penetration is increasing.
1925 const Vec3 vel = vMO + wMO % contactPtO_M; // 12 flops
1926
1927 // Want odot > 0 when overlap is increasing; normal points the
1928 // other way. odot is signed penetration (overlap) rate.
1929 const Real odot = -dot(vel, normal_M); // 6 flops
1930 const Vec3 velNormal = -odot*normal_M; // 4 flops
1931 const Vec3 velTangent = vel-velNormal; // 3 flops
1932
1933 // Calculate scalar normal force (5 flops)
1934 // Here kh has units of pressure/area/displacement
1935 const Real fK = kh*faceArea*overlap; // normal elastic force (conservative)
1936 const Real fC = fK*c*odot; // normal dissipation force (loss)
1937 const Real fNormal = fK + fC; // normal force
1938
1939 // Total force can be negative under unusual circumstances ("yanking");
1940 // that means no force is generated and no stored PE will be recovered.
1941 // This will most often occur in to-be-rejected trial steps but can
1942 // occasionally be real.
1943 if (fNormal <= 0) {
1944 //SimTK_DEBUG1("YANKING!!! (face %d)\n", face);
1945 continue;
1946 }
1947
1948 // 12 flops in this series.
1949 const Vec3 forceK = fK*normal_M; // as applied to other surf
1950 const Vec3 forceC = fC*normal_M;
1951 const Real PE = fK*overlap/2; // 1/2 kAx^2
1952 const Real powerC = fC*odot; // rate of energy loss, >= 0
1953 const Vec3 forceNormal = forceK + forceC;
1954
1955 // This is the moment r X f about the resultant point produced by
1956 // applying this pure force at the contact point. Cost ~60 flops.
1957 const Vec3 r = contactPt_M - resultantPt_M;
1958 const Real pressureMoment = (r % forceNormal).norm();
1959 weightedCenterOfPressure_M += pressureMoment*r;
1960 sumOfAllPressureMoments += pressureMoment;
1961
1962 // Calculate the friction force. Cost is about 60 flops.
1963 Vec3 forceFriction(0);
1964 Real powerFriction = 0;
1965 const Real vslipSq = velTangent.normSqr(); // 5 flops
1966 if (vslipSq > square(SignificantReal)) {
1967 const Real vslip = std::sqrt(vslipSq); // expensive: ~25 flops
1968 // Express slip velocity as unitless multiple of transition velocity.
1969 const Real v = vslip * ooVtrans;
1970 // Must scale viscous coefficient to match unitless velocity.
1971 const Real mu=stribeck(us,ud,uv*vtrans,v); // ~10 flops
1972 //const Real mu=hollars(us,ud,uv*vtrans,v);
1973 const Real fFriction = fNormal * mu;
1974 // Force direction on O opposes O's velocity.
1975 forceFriction = (-fFriction/vslip)*velTangent; // ~20 flops
1976 powerFriction = fFriction * vslip; // always >= 0
1977 }
1978
1979 const Vec3 forceLoss = forceC + forceFriction; // 3 flops
1980 const Vec3 forceTotal = forceK + forceLoss; // 3 flops
1981
1982 // Accumulate the moment and force on the *other* surface as though
1983 // applied at the point of O that is coincident with the resultant
1984 // point; we'll move it later. (15 flops)
1985 resultantForceOnOther_M += SpatialVec(r % forceTotal, forceTotal);
1986
1987 // Accumulate potential energy stored in elastic displacement.
1988 potentialEnergy += PE; // 1 flop
1989
1990 // Don't include dot(forceK,velNormal) power due to conservative force
1991 // here. This way we don't double-count the energy on the way in as
1992 // integrated power and potential energy. Although the books would
1993 // balance again when the contact is broken, it makes continuous
1994 // contact look as though some energy has been lost. In the "yanking"
1995 // case above, without including the conservative power term we will
1996 // actually lose energy because the deformed material isn't allowed to
1997 // push back on us so the energy is lost to surface vibrations or some
1998 // other unmodeled effect.
1999 const Real powerLossThisElement = powerC + powerFriction; // 1 flop
2000 powerLoss += powerLossThisElement; // 1 flop
2001
2002 if (wantDetails) {
2003 contactDetails_M->push_back();
2004 ContactDetail& detail = contactDetails_M->back();
2005 detail.m_contactPt = contactPt_M;
2006 detail.m_patchNormal = normal_M;
2007 detail.m_slipVelocity = velTangent;
2008 detail.m_forceOnSurface2 = forceTotal;
2009 detail.m_deformation = overlap;
2010 detail.m_deformationRate = odot;
2011 detail.m_patchArea = faceArea;
2012 detail.m_peakPressure = (faceArea != 0 ? fNormal/faceArea
2013 : Real(0));
2014 detail.m_potentialEnergy = PE;
2015 detail.m_powerLoss = powerLossThisElement;
2016 }
2017 }
2018 }
2019
2020 } // namespace SimTK
2021
2022