Simbody  3.5
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
ConditionalConstraint.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
2 #define SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm) *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2014 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKmath.h"
32 
33 namespace SimTK {
34 
39 public:
40 
47 static Real calcEffectiveCOR(Real minCOR, Real captureSpeed, Real minCORSpeed,
48  Real impactSpeed)
49 {
50  assert(0 <= minCOR && minCOR <= 1);
51  assert(0 <= captureSpeed && captureSpeed <= minCORSpeed);
52  assert(impactSpeed >= 0);
53 
54  if (impactSpeed <= captureSpeed) return 0;
55  if (impactSpeed >= minCORSpeed) return minCOR;
56  // captureSpeed < impactSpeed < minCORSpeed
57  const Real slope = (1-minCOR) / minCORSpeed;
58  return 1 - slope*impactSpeed;
59 }
60 
66 static Real calcEffectiveCOF(Real mu_s, Real mu_d, Real mu_v,
67  Real transitionSpeed, Real slipSpeed)
68 {
69  assert(mu_s>=0 && mu_d>=0 && mu_v>=0);
70  assert(mu_s>=mu_d);
71  assert(transitionSpeed>=0 && slipSpeed>=0);
72  const Real viscous = mu_v*slipSpeed; // typically zero
73  return viscous + (slipSpeed <= transitionSpeed ? mu_s : mu_d);
74 }
75 
76 };
77 
78 class UnilateralContact; // normal + friction
79 class UnilateralSpeedConstraint;
80 class BoundedSpeedConstraint;
81 
82 class ConstraintLimitedFriction;
83 class StateLimitedFriction;
84 
85 //==============================================================================
86 // UNILATERAL CONTACT CONSTRAINT
87 //==============================================================================
121 public:
125  explicit UnilateralContact(int sign=1) : m_sign((Real)sign)
126  { assert(sign==1 || sign==-1); }
127 
128  virtual ~UnilateralContact() {}
129 
131  Real getSignConvention() const {return m_sign;}
132 
135  virtual bool disable(State& state) const = 0;
136 
139  virtual bool enable(State& state) const = 0;
140 
142  virtual bool isEnabled(const State& state) const = 0;
143 
146  virtual Vec3 whereToDisplay(const State& state) const = 0;
147 
155  virtual Real calcEffectiveCOR(const State& state,
156  Real defaultCaptureSpeed,
157  Real defaultMinCORSpeed,
158  Real impactSpeed) const = 0;
159 
163  virtual Real getPerr(const State& state) const = 0;
166  virtual Real getVerr(const State& state) const = 0;
169  virtual Real getAerr(const State& state) const = 0;
170 
175  virtual bool isProximal(const State& state, Real ptol) const
176  { return m_sign*getPerr(state) <= ptol; }
177 
182  virtual MultiplierIndex
183  getContactMultiplierIndex(const State& state) const = 0;
184 
187  virtual bool hasFriction(const State& state) const {return false;}
188 
196  virtual Real calcEffectiveCOF(const State& state,
197  Real defaultTransitionSpeed,
198  Real slipSpeed) const
199  { return NaN; }
200 
201  virtual Vec2 getSlipVelocity(const State& state) const
202  { return Vec2(NaN); }
203 
207  virtual void
209  MultiplierIndex& ix_x,
210  MultiplierIndex& ix_y) const
211  { ix_x.invalidate(); ix_y.invalidate(); }
212 
215  virtual Vec3 getPositionInfo(const State& state) const
216  { return Vec3(NaN); }
219  virtual void setInstanceParameter(State& state, const Vec3& pos) const {}
220 
221 
222  void setMyIndex(UnilateralContactIndex cx) {m_myIx = cx;}
223  UnilateralContactIndex getMyIndex() const {return m_myIx;}
224 private:
225  Real m_sign; // 1 or -1
226  UnilateralContactIndex m_myIx;
227 };
228 
229 //==============================================================================
230 // UNILATERAL SPEED CONSTRAINT
231 //==============================================================================
239 public:
242 
243 private:
244 };
245 
246 //==============================================================================
247 // BOUNDED SPEED CONSTRAINT
248 //==============================================================================
273 public:
276 
281  virtual Vec2 calcEffectiveBounds(const State& state) const = 0;
282 private:
283 };
284 
285 //==============================================================================
286 // STATE LIMITED FRICTION
287 //==============================================================================
290 public:
293 
296  virtual bool disable(State& state) const = 0;
297 
300  virtual bool enable(State& state) const = 0;
301 
304  virtual Real getNormalForceMagnitude(const State& state) const = 0;
305 
306  virtual Real calcEffectiveCOF(const State& state,
307  Real defaultTransitionSpeed,
308  Real slipSpeed) const = 0;
309 
310  virtual Real getSlipSpeed(const State& state) const = 0;
311 
314  virtual Vec3 getPositionInfo(const State& state) const
315  { return Vec3(NaN); }
318  virtual void setInstanceParameter(State& state, const Vec3& pos) const {}
319 
320  void setMyIndex(StateLimitedFrictionIndex fx) {m_myIx = fx;}
321  StateLimitedFrictionIndex getMyIndex() const {return m_myIx;}
322 private:
323  StateLimitedFrictionIndex m_myIx;
324 };
325 
326 
327 //==============================================================================
328 // HARD STOP UPPER
329 //==============================================================================
343 public:
345  Real defaultUpperLimit, Real minCOR);
346 
347  bool disable(State& state) const OVERRIDE_11
348  { if (m_upper.isDisabled(state)) return false;
349  else {m_upper.disable(state); return true;} }
350  bool enable(State& state) const OVERRIDE_11
351  { if (!m_upper.isDisabled(state)) return false;
352  else {m_upper.enable(state); return true;} }
353  bool isEnabled(const State& state) const OVERRIDE_11
354  { return !m_upper.isDisabled(state); }
355 
356  // Returns the contact point in the Ground frame.
357  Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
358 
359  // Currently have to fake the perr because the constraint might be
360  // disabled in which case it won't calculate perr. Also, we want
361  // negative to mean violated so may need to adjust the sign.
362  Real getPerr(const State& state) const OVERRIDE_11;
363  Real getVerr(const State& state) const OVERRIDE_11;
364  Real getAerr(const State& state) const OVERRIDE_11;
365 
367  Real defaultCaptureSpeed,
368  Real defaultMinCORSpeed,
369  Real impactSpeed) const OVERRIDE_11
370  {
372  (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
373  impactSpeed);
374  }
375 
376  MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
377 
378 private:
379  MobilizedBody m_mobod;
380  Real m_defaultUpperLimit;
381  Real m_minCOR;
383 };
384 
385 //==============================================================================
386 // HARD STOP LOWER
387 //==============================================================================
401 public:
403  Real defaultLowerLimit, Real minCOR);
404 
405  bool disable(State& state) const OVERRIDE_11
406  { if (m_lower.isDisabled(state)) return false;
407  else {m_lower.disable(state); return true;} }
408  bool enable(State& state) const OVERRIDE_11
409  { if (!m_lower.isDisabled(state)) return false;
410  else {m_lower.enable(state); return true;} }
411  bool isEnabled(const State& state) const OVERRIDE_11
412  { return !m_lower.isDisabled(state); }
413 
414  // Returns the contact point in the Ground frame.
415  Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
416 
417  // Currently have to fake the perr because the constraint might be
418  // disabled in which case it won't calculate perr. Also, we want
419  // negative to mean violated so may need to adjust the sign.
420  Real getPerr(const State& state) const OVERRIDE_11;
421  Real getVerr(const State& state) const OVERRIDE_11;
422  Real getAerr(const State& state) const OVERRIDE_11;
423 
425  Real defaultCaptureSpeed,
426  Real defaultMinCORSpeed,
427  Real impactSpeed) const OVERRIDE_11
428  {
430  (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
431  impactSpeed);
432  }
433 
434  MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
435 private:
436  MobilizedBody m_mobod;
437  Real m_defaultLowerLimit;
438  Real m_minCOR;
440 };
441 
442 
443 
444 //==============================================================================
445 // ROPE
446 //==============================================================================
460 public:
461  Rope(MobilizedBody& mobod1, const Vec3& point1,
462  MobilizedBody& mobod2, const Vec3& point2,
463  Real defaultLengthLimit, Real minCOR);
464 
465  bool disable(State& state) const OVERRIDE_11
466  { if (m_rod.isDisabled(state)) return false;
467  else {m_rod.disable(state); return true;} }
468  bool enable(State& state) const OVERRIDE_11
469  { if (!m_rod.isDisabled(state)) return false;
470  else {m_rod.enable(state); return true;} }
471  bool isEnabled(const State& state) const OVERRIDE_11
472  { return !m_rod.isDisabled(state); }
473 
474  // Returns half-way location in the Ground frame.
475  Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
476 
477  // Currently have to fake the perr because the constraint might be
478  // disabled in which case it won't calculate perr.
479  Real getPerr(const State& state) const OVERRIDE_11;
480  Real getVerr(const State& state) const OVERRIDE_11;
481  Real getAerr(const State& state) const OVERRIDE_11;
482 
484  Real defaultCaptureSpeed,
485  Real defaultMinCORSpeed,
486  Real impactSpeed) const OVERRIDE_11
487  {
489  (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
490  impactSpeed);
491  }
492 
493  MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
494 private:
495  Real m_minCOR;
496  Constraint::Rod m_rod;
497 };
498 
499 //==============================================================================
500 // POINT PLANE FRICTIONLESS CONTACT
501 //==============================================================================
507 : public UnilateralContact {
508 public:
510  MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
511  MobilizedBody& followerBodyF, const Vec3& point_F, Real minCOR);
512 
513  bool disable(State& state) const OVERRIDE_11 {
514  if (m_ptInPlane.isDisabled(state)) return false;
515  m_ptInPlane.disable(state);
516  return true;
517  }
518 
519  bool enable(State& state) const OVERRIDE_11 {
520  if (!m_ptInPlane.isDisabled(state)) return false;
521  m_ptInPlane.enable(state);
522  return true;
523  }
524 
525  bool isEnabled(const State& state) const OVERRIDE_11 {
526  return !m_ptInPlane.isDisabled(state);
527  }
528 
529  // Returns the contact point in the Ground frame.
530  Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
531 
532  // Currently have to fake the perr because the constraint might be
533  // disabled in which case it won't calculate perr.
534  Real getPerr(const State& state) const OVERRIDE_11;
535 
536  // We won't need to look at these except for proximal constraints which
537  // will already have been enabled, so no need to fake.
538  Real getVerr(const State& state) const OVERRIDE_11
539  { return m_ptInPlane.getVelocityError(state); }
540  Real getAerr(const State& state) const OVERRIDE_11
541  { return m_ptInPlane.getAccelerationError(state); }
542 
543 
545  Real defaultCaptureSpeed,
546  Real defaultMinCORSpeed,
547  Real impactSpeed) const OVERRIDE_11
548  {
550  (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
551  impactSpeed);
552  }
553 
554  MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
555 
556 private:
557  MobilizedBody m_planeBody; // body P
558  const Rotation m_frame; // z is normal; expressed in P
559  const Real m_height;
560 
561  MobilizedBody m_follower; // body F
562  const Vec3 m_point; // measured & expressed in F
563 
564  Real m_minCOR;
565 
566  Constraint::PointInPlane m_ptInPlane;
567 };
568 
569 
570 //==============================================================================
571 // POINT PLANE CONTACT
572 //==============================================================================
579 public:
581  MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
582  MobilizedBody& followerBodyF, const Vec3& point_F,
583  Real minCOR, Real mu_s, Real mu_d, Real mu_v);
584 
585  bool disable(State& state) const OVERRIDE_11 {
586  if (m_ptInPlane.isDisabled(state)) return false;
587  m_ptInPlane.disable(state);
588  return true;
589  }
590 
591  bool enable(State& state) const OVERRIDE_11 {
592  if (!m_ptInPlane.isDisabled(state)) return false;
593  m_ptInPlane.enable(state);
594  return true;
595  }
596 
597  bool isEnabled(const State& state) const OVERRIDE_11 {
598  return !m_ptInPlane.isDisabled(state);
599  }
600 
601  // Returns the contact point in the Ground frame.
602  Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
603 
604  // Currently have to fake the perr because the constraint might be
605  // disabled in which case it won't calculate perr.
606  Real getPerr(const State& state) const OVERRIDE_11;
607 
608  // We won't need to look at these except for proximal constraints which
609  // will already have been enabled, so no need to fake.
610  Real getVerr(const State& state) const OVERRIDE_11
611  { return m_ptInPlane.getVelocityErrors(state)[2]; }
612  Real getAerr(const State& state) const OVERRIDE_11
613  { return m_ptInPlane.getAccelerationErrors(state)[2]; }
614 
615 
617  Real defaultCaptureSpeed,
618  Real defaultMinCORSpeed,
619  Real impactSpeed) const OVERRIDE_11
620  {
622  (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
623  impactSpeed);
624  }
625 
626  bool hasFriction(const State& state) const OVERRIDE_11
627  { return true; }
628 
629  Vec2 getSlipVelocity(const State& state) const OVERRIDE_11 {
630  const Vec3 v = m_ptInPlane.getVelocityErrors(state);
631  return Vec2(v[0], v[1]);
632  }
633 
635  Real defaultTransitionSpeed,
636  Real slipSpeed) const OVERRIDE_11
637  {
639  (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
640  }
641 
642  MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
643 
644  void getFrictionMultiplierIndices(const State& s,
645  MultiplierIndex& ix_x,
646  MultiplierIndex& ix_y) const OVERRIDE_11;
647 
648 private:
649  MobilizedBody m_planeBody; // body P
650  const Rotation m_frame; // z is normal; expressed in P
651  const Real m_height;
652 
653  MobilizedBody m_follower; // body F
654  const Vec3 m_point; // measured & expressed in F
655 
656  Real m_minCOR;
657  Real m_mu_s, m_mu_d, m_mu_v;
658 
660 };
661 
662 //==============================================================================
663 // SPHERE PLANE CONTACT
664 //==============================================================================
671 public:
673  MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
674  MobilizedBody& followerBodyF, const Vec3& point_F, Real radius,
675  Real minCOR, Real mu_s, Real mu_d, Real mu_v);
676 
677  bool disable(State& state) const OVERRIDE_11 {
678  if (m_sphereOnPlane.isDisabled(state)) return false;
679  m_sphereOnPlane.disable(state);
680  return true;
681  }
682 
683  bool enable(State& state) const OVERRIDE_11 {
684  if (!m_sphereOnPlane.isDisabled(state)) return false;
685  m_sphereOnPlane.enable(state);
686  return true;
687  }
688 
689  bool isEnabled(const State& state) const OVERRIDE_11 {
690  return !m_sphereOnPlane.isDisabled(state);
691  }
692 
693  // Returns the contact point in the Ground frame.
694  Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
695 
696  // Currently have to fake the perr because the constraint might be
697  // disabled in which case it won't calculate perr.
698  Real getPerr(const State& state) const OVERRIDE_11;
699 
700  // We won't need to look at these except for proximal constraints which
701  // will already have been enabled, so no need to fake.
702  Real getVerr(const State& state) const OVERRIDE_11
703  { return m_sphereOnPlane.getVelocityErrors(state)[2]; }
704  Real getAerr(const State& state) const OVERRIDE_11
705  { return m_sphereOnPlane.getAccelerationErrors(state)[2]; }
706 
707 
709  Real defaultCaptureSpeed,
710  Real defaultMinCORSpeed,
711  Real impactSpeed) const OVERRIDE_11
712  {
714  (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
715  impactSpeed);
716  }
717 
718  bool hasFriction(const State& state) const OVERRIDE_11
719  { return true; }
720 
721  Vec2 getSlipVelocity(const State& state) const OVERRIDE_11 {
722  const Vec3 v = m_sphereOnPlane.getVelocityErrors(state);
723  return Vec2(v[0], v[1]);
724  }
725 
727  Real defaultTransitionSpeed,
728  Real slipSpeed) const OVERRIDE_11
729  {
731  (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
732  }
733 
734  MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
735 
736  void getFrictionMultiplierIndices(const State& s,
737  MultiplierIndex& ix_x,
738  MultiplierIndex& ix_y) const OVERRIDE_11;
739 
740 private:
741  Real m_minCOR;
742  Real m_mu_s, m_mu_d, m_mu_v;
743 
744  Constraint::SphereOnPlaneContact m_sphereOnPlane;
745 };
746 
747 //==============================================================================
748 // SPHERE SPHERE CONTACT
749 //==============================================================================
757 public:
759  MobilizedBody& mobod_F,
760  const Vec3& defaultCenterOnF,
761  Real defaultRadiusOnF,
762  MobilizedBody& mobod_B,
763  const Vec3& defaultCenterOnB,
764  Real defaultRadiusOnB,
765  Real minCOR, Real mu_s, Real mu_d, Real mu_v);
766 
767  bool disable(State& state) const OVERRIDE_11 {
768  if (m_sphereOnSphere.isDisabled(state)) return false;
769  m_sphereOnSphere.disable(state);
770  return true;
771  }
772 
773  bool enable(State& state) const OVERRIDE_11 {
774  if (!m_sphereOnSphere.isDisabled(state)) return false;
775  m_sphereOnSphere.enable(state);
776  return true;
777  }
778 
779  bool isEnabled(const State& state) const OVERRIDE_11 {
780  return !m_sphereOnSphere.isDisabled(state);
781  }
782 
783  // Returns the contact point in the Ground frame.
784  Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
785 
786  // Currently have to fake the perr because the constraint might be
787  // disabled in which case it won't calculate perr.
788  Real getPerr(const State& state) const OVERRIDE_11;
789 
790  // We won't need to look at these except for proximal constraints which
791  // will already have been enabled, so no need to fake.
792  Real getVerr(const State& state) const OVERRIDE_11
793  { return m_sphereOnSphere.getVelocityErrors(state)[2]; }
794  Real getAerr(const State& state) const OVERRIDE_11
795  { return m_sphereOnSphere.getAccelerationErrors(state)[2]; }
796 
797 
799  Real defaultCaptureSpeed,
800  Real defaultMinCORSpeed,
801  Real impactSpeed) const OVERRIDE_11
802  {
804  (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
805  impactSpeed);
806  }
807 
808  bool hasFriction(const State& state) const OVERRIDE_11
809  { return true; }
810 
811  Vec2 getSlipVelocity(const State& state) const OVERRIDE_11 {
812  const Vec3 v = m_sphereOnSphere.getVelocityErrors(state);
813  return Vec2(v[0], v[1]);
814  }
815 
817  Real defaultTransitionSpeed,
818  Real slipSpeed) const OVERRIDE_11
819  {
821  (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
822  }
823 
824  MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
825 
826  void getFrictionMultiplierIndices(const State& s,
827  MultiplierIndex& ix_x,
828  MultiplierIndex& ix_y) const OVERRIDE_11;
829 
830 private:
831  Real m_minCOR;
832  Real m_mu_s, m_mu_d, m_mu_v;
833 
834  Constraint::SphereOnSphereContact m_sphereOnSphere;
835 };
836 
837 
838 //==============================================================================
839 // EDGE EDGE CONTACT
840 //==============================================================================
853 public:
855  MobilizedBody& mobod_F,
856  const Transform& defaultEdgeFrameF,
857  Real defaultHalfLengthF,
858  MobilizedBody& mobod_B,
859  const Transform& defaultEdgeFrameB,
860  Real defaultHalfLengthB,
861  Real minCOR, Real mu_s, Real mu_d, Real mu_v);
862 
863  bool disable(State& state) const OVERRIDE_11 {
864  if (m_lineOnLine.isDisabled(state)) return false;
865  m_lineOnLine.disable(state);
866  return true;
867  }
868 
869  bool enable(State& state) const OVERRIDE_11 {
870  if (!m_lineOnLine.isDisabled(state)) return false;
871  m_lineOnLine.enable(state);
872  return true;
873  }
874 
875  bool isEnabled(const State& state) const OVERRIDE_11 {
876  return !m_lineOnLine.isDisabled(state);
877  }
878 
879  // Returns the contact point in the Ground frame.
880  Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
881 
882  // Currently have to fake the perr because the constraint might be
883  // disabled in which case it won't calculate perr.
884  Real getPerr(const State& state) const OVERRIDE_11;
885 
886  // We won't need to look at these except for proximal constraints which
887  // will already have been enabled, so no need to fake.
888  Real getVerr(const State& state) const OVERRIDE_11
889  { return m_lineOnLine.getVelocityErrors(state)[2]; }
890  Real getAerr(const State& state) const OVERRIDE_11
891  { return m_lineOnLine.getAccelerationErrors(state)[2]; }
892 
893 
895  Real defaultCaptureSpeed,
896  Real defaultMinCORSpeed,
897  Real impactSpeed) const OVERRIDE_11
898  {
900  (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
901  impactSpeed);
902  }
903 
904  bool hasFriction(const State& state) const OVERRIDE_11
905  { return true; }
906 
907  Vec2 getSlipVelocity(const State& state) const OVERRIDE_11 {
908  const Vec3 v = m_lineOnLine.getVelocityErrors(state);
909  return Vec2(v[0], v[1]);
910  }
911 
913  Real defaultTransitionSpeed,
914  Real slipSpeed) const OVERRIDE_11
915  {
917  (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
918  }
919 
920  MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
921 
922  void getFrictionMultiplierIndices(const State& s,
923  MultiplierIndex& ix_x,
924  MultiplierIndex& ix_y) const OVERRIDE_11;
925 
926 private:
927  Real m_minCOR;
928  Real m_mu_s, m_mu_d, m_mu_v;
929 
930  Constraint::LineOnLineContact m_lineOnLine;
931 };
932 
933 } // namespace SimTK
934 
935 #endif // SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
Vec2 getSlipVelocity(const State &state) const override
Definition: ConditionalConstraint.h:811
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:863
bool enable(State &state) const override
Enable the normal and friction constraints if they were disabled.
Definition: ConditionalConstraint.h:591
Real calcEffectiveCOR(const State &state, Real defaultCaptureSpeed, Real defaultMinCORSpeed, Real impactSpeed) const override
Returns the effective coefficient of restitution (COR) for this contact, given an impact speed (a non...
Definition: ConditionalConstraint.h:798
bool enable(State &state) const override
Enable the normal and friction constraints if they were disabled.
Definition: ConditionalConstraint.h:350
virtual ~BoundedSpeedConstraint()
Definition: ConditionalConstraint.h:275
bool enable(State &state) const override
Enable the normal and friction constraints if they were disabled.
Definition: ConditionalConstraint.h:468
This constraint represents a bilateral connection between an edge on one body and a non-parallel edge...
Definition: Constraint_LineOnLineContact.h:142
(Experimental – API will change – use at your own risk) Define a point on one body that cannot pene...
Definition: ConditionalConstraint.h:578
Include the header files that define each of the built-in constraint subclasses of class Constraint...
UnilateralContactIndex getMyIndex() const
Definition: ConditionalConstraint.h:223
Real getVerr(const State &state) const override
Return the time derivative of the contact constraint position error.
Definition: ConditionalConstraint.h:702
void setMyIndex(StateLimitedFrictionIndex fx)
Definition: ConditionalConstraint.h:320
(Experimental – API will change – use at your own risk) Define a sphere on one body that cannot pen...
Definition: ConditionalConstraint.h:670
virtual void setInstanceParameter(State &state, const Vec3 &pos) const
TODO: kludge to set instance parameters on internal constraints; this should be the same Vec3 you got...
Definition: ConditionalConstraint.h:318
Real calcEffectiveCOR(const State &state, Real defaultCaptureSpeed, Real defaultMinCORSpeed, Real impactSpeed) const override
Returns the effective coefficient of restitution (COR) for this contact, given an impact speed (a non...
Definition: ConditionalConstraint.h:366
(Experimental – API will change – use at your own risk) Define an edge on each of two bodies...
Definition: ConditionalConstraint.h:852
StateLimitedFrictionIndex getMyIndex() const
Definition: ConditionalConstraint.h:321
static Real calcEffectiveCOF(Real mu_s, Real mu_d, Real mu_v, Real transitionSpeed, Real slipSpeed)
Given the coefficients of friction and slip-to-rolling transition speed, calculate the effective COF ...
Definition: ConditionalConstraint.h:66
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:689
Real calcEffectiveCOF(const State &state, Real defaultTransitionSpeed, Real slipSpeed) const override
Returns the effective coefficient of friction mu for this contact, given a relative slip speed (a non...
Definition: ConditionalConstraint.h:912
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:597
virtual Vec2 getSlipVelocity(const State &state) const
Definition: ConditionalConstraint.h:201
Real getAerr(const State &state) const override
Return the time derivative of the contact constraint velocity error.
Definition: ConditionalConstraint.h:794
Every Simbody header and source file should include this header before any other Simbody header...
Real getAerr(const State &state) const override
Return the time derivative of the contact constraint velocity error.
Definition: ConditionalConstraint.h:612
This constraint represents a bilateral connection between a sphere on one body and a sphere on anothe...
Definition: Constraint_SphereOnSphereContact.h:102
bool enable(State &state) const override
Enable the normal and friction constraints if they were disabled.
Definition: ConditionalConstraint.h:683
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:577
Real getVerr(const State &state) const override
Return the time derivative of the contact constraint position error.
Definition: ConditionalConstraint.h:888
TODO: not implemented yet.
Definition: ConditionalConstraint.h:272
TODO: Simbody model element representing a conditionally-enforced constraint.
Definition: ConditionalConstraint.h:38
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:411
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:347
UnilateralContact(int sign=1)
The base class constructor allows specification of the sign convention to be used with this constrain...
Definition: ConditionalConstraint.h:125
Real getAerr(const State &state) const override
Return the time derivative of the contact constraint velocity error.
Definition: ConditionalConstraint.h:540
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
Real calcEffectiveCOR(const State &state, Real defaultCaptureSpeed, Real defaultMinCORSpeed, Real impactSpeed) const override
Returns the effective coefficient of restitution (COR) for this contact, given an impact speed (a non...
Definition: ConditionalConstraint.h:483
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:465
Vec2 getSlipVelocity(const State &state) const override
Definition: ConditionalConstraint.h:721
Real getSignConvention() const
Report the sign convention (1 or -1) supplied at construction.
Definition: ConditionalConstraint.h:131
(Experimental – API will change – use at your own risk) A unilateral contact constraint uses a sing...
Definition: ConditionalConstraint.h:120
Constrain a single mobilizer coordinate q to have a particular value.
Definition: Constraint.h:858
TODO: not implemented yet.
Definition: ConditionalConstraint.h:289
Real getVerr(const State &state) const override
Return the time derivative of the contact constraint position error.
Definition: ConditionalConstraint.h:792
bool enable(State &state) const override
Enable the normal and friction constraints if they were disabled.
Definition: ConditionalConstraint.h:519
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:471
(Advanced) This is the underlying constraint for unilateral contact with friction but must be combine...
Definition: Constraint_PointOnPlaneContact.h:87
Real getAerr(const State &state) const override
Return the time derivative of the contact constraint velocity error.
Definition: ConditionalConstraint.h:704
#define OVERRIDE_11
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:251
This constraint represents a bilateral connection between a sphere on one body and a plane on another...
Definition: Constraint_SphereOnPlaneContact.h:86
Real getAerr(const State &state) const override
Return the time derivative of the contact constraint velocity error.
Definition: ConditionalConstraint.h:890
static Real calcEffectiveCOR(Real minCOR, Real captureSpeed, Real minCORSpeed, Real impactSpeed)
Given the specified minimum coefficient of restitution (COR), capture speed, and the speed at which t...
Definition: ConditionalConstraint.h:47
void setMyIndex(UnilateralContactIndex cx)
Definition: ConditionalConstraint.h:222
UnilateralSpeedConstraint()
Definition: ConditionalConstraint.h:240
virtual Vec3 getPositionInfo(const State &state) const
TODO: kludge needed because we're misusing existing constraints.
Definition: ConditionalConstraint.h:314
virtual void getFrictionMultiplierIndices(const State &state, MultiplierIndex &ix_x, MultiplierIndex &ix_y) const
If hasFriction(), this method returns the multipliers used for the x- and y-direction friction constr...
Definition: ConditionalConstraint.h:208
bool hasFriction(const State &state) const override
Returns true if there is a friction constraint associated with this contact constraint.
Definition: ConditionalConstraint.h:626
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:585
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:767
One constraint equation.
Definition: Constraint_PointInPlane.h:47
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:353
Real calcEffectiveCOR(const State &state, Real defaultCaptureSpeed, Real defaultMinCORSpeed, Real impactSpeed) const override
Returns the effective coefficient of restitution (COR) for this contact, given an impact speed (a non...
Definition: ConditionalConstraint.h:616
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:525
(Experimental – API will change – use at your own risk) Set a hard limit on the minimum value of a ...
Definition: ConditionalConstraint.h:400
Real calcEffectiveCOR(const State &state, Real defaultCaptureSpeed, Real defaultMinCORSpeed, Real impactSpeed) const override
Returns the effective coefficient of restitution (COR) for this contact, given an impact speed (a non...
Definition: ConditionalConstraint.h:708
virtual Real calcEffectiveCOF(const State &state, Real defaultTransitionSpeed, Real slipSpeed) const
Returns the effective coefficient of friction mu for this contact, given a relative slip speed (a non...
Definition: ConditionalConstraint.h:196
bool enable(State &state) const override
Enable the normal and friction constraints if they were disabled.
Definition: ConditionalConstraint.h:869
Real calcEffectiveCOF(const State &state, Real defaultTransitionSpeed, Real slipSpeed) const override
Returns the effective coefficient of friction mu for this contact, given a relative slip speed (a non...
Definition: ConditionalConstraint.h:634
(Experimental – API will change – use at your own risk) Define a point on one body that cannot pene...
Definition: ConditionalConstraint.h:506
(Experimental – API will change – use at your own risk) Set a hard upper limit on the separation be...
Definition: ConditionalConstraint.h:459
Real calcEffectiveCOF(const State &state, Real defaultTransitionSpeed, Real slipSpeed) const override
Returns the effective coefficient of friction mu for this contact, given a relative slip speed (a non...
Definition: ConditionalConstraint.h:726
virtual ~UnilateralSpeedConstraint()
Definition: ConditionalConstraint.h:241
This defines the base Constraint class and related classes, which are used to specify limitations on ...
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:677
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:405
This constraint consists of one constraint equation that enforces a constant distance between a point...
Definition: Constraint_Rod.h:52
Real calcEffectiveCOR(const State &state, Real defaultCaptureSpeed, Real defaultMinCORSpeed, Real impactSpeed) const override
Returns the effective coefficient of restitution (COR) for this contact, given an impact speed (a non...
Definition: ConditionalConstraint.h:424
StateLimitedFriction()
Definition: ConditionalConstraint.h:291
This defines the MobilizedBody class, which associates a new body (the "child", "outboard", or "successor" body) with a mobilizer and a reference frame on an existing body (the "parent", "inboard", or "predecessor" body) that is already part of a SimbodyMatterSubsystem.
Vec2 getSlipVelocity(const State &state) const override
Definition: ConditionalConstraint.h:629
bool enable(State &state) const override
Enable the normal and friction constraints if they were disabled.
Definition: ConditionalConstraint.h:773
virtual ~StateLimitedFriction()
Definition: ConditionalConstraint.h:292
BoundedSpeedConstraint()
Definition: ConditionalConstraint.h:274
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:513
virtual void setInstanceParameter(State &state, const Vec3 &pos) const
TODO: kludge to set instance parameters on internal constraints; this should be the same Vec3 you got...
Definition: ConditionalConstraint.h:219
bool hasFriction(const State &state) const override
Returns true if there is a friction constraint associated with this contact constraint.
Definition: ConditionalConstraint.h:904
Vec2 getSlipVelocity(const State &state) const override
Definition: ConditionalConstraint.h:907
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:167
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
(Experimental – API will change – use at your own risk) Define a sphere on each of two bodies...
Definition: ConditionalConstraint.h:756
(Experimental – API will change – use at your own risk) Set a hard limit on the maximum value of a ...
Definition: ConditionalConstraint.h:342
virtual bool isProximal(const State &state, Real ptol) const
Given the position constraint tolerance currently in use, is this contact close enough to contacting ...
Definition: ConditionalConstraint.h:175
TODO: not implemented yet.
Definition: ConditionalConstraint.h:238
Vec< 3 > Vec3
This is the most common 3D vector type: a column of 3 Real values stored consecutively in memory (pac...
Definition: SmallMatrix.h:129
Real calcEffectiveCOR(const State &state, Real defaultCaptureSpeed, Real defaultMinCORSpeed, Real impactSpeed) const override
Returns the effective coefficient of restitution (COR) for this contact, given an impact speed (a non...
Definition: ConditionalConstraint.h:894
Real calcEffectiveCOR(const State &state, Real defaultCaptureSpeed, Real defaultMinCORSpeed, Real impactSpeed) const override
Returns the effective coefficient of restitution (COR) for this contact, given an impact speed (a non...
Definition: ConditionalConstraint.h:544
Real getVerr(const State &state) const override
Return the time derivative of the contact constraint position error.
Definition: ConditionalConstraint.h:610
virtual Vec3 getPositionInfo(const State &state) const
TODO: kludge needed because we're misusing existing constraints.
Definition: ConditionalConstraint.h:215
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:875
virtual bool hasFriction(const State &state) const
Returns true if there is a friction constraint associated with this contact constraint.
Definition: ConditionalConstraint.h:187
Unique integer type for Subsystem-local multiplier indexing.
bool hasFriction(const State &state) const override
Returns true if there is a friction constraint associated with this contact constraint.
Definition: ConditionalConstraint.h:718
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:779
bool hasFriction(const State &state) const override
Returns true if there is a friction constraint associated with this contact constraint.
Definition: ConditionalConstraint.h:808
Real calcEffectiveCOF(const State &state, Real defaultTransitionSpeed, Real slipSpeed) const override
Returns the effective coefficient of friction mu for this contact, given a relative slip speed (a non...
Definition: ConditionalConstraint.h:816
Real getVerr(const State &state) const override
Return the time derivative of the contact constraint position error.
Definition: ConditionalConstraint.h:538
virtual ~UnilateralContact()
Definition: ConditionalConstraint.h:128
bool enable(State &state) const override
Enable the normal and friction constraints if they were disabled.
Definition: ConditionalConstraint.h:408
Vec< 2 > Vec2
This is the most common 2D vector type: a column of 2 Real values stored consecutively in memory (pac...
Definition: SmallMatrix.h:126