Constraint.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_CONSTRAINT_H_
#define SimTK_SIMBODY_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) 2007-14 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 <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);

operator ConstraintIndex() const {return getConstraintIndex();}

const SimbodyMatterSubsystem& getMatterSubsystem()      const;

SimbodyMatterSubsystem& updMatterSubsystem();

ConstraintIndex getConstraintIndex() const;

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

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

int getNumConstrainedBodies() const;

const MobilizedBody& getMobilizedBodyFromConstrainedBody
   (ConstrainedBodyIndex consBodyIx) const;

const MobilizedBody& getAncestorMobilizedBody() const;

int getNumConstrainedMobilizers() const;

const MobilizedBody& getMobilizedBodyFromConstrainedMobilizer
   (ConstrainedMobilizerIndex consMobilizerIx) const;

const SimbodyMatterSubtree& getSubtree() const;

    // MODEL STAGE //
// nothing in base class currently

    // INSTANCE 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;

QIndex getQIndexOfConstrainedQ(const State&      state,
                               ConstrainedQIndex consQIndex) const;
UIndex getUIndexOfConstrainedU(const State&      state,
                               ConstrainedUIndex consUIndex) const;

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

void getIndexOfMultipliersInUse(const State& state,
                                MultiplierIndex& px0, 
                                MultiplierIndex& vx0, 
                                MultiplierIndex& ax0) const;

void setMyPartInConstraintSpaceVector(const State& state,
                                      const Vector& myPart,
                                      Vector& constraintSpace) const;

void getMyPartFromConstraintSpaceVector(const State& state,
                                        const Vector& constraintSpace,
                                        Vector& myPart) const;

    // 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   

void getConstraintForcesAsVectors
   (const State&         state,
    Vector_<SpatialVec>& bodyForcesInG, // numConstrainedBodies
    Vector&              mobilityForces) const; // numConstrainedU

Vector_<SpatialVec> getConstrainedBodyForcesAsVector(const State& state) const {
    Vector_<SpatialVec> bodyForcesInG;
    Vector              mobilityForces;
    getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
    return bodyForcesInG;
}
Vector getConstrainedMobilityForcesAsVector(const State& state) const {
    Vector_<SpatialVec> bodyForcesInG;
    Vector              mobilityForces;
    getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
    return mobilityForces;
}

Real calcPower(const State& state) const;

// 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

void setIsConditional(bool isConditional);
bool isConditional() const;
                  
//------------------------------------------------------------------------------
// These are the built-in Constraint types, and some synonyms. Each built in 
// Constraint type is declared in its own header file using naming convention 
// Constraint_Rod.h, for example. All the built-in headers are collected in 
// Constraint_BuiltIns.h; you should include new ones there also.
class Rod;  
typedef Rod  ConstantDistance;  

class Ball; 
typedef Ball CoincidentPoints;  
typedef Ball Spherical;         

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 ConstantCoordinate; // prescribe generalized coordinate value
class ConstantSpeed; // prescribe generalized speed value
class ConstantAcceleration; // prescribe generalized acceleration value
class Custom;
class CoordinateCoupler;
class SpeedCoupler;
class PrescribedMotion;
class PointOnPlaneContact; 
class SphereOnPlaneContact; // ball in contact with plane (sliding or rolling)
class SphereOnSphereContact; // ball in contact with ball (sliding or rolling)
class LineOnLineContact;    // edge/edge contact

// Internal use only.
class RodImpl;
class BallImpl;
class WeldImpl;
class PointInPlaneImpl;
class PointOnLineImpl;
class ConstantAngleImpl;
class ConstantOrientationImpl;
class NoSlip1DImpl;
class ConstantCoordinateImpl;
class ConstantSpeedImpl;
class ConstantAccelerationImpl;
class CustomImpl;
class CoordinateCouplerImpl;
class SpeedCouplerImpl;
class PrescribedMotionImpl;
class PointOnPlaneContactImpl; 
class SphereOnPlaneContactImpl;
class SphereOnSphereContactImpl;
class LineOnLineContactImpl;

};

    // 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);
    
    PointOnLine() {}

    // 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
     // hide from Doxygen
    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);
    
    ConstantAngle() {}

    // 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
     // hide from Doxygen
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ConstantAngle, ConstantAngleImpl, 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); 
    
    ConstantOrientation() {}

    //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;
 // hide from Doxygen
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
       (ConstantOrientation, ConstantOrientationImpl, 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);
    
    NoSlip1D() {}

    void setContactPoint(State& state, const Vec3& point_C) const;
    void setDirection(State& state, const UnitVec3& direction_C) const;

    const Vec3& getContactPoint(const State& state) const;
    const UnitVec3& getDirection(const State& state) const;

    // 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& setDefaultContactPoint(const Vec3&); 
    NoSlip1D& setDefaultDirection(const UnitVec3&);


    // Stage::Topology

    MobilizedBodyIndex getCaseMobilizedBodyIndex() const;
    MobilizedBodyIndex getMovingBodyMobilizedBodyIndex(int which) const;

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


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

    Real getVelocityError(const State& state) const;

    // Stage::Acceleration

    Real getAccelerationError(const State&) const;

    Real getMultiplier(const State&) const;

    Real getForceAtContactPoint(const State&) const;
 // hide from Doxygen
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(NoSlip1D, NoSlip1DImpl, Constraint);
};

    // CONSTANT COORDINATE //

class SimTK_SIMBODY_EXPORT Constraint::ConstantCoordinate : public Constraint {
public:
    ConstantCoordinate(MobilizedBody& mobilizer, MobilizerQIndex whichQ, 
                       Real defaultPosition);

    ConstantCoordinate(MobilizedBody& mobilizer, Real defaultPosition); 
    
    ConstantCoordinate() {}

    MobilizedBodyIndex getMobilizedBodyIndex() const;

    MobilizerQIndex    getWhichQ() const;

    Real getDefaultPosition() const;

    ConstantCoordinate& setDefaultPosition(Real position);

    void setPosition(State& state, Real position) const;

    Real getPosition(const State& state) const;

    Real getPositionError(const State& state) const;

    Real getVelocityError(const State& state) const;

    Real getAccelerationError(const State& state) const;

    Real getMultiplier(const State& state) const;
 // hide from Doxygen
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
       (ConstantCoordinate, ConstantCoordinateImpl, Constraint);
};

    // CONSTANT SPEED //

class SimTK_SIMBODY_EXPORT Constraint::ConstantSpeed : public Constraint {
public:
    ConstantSpeed(MobilizedBody& mobilizer, MobilizerUIndex whichU, 
                  Real defaultSpeed);
    ConstantSpeed(MobilizedBody& mobilizer, Real defaultSpeed); 
    
    ConstantSpeed() {}

    MobilizedBodyIndex getMobilizedBodyIndex() const;

    MobilizerUIndex    getWhichU() const;

    Real               getDefaultSpeed() const;

    ConstantSpeed&     setDefaultSpeed(Real speed);

    void setSpeed(State& state, Real speed) const;

    Real getSpeed(const State& state) const;

    // no position error

    Real getVelocityError(const State& state) const;

    Real getAccelerationError(const State& state) const;

    Real getMultiplier(const State& state) const;
 // hide from Doxygen
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
       (ConstantSpeed, ConstantSpeedImpl, Constraint);
};

    // CONSTANT ACCELERATION //

class SimTK_SIMBODY_EXPORT Constraint::ConstantAcceleration : public Constraint
{
public:
    ConstantAcceleration(MobilizedBody& mobilizer, MobilizerUIndex whichU, 
                         Real defaultAcceleration);

    ConstantAcceleration(MobilizedBody& mobilizer, 
                         Real defaultAcceleration);
    
    ConstantAcceleration() {}

    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;

    // no position or velocity error

    Real getAccelerationError(const State&) const;

    Real getMultiplier(const State&) const;
 // hide from Doxygen
    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS
       (ConstantAcceleration, ConstantAccelerationImpl, Constraint);
};

//==============================================================================
//                                 CUSTOM
//==============================================================================
class SimTK_SIMBODY_EXPORT Constraint::Custom : public Constraint {
public:
    class Implementation;
    class ImplementationImpl;

    explicit Custom(Implementation* implementation);

    
    Custom() {}

    SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, Constraint);
protected:
    const Implementation& getImplementation() const;
    Implementation&       updImplementation();
};

//==============================================================================
//                           CUSTOM::IMPLEMENTATION
//==============================================================================

// 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;


Real getOneQ(const State&                           state,
             const Array_<Real,ConstrainedQIndex>&  constrainedQ,
             ConstrainedMobilizerIndex              mobilizer, 
             MobilizerQIndex                        whichQ) const;

Real getOneQFromState(const State&              state, 
                      ConstrainedMobilizerIndex mobilizer, 
                      MobilizerQIndex           whichQ) const;

Real getOneQDot(const State&                           state,
                const Array_<Real,ConstrainedQIndex>&  constrainedQDot,
                ConstrainedMobilizerIndex              mobilizer, 
                MobilizerQIndex                        whichQ) const;

Real getOneQDotFromState(const State&             state, 
                        ConstrainedMobilizerIndex mobilizer, 
                        MobilizerQIndex           whichQ) const;


Real getOneQDotDot(const State&                           state,
                   const Array_<Real,ConstrainedQIndex>&  constrainedQDotDot,
                   ConstrainedMobilizerIndex              mobilizer, 
                   MobilizerQIndex                        whichQ) const;

Real getOneU(const State&                           state,
             const Array_<Real,ConstrainedUIndex>&  constrainedU,
             ConstrainedMobilizerIndex              mobilizer, 
             MobilizerUIndex                        whichU) const;

Real getOneUFromState(const State&              state,
                      ConstrainedMobilizerIndex mobilizer, 
                      MobilizerUIndex           whichU) const;

Real getOneUDot(const State&                           state,
                const Array_<Real,ConstrainedUIndex>&  constrainedUDot,
                ConstrainedMobilizerIndex              mobilizer, 
                MobilizerUIndex                        whichU) const;

void addInOneMobilityForce
   (const State&                    state, 
    ConstrainedMobilizerIndex       mobilizer, 
    MobilizerUIndex                 whichU,
    Real                            fu, 
    Array_<Real,ConstrainedUIndex>& mobilityForces) const;

void addInOneQForce
   (const State&                    state, 
    ConstrainedMobilizerIndex       mobilizer, 
    MobilizerQIndex                 whichQ,
    Real                            fq, 
    Array_<Real,ConstrainedQIndex>& qForces) const;
const Transform& getBodyTransform
   (const Array_<Transform,ConstrainedBodyIndex>&   allX_AB, 
    ConstrainedBodyIndex                            bodyB) const;
const Rotation& getBodyRotation
   (const Array_<Transform,ConstrainedBodyIndex>&   allX_AB, 
    ConstrainedBodyIndex                            bodyB) const
{   return getBodyTransform(allX_AB,bodyB).R(); }
const Vec3& getBodyOriginLocation
   (const Array_<Transform,ConstrainedBodyIndex>&   allX_AB, 
    ConstrainedBodyIndex                            bodyB) const
{   return getBodyTransform(allX_AB,bodyB).p(); }

const Transform& getBodyTransformFromState
   (const State& state, ConstrainedBodyIndex B)    const; // X_AB
const Rotation& getBodyRotationFromState
   (const State& state, ConstrainedBodyIndex bodyB) const
{   return getBodyTransformFromState(state,bodyB).R(); }
const Vec3& getBodyOriginLocationFromState
   (const State& state, ConstrainedBodyIndex bodyB) const
{   return getBodyTransformFromState(state,bodyB).p(); }

const SpatialVec& getBodyVelocity
   (const Array_<SpatialVec,ConstrainedBodyIndex>&  allV_AB, 
    ConstrainedBodyIndex                            bodyB) const;
const Vec3& getBodyAngularVelocity
   (const Array_<SpatialVec,ConstrainedBodyIndex>&  allV_AB, 
    ConstrainedBodyIndex                            bodyB) const
{   return getBodyVelocity(allV_AB,bodyB)[0]; }
const Vec3& getBodyOriginVelocity
   (const Array_<SpatialVec,ConstrainedBodyIndex>&  allV_AB, 
    ConstrainedBodyIndex                            bodyB) const
{   return getBodyVelocity(allV_AB,bodyB)[1]; }

const SpatialVec& getBodyVelocityFromState
   (const State& state, ConstrainedBodyIndex bodyB)     const; // V_AB
const Vec3& getBodyAngularVelocityFromState
   (const State& state, ConstrainedBodyIndex bodyB) const
{   return getBodyVelocityFromState(state,bodyB)[0]; }
const Vec3& getBodyOriginVelocityFromState
   (const State& state, ConstrainedBodyIndex bodyB) const 
{   return getBodyVelocityFromState(state,bodyB)[1]; }

const SpatialVec& getBodyAcceleration
   (const Array_<SpatialVec,ConstrainedBodyIndex>&  allA_AB, 
    ConstrainedBodyIndex                            bodyB) const;
const Vec3& getBodyAngularAcceleration
   (const Array_<SpatialVec,ConstrainedBodyIndex>&  allA_AB, 
    ConstrainedBodyIndex                            bodyB) const
{   return getBodyAcceleration(allA_AB,bodyB)[0]; }
const Vec3& getBodyOriginAcceleration
   (const Array_<SpatialVec,ConstrainedBodyIndex>&  allA_AB, 
    ConstrainedBodyIndex                            bodyB) const
{   return getBodyAcceleration(allA_AB,bodyB)[1]; }

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

Vec3 findStationLocation
   (const Array_<Transform, ConstrainedBodyIndex>&  allX_AB, 
    ConstrainedBodyIndex                            bodyB, 
    const Vec3&                                     p_BS) const 
{
    const Transform& X_AB = allX_AB[bodyB];
    return X_AB * p_BS; // re-measure and re-express
}
Vec3 findStationLocationFromState
   (const State&            state, 
    ConstrainedBodyIndex    bodyB, 
    const Vec3&             p_BS) const 
{   
    const Transform& X_AB = getBodyTransformFromState(state,bodyB);
    return X_AB * p_BS; // re-measure and re-express
}

Vec3 findStationVelocity
   (const State&                                    state,
    const Array_<SpatialVec,ConstrainedBodyIndex>&  allV_AB, 
    ConstrainedBodyIndex                            bodyB, 
    const Vec3&                                     p_BS) const 
{   // p_BS_A is p_BS rexpressed in A but not shifted to Ao
    const Rotation&   R_AB   = getBodyRotationFromState(state,bodyB);
    const Vec3        p_BS_A = R_AB * p_BS; 
    const SpatialVec& V_AB   = allV_AB[bodyB];
    return V_AB[1] + (V_AB[0] % p_BS_A);    // v + w X r
}
Vec3 findStationVelocityFromState
   (const State&            state, 
    ConstrainedBodyIndex    bodyB, 
    const Vec3&             p_BS) const 
{   // p_BS_A is p_BS rexpressed in A but not shifted to Ao
    const Rotation&   R_AB   = getBodyRotationFromState(state,bodyB);
    const Vec3        p_BS_A = R_AB * p_BS;
    const SpatialVec& V_AB   = getBodyVelocityFromState(state,bodyB);
    return V_AB[1] + (V_AB[0] % p_BS_A);    // v + w X r
}

Vec3 findStationAcceleration
   (const State&                                    state, 
    const Array_<SpatialVec,ConstrainedBodyIndex>&  allA_AB, 
    ConstrainedBodyIndex                            bodyB, 
    const Vec3&                                     p_BS) const 
{   // p_BS_A is p_BS rexpressed in A but not shifted to Ao
    const Rotation&   R_AB   = getBodyRotationFromState(state,bodyB);
    const Vec3        p_BS_A = R_AB * p_BS; 
    const Vec3&       w_AB   = getBodyAngularVelocityFromState(state,bodyB);
    const SpatialVec& A_AB   = allA_AB[bodyB];

    // Result is a + b X r + w X (w X r).
    // ("b" is angular acceleration; w is angular velocity).
    const Vec3 a_AS = A_AB[1] + (A_AB[0] % p_BS_A) 
                              + w_AB % (w_AB % p_BS_A); // % is not associative
    return a_AS;
}

    // Utilities for applying constraint forces to ConstrainedBodies.

void addInStationForce
   (const State&                                state,  
    ConstrainedBodyIndex                        bodyB,
    const Vec3&                                 p_BS, 
    const Vec3&                                 forceInA, 
    Array_<SpatialVec,ConstrainedBodyIndex>&    bodyForcesInA) const;

void addInBodyTorque
   (const State&                                state, 
    ConstrainedBodyIndex                        bodyB,
    const Vec3&                                 torqueInA, 
    Array_<SpatialVec,ConstrainedBodyIndex>&    bodyForcesInA) const;
void getMultipliers(const State&  state, 
                    Array_<Real>& multipliers) 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 calcPositionErrors     
   (const State&                                    state,      // Stage::Time
    const Array_<Transform,ConstrainedBodyIndex>&   X_AB, 
    const Array_<Real,     ConstrainedQIndex>&      constrainedQ,
    Array_<Real>&                                   perr)       // mp of these
    const;

virtual void calcPositionDotErrors      
   (const State&                                    state, // Stage::Position
    const Array_<SpatialVec,ConstrainedBodyIndex>&  V_AB, 
    const Array_<Real,      ConstrainedQIndex>&     constrainedQDot,
    Array_<Real>&                                   pverr) // mp of these
    const;

virtual void calcPositionDotDotErrors     
   (const State&                                    state, // Stage::Velocity
    const Array_<SpatialVec,ConstrainedBodyIndex>&  A_AB, 
    const Array_<Real,      ConstrainedQIndex>&     constrainedQDotDot,
    Array_<Real>&                                   paerr) // mp of these
    const;

virtual void addInPositionConstraintForces
   (const State&                                state, 
    const Array_<Real>&                         multipliers,
    Array_<SpatialVec,ConstrainedBodyIndex>&    bodyForcesInA,
    Array_<Real,      ConstrainedQIndex>&       qForces) const;
virtual void calcVelocityErrors     
   (const State&                                    state,  // Stage::Position
    const Array_<SpatialVec,ConstrainedBodyIndex>&  V_AB, 
    const Array_<Real,      ConstrainedUIndex>&     constrainedU,
    Array_<Real>&                                   verr)   // mv of these
    const;

virtual void calcVelocityDotErrors     
   (const State&                                    state,  // Stage::Velocity
    const Array_<SpatialVec,ConstrainedBodyIndex>&  A_AB, 
    const Array_<Real,      ConstrainedUIndex>&     constrainedUDot,
    Array_<Real>&                                   vaerr)  // mv of these
    const;

virtual void addInVelocityConstraintForces
   (const State&                                state, 
    const Array_<Real>&                         multipliers,
    Array_<SpatialVec,ConstrainedBodyIndex>&    bodyForcesInA,
    Array_<Real,      ConstrainedUIndex>&       mobilityForces) const;
virtual void calcAccelerationErrors      
   (const State&                                    state, // Stage::Velocity
    const Array_<SpatialVec,ConstrainedBodyIndex>&  A_AB, 
    const Array_<Real,      ConstrainedUIndex>&     constrainedUDot,
    Array_<Real>&                                   aerr)  // ma of these
    const;

virtual void addInAccelerationConstraintForces
   (const State&                                state, 
    const Array_<Real>&                         multipliers,
    Array_<SpatialVec,ConstrainedBodyIndex>&    bodyForcesInA,
    Array_<Real,      ConstrainedUIndex>&       mobilityForces) const;
virtual void calcDecorativeGeometryAndAppend
    (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
{
}

friend class Constraint::CustomImpl;
};



//==============================================================================
//                           COORDINATE COUPLER
//==============================================================================
class SimTK_SIMBODY_EXPORT Constraint::CoordinateCoupler 
:   public Constraint::Custom {
public:
    CoordinateCoupler(SimbodyMatterSubsystem&           matter, 
                      const Function*                   function, 
                      const Array_<MobilizedBodyIndex>& coordMobod, 
                      const Array_<MobilizerQIndex>&    coordQIndex);

    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));
    }
    
    CoordinateCoupler() {}
};



//==============================================================================
//                              SPEED COUPLER
//==============================================================================
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));
    }

    SpeedCoupler() {}
};



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

    
    PrescribedMotion() {}
};



} // namespace SimTK

#endif // SimTK_SIMBODY_CONSTRAINT_H_