Simbody  3.5
ConditionalConstraint.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
00002 #define SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                               Simbody(tm)                                  *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2014 Stanford University and the Authors.           *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00027 #include "SimTKmath.h"
00028 #include "simbody/internal/common.h"
00029 #include "simbody/internal/Constraint.h"
00030 #include "simbody/internal/Constraint_BuiltIns.h"
00031 #include "simbody/internal/MobilizedBody.h"
00032 
00033 namespace SimTK {
00034 
00038 class SimTK_SIMBODY_EXPORT ConditionalConstraint {
00039 public:
00040 
00047 static Real calcEffectiveCOR(Real minCOR, Real captureSpeed, Real minCORSpeed, 
00048                              Real impactSpeed)
00049 {
00050     assert(0 <= minCOR && minCOR <= 1);
00051     assert(0 <= captureSpeed && captureSpeed <= minCORSpeed);
00052     assert(impactSpeed >= 0);
00053 
00054     if (impactSpeed <= captureSpeed) return 0;
00055     if (impactSpeed >= minCORSpeed)  return minCOR;
00056     // captureSpeed < impactSpeed < minCORSpeed
00057     const Real slope = (1-minCOR) / minCORSpeed;
00058     return 1 - slope*impactSpeed;
00059 }
00060 
00066 static Real calcEffectiveCOF(Real mu_s, Real mu_d, Real mu_v,
00067                              Real transitionSpeed, Real slipSpeed)
00068 {
00069     assert(mu_s>=0 && mu_d>=0 && mu_v>=0);
00070     assert(mu_s>=mu_d);
00071     assert(transitionSpeed>=0 && slipSpeed>=0);
00072     const Real viscous = mu_v*slipSpeed; // typically zero
00073     return viscous + (slipSpeed <= transitionSpeed ? mu_s : mu_d);
00074 }
00075 
00076 };
00077 
00078 class UnilateralContact; // normal + friction
00079 class UnilateralSpeedConstraint;
00080 class BoundedSpeedConstraint;
00081 
00082 class ConstraintLimitedFriction;
00083 class StateLimitedFriction;
00084 
00085 //==============================================================================
00086 //                       UNILATERAL CONTACT CONSTRAINT
00087 //==============================================================================
00120 class SimTK_SIMBODY_EXPORT UnilateralContact {
00121 public:
00125     explicit UnilateralContact(int sign=1) : m_sign((Real)sign) 
00126     {   assert(sign==1 || sign==-1); }
00127 
00128     virtual ~UnilateralContact() {}
00129 
00131     Real getSignConvention() const {return m_sign;}
00132 
00135     virtual bool disable(State& state) const = 0;
00136 
00139     virtual bool enable(State& state) const = 0;
00140 
00142     virtual bool isEnabled(const State& state) const = 0;
00143 
00146     virtual Vec3 whereToDisplay(const State& state) const = 0;
00147 
00155     virtual Real calcEffectiveCOR(const State& state,
00156                                   Real defaultCaptureSpeed,
00157                                   Real defaultMinCORSpeed,
00158                                   Real impactSpeed) const = 0;
00159 
00163     virtual Real getPerr(const State& state) const = 0;
00166     virtual Real getVerr(const State& state) const = 0;
00169     virtual Real getAerr(const State& state) const = 0;
00170 
00175     virtual bool isProximal(const State& state, Real ptol) const 
00176     {   return m_sign*getPerr(state) <= ptol; }
00177 
00182     virtual MultiplierIndex 
00183     getContactMultiplierIndex(const State& state) const = 0;
00184 
00187     virtual bool hasFriction(const State& state) const {return false;}
00188 
00196     virtual Real calcEffectiveCOF(const State& state,
00197                                   Real defaultTransitionSpeed,
00198                                   Real slipSpeed) const
00199     {   return NaN; }
00200 
00201     virtual Vec2 getSlipVelocity(const State& state) const 
00202     {   return Vec2(NaN); }
00203 
00207     virtual void
00208     getFrictionMultiplierIndices(const State&       state, 
00209                                  MultiplierIndex&   ix_x, 
00210                                  MultiplierIndex&   ix_y) const
00211     {   ix_x.invalidate(); ix_y.invalidate(); }
00212 
00215     virtual Vec3 getPositionInfo(const State& state) const 
00216     {   return Vec3(NaN); }
00219     virtual void setInstanceParameter(State& state, const Vec3& pos) const {}
00220 
00221 
00222     void setMyIndex(UnilateralContactIndex cx) {m_myIx = cx;}
00223     UnilateralContactIndex getMyIndex() const {return m_myIx;}
00224 private:
00225     Real                    m_sign; // 1 or -1
00226     UnilateralContactIndex  m_myIx;
00227 };
00228 
00229 //==============================================================================
00230 //                       UNILATERAL SPEED CONSTRAINT
00231 //==============================================================================
00238 class SimTK_SIMBODY_EXPORT UnilateralSpeedConstraint {
00239 public:
00240     UnilateralSpeedConstraint() {}
00241     virtual ~UnilateralSpeedConstraint() {}
00242 
00243 private:
00244 };
00245 
00246 //==============================================================================
00247 //                         BOUNDED SPEED CONSTRAINT
00248 //==============================================================================
00272 class SimTK_SIMBODY_EXPORT BoundedSpeedConstraint {
00273 public:
00274     BoundedSpeedConstraint() {}
00275     virtual ~BoundedSpeedConstraint() {}
00276 
00281     virtual Vec2 calcEffectiveBounds(const State& state) const = 0;
00282 private:
00283 };
00284 
00285 //==============================================================================
00286 //                          STATE LIMITED FRICTION
00287 //==============================================================================
00289 class SimTK_SIMBODY_EXPORT StateLimitedFriction {
00290 public:
00291     StateLimitedFriction() {}
00292     virtual ~StateLimitedFriction() {}
00293 
00296     virtual bool disable(State& state) const = 0;
00297 
00300     virtual bool enable(State& state) const = 0;
00301 
00304     virtual Real getNormalForceMagnitude(const State& state) const = 0;
00305 
00306     virtual Real calcEffectiveCOF(const State& state,
00307                                   Real defaultTransitionSpeed,
00308                                   Real slipSpeed) const = 0;
00309 
00310     virtual Real getSlipSpeed(const State& state) const = 0; 
00311 
00314     virtual Vec3 getPositionInfo(const State& state) const 
00315     {   return Vec3(NaN); }
00318     virtual void setInstanceParameter(State& state, const Vec3& pos) const {}
00319 
00320     void setMyIndex(StateLimitedFrictionIndex fx) {m_myIx = fx;}
00321     StateLimitedFrictionIndex getMyIndex() const {return m_myIx;}
00322 private:
00323     StateLimitedFrictionIndex   m_myIx;
00324 };
00325 
00326 
00327 //==============================================================================
00328 //                              HARD STOP UPPER
00329 //==============================================================================
00342 class SimTK_SIMBODY_EXPORT HardStopUpper : public UnilateralContact {
00343 public:
00344     HardStopUpper(MobilizedBody& mobod, MobilizerQIndex whichQ,
00345              Real defaultUpperLimit, Real minCOR);
00346 
00347     bool disable(State& state) const OVERRIDE_11 
00348     {   if (m_upper.isDisabled(state)) return false;
00349         else {m_upper.disable(state); return true;} }
00350     bool enable(State& state) const OVERRIDE_11 
00351     {   if (!m_upper.isDisabled(state)) return false;
00352         else {m_upper.enable(state); return true;} }
00353     bool isEnabled(const State& state) const OVERRIDE_11 
00354     {   return !m_upper.isDisabled(state); }
00355 
00356     // Returns the contact point in the Ground frame.
00357     Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
00358 
00359     // Currently have to fake the perr because the constraint might be
00360     // disabled in which case it won't calculate perr. Also, we want 
00361     // negative to mean violated so may need to adjust the sign.
00362     Real getPerr(const State& state) const OVERRIDE_11;
00363     Real getVerr(const State& state) const OVERRIDE_11;
00364     Real getAerr(const State& state) const OVERRIDE_11;
00365 
00366     Real calcEffectiveCOR(const State& state,
00367                           Real defaultCaptureSpeed,
00368                           Real defaultMinCORSpeed,
00369                           Real impactSpeed) const OVERRIDE_11 
00370     {
00371        return ConditionalConstraint::calcEffectiveCOR
00372                (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
00373                 impactSpeed);
00374     }
00375 
00376     MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
00377 
00378 private:
00379     MobilizedBody                   m_mobod;
00380     Real                            m_defaultUpperLimit;
00381     Real                            m_minCOR;
00382     Constraint::ConstantCoordinate  m_upper;
00383 };
00384 
00385 //==============================================================================
00386 //                              HARD STOP LOWER
00387 //==============================================================================
00400 class SimTK_SIMBODY_EXPORT HardStopLower : public UnilateralContact {
00401 public:
00402     HardStopLower(MobilizedBody& mobod, MobilizerQIndex whichQ,
00403                   Real defaultLowerLimit, Real minCOR);
00404 
00405     bool disable(State& state) const OVERRIDE_11 
00406     {   if (m_lower.isDisabled(state)) return false;
00407         else {m_lower.disable(state); return true;} }
00408     bool enable(State& state) const OVERRIDE_11 
00409     {   if (!m_lower.isDisabled(state)) return false;
00410         else {m_lower.enable(state); return true;} }
00411     bool isEnabled(const State& state) const OVERRIDE_11 
00412     {   return !m_lower.isDisabled(state); }
00413 
00414     // Returns the contact point in the Ground frame.
00415     Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
00416 
00417     // Currently have to fake the perr because the constraint might be
00418     // disabled in which case it won't calculate perr. Also, we want 
00419     // negative to mean violated so may need to adjust the sign.
00420     Real getPerr(const State& state) const OVERRIDE_11;
00421     Real getVerr(const State& state) const OVERRIDE_11;
00422     Real getAerr(const State& state) const OVERRIDE_11;
00423 
00424     Real calcEffectiveCOR(const State& state,
00425                           Real defaultCaptureSpeed,
00426                           Real defaultMinCORSpeed,
00427                           Real impactSpeed) const OVERRIDE_11 
00428     {
00429        return ConditionalConstraint::calcEffectiveCOR
00430                (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
00431                 impactSpeed);
00432     }
00433 
00434     MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
00435 private:
00436     MobilizedBody                   m_mobod;
00437     Real                            m_defaultLowerLimit;
00438     Real                            m_minCOR;
00439     Constraint::ConstantCoordinate  m_lower;
00440 };
00441 
00442 
00443 
00444 //==============================================================================
00445 //                                  ROPE
00446 //==============================================================================
00459 class SimTK_SIMBODY_EXPORT Rope : public UnilateralContact {
00460 public:
00461     Rope(MobilizedBody& mobod1, const Vec3& point1, 
00462          MobilizedBody& mobod2, const Vec3& point2,
00463          Real defaultLengthLimit, Real minCOR);
00464 
00465     bool disable(State& state) const OVERRIDE_11 
00466     {   if (m_rod.isDisabled(state)) return false;
00467         else {m_rod.disable(state); return true;} }
00468     bool enable(State& state) const OVERRIDE_11 
00469     {   if (!m_rod.isDisabled(state)) return false;
00470         else {m_rod.enable(state); return true;} }
00471     bool isEnabled(const State& state) const OVERRIDE_11 
00472     {   return !m_rod.isDisabled(state); }
00473 
00474     // Returns half-way location in the Ground frame.
00475     Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
00476 
00477     // Currently have to fake the perr because the constraint might be
00478     // disabled in which case it won't calculate perr.
00479     Real getPerr(const State& state) const OVERRIDE_11;
00480     Real getVerr(const State& state) const OVERRIDE_11;
00481     Real getAerr(const State& state) const OVERRIDE_11;
00482 
00483     Real calcEffectiveCOR(const State& state,
00484                           Real defaultCaptureSpeed,
00485                           Real defaultMinCORSpeed,
00486                           Real impactSpeed) const OVERRIDE_11 
00487     {
00488        return ConditionalConstraint::calcEffectiveCOR
00489                (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
00490                 impactSpeed);
00491     }
00492 
00493     MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
00494 private:
00495     Real                            m_minCOR;
00496     Constraint::Rod                 m_rod;
00497 };
00498 
00499 //==============================================================================
00500 //                    POINT PLANE FRICTIONLESS CONTACT
00501 //==============================================================================
00506 class SimTK_SIMBODY_EXPORT PointPlaneFrictionlessContact 
00507 :   public UnilateralContact {
00508 public:
00509     PointPlaneFrictionlessContact(
00510         MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
00511         MobilizedBody& followerBodyF, const Vec3& point_F, Real minCOR);
00512 
00513     bool disable(State& state) const OVERRIDE_11 {
00514         if (m_ptInPlane.isDisabled(state)) return false;
00515         m_ptInPlane.disable(state);
00516         return true;
00517     }
00518 
00519     bool enable(State& state) const OVERRIDE_11 {
00520         if (!m_ptInPlane.isDisabled(state)) return false;
00521         m_ptInPlane.enable(state);
00522         return true;
00523     }
00524 
00525     bool isEnabled(const State& state) const OVERRIDE_11 {
00526         return !m_ptInPlane.isDisabled(state);
00527     }
00528 
00529     // Returns the contact point in the Ground frame.
00530     Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
00531 
00532     // Currently have to fake the perr because the constraint might be
00533     // disabled in which case it won't calculate perr.
00534     Real getPerr(const State& state) const OVERRIDE_11;
00535 
00536     // We won't need to look at these except for proximal constraints which
00537     // will already have been enabled, so no need to fake.
00538     Real getVerr(const State& state) const OVERRIDE_11
00539     {   return m_ptInPlane.getVelocityError(state); }
00540     Real getAerr(const State& state) const OVERRIDE_11
00541     {   return m_ptInPlane.getAccelerationError(state); }
00542 
00543 
00544     Real calcEffectiveCOR(const State& state,
00545                           Real defaultCaptureSpeed,
00546                           Real defaultMinCORSpeed,
00547                           Real impactSpeed) const OVERRIDE_11 
00548     {
00549        return ConditionalConstraint::calcEffectiveCOR
00550                (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
00551                 impactSpeed);
00552     }
00553 
00554     MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
00555 
00556 private:
00557     MobilizedBody               m_planeBody;    // body P
00558     const Rotation              m_frame;        // z is normal; expressed in P
00559     const Real                  m_height;
00560 
00561     MobilizedBody               m_follower;     // body F
00562     const Vec3                  m_point;        // measured & expressed in F
00563 
00564     Real                        m_minCOR;
00565 
00566     Constraint::PointInPlane    m_ptInPlane;
00567 };
00568 
00569 
00570 //==============================================================================
00571 //                          POINT PLANE CONTACT
00572 //==============================================================================
00578 class SimTK_SIMBODY_EXPORT PointPlaneContact : public UnilateralContact {
00579 public:
00580     PointPlaneContact(
00581         MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
00582         MobilizedBody& followerBodyF, const Vec3& point_F, 
00583         Real minCOR, Real mu_s, Real mu_d, Real mu_v);
00584 
00585     bool disable(State& state) const OVERRIDE_11 {
00586         if (m_ptInPlane.isDisabled(state)) return false;
00587         m_ptInPlane.disable(state);
00588         return true;
00589     }
00590 
00591     bool enable(State& state) const OVERRIDE_11 {
00592         if (!m_ptInPlane.isDisabled(state)) return false;
00593         m_ptInPlane.enable(state);
00594         return true;
00595     }
00596 
00597     bool isEnabled(const State& state) const OVERRIDE_11 {
00598         return !m_ptInPlane.isDisabled(state);
00599     }
00600 
00601     // Returns the contact point in the Ground frame.
00602     Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
00603 
00604     // Currently have to fake the perr because the constraint might be
00605     // disabled in which case it won't calculate perr.
00606     Real getPerr(const State& state) const OVERRIDE_11;
00607 
00608     // We won't need to look at these except for proximal constraints which
00609     // will already have been enabled, so no need to fake.
00610     Real getVerr(const State& state) const OVERRIDE_11
00611     {   return m_ptInPlane.getVelocityErrors(state)[2]; }
00612     Real getAerr(const State& state) const OVERRIDE_11
00613     {   return m_ptInPlane.getAccelerationErrors(state)[2]; }
00614 
00615 
00616     Real calcEffectiveCOR(const State& state,
00617                           Real defaultCaptureSpeed,
00618                           Real defaultMinCORSpeed,
00619                           Real impactSpeed) const OVERRIDE_11 
00620     {
00621        return ConditionalConstraint::calcEffectiveCOR
00622                (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
00623                 impactSpeed);
00624     }
00625 
00626     bool hasFriction(const State& state) const OVERRIDE_11
00627     {   return true; }
00628 
00629     Vec2 getSlipVelocity(const State& state) const  OVERRIDE_11 {
00630         const Vec3 v = m_ptInPlane.getVelocityErrors(state);
00631         return Vec2(v[0], v[1]);
00632     }
00633 
00634     Real calcEffectiveCOF(const State& state,
00635                           Real defaultTransitionSpeed,
00636                           Real slipSpeed) const OVERRIDE_11
00637     {
00638        return ConditionalConstraint::calcEffectiveCOF
00639                (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
00640     }
00641 
00642     MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
00643 
00644     void getFrictionMultiplierIndices(const State&     s, 
00645                                       MultiplierIndex& ix_x, 
00646                                       MultiplierIndex& ix_y) const OVERRIDE_11;
00647 
00648 private:
00649     MobilizedBody               m_planeBody;    // body P
00650     const Rotation              m_frame;        // z is normal; expressed in P
00651     const Real                  m_height;
00652 
00653     MobilizedBody               m_follower;     // body F
00654     const Vec3                  m_point;        // measured & expressed in F
00655 
00656     Real                        m_minCOR;
00657     Real                        m_mu_s, m_mu_d, m_mu_v;
00658 
00659     Constraint::PointOnPlaneContact m_ptInPlane;
00660 };
00661 
00662 //==============================================================================
00663 //                          SPHERE PLANE CONTACT
00664 //==============================================================================
00670 class SimTK_SIMBODY_EXPORT SpherePlaneContact : public UnilateralContact {
00671 public:
00672     SpherePlaneContact(
00673         MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
00674         MobilizedBody& followerBodyF, const Vec3& point_F, Real radius,
00675         Real minCOR, Real mu_s, Real mu_d, Real mu_v);
00676 
00677     bool disable(State& state) const OVERRIDE_11 {
00678         if (m_sphereOnPlane.isDisabled(state)) return false;
00679         m_sphereOnPlane.disable(state);
00680         return true;
00681     }
00682 
00683     bool enable(State& state) const OVERRIDE_11 {
00684         if (!m_sphereOnPlane.isDisabled(state)) return false;
00685         m_sphereOnPlane.enable(state);
00686         return true;
00687     }
00688 
00689     bool isEnabled(const State& state) const OVERRIDE_11 {
00690         return !m_sphereOnPlane.isDisabled(state);
00691     }
00692 
00693     // Returns the contact point in the Ground frame.
00694     Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
00695 
00696     // Currently have to fake the perr because the constraint might be
00697     // disabled in which case it won't calculate perr.
00698     Real getPerr(const State& state) const OVERRIDE_11;
00699 
00700     // We won't need to look at these except for proximal constraints which
00701     // will already have been enabled, so no need to fake.
00702     Real getVerr(const State& state) const OVERRIDE_11
00703     {   return m_sphereOnPlane.getVelocityErrors(state)[2]; }
00704     Real getAerr(const State& state) const OVERRIDE_11
00705     {   return m_sphereOnPlane.getAccelerationErrors(state)[2]; }
00706 
00707 
00708     Real calcEffectiveCOR(const State& state,
00709                           Real defaultCaptureSpeed,
00710                           Real defaultMinCORSpeed,
00711                           Real impactSpeed) const OVERRIDE_11 
00712     {
00713        return ConditionalConstraint::calcEffectiveCOR
00714                (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
00715                 impactSpeed);
00716     }
00717 
00718     bool hasFriction(const State& state) const OVERRIDE_11
00719     {   return true; }
00720 
00721     Vec2 getSlipVelocity(const State& state) const  OVERRIDE_11 {
00722         const Vec3 v = m_sphereOnPlane.getVelocityErrors(state);
00723         return Vec2(v[0], v[1]);
00724     }
00725 
00726     Real calcEffectiveCOF(const State& state,
00727                           Real defaultTransitionSpeed,
00728                           Real slipSpeed) const OVERRIDE_11
00729     {
00730        return ConditionalConstraint::calcEffectiveCOF
00731                (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
00732     }
00733 
00734     MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
00735 
00736     void getFrictionMultiplierIndices(const State&     s, 
00737                                       MultiplierIndex& ix_x, 
00738                                       MultiplierIndex& ix_y) const OVERRIDE_11;
00739 
00740 private:
00741     Real                        m_minCOR;
00742     Real                        m_mu_s, m_mu_d, m_mu_v;
00743 
00744     Constraint::SphereOnPlaneContact    m_sphereOnPlane;
00745 };
00746 
00747 //==============================================================================
00748 //                          SPHERE SPHERE CONTACT
00749 //==============================================================================
00756 class SimTK_SIMBODY_EXPORT SphereSphereContact : public UnilateralContact {
00757 public:
00758     SphereSphereContact(
00759         MobilizedBody&      mobod_F, 
00760         const Vec3&         defaultCenterOnF, 
00761         Real                defaultRadiusOnF, 
00762         MobilizedBody&      mobod_B, 
00763         const Vec3&         defaultCenterOnB,
00764         Real                defaultRadiusOnB,
00765         Real minCOR, Real mu_s, Real mu_d, Real mu_v);
00766 
00767     bool disable(State& state) const OVERRIDE_11 {
00768         if (m_sphereOnSphere.isDisabled(state)) return false;
00769         m_sphereOnSphere.disable(state);
00770         return true;
00771     }
00772 
00773     bool enable(State& state) const OVERRIDE_11 {
00774         if (!m_sphereOnSphere.isDisabled(state)) return false;
00775         m_sphereOnSphere.enable(state);
00776         return true;
00777     }
00778 
00779     bool isEnabled(const State& state) const OVERRIDE_11 {
00780         return !m_sphereOnSphere.isDisabled(state);
00781     }
00782 
00783     // Returns the contact point in the Ground frame.
00784     Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
00785 
00786     // Currently have to fake the perr because the constraint might be
00787     // disabled in which case it won't calculate perr.
00788     Real getPerr(const State& state) const OVERRIDE_11;
00789 
00790     // We won't need to look at these except for proximal constraints which
00791     // will already have been enabled, so no need to fake.
00792     Real getVerr(const State& state) const OVERRIDE_11
00793     {   return m_sphereOnSphere.getVelocityErrors(state)[2]; }
00794     Real getAerr(const State& state) const OVERRIDE_11
00795     {   return m_sphereOnSphere.getAccelerationErrors(state)[2]; }
00796 
00797 
00798     Real calcEffectiveCOR(const State& state,
00799                           Real defaultCaptureSpeed,
00800                           Real defaultMinCORSpeed,
00801                           Real impactSpeed) const OVERRIDE_11 
00802     {
00803        return ConditionalConstraint::calcEffectiveCOR
00804                (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
00805                 impactSpeed);
00806     }
00807 
00808     bool hasFriction(const State& state) const OVERRIDE_11
00809     {   return true; }
00810 
00811     Vec2 getSlipVelocity(const State& state) const  OVERRIDE_11 {
00812         const Vec3 v = m_sphereOnSphere.getVelocityErrors(state);
00813         return Vec2(v[0], v[1]);
00814     }
00815 
00816     Real calcEffectiveCOF(const State& state,
00817                           Real defaultTransitionSpeed,
00818                           Real slipSpeed) const OVERRIDE_11
00819     {
00820        return ConditionalConstraint::calcEffectiveCOF
00821                (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
00822     }
00823 
00824     MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
00825 
00826     void getFrictionMultiplierIndices(const State&     s, 
00827                                       MultiplierIndex& ix_x, 
00828                                       MultiplierIndex& ix_y) const OVERRIDE_11;
00829 
00830 private:
00831     Real                        m_minCOR;
00832     Real                        m_mu_s, m_mu_d, m_mu_v;
00833 
00834     Constraint::SphereOnSphereContact    m_sphereOnSphere;
00835 };
00836 
00837 
00838 //==============================================================================
00839 //                            EDGE EDGE CONTACT
00840 //==============================================================================
00852 class SimTK_SIMBODY_EXPORT EdgeEdgeContact : public UnilateralContact {
00853 public:
00854     EdgeEdgeContact(
00855         MobilizedBody&      mobod_F, 
00856         const Transform&    defaultEdgeFrameF, 
00857         Real                defaultHalfLengthF, 
00858         MobilizedBody&      mobod_B, 
00859         const Transform&    defaultEdgeFrameB, 
00860         Real                defaultHalfLengthB,
00861         Real minCOR, Real mu_s, Real mu_d, Real mu_v);
00862 
00863     bool disable(State& state) const OVERRIDE_11 {
00864         if (m_lineOnLine.isDisabled(state)) return false;
00865         m_lineOnLine.disable(state);
00866         return true;
00867     }
00868 
00869     bool enable(State& state) const OVERRIDE_11 {
00870         if (!m_lineOnLine.isDisabled(state)) return false;
00871         m_lineOnLine.enable(state);
00872         return true;
00873     }
00874 
00875     bool isEnabled(const State& state) const OVERRIDE_11 {
00876         return !m_lineOnLine.isDisabled(state);
00877     }
00878 
00879     // Returns the contact point in the Ground frame.
00880     Vec3 whereToDisplay(const State& state) const OVERRIDE_11;
00881 
00882     // Currently have to fake the perr because the constraint might be
00883     // disabled in which case it won't calculate perr.
00884     Real getPerr(const State& state) const OVERRIDE_11;
00885 
00886     // We won't need to look at these except for proximal constraints which
00887     // will already have been enabled, so no need to fake.
00888     Real getVerr(const State& state) const OVERRIDE_11
00889     {   return m_lineOnLine.getVelocityErrors(state)[2]; }
00890     Real getAerr(const State& state) const OVERRIDE_11
00891     {   return m_lineOnLine.getAccelerationErrors(state)[2]; }
00892 
00893 
00894     Real calcEffectiveCOR(const State& state,
00895                           Real defaultCaptureSpeed,
00896                           Real defaultMinCORSpeed,
00897                           Real impactSpeed) const OVERRIDE_11 
00898     {
00899        return ConditionalConstraint::calcEffectiveCOR
00900                (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
00901                 impactSpeed);
00902     }
00903 
00904     bool hasFriction(const State& state) const OVERRIDE_11
00905     {   return true; }
00906 
00907     Vec2 getSlipVelocity(const State& state) const  OVERRIDE_11 {
00908         const Vec3 v = m_lineOnLine.getVelocityErrors(state);
00909         return Vec2(v[0], v[1]);
00910     }
00911 
00912     Real calcEffectiveCOF(const State& state,
00913                           Real defaultTransitionSpeed,
00914                           Real slipSpeed) const OVERRIDE_11
00915     {
00916        return ConditionalConstraint::calcEffectiveCOF
00917                (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
00918     }
00919 
00920     MultiplierIndex getContactMultiplierIndex(const State& s) const OVERRIDE_11;
00921 
00922     void getFrictionMultiplierIndices(const State&     s, 
00923                                       MultiplierIndex& ix_x, 
00924                                       MultiplierIndex& ix_y) const OVERRIDE_11;
00925 
00926 private:
00927     Real                        m_minCOR;
00928     Real                        m_mu_s, m_mu_d, m_mu_v;
00929 
00930     Constraint::LineOnLineContact    m_lineOnLine;
00931 };
00932 
00933 } // namespace SimTK
00934 
00935 #endif // SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines