Constraint.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_CONSTRAINT_H_
#define SimTK_SIMBODY_CONSTRAINT_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simbody(tm)                         *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2007-9 Stanford University and the Authors.         *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKmath.h"
#include "simbody/internal/common.h"

#include <cassert>

namespace SimTK {

class SimbodyMatterSubsystem;
class SimbodyMatterSubtree;
class MobilizedBody;
class Constraint;
class ConstraintImpl;

// We only want the template instantiation to occur once. This symbol is 
// defined in the SimTK core compilation unit that defines the Constraint 
// class but should not be defined any other time.
#ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT
    extern template class PIMPLHandle<Constraint, ConstraintImpl, true>;
#endif

    // CONSTRAINT BASE CLASS //

class SimTK_SIMBODY_EXPORT Constraint : public PIMPLHandle<Constraint, ConstraintImpl, true> {
public:
    Constraint() { }
    explicit Constraint(ConstraintImpl* r) : HandleBase(r) { }

    void disable(State&) const;
    void enable(State&) const;
    bool isDisabled(const State&) const;
    bool isDisabledByDefault() const;

    void setDisabledByDefault(bool shouldBeDisabled);

    // Implicit conversion to ConstraintIndex when needed.
    operator ConstraintIndex() const {return getConstraintIndex();}

    // These will fail unless this Constraint is owned by a MatterSubsystem.
    ConstraintIndex               getConstraintIndex()      const;
    const SimbodyMatterSubsystem& getMatterSubsystem()      const;
    SimbodyMatterSubsystem&       updMatterSubsystem();

    bool isInSubsystem() const;
    bool isInSameSubsystem(const MobilizedBody&) const;

        // TOPOLOGY STAGE (i.e., post-construction) //

    int getNumConstrainedBodies() const;

    const MobilizedBody& 
        getMobilizedBodyFromConstrainedBody(ConstrainedBodyIndex) const;

    const MobilizedBody& getAncestorMobilizedBody() const;

    int getNumConstrainedMobilizers() const;

    const MobilizedBody& 
        getMobilizedBodyFromConstrainedMobilizer(ConstrainedMobilizerIndex) const;

    const SimbodyMatterSubtree& getSubtree() const;

        // MODEL STAGE //

    int getNumConstrainedQ(const State&, ConstrainedMobilizerIndex) const;
    int getNumConstrainedU(const State&, ConstrainedMobilizerIndex) const;

    ConstrainedUIndex getConstrainedUIndex
        (const State&, ConstrainedMobilizerIndex, MobilizerUIndex which) const;
    ConstrainedQIndex getConstrainedQIndex
        (const State&, ConstrainedMobilizerIndex, MobilizerQIndex which) const;

    int getNumConstrainedQ(const State&) const;

    int getNumConstrainedU(const State&) const;
    
    void getNumConstraintEquationsInUse(const State&, int& mp, int& mv, int& ma) const;

        // INSTANCE STAGE //
    // nothing in base class currently

        // POSITION STAGE //
    Vector getPositionErrorsAsVector(const State&) const;   // mp of these
    Vector calcPositionErrorFromQ(const State&, const Vector& q) const;

    // Matrix P = partial(perr_dot)/partial(u). (just the holonomic constraints)
    Matrix calcPositionConstraintMatrixP(const State&) const; // mp X nu
    Matrix calcPositionConstraintMatrixPt(const State&) const; // nu X mp

    // Matrix PNInv = partial(perr)/partial(q) = P*N^-1
    Matrix calcPositionConstraintMatrixPNInv(const State&) const; // mp X nq

    void calcConstraintForcesFromMultipliers(const State&,
        const Vector&        lambda,                // mp+mv+ma of these
        Vector_<SpatialVec>& bodyForcesInA,         // numConstrainedBodies
        Vector&              mobilityForces) const; // numConstrainedU

        // VELOCITY STAGE //
    Vector getVelocityErrorsAsVector(const State&) const;   // mp+mv of these
    Vector calcVelocityErrorFromU(const State&,     // mp+mv of these
                                  const Vector& u) const;   // numParticipatingU u's

    // Matrix V = partial(verr)/partial(u) for just the non-holonomic 
    // constraints.
    Matrix calcVelocityConstraintMatrixV(const State&) const;  // mv X nu
    Matrix calcVelocityConstraintMatrixVt(const State&) const; // nu X mv

        // DYNAMICS STAGE //
    // nothing in base class currently

        // ACCELERATION STAGE //
    Vector getAccelerationErrorsAsVector(const State&) const;   // mp+mv+ma of these
    Vector calcAccelerationErrorFromUDot(const State&,  // mp+mv+ma of these
                                         const Vector& udot) const; // numParticipatingU udot's

    Vector getMultipliersAsVector(const State&) const;  // mp+mv+ma of these   

    // Matrix A = partial(aerr)/partial(udot) for just the acceleration-only 
    // constraints.
    Matrix calcAccelerationConstraintMatrixA(const State&) const;  // ma X nu
    Matrix calcAccelerationConstraintMatrixAt(const State&) const; // nu X ma

    // These are the built-in Constraint types. Types on the same line are
    // synonymous.
    class Rod;  typedef Rod  ConstantDistance;
    class Ball; typedef Ball CoincidentPoints;
    class Weld; typedef Weld CoincidentFrames;
    class PointInPlane;  // translations perpendicular to plane normal only
    class PointOnLine;   // translations along a line only
    class ConstantAngle; // prevent rotation about common normal of two vectors
    class ConstantOrientation; // allows any translation but no rotation
    class NoSlip1D; // same velocity at a point along a direction
    class ConstantSpeed; // prescribe generalized speed value
    class ConstantAcceleration; // prescribe generalized acceleration value
    class Custom;
    class CoordinateCoupler;
    class SpeedCoupler;
    class PrescribedMotion;

    class RodImpl;
    class BallImpl;
    class WeldImpl;
    class PointInPlaneImpl;
    class PointOnLineImpl;
    class ConstantAngleImpl;
    class ConstantOrientationImpl;
    class NoSlip1DImpl;
    class ConstantSpeedImpl;
    class ConstantAccelerationImpl;
    class CustomImpl;
    class CoordinateCouplerImpl;
    class SpeedCouplerImpl;
    class PrescribedMotionImpl;
};

    // ROD (CONSTANT DISTANCE) CONSTRAINT //

 class SimTK_SIMBODY_EXPORT Constraint::Rod : public Constraint {
public:
    // no default constructor
    Rod(MobilizedBody& body1, MobilizedBody& body2,
        Real defaultLength=1);
    Rod(MobilizedBody& body1, const Vec3& defaultPoint1,
        MobilizedBody& body2, const Vec3& defaultPoint2,
        Real defaultLength=1);

    // Defaults for Instance variables.
    Rod& setDefaultPointOnBody1(const Vec3&);
    Rod& setDefaultPointOnBody2(const Vec3&);
    Rod& setDefaultRodLength(Real);

    // Stage::Topology
    MobilizedBodyIndex getBody1MobilizedBodyIndex() const;
    MobilizedBodyIndex getBody2MobilizedBodyIndex() const;
    const Vec3& getDefaultPointOnBody1() const;
    const Vec3& getDefaultPointOnBody2() const;
    Real getDefaultRodLength() const;

    // Stage::Instance
    const Vec3& getPointOnBody1(const State&) const;
    const Vec3& getPointOnBody2(const State&) const;
    Real        getRodLength   (const State&) const;

    // Stage::Position, Velocity, Acceleration
    Real getPositionError(const State&) const;
    Real getVelocityError(const State&) const;

    // Stage::Acceleration
    Real getAccelerationError(const State&) const;
    Real getMultiplier(const State&) const;
    Real getRodTension(const State&) const; // negative means compression
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Rod, RodImpl, Constraint);
};

    // POINT IN PLANE CONSTRAINT //

class SimTK_SIMBODY_EXPORT Constraint::PointInPlane : public Constraint  {
public:
    // no default constructor
    PointInPlane(MobilizedBody& planeBody_B, const UnitVec3& defaultPlaneNormal_B, Real defaultHeight,
                 MobilizedBody& followerBody_F, const Vec3& defaultFollowerPoint_F);

    // These affect only generated decorative geometry for visualization;
    // the plane is really infinite in extent with zero depth and the
    // point is really of zero radius.
    PointInPlane& setPlaneDisplayHalfWidth(Real);
    PointInPlane& setPointDisplayRadius(Real);
    Real getPlaneDisplayHalfWidth() const;
    Real getPointDisplayRadius() const;

    // Defaults for Instance variables.
    PointInPlane& setDefaultPlaneNormal(const UnitVec3&);
    PointInPlane& setDefaultPlaneHeight(Real);
    PointInPlane& setDefaultFollowerPoint(const Vec3&);

    // Stage::Topology
    MobilizedBodyIndex getPlaneMobilizedBodyIndex() const;
    MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;

    const UnitVec3& getDefaultPlaneNormal() const;
    Real            getDefaultPlaneHeight() const;
    const Vec3&     getDefaultFollowerPoint() const;

    // Stage::Instance
    const UnitVec3& getPlaneNormal(const State&) const;
    Real            getPlaneHeight(const State&) const;
    const Vec3&     getFollowerPoint(const State&) const;

    // Stage::Position, Velocity
    Real getPositionError(const State&) const;
    Real getVelocityError(const State&) const;

    // Stage::Acceleration
    Real getAccelerationError(const State&) const;
    Real getMultiplier(const State&) const;
    Real getForceOnFollowerPoint(const State&) const; // in normal direction
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(PointInPlane, PointInPlaneImpl, Constraint);
};

    // POINT ON LINE CONSTRAINT //

class SimTK_SIMBODY_EXPORT Constraint::PointOnLine : public Constraint  {
public:
    // no default constructor
    PointOnLine(MobilizedBody& lineBody_B, const UnitVec3& defaultLineDirection_B, const Vec3& defaultPointOnLine_B,
                MobilizedBody& followerBody_F, const Vec3& defaultFollowerPoint_F);

    // These affect only generated decorative geometry for visualization;
    // the line is really infinite in extent and the
    // point is really of zero radius.
    PointOnLine& setLineDisplayHalfLength(Real);
    PointOnLine& setPointDisplayRadius(Real);
    Real getLineDisplayHalfLength() const;
    Real getPointDisplayRadius() const;

    // Defaults for Instance variables.
    PointOnLine& setDefaultLineDirection(const UnitVec3&);
    PointOnLine& setDefaultPointOnLine(const Vec3&);
    PointOnLine& setDefaultFollowerPoint(const Vec3&);

    // Stage::Topology
    MobilizedBodyIndex getLineMobilizedBodyIndex() const;
    MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;

    const UnitVec3& getDefaultLineDirection() const;
    const Vec3&     getDefaultPointOnLine() const;
    const Vec3&     getDefaultFollowerPoint() const;

    // Stage::Instance
    const UnitVec3& getLineDirection(const State&) const;
    const Vec3&     getPointOnLine(const State&) const;
    const Vec3&     getFollowerPoint(const State&) const;

    // Stage::Position, Velocity
    Vec2 getPositionErrors(const State&) const;
    Vec2 getVelocityErrors(const State&) const;

    // Stage::Acceleration
    Vec2 getAccelerationErrors(const State&) const;
    Vec2 getMultipliers(const State&) const;
    const Vec2& getForceOnFollowerPoint(const State&) const; // in normal direction
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(PointOnLine, PointOnLineImpl, Constraint);
};

    // CONSTANT ANGLE CONSTRAINT //

class SimTK_SIMBODY_EXPORT Constraint::ConstantAngle : public Constraint {
public:
    // no default constructor
    ConstantAngle(MobilizedBody& baseBody_B,     const UnitVec3& defaultAxis_B,
                  MobilizedBody& followerBody_F, const UnitVec3& defaultAxis_F, 
                  Real angle = Pi/2);

    // These affect only generated decorative geometry for visualization.
    ConstantAngle& setAxisDisplayLength(Real);
    ConstantAngle& setAxisDisplayWidth(Real);
    Real getAxisDisplayLength() const;
    Real getAxisDisplayWidth() const;

    // Defaults for Instance variables.
    ConstantAngle& setDefaultBaseAxis(const UnitVec3&);
    ConstantAngle& setDefaultFollowerAxis(const UnitVec3&);
    ConstantAngle& setDefaultAngle(Real);

    // Stage::Topology
    MobilizedBodyIndex getBaseMobilizedBodyIndex() const;
    MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;

    const UnitVec3& getDefaultBaseAxis() const;
    const UnitVec3& getDefaultFollowerAxis() const;
    Real getDefaultAngle() const;

    // Stage::Instance
    const UnitVec3& getBaseAxis(const State&) const;
    const UnitVec3& getFollowerAxis(const State&) const;
    Real getAngle(const State&) const;

    // Stage::Position, Velocity
    Real getPositionError(const State&) const;
    Real getVelocityError(const State&) const;

    // Stage::Acceleration
    Real getAccelerationError(const State&) const;
    Real getMultiplier(const State&) const;
    Real getTorqueOnFollowerBody(const State&) const; // about f X b
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ConstantAngle, ConstantAngleImpl, Constraint);
};

    // BALL (COINCIDENT POINTS) CONSTRAINT //

class SimTK_SIMBODY_EXPORT Constraint::Ball : public Constraint {
public:
    // no default constructor
    Ball(MobilizedBody& body1, MobilizedBody& body2);
    Ball(MobilizedBody& body1, const Vec3& defaultPoint1,
         MobilizedBody& body2, const Vec3& defaultPoint2);

    // Defaults for Instance variables.
    Ball& setDefaultPointOnBody1(const Vec3&);
    Ball& setDefaultPointOnBody2(const Vec3&);

    // This is used only for visualization.
    Ball& setDefaultRadius(Real r);
    Real getDefaultRadius() const;

    // Stage::Topology
    MobilizedBodyIndex getBody1MobilizedBodyIndex() const;
    MobilizedBodyIndex getBody2MobilizedBodyIndex() const;
    const Vec3& getDefaultPointOnBody1() const;
    const Vec3& getDefaultPointOnBody2() const;

    // Stage::Instance
    const Vec3& getPointOnBody1(const State&) const;
    const Vec3& getPointOnBody2(const State&) const;

    // Stage::Position, Velocity, Acceleration
    Vec3 getPositionErrors(const State&) const;
    Vec3 getVelocityErrors(const State&) const;

    // Stage::Acceleration
    Vec3 getAccelerationErrors(const State&) const;
    Vec3 getMultipliers(const State&) const;

    // Forces are reported expressed in the body frame of the indicated body.
    const Vec3& getBallReactionForceOnBody1(const State&) const;
    const Vec3& getBallReactionForceOnBody2(const State&) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ball, BallImpl, Constraint);
};

    // CONSTANT ORIENTATION CONSTRAINT //

class SimTK_SIMBODY_EXPORT Constraint::ConstantOrientation : public Constraint
{
public:
    // no default constructor
    ConstantOrientation(MobilizedBody& baseBody_B,     const Rotation& defaultRB,
                        MobilizedBody& followerBody_F, const Rotation& defaultRF); 

    //TODO: default visualization geometry?

    // Defaults for Instance variables.
    ConstantOrientation& setDefaultBaseRotation(const Rotation&);
    ConstantOrientation& setDefaultFollowerRotation(const Rotation&);

    // Stage::Topology
    MobilizedBodyIndex getBaseMobilizedBodyIndex() const;
    MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;

    const Rotation& getDefaultBaseRotation() const;
    const Rotation& getDefaultFollowerRotation() const;

    // Stage::Instance
    const Rotation& getBaseRotation(const State&) const;
    const Rotation& getFollowerRotation(const State&) const;

    // Stage::Position, Velocity
    Vec3 getPositionErrors(const State&) const;
    Vec3 getVelocityErrors(const State&) const;

    // Stage::Acceleration
    Vec3 getAccelerationErrors(const State&) const;
    Vec3 getMultipliers(const State&) const;
    Vec3 getTorqueOnFollowerBody(const State&) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ConstantOrientation, ConstantOrientationImpl, Constraint);
};

    // WELD (COINCIDENT FRAMES) CONSTRAINT //

class SimTK_SIMBODY_EXPORT Constraint::Weld : public Constraint {
public:
        // no default constructor

    Weld(MobilizedBody& body1, MobilizedBody& body2);

    Weld(MobilizedBody& body1, const Transform& frame1,
         MobilizedBody& body2, const Transform& frame2);

        // Control over generated decorative geometry.

    Weld& setAxisDisplayLength(Real r);

    Real getAxisDisplayLength() const;

        // Defaults for Instance variables.

    Weld& setDefaultFrameOnBody1(const Transform&);

    const Transform& getDefaultFrameOnBody1() const;

    Weld& setDefaultFrameOnBody2(const Transform&);

    const Transform& getDefaultFrameOnBody2() const;


        // Stage::Topology

    MobilizedBodyIndex getBody1MobilizedBodyIndex() const;

    MobilizedBodyIndex getBody2MobilizedBodyIndex() const;


        // Stage::Instance
    const Transform& getFrameOnBody1(const State&) const;
    const Transform& getFrameOnBody2(const State&) const;

        // Stage::Position, Velocity, Acceleration
    Vec6 getPositionErrors(const State&) const;
    Vec6 getVelocityErrors(const State&) const;

        // Stage::Acceleration
    Vec6 getAccelerationErrors(const State&) const;
    Vec6 getMultipliers(const State&) const;

        // Forces are reported expressed in the body frame of the indicated body.
    const SpatialVec& getWeldReactionOnBody1(const State&) const;
    const SpatialVec& getWeldReactionOnBody2(const State&) const;
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Weld, WeldImpl, Constraint);
};

    // NO SLIP 1D CONSTRAINT //

class SimTK_SIMBODY_EXPORT Constraint::NoSlip1D : public Constraint {
public:
    // no default constructor
    NoSlip1D(MobilizedBody& caseBodyC, const Vec3& P_C, const UnitVec3& n_C,
             MobilizedBody& movingBody0, MobilizedBody& movingBody1);

    // These affect only generated decorative geometry for visualization;
    // the plane is really infinite in extent with zero depth and the
    // point is really of zero radius.
    NoSlip1D& setDirectionDisplayLength(Real);
    NoSlip1D& setPointDisplayRadius(Real);
    Real getDirectionDisplayLength() const;
    Real getPointDisplayRadius() const;

    // Defaults for Instance variables.
    NoSlip1D& setDefaultDirection(const UnitVec3&);
    NoSlip1D& setDefaultContactPoint(const Vec3&);

    // Stage::Topology
    MobilizedBodyIndex getCaseMobilizedBodyIndex() const;
    MobilizedBodyIndex getMovingBodyMobilizedBodyIndex(int which) const;

    const UnitVec3& getDefaultDirection() const;
    const Vec3&     getDefaultContactPoint() const;

    // Stage::Instance
    const UnitVec3& getDirection(const State&) const;
    const Vec3&     getContactPoint(const State&) const;

    // Stage::Position, Velocity
        // no position error
    Real getVelocityError(const State&) const;

    // Stage::Acceleration
    Real getAccelerationError(const State&) const;
    Real getMultiplier(const State&) const;
    Real getForceAtContactPoint(const State&) const; // in normal direction, no body 2
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(NoSlip1D, NoSlip1DImpl, Constraint);
};

    // CONSTANT SPEED //

class SimTK_SIMBODY_EXPORT Constraint::ConstantSpeed : public Constraint
{
public:
    // no default constructor
    ConstantSpeed(MobilizedBody& mobilizer, MobilizerUIndex, Real speed);
    ConstantSpeed(MobilizedBody& mobilizer, Real speed); 

    // Stage::Topology
    MobilizedBodyIndex getMobilizedBodyIndex() const;
    MobilizerUIndex    getWhichU() const;
    Real               getDefaultSpeed() const;

    // Stage::Position, Velocity
        // no position error
    Real getVelocityError(const State&) const;

    // Stage::Acceleration
    Real getAccelerationError(const State&) const;
    Real getMultiplier(const State&) const;
    Real getGeneralizedForce(const State&) const;
 // hide from Doxygen
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
       (ConstantSpeed, ConstantSpeedImpl, Constraint);
};

    // CONSTANT ACCELERATION //

class SimTK_SIMBODY_EXPORT Constraint::ConstantAcceleration : public Constraint
{
public:
    // no default constructor
    ConstantAcceleration(MobilizedBody& mobilizer, MobilizerUIndex, 
                         Real defaultAcceleration);
    ConstantAcceleration(MobilizedBody& mobilizer, 
                         Real defaultAcceleration);

    // Stage::Topology
    MobilizedBodyIndex getMobilizedBodyIndex() const;
    MobilizerUIndex    getWhichU() const;
    Real               getDefaultAcceleration() const;
    ConstantAcceleration& setDefaultAcceleration(Real accel);

    void setAcceleration(State& state, Real accel) const;
    Real getAcceleration(const State& state) const;

    // Stage::Position, Velocity
        // no position or velocity error

    // Stage::Acceleration
    Real getAccelerationError(const State&) const;
    Real getMultiplier(const State&) const;
    Real getGeneralizedForce(const State&) const;
 // hide from Doxygen
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
       (ConstantAcceleration, ConstantAccelerationImpl, Constraint);
};

class SimTK_SIMBODY_EXPORT Constraint::Custom : public Constraint {
public:
    class Implementation;
    class ImplementationImpl;

    explicit Custom(Implementation* implementation);
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, Constraint);
protected:
    const Implementation& getImplementation() const;
    Implementation&       updImplementation();

    Custom() {}
};

// We only want the template instantiation to occur once. This symbol is 
// defined in the SimTK core compilation unit that defines the Constraint 
// class but should not be defined any other time.
#ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT
    extern template class PIMPLHandle<Constraint::Custom::Implementation, 
                                      Constraint::Custom::ImplementationImpl>;
#endif

class SimTK_SIMBODY_EXPORT Constraint::Custom::Implementation 
:   public PIMPLHandle<Implementation,ImplementationImpl> {
public:
    // No default constructor because you have to supply at least the 
    // SimbodyMatterSubsystem to which this Constraint belongs.

    virtual ~Implementation() { }

    virtual Implementation* clone() const = 0;

    Implementation(SimbodyMatterSubsystem&, int mp, int mv, int ma);

    explicit Implementation(SimbodyMatterSubsystem&);

    const SimbodyMatterSubsystem& getMatterSubsystem() const;

        // Topological information//

    void invalidateTopologyCache() const;

    Implementation& setDefaultNumConstraintEquations(int mp, int mv, int ma);

    Implementation& setDisabledByDefault(bool shouldBeDisabled);

    ConstrainedBodyIndex addConstrainedBody(const MobilizedBody&);

    ConstrainedMobilizerIndex addConstrainedMobilizer(const MobilizedBody&);

    MobilizedBodyIndex 
    getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex) const;
    MobilizedBodyIndex 
    getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex) const;

        // Model stage information //

        // Methods for use with ConstrainedMobilizers.

    Real getOneQ(const State&, ConstrainedMobilizerIndex, MobilizerQIndex) const;
    Real getOneU(const State&, ConstrainedMobilizerIndex, MobilizerUIndex) const;

    Real getOneQDot   (const State&, ConstrainedMobilizerIndex, MobilizerQIndex, bool realizingVelocity=false) const;
    Real getOneQDotDot(const State&, ConstrainedMobilizerIndex, MobilizerQIndex, bool realizingAcceleration=false) const;
    Real getOneUDot(const State&, ConstrainedMobilizerIndex, MobilizerUIndex, bool realizingAcceleration=false) const;

    void addInOneMobilityForce(const State&, 
                               ConstrainedMobilizerIndex, MobilizerUIndex whichU,
                               Real force, Vector& mobilityForces) const;

        // Methods for use with ConstrainedBodies

    const Transform&  getBodyTransform(const State& s, ConstrainedBodyIndex B)    const; // X_AB
    const SpatialVec& getBodyVelocity(const State& s, ConstrainedBodyIndex B)     const; // V_AB
    const SpatialVec& getBodyAcceleration(const State& s, ConstrainedBodyIndex B) const; // A_AB

        // Extract just the rotational part of spatial quantities above.

    const Rotation& getBodyRotation(const State& s, ConstrainedBodyIndex B) const
       {return getBodyTransform(s,B).R();}   // R_AB
    const Vec3& getBodyAngularVelocity(const State& s, ConstrainedBodyIndex B) const
       {return getBodyVelocity(s,B)[0];}     // w_AB
    const Vec3& getBodyAngularAcceleration(const State& s, ConstrainedBodyIndex B) const
       {return getBodyAcceleration(s,B)[0];} // b_AB

        // Extract just the translational (linear) part of spatial quantities above.

    const Vec3& getBodyOriginLocation(const State& s, ConstrainedBodyIndex B) const
       {return getBodyTransform(s,B).p();}   // p_AB
    const Vec3& getBodyOriginVelocity    (const State& s, ConstrainedBodyIndex B) const 
       {return getBodyVelocity(s,B)[1];}     // v_AB
    const Vec3& getBodyOriginAcceleration(const State& s, ConstrainedBodyIndex B) const 
       {return getBodyAcceleration(s,B)[1];} // a_AB

        // Calculate location, velocity, and acceleration for a given station.

    Vec3 findStationLocation(const State& s, ConstrainedBodyIndex B, const Vec3& p_BS) const {
        return getBodyTransform(s,B) * p_BS; // re-measure and re-express
    }
    Vec3 findStationVelocity(const State& s, ConstrainedBodyIndex B, const Vec3& p_BS) const {
        const Vec3        p_AS = getBodyRotation(s,B) * p_BS; // rexpressed but not shifted
        const SpatialVec& V_AB = getBodyVelocity(s,B);
        return V_AB[1] + (V_AB[0] % p_AS);
    }
    Vec3 findStationAcceleration(const State& s, ConstrainedBodyIndex B, const Vec3& p_BS) const {
        const Vec3        p_AS = getBodyRotation(s,B) * p_BS; // rexpressed but not shifted
        const Vec3&       w_AB = getBodyAngularVelocity(s,B);
        const SpatialVec& A_AB = getBodyAcceleration(s,B);
        return A_AB[1] + (A_AB[0] % p_AS) + w_AB % (w_AB % p_AS); // careful: cross product is not associative
    }

        // Utilities for applying constraint forces to ConstrainedBodies.

    void addInStationForce(const State& s,  ConstrainedBodyIndex B,
                           const Vec3& p_BS, 
                           const Vec3& forceInA, Vector_<SpatialVec>& bodyForcesInA) const;

    void addInBodyTorque(const State& s, ConstrainedBodyIndex B,
                         const Vec3& torqueInA, Vector_<SpatialVec>& bodyForcesInA) const;


protected:









    virtual void realizeTopology(State&) const { }

    virtual void realizeModel(State&) const { }

    virtual void realizeInstance(const State&) const { }

    virtual void realizeTime(const State&) const { }

    virtual void realizePosition(const State&) const { }

    virtual void realizeVelocity(const State&) const { }

    virtual void realizeDynamics(const State&) const { }

    virtual void realizeAcceleration(const State&) const { }

    virtual void realizeReport(const State&) const { }






    virtual void realizePositionErrors(const State&, int mp,  Real* perr) const;

    virtual void realizePositionDotErrors(const State&, int mp,  Real* pverr) const;

    virtual void realizePositionDotDotErrors(const State&, int mp,  Real* paerr) const;

    virtual void applyPositionConstraintForces
       (const State&, int mp, const Real* multipliers,
        Vector_<SpatialVec>& bodyForces,
        Vector&              mobilityForces) const;






    virtual void realizeVelocityErrors(const State&, int mv,  Real* verr) const;

    virtual void realizeVelocityDotErrors(const State&, int mv,  Real* vaerr) const;

    virtual void applyVelocityConstraintForces
       (const State&, int mv, const Real* multipliers,
        Vector_<SpatialVec>& bodyForces,
        Vector&              mobilityForces) const;






    virtual void realizeAccelerationErrors(const State&, int ma,  Real* aerr) const;
    virtual void applyAccelerationConstraintForces
       (const State&, int ma, const Real* multipliers,
        Vector_<SpatialVec>& bodyForces,
        Vector&              mobilityForces) const;

    virtual void calcDecorativeGeometryAndAppend
       (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
    {
    }

    friend class Constraint::CustomImpl;
};

class SimTK_SIMBODY_EXPORT Constraint::CoordinateCoupler 
:   public Constraint::Custom {
public:
    CoordinateCoupler(SimbodyMatterSubsystem& matter, const Function* function, 
                      const Array_<MobilizedBodyIndex>& coordBody, 
                      const Array_<MobilizerQIndex>& coordIndex);

    CoordinateCoupler(SimbodyMatterSubsystem& matter, const Function* function, 
                      const std::vector<MobilizedBodyIndex>& coordBody, 
                      const std::vector<MobilizerQIndex>& coordIndex) 
    {   // Invoke the above constructor with converted arguments.
        new (this) CoordinateCoupler(matter,function,
            ArrayViewConst_<MobilizedBodyIndex>(coordBody),
            ArrayViewConst_<MobilizerQIndex>(coordIndex));
    }
};

class SimTK_SIMBODY_EXPORT Constraint::SpeedCoupler : public Constraint::Custom {
public:
    SpeedCoupler(SimbodyMatterSubsystem& matter, const Function* function, 
                 const Array_<MobilizedBodyIndex>& speedBody, 
                 const Array_<MobilizerUIndex>& speedIndex);

    SpeedCoupler(SimbodyMatterSubsystem& matter, const Function* function, 
                 const std::vector<MobilizedBodyIndex>& speedBody, 
                 const std::vector<MobilizerUIndex>& speedIndex) 
    {   // Invoke above constructor with converted arguments.
        new (this) SpeedCoupler(matter, function,
                                ArrayViewConst_<MobilizedBodyIndex>(speedBody),
                                ArrayViewConst_<MobilizerUIndex>(speedIndex));
    }

    SpeedCoupler(SimbodyMatterSubsystem& matter, const Function* function, 
                 const Array_<MobilizedBodyIndex>& speedBody, 
                 const Array_<MobilizerUIndex>& speedIndex,
                 const Array_<MobilizedBodyIndex>& coordBody, 
                 const Array_<MobilizerQIndex>& coordIndex);

    SpeedCoupler(SimbodyMatterSubsystem& matter, const Function* function, 
                 const std::vector<MobilizedBodyIndex>& speedBody, 
                 const std::vector<MobilizerUIndex>& speedIndex,
                 const std::vector<MobilizedBodyIndex>& coordBody, 
                 const std::vector<MobilizerQIndex>& coordIndex)
    {   // Invoke above constructor with converted arguments.
        new (this) SpeedCoupler(matter, function,
                                ArrayViewConst_<MobilizedBodyIndex>(speedBody),
                                ArrayViewConst_<MobilizerUIndex>(speedIndex),
                                ArrayViewConst_<MobilizedBodyIndex>(coordBody),
                                ArrayViewConst_<MobilizerQIndex>(coordIndex));
    }
};

class SimTK_SIMBODY_EXPORT Constraint::PrescribedMotion : public Constraint::Custom {
public:
    PrescribedMotion(SimbodyMatterSubsystem& matter, const Function* function, 
                     MobilizedBodyIndex coordBody, MobilizerQIndex coordIndex);
};

} // namespace SimTK

#endif // SimTK_SIMBODY_CONSTRAINT_H_