ConditionalConstraint.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
#define SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_
 
/* -------------------------------------------------------------------------- *
 *                               Simbody(tm)                                  *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2014 Stanford University and the Authors.           *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */
 
#include "SimTKmath.h"
#include "simbody/internal/common.h"
#include "simbody/internal/Constraint.h"
#include "simbody/internal/Constraint_BuiltIns.h"
#include "simbody/internal/MobilizedBody.h"
 
namespace SimTK {
 
class SimTK_SIMBODY_EXPORT ConditionalConstraint {
public:
 
static Real calcEffectiveCOR(Real minCOR, Real captureSpeed, Real minCORSpeed, 
                             Real impactSpeed)
{
    assert(0 <= minCOR && minCOR <= 1);
    assert(0 <= captureSpeed && captureSpeed <= minCORSpeed);
    assert(impactSpeed >= 0);
 
    if (impactSpeed <= captureSpeed) return 0;
    if (impactSpeed >= minCORSpeed)  return minCOR;
    // captureSpeed < impactSpeed < minCORSpeed
    const Real slope = (1-minCOR) / minCORSpeed;
    return 1 - slope*impactSpeed;
}
 
static Real calcEffectiveCOF(Real mu_s, Real mu_d, Real mu_v,
                             Real transitionSpeed, Real slipSpeed)
{
    assert(mu_s>=0 && mu_d>=0 && mu_v>=0);
    assert(mu_s>=mu_d);
    assert(transitionSpeed>=0 && slipSpeed>=0);
    const Real viscous = mu_v*slipSpeed; // typically zero
    return viscous + (slipSpeed <= transitionSpeed ? mu_s : mu_d);
}
 
};
 
class UnilateralContact; // normal + friction
class UnilateralSpeedConstraint;
class BoundedSpeedConstraint;
 
class ConstraintLimitedFriction;
class StateLimitedFriction;
 
//==============================================================================
//                       UNILATERAL CONTACT CONSTRAINT
//==============================================================================
class SimTK_SIMBODY_EXPORT UnilateralContact {
public:
    explicit UnilateralContact(int sign=1) : m_sign((Real)sign) 
    {   assert(sign==1 || sign==-1); }
 
    virtual ~UnilateralContact() {}
 
    Real getSignConvention() const {return m_sign;}
 
    virtual bool disable(State& state) const = 0;
 
    virtual bool enable(State& state) const = 0;
 
    virtual bool isEnabled(const State& state) const = 0;
 
    virtual Vec3 whereToDisplay(const State& state) const = 0;
 
    virtual Real calcEffectiveCOR(const State& state,
                                  Real defaultCaptureSpeed,
                                  Real defaultMinCORSpeed,
                                  Real impactSpeed) const = 0;
 
    virtual Real getPerr(const State& state) const = 0;
    virtual Real getVerr(const State& state) const = 0;
    virtual Real getAerr(const State& state) const = 0;
 
    virtual bool isProximal(const State& state, Real ptol) const 
    {   return m_sign*getPerr(state) <= ptol; }
 
    virtual MultiplierIndex 
    getContactMultiplierIndex(const State& state) const = 0;
 
    virtual bool hasFriction(const State& state) const {return false;}
 
    virtual Real calcEffectiveCOF(const State& state,
                                  Real defaultTransitionSpeed,
                                  Real slipSpeed) const
    {   return NaN; }
 
    virtual Vec2 getSlipVelocity(const State& state) const 
    {   return Vec2(NaN); }
 
    virtual void
    getFrictionMultiplierIndices(const State&       state, 
                                 MultiplierIndex&   ix_x, 
                                 MultiplierIndex&   ix_y) const
    {   ix_x.invalidate(); ix_y.invalidate(); }
 
    virtual Vec3 getPositionInfo(const State& state) const 
    {   return Vec3(NaN); }
    virtual void setInstanceParameter(State& state, const Vec3& pos) const {}
 
 
    void setMyIndex(UnilateralContactIndex cx) {m_myIx = cx;}
    UnilateralContactIndex getMyIndex() const {return m_myIx;}
private:
    Real                    m_sign; // 1 or -1
    UnilateralContactIndex  m_myIx;
};
 
//==============================================================================
//                       UNILATERAL SPEED CONSTRAINT
//==============================================================================
class SimTK_SIMBODY_EXPORT UnilateralSpeedConstraint {
public:
    UnilateralSpeedConstraint() {}
    virtual ~UnilateralSpeedConstraint() {}
 
private:
};
 
//==============================================================================
//                         BOUNDED SPEED CONSTRAINT
//==============================================================================
class SimTK_SIMBODY_EXPORT BoundedSpeedConstraint {
public:
    BoundedSpeedConstraint() {}
    virtual ~BoundedSpeedConstraint() {}
 
    virtual Vec2 calcEffectiveBounds(const State& state) const = 0;
private:
};
 
//==============================================================================
//                          STATE LIMITED FRICTION
//==============================================================================
class SimTK_SIMBODY_EXPORT StateLimitedFriction {
public:
    StateLimitedFriction() {}
    virtual ~StateLimitedFriction() {}
 
    virtual bool disable(State& state) const = 0;
 
    virtual bool enable(State& state) const = 0;
 
    virtual Real getNormalForceMagnitude(const State& state) const = 0;
 
    virtual Real calcEffectiveCOF(const State& state,
                                  Real defaultTransitionSpeed,
                                  Real slipSpeed) const = 0;
 
    virtual Real getSlipSpeed(const State& state) const = 0; 
 
    virtual Vec3 getPositionInfo(const State& state) const 
    {   return Vec3(NaN); }
    virtual void setInstanceParameter(State& state, const Vec3& pos) const {}
 
    void setMyIndex(StateLimitedFrictionIndex fx) {m_myIx = fx;}
    StateLimitedFrictionIndex getMyIndex() const {return m_myIx;}
private:
    StateLimitedFrictionIndex   m_myIx;
};
 
 
//==============================================================================
//                              HARD STOP UPPER
//==============================================================================
class SimTK_SIMBODY_EXPORT HardStopUpper : public UnilateralContact {
public:
    HardStopUpper(MobilizedBody& mobod, MobilizerQIndex whichQ,
             Real defaultUpperLimit, Real minCOR);
 
    bool disable(State& state) const override 
    {   if (m_upper.isDisabled(state)) return false;
        else {m_upper.disable(state); return true;} }
    bool enable(State& state) const override 
    {   if (!m_upper.isDisabled(state)) return false;
        else {m_upper.enable(state); return true;} }
    bool isEnabled(const State& state) const override 
    {   return !m_upper.isDisabled(state); }
 
    // Returns the contact point in the Ground frame.
    Vec3 whereToDisplay(const State& state) const override;
 
    // Currently have to fake the perr because the constraint might be
    // disabled in which case it won't calculate perr. Also, we want 
    // negative to mean violated so may need to adjust the sign.
    Real getPerr(const State& state) const override;
    Real getVerr(const State& state) const override;
    Real getAerr(const State& state) const override;
 
    Real calcEffectiveCOR(const State& state,
                          Real defaultCaptureSpeed,
                          Real defaultMinCORSpeed,
                          Real impactSpeed) const override 
    {
       return ConditionalConstraint::calcEffectiveCOR
               (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
                impactSpeed);
    }
 
    MultiplierIndex getContactMultiplierIndex(const State& s) const override;
 
private:
    MobilizedBody                   m_mobod;
    Real                            m_defaultUpperLimit;
    Real                            m_minCOR;
    Constraint::ConstantCoordinate  m_upper;
};
 
//==============================================================================
//                              HARD STOP LOWER
//==============================================================================
class SimTK_SIMBODY_EXPORT HardStopLower : public UnilateralContact {
public:
    HardStopLower(MobilizedBody& mobod, MobilizerQIndex whichQ,
                  Real defaultLowerLimit, Real minCOR);
 
    bool disable(State& state) const override 
    {   if (m_lower.isDisabled(state)) return false;
        else {m_lower.disable(state); return true;} }
    bool enable(State& state) const override 
    {   if (!m_lower.isDisabled(state)) return false;
        else {m_lower.enable(state); return true;} }
    bool isEnabled(const State& state) const override 
    {   return !m_lower.isDisabled(state); }
 
    // Returns the contact point in the Ground frame.
    Vec3 whereToDisplay(const State& state) const override;
 
    // Currently have to fake the perr because the constraint might be
    // disabled in which case it won't calculate perr. Also, we want 
    // negative to mean violated so may need to adjust the sign.
    Real getPerr(const State& state) const override;
    Real getVerr(const State& state) const override;
    Real getAerr(const State& state) const override;
 
    Real calcEffectiveCOR(const State& state,
                          Real defaultCaptureSpeed,
                          Real defaultMinCORSpeed,
                          Real impactSpeed) const override 
    {
       return ConditionalConstraint::calcEffectiveCOR
               (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
                impactSpeed);
    }
 
    MultiplierIndex getContactMultiplierIndex(const State& s) const override;
private:
    MobilizedBody                   m_mobod;
    Real                            m_defaultLowerLimit;
    Real                            m_minCOR;
    Constraint::ConstantCoordinate  m_lower;
};
 
 
 
//==============================================================================
//                                  ROPE
//==============================================================================
class SimTK_SIMBODY_EXPORT Rope : public UnilateralContact {
public:
    Rope(MobilizedBody& mobod1, const Vec3& point1, 
         MobilizedBody& mobod2, const Vec3& point2,
         Real defaultLengthLimit, Real minCOR);
 
    bool disable(State& state) const override 
    {   if (m_rod.isDisabled(state)) return false;
        else {m_rod.disable(state); return true;} }
    bool enable(State& state) const override 
    {   if (!m_rod.isDisabled(state)) return false;
        else {m_rod.enable(state); return true;} }
    bool isEnabled(const State& state) const override 
    {   return !m_rod.isDisabled(state); }
 
    // Returns half-way location in the Ground frame.
    Vec3 whereToDisplay(const State& state) const override;
 
    // Currently have to fake the perr because the constraint might be
    // disabled in which case it won't calculate perr.
    Real getPerr(const State& state) const override;
    Real getVerr(const State& state) const override;
    Real getAerr(const State& state) const override;
 
    Real calcEffectiveCOR(const State& state,
                          Real defaultCaptureSpeed,
                          Real defaultMinCORSpeed,
                          Real impactSpeed) const override 
    {
       return ConditionalConstraint::calcEffectiveCOR
               (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
                impactSpeed);
    }
 
    MultiplierIndex getContactMultiplierIndex(const State& s) const override;
private:
    Real                            m_minCOR;
    Constraint::Rod                 m_rod;
};
 
//==============================================================================
//                    POINT PLANE FRICTIONLESS CONTACT
//==============================================================================
class SimTK_SIMBODY_EXPORT PointPlaneFrictionlessContact 
:   public UnilateralContact {
public:
    PointPlaneFrictionlessContact(
        MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
        MobilizedBody& followerBodyF, const Vec3& point_F, Real minCOR);
 
    bool disable(State& state) const override {
        if (m_ptInPlane.isDisabled(state)) return false;
        m_ptInPlane.disable(state);
        return true;
    }
 
    bool enable(State& state) const override {
        if (!m_ptInPlane.isDisabled(state)) return false;
        m_ptInPlane.enable(state);
        return true;
    }
 
    bool isEnabled(const State& state) const override {
        return !m_ptInPlane.isDisabled(state);
    }
 
    // Returns the contact point in the Ground frame.
    Vec3 whereToDisplay(const State& state) const override;
 
    // Currently have to fake the perr because the constraint might be
    // disabled in which case it won't calculate perr.
    Real getPerr(const State& state) const override;
 
    // We won't need to look at these except for proximal constraints which
    // will already have been enabled, so no need to fake.
    Real getVerr(const State& state) const override
    {   return m_ptInPlane.getVelocityError(state); }
    Real getAerr(const State& state) const override
    {   return m_ptInPlane.getAccelerationError(state); }
 
 
    Real calcEffectiveCOR(const State& state,
                          Real defaultCaptureSpeed,
                          Real defaultMinCORSpeed,
                          Real impactSpeed) const override 
    {
       return ConditionalConstraint::calcEffectiveCOR
               (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
                impactSpeed);
    }
 
    MultiplierIndex getContactMultiplierIndex(const State& s) const override;
 
private:
    MobilizedBody               m_planeBody;    // body P
    const Rotation              m_frame;        // z is normal; expressed in P
    const Real                  m_height;
 
    MobilizedBody               m_follower;     // body F
    const Vec3                  m_point;        // measured & expressed in F
 
    Real                        m_minCOR;
 
    Constraint::PointInPlane    m_ptInPlane;
};
 
 
//==============================================================================
//                          POINT PLANE CONTACT
//==============================================================================
class SimTK_SIMBODY_EXPORT PointPlaneContact : public UnilateralContact {
public:
    PointPlaneContact(
        MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
        MobilizedBody& followerBodyF, const Vec3& point_F, 
        Real minCOR, Real mu_s, Real mu_d, Real mu_v);
 
    bool disable(State& state) const override {
        if (m_ptInPlane.isDisabled(state)) return false;
        m_ptInPlane.disable(state);
        return true;
    }
 
    bool enable(State& state) const override {
        if (!m_ptInPlane.isDisabled(state)) return false;
        m_ptInPlane.enable(state);
        return true;
    }
 
    bool isEnabled(const State& state) const override {
        return !m_ptInPlane.isDisabled(state);
    }
 
    // Returns the contact point in the Ground frame.
    Vec3 whereToDisplay(const State& state) const override;
 
    // Currently have to fake the perr because the constraint might be
    // disabled in which case it won't calculate perr.
    Real getPerr(const State& state) const override;
 
    // We won't need to look at these except for proximal constraints which
    // will already have been enabled, so no need to fake.
    Real getVerr(const State& state) const override
    {   return m_ptInPlane.getVelocityErrors(state)[2]; }
    Real getAerr(const State& state) const override
    {   return m_ptInPlane.getAccelerationErrors(state)[2]; }
 
 
    Real calcEffectiveCOR(const State& state,
                          Real defaultCaptureSpeed,
                          Real defaultMinCORSpeed,
                          Real impactSpeed) const override 
    {
       return ConditionalConstraint::calcEffectiveCOR
               (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
                impactSpeed);
    }
 
    bool hasFriction(const State& state) const override
    {   return true; }
 
    Vec2 getSlipVelocity(const State& state) const  override {
        const Vec3 v = m_ptInPlane.getVelocityErrors(state);
        return Vec2(v[0], v[1]);
    }
 
    Real calcEffectiveCOF(const State& state,
                          Real defaultTransitionSpeed,
                          Real slipSpeed) const override
    {
       return ConditionalConstraint::calcEffectiveCOF
               (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
    }
 
    MultiplierIndex getContactMultiplierIndex(const State& s) const override;
 
    void getFrictionMultiplierIndices(const State&     s, 
                                      MultiplierIndex& ix_x, 
                                      MultiplierIndex& ix_y) const override;
 
private:
    MobilizedBody               m_planeBody;    // body P
    const Rotation              m_frame;        // z is normal; expressed in P
    const Real                  m_height;
 
    MobilizedBody               m_follower;     // body F
    const Vec3                  m_point;        // measured & expressed in F
 
    Real                        m_minCOR;
    Real                        m_mu_s, m_mu_d, m_mu_v;
 
    Constraint::PointOnPlaneContact m_ptInPlane;
};
 
//==============================================================================
//                          SPHERE PLANE CONTACT
//==============================================================================
class SimTK_SIMBODY_EXPORT SpherePlaneContact : public UnilateralContact {
public:
    SpherePlaneContact(
        MobilizedBody& planeBodyB, const UnitVec3& normal_B, Real height,
        MobilizedBody& followerBodyF, const Vec3& point_F, Real radius,
        Real minCOR, Real mu_s, Real mu_d, Real mu_v);
 
    bool disable(State& state) const override {
        if (m_sphereOnPlane.isDisabled(state)) return false;
        m_sphereOnPlane.disable(state);
        return true;
    }
 
    bool enable(State& state) const override {
        if (!m_sphereOnPlane.isDisabled(state)) return false;
        m_sphereOnPlane.enable(state);
        return true;
    }
 
    bool isEnabled(const State& state) const override {
        return !m_sphereOnPlane.isDisabled(state);
    }
 
    // Returns the contact point in the Ground frame.
    Vec3 whereToDisplay(const State& state) const override;
 
    // Currently have to fake the perr because the constraint might be
    // disabled in which case it won't calculate perr.
    Real getPerr(const State& state) const override;
 
    // We won't need to look at these except for proximal constraints which
    // will already have been enabled, so no need to fake.
    Real getVerr(const State& state) const override
    {   return m_sphereOnPlane.getVelocityErrors(state)[2]; }
    Real getAerr(const State& state) const override
    {   return m_sphereOnPlane.getAccelerationErrors(state)[2]; }
 
 
    Real calcEffectiveCOR(const State& state,
                          Real defaultCaptureSpeed,
                          Real defaultMinCORSpeed,
                          Real impactSpeed) const override 
    {
       return ConditionalConstraint::calcEffectiveCOR
               (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
                impactSpeed);
    }
 
    bool hasFriction(const State& state) const override
    {   return true; }
 
    Vec2 getSlipVelocity(const State& state) const  override {
        const Vec3 v = m_sphereOnPlane.getVelocityErrors(state);
        return Vec2(v[0], v[1]);
    }
 
    Real calcEffectiveCOF(const State& state,
                          Real defaultTransitionSpeed,
                          Real slipSpeed) const override
    {
       return ConditionalConstraint::calcEffectiveCOF
               (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
    }
 
    MultiplierIndex getContactMultiplierIndex(const State& s) const override;
 
    void getFrictionMultiplierIndices(const State&     s, 
                                      MultiplierIndex& ix_x, 
                                      MultiplierIndex& ix_y) const override;
 
private:
    Real                        m_minCOR;
    Real                        m_mu_s, m_mu_d, m_mu_v;
 
    Constraint::SphereOnPlaneContact    m_sphereOnPlane;
};
 
//==============================================================================
//                          SPHERE SPHERE CONTACT
//==============================================================================
class SimTK_SIMBODY_EXPORT SphereSphereContact : public UnilateralContact {
public:
    SphereSphereContact(
        MobilizedBody&      mobod_F, 
        const Vec3&         defaultCenterOnF, 
        Real                defaultRadiusOnF, 
        MobilizedBody&      mobod_B, 
        const Vec3&         defaultCenterOnB,
        Real                defaultRadiusOnB,
        Real minCOR, Real mu_s, Real mu_d, Real mu_v);
 
    bool disable(State& state) const override {
        if (m_sphereOnSphere.isDisabled(state)) return false;
        m_sphereOnSphere.disable(state);
        return true;
    }
 
    bool enable(State& state) const override {
        if (!m_sphereOnSphere.isDisabled(state)) return false;
        m_sphereOnSphere.enable(state);
        return true;
    }
 
    bool isEnabled(const State& state) const override {
        return !m_sphereOnSphere.isDisabled(state);
    }
 
    // Returns the contact point in the Ground frame.
    Vec3 whereToDisplay(const State& state) const override;
 
    // Currently have to fake the perr because the constraint might be
    // disabled in which case it won't calculate perr.
    Real getPerr(const State& state) const override;
 
    // We won't need to look at these except for proximal constraints which
    // will already have been enabled, so no need to fake.
    Real getVerr(const State& state) const override
    {   return m_sphereOnSphere.getVelocityErrors(state)[2]; }
    Real getAerr(const State& state) const override
    {   return m_sphereOnSphere.getAccelerationErrors(state)[2]; }
 
 
    Real calcEffectiveCOR(const State& state,
                          Real defaultCaptureSpeed,
                          Real defaultMinCORSpeed,
                          Real impactSpeed) const override 
    {
       return ConditionalConstraint::calcEffectiveCOR
               (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
                impactSpeed);
    }
 
    bool hasFriction(const State& state) const override
    {   return true; }
 
    Vec2 getSlipVelocity(const State& state) const  override {
        const Vec3 v = m_sphereOnSphere.getVelocityErrors(state);
        return Vec2(v[0], v[1]);
    }
 
    Real calcEffectiveCOF(const State& state,
                          Real defaultTransitionSpeed,
                          Real slipSpeed) const override
    {
       return ConditionalConstraint::calcEffectiveCOF
               (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
    }
 
    MultiplierIndex getContactMultiplierIndex(const State& s) const override;
 
    void getFrictionMultiplierIndices(const State&     s, 
                                      MultiplierIndex& ix_x, 
                                      MultiplierIndex& ix_y) const override;
 
private:
    Real                        m_minCOR;
    Real                        m_mu_s, m_mu_d, m_mu_v;
 
    Constraint::SphereOnSphereContact    m_sphereOnSphere;
};
 
 
//==============================================================================
//                            EDGE EDGE CONTACT
//==============================================================================
class SimTK_SIMBODY_EXPORT EdgeEdgeContact : public UnilateralContact {
public:
    EdgeEdgeContact(
        MobilizedBody&      mobod_F, 
        const Transform&    defaultEdgeFrameF, 
        Real                defaultHalfLengthF, 
        MobilizedBody&      mobod_B, 
        const Transform&    defaultEdgeFrameB, 
        Real                defaultHalfLengthB,
        Real minCOR, Real mu_s, Real mu_d, Real mu_v);
 
    bool disable(State& state) const override {
        if (m_lineOnLine.isDisabled(state)) return false;
        m_lineOnLine.disable(state);
        return true;
    }
 
    bool enable(State& state) const override {
        if (!m_lineOnLine.isDisabled(state)) return false;
        m_lineOnLine.enable(state);
        return true;
    }
 
    bool isEnabled(const State& state) const override {
        return !m_lineOnLine.isDisabled(state);
    }
 
    // Returns the contact point in the Ground frame.
    Vec3 whereToDisplay(const State& state) const override;
 
    // Currently have to fake the perr because the constraint might be
    // disabled in which case it won't calculate perr.
    Real getPerr(const State& state) const override;
 
    // We won't need to look at these except for proximal constraints which
    // will already have been enabled, so no need to fake.
    Real getVerr(const State& state) const override
    {   return m_lineOnLine.getVelocityErrors(state)[2]; }
    Real getAerr(const State& state) const override
    {   return m_lineOnLine.getAccelerationErrors(state)[2]; }
 
 
    Real calcEffectiveCOR(const State& state,
                          Real defaultCaptureSpeed,
                          Real defaultMinCORSpeed,
                          Real impactSpeed) const override 
    {
       return ConditionalConstraint::calcEffectiveCOR
               (m_minCOR, defaultCaptureSpeed, defaultMinCORSpeed,
                impactSpeed);
    }
 
    bool hasFriction(const State& state) const override
    {   return true; }
 
    Vec2 getSlipVelocity(const State& state) const  override {
        const Vec3 v = m_lineOnLine.getVelocityErrors(state);
        return Vec2(v[0], v[1]);
    }
 
    Real calcEffectiveCOF(const State& state,
                          Real defaultTransitionSpeed,
                          Real slipSpeed) const override
    {
       return ConditionalConstraint::calcEffectiveCOF
               (m_mu_s, m_mu_d, m_mu_v, defaultTransitionSpeed, slipSpeed);
    }
 
    MultiplierIndex getContactMultiplierIndex(const State& s) const override;
 
    void getFrictionMultiplierIndices(const State&     s, 
                                      MultiplierIndex& ix_x, 
                                      MultiplierIndex& ix_y) const override;
 
private:
    Real                        m_minCOR;
    Real                        m_mu_s, m_mu_d, m_mu_v;
 
    Constraint::LineOnLineContact    m_lineOnLine;
};
 
} // namespace SimTK
 
#endif // SimTK_SIMBODY_CONDITIONAL_CONSTRAINT_H_