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