1 #ifndef SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_ 2 #define SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_ 50 assert(0 <= minCOR && minCOR <= 1);
51 assert(0 <= captureSpeed && captureSpeed <= minCORSpeed);
52 assert(impactSpeed >= 0);
54 if (impactSpeed <= captureSpeed)
return 0;
55 if (impactSpeed >= minCORSpeed)
return minCOR;
57 const Real slope = (1-minCOR) / minCORSpeed;
58 return 1 - slope*impactSpeed;
67 Real transitionSpeed,
Real slipSpeed)
69 assert(mu_s>=0 && mu_d>=0 && mu_v>=0);
71 assert(transitionSpeed>=0 && slipSpeed>=0);
72 const Real viscous = mu_v*slipSpeed;
73 return viscous + (slipSpeed <= transitionSpeed ? mu_s : mu_d);
78 class UnilateralContact;
79 class UnilateralSpeedConstraint;
80 class BoundedSpeedConstraint;
82 class ConstraintLimitedFriction;
83 class StateLimitedFriction;
135 virtual bool disable(
State& state)
const = 0;
139 virtual bool enable(
State& state)
const = 0;
142 virtual bool isEnabled(
const State& state)
const = 0;
146 virtual Vec3 whereToDisplay(
const State& state)
const = 0;
155 virtual Real calcEffectiveCOR(
const State& state,
156 Real defaultCaptureSpeed,
157 Real defaultMinCORSpeed,
158 Real impactSpeed)
const = 0;
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;
176 {
return m_sign*getPerr(state) <= ptol; }
183 getContactMultiplierIndex(
const State& state)
const = 0;
197 Real defaultTransitionSpeed,
198 Real slipSpeed)
const 211 { ix_x.invalidate(); ix_y.invalidate(); }
226 UnilateralContactIndex m_myIx;
281 virtual Vec2 calcEffectiveBounds(
const State& state)
const = 0;
296 virtual bool disable(
State& state)
const = 0;
300 virtual bool enable(
State& state)
const = 0;
304 virtual Real getNormalForceMagnitude(
const State& state)
const = 0;
306 virtual Real calcEffectiveCOF(
const State& state,
307 Real defaultTransitionSpeed,
308 Real slipSpeed)
const = 0;
310 virtual Real getSlipSpeed(
const State& state)
const = 0;
321 StateLimitedFrictionIndex
getMyIndex()
const {
return m_myIx;}
323 StateLimitedFrictionIndex m_myIx;
345 Real defaultUpperLimit,
Real minCOR);
348 {
if (m_upper.isDisabled(state))
return false;
349 else {m_upper.disable(state);
return true;} }
351 {
if (!m_upper.isDisabled(state))
return false;
352 else {m_upper.enable(state);
return true;} }
354 {
return !m_upper.isDisabled(state); }
357 Vec3 whereToDisplay(
const State& state)
const override;
362 Real getPerr(
const State& state)
const override;
363 Real getVerr(
const State& state)
const override;
364 Real getAerr(
const State& state)
const override;
367 Real defaultCaptureSpeed,
368 Real defaultMinCORSpeed,
369 Real impactSpeed)
const override 372 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
380 Real m_defaultUpperLimit;
403 Real defaultLowerLimit,
Real minCOR);
406 {
if (m_lower.isDisabled(state))
return false;
407 else {m_lower.disable(state);
return true;} }
409 {
if (!m_lower.isDisabled(state))
return false;
410 else {m_lower.enable(state);
return true;} }
412 {
return !m_lower.isDisabled(state); }
415 Vec3 whereToDisplay(
const State& state)
const override;
420 Real getPerr(
const State& state)
const override;
421 Real getVerr(
const State& state)
const override;
422 Real getAerr(
const State& state)
const override;
425 Real defaultCaptureSpeed,
426 Real defaultMinCORSpeed,
427 Real impactSpeed)
const override 430 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
437 Real m_defaultLowerLimit;
463 Real defaultLengthLimit,
Real minCOR);
466 {
if (m_rod.isDisabled(state))
return false;
467 else {m_rod.disable(state);
return true;} }
469 {
if (!m_rod.isDisabled(state))
return false;
470 else {m_rod.enable(state);
return true;} }
472 {
return !m_rod.isDisabled(state); }
475 Vec3 whereToDisplay(
const State& state)
const override;
479 Real getPerr(
const State& state)
const override;
480 Real getVerr(
const State& state)
const override;
481 Real getAerr(
const State& state)
const override;
484 Real defaultCaptureSpeed,
485 Real defaultMinCORSpeed,
486 Real impactSpeed)
const override 489 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
514 if (m_ptInPlane.isDisabled(state))
return false;
515 m_ptInPlane.disable(state);
520 if (!m_ptInPlane.isDisabled(state))
return false;
521 m_ptInPlane.enable(state);
526 return !m_ptInPlane.isDisabled(state);
530 Vec3 whereToDisplay(
const State& state)
const override;
534 Real getPerr(
const State& state)
const override;
539 {
return m_ptInPlane.getVelocityError(state); }
541 {
return m_ptInPlane.getAccelerationError(state); }
545 Real defaultCaptureSpeed,
546 Real defaultMinCORSpeed,
547 Real impactSpeed)
const override 550 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
586 if (m_ptInPlane.isDisabled(state))
return false;
587 m_ptInPlane.disable(state);
592 if (!m_ptInPlane.isDisabled(state))
return false;
593 m_ptInPlane.enable(state);
598 return !m_ptInPlane.isDisabled(state);
602 Vec3 whereToDisplay(
const State& state)
const override;
606 Real getPerr(
const State& state)
const override;
611 {
return m_ptInPlane.getVelocityErrors(state)[2]; }
613 {
return m_ptInPlane.getAccelerationErrors(state)[2]; }
617 Real defaultCaptureSpeed,
618 Real defaultMinCORSpeed,
619 Real impactSpeed)
const override 622 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
630 const Vec3 v = m_ptInPlane.getVelocityErrors(state);
631 return Vec2(v[0], v[1]);
635 Real defaultTransitionSpeed,
636 Real slipSpeed)
const override 639 (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
644 void getFrictionMultiplierIndices(
const State& s,
657 Real m_mu_s, m_mu_d, m_mu_v;
678 if (m_sphereOnPlane.isDisabled(state))
return false;
679 m_sphereOnPlane.disable(state);
684 if (!m_sphereOnPlane.isDisabled(state))
return false;
685 m_sphereOnPlane.enable(state);
690 return !m_sphereOnPlane.isDisabled(state);
694 Vec3 whereToDisplay(
const State& state)
const override;
698 Real getPerr(
const State& state)
const override;
703 {
return m_sphereOnPlane.getVelocityErrors(state)[2]; }
705 {
return m_sphereOnPlane.getAccelerationErrors(state)[2]; }
709 Real defaultCaptureSpeed,
710 Real defaultMinCORSpeed,
711 Real impactSpeed)
const override 714 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
722 const Vec3 v = m_sphereOnPlane.getVelocityErrors(state);
723 return Vec2(v[0], v[1]);
727 Real defaultTransitionSpeed,
728 Real slipSpeed)
const override 731 (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
736 void getFrictionMultiplierIndices(
const State& s,
742 Real m_mu_s, m_mu_d, m_mu_v;
760 const Vec3& defaultCenterOnF,
761 Real defaultRadiusOnF,
763 const Vec3& defaultCenterOnB,
764 Real defaultRadiusOnB,
768 if (m_sphereOnSphere.isDisabled(state))
return false;
769 m_sphereOnSphere.disable(state);
774 if (!m_sphereOnSphere.isDisabled(state))
return false;
775 m_sphereOnSphere.enable(state);
780 return !m_sphereOnSphere.isDisabled(state);
784 Vec3 whereToDisplay(
const State& state)
const override;
788 Real getPerr(
const State& state)
const override;
793 {
return m_sphereOnSphere.getVelocityErrors(state)[2]; }
795 {
return m_sphereOnSphere.getAccelerationErrors(state)[2]; }
799 Real defaultCaptureSpeed,
800 Real defaultMinCORSpeed,
801 Real impactSpeed)
const override 804 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
812 const Vec3 v = m_sphereOnSphere.getVelocityErrors(state);
813 return Vec2(v[0], v[1]);
817 Real defaultTransitionSpeed,
818 Real slipSpeed)
const override 821 (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
826 void getFrictionMultiplierIndices(
const State& s,
832 Real m_mu_s, m_mu_d, m_mu_v;
857 Real defaultHalfLengthF,
860 Real defaultHalfLengthB,
864 if (m_lineOnLine.isDisabled(state))
return false;
865 m_lineOnLine.disable(state);
870 if (!m_lineOnLine.isDisabled(state))
return false;
871 m_lineOnLine.enable(state);
876 return !m_lineOnLine.isDisabled(state);
880 Vec3 whereToDisplay(
const State& state)
const override;
884 Real getPerr(
const State& state)
const override;
889 {
return m_lineOnLine.getVelocityErrors(state)[2]; }
891 {
return m_lineOnLine.getAccelerationErrors(state)[2]; }
895 Real defaultCaptureSpeed,
896 Real defaultMinCORSpeed,
897 Real impactSpeed)
const override 900 (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
908 const Vec3 v = m_lineOnLine.getVelocityErrors(state);
909 return Vec2(v[0], v[1]);
913 Real defaultTransitionSpeed,
914 Real slipSpeed)
const override 917 (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
922 void getFrictionMultiplierIndices(
const State& s,
928 Real m_mu_s, m_mu_d, m_mu_v;
935 #endif // SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
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
Include the header files that define each of the built-in constraint subclasses of class Constraint...
void setMyIndex(StateLimitedFrictionIndex fx)
Definition: ConditionalConstraint.h:320
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
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
StateLimitedFrictionIndex getMyIndex() const
Definition: ConditionalConstraint.h:321
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
Every Simbody header and source file should include this header before any other Simbody header...
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:606
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:280
bool disable(State &state) const override
Disable the normal and friction constraints if they were enabled.
Definition: ConditionalConstraint.h:347
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
Constrain a single mobilizer coordinate q to have a particular value.
Definition: Constraint.h:858
TODO: not implemented yet.
Definition: ConditionalConstraint.h:289
bool isEnabled(const State &state) const override
Return true if this contact is enabled.
Definition: ConditionalConstraint.h:471
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
UnilateralSpeedConstraint()
Definition: ConditionalConstraint.h:240
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
(Experimental – API will change – use at your own risk) Set a hard limit on the minimum value of a ...
Definition: ConditionalConstraint.h:400
(Experimental – API will change – use at your own risk) Set a hard upper limit on the separation be...
Definition: ConditionalConstraint.h:459
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: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.
virtual Vec3 getPositionInfo(const State &state) const
TODO: kludge needed because we're misusing existing constraints.
Definition: ConditionalConstraint.h:314
virtual ~StateLimitedFriction()
Definition: ConditionalConstraint.h:292
BoundedSpeedConstraint()
Definition: ConditionalConstraint.h:274
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
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:168
unsigned int sign(unsigned char u)
Definition: Scalar.h:311
(Experimental – API will change – use at your own risk) Set a hard limit on the maximum value of a ...
Definition: ConditionalConstraint.h:342
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
Unique integer type for Subsystem-local multiplier indexing.
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