MobilizedBody.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_MOBILIZED_BODY_H_
#define SimTK_SIMBODY_MOBILIZED_BODY_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-13 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors: Paul Mitiguy, Peter Eastman                                  *
 *                                                                            *
 * 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/Body.h"
#include "simbody/internal/Motion.h"
 
#include <cassert>
 
namespace SimTK {
 
class SimbodyMatterSubsystem;
class Motion;
class MobilizedBody;
class MobilizedBodyImpl;
 
// We only want the template instantiation to occur once. This symbol is 
// defined in the SimTK core compilation unit that instantiates the mobilized 
// body class but should not be defined any other time.
#ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
    extern template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
#endif
 
typedef MobilizedBody Mobod;
 
 
 
//==============================================================================
//                             MOBILIZED BODY
//==============================================================================
class SimTK_SIMBODY_EXPORT MobilizedBody 
:   public PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true> {
public:
 
enum Direction {
    Forward = 0, 
    Reverse = 1  
};
 
//------------------------------------------------------------------------------
void lock(State& state, Motion::Level level=Motion::Position) const;
 
void lockAt(State& state, Real value, 
            Motion::Level level=Motion::Position) const;
 
void lockAt(State& state, const Vector& value, 
            Motion::Level level=Motion::Position) const;
 
template <int N> SimTK_SIMBODY_EXPORT // instantiated in library
void lockAt(State& state, const Vec<N>& value,
            Motion::Level level=Motion::Position) const;
 
void unlock(State& state) const;
 
bool isLocked(const State& state) const 
{   return getLockLevel(state)!=Motion::NoLevel; }
 
Motion::Level getLockLevel(const State& state) const;
 
Vector getLockValueAsVector(const State& state) const;
 
MobilizedBody& lockByDefault(Motion::Level level=Motion::Position);
 
bool isLockedByDefault() const 
{   return getLockByDefaultLevel()!=Motion::NoLevel; }
 
Motion::Level getLockByDefaultLevel() const;
    // STATE ACCESS METHODS //
 
//------------------------------------------------------------------------------
const Transform& getBodyTransform(const State& state) const; // X_GB
 
const Rotation& getBodyRotation(const State& state) const {
    return getBodyTransform(state).R();
}
const Vec3& getBodyOriginLocation(const State& state) const {
    return getBodyTransform(state).p();
}
 
const Transform& getMobilizerTransform(const State& state) const;   // X_FM
 
const SpatialVec& getBodyVelocity(const State& state) const;        // V_GB
 
const Vec3& getBodyAngularVelocity(const State& state) const {      // w_GB
    return getBodyVelocity(state)[0]; 
}
const Vec3& getBodyOriginVelocity(const State& state) const {       // v_GB
    return getBodyVelocity(state)[1];
}
 
const SpatialVec& getMobilizerVelocity(const State& state) const;   // V_FM
 
const SpatialVec& getBodyAcceleration(const State& state) const;    // A_GB
 
const Vec3& getBodyAngularAcceleration(const State& state) const {  // b_GB
    return getBodyAcceleration(state)[0]; 
}
 
const Vec3& getBodyOriginAcceleration(const State& state) const {   // a_GB
    return getBodyAcceleration(state)[1];
}
 
const SpatialVec& getMobilizerAcceleration(const State& state) const { // A_FM
    SimTK_ASSERT_ALWAYS(!"unimplemented method", 
"MobilizedBody::getMobilizerAcceleration() not yet implemented -- volunteers?");
    return *(new SpatialVec());
}
 
const MassProperties& getBodyMassProperties(const State& state) const;
 
const SpatialInertia& getBodySpatialInertiaInGround(const State& state) const;
 
Real getBodyMass(const State& state) const {
    return getBodyMassProperties(state).getMass();
}
 
const Vec3& getBodyMassCenterStation(const State& state) const {
    return getBodyMassProperties(state).getMassCenter();
}
 
const UnitInertia& getBodyUnitInertiaAboutBodyOrigin(const State& state) const {
    return getBodyMassProperties(state).getUnitInertia();
}
 
const Transform& getInboardFrame (const State& state) const;    // X_PF
const Transform& getOutboardFrame(const State& state) const;    // X_BM
 
void setInboardFrame (State& state, const Transform& X_PF) const;
void setOutboardFrame(State& state, const Transform& X_BM) const;
 
// End of State Access - Bodies
//------------------------------------------------------------------------------
int getNumQ(const State& state) const;
int getNumU(const State& state) const;
 
QIndex getFirstQIndex(const State& state) const;
 
UIndex getFirstUIndex(const State& state) const;
 
Motion::Method getQMotionMethod(const State& state) const;
Motion::Method getUMotionMethod(const State& state) const;
Motion::Method getUDotMotionMethod(const State& state) const;
 
Real getOneQ(const State& state, int which) const;
 
Real getOneU(const State& state, int which) const;
 
Vector getQAsVector(const State& state) const;
Vector getUAsVector(const State& state) const;
 
Real getOneQDot   (const State& state, int which) const;
Vector getQDotAsVector(const State& state) const;
 
Real getOneUDot(const State& state, int which) const;
Real getOneQDotDot(const State& state, int which) const;
Vector getUDotAsVector(const State& state) const;
Vector getQDotDotAsVector(const State& state) const;
 
Vector getTauAsVector(const State& state) const;
 
Real getOneTau(const State& state, MobilizerUIndex which) const;
 
void setOneQ(State& state, int which, Real v) const;
void setOneU(State& state, int which, Real v) const;
 
void setQFromVector(State& state, const Vector& v) const;
void setUFromVector(State& state, const Vector& v) const;
 
void setQToFitTransform(State& state, const Transform& X_FM) const;
 
void setQToFitRotation(State& state, const Rotation& R_FM) const;
 
void setQToFitTranslation(State& state, const Vec3& p_FM) const;
 
void setUToFitVelocity(State& state, const SpatialVec& V_FM) const;
 
void setUToFitAngularVelocity(State& state, const Vec3& w_FM) const;
 
void setUToFitLinearVelocity(State& state, const Vec3& v_FM) const;
 
SpatialVec getHCol(const State& state, MobilizerUIndex ux) const;
 
SpatialVec getH_FMCol(const State& state, MobilizerUIndex ux) const;
 
// End of State Access Methods.
    // BASIC OPERATORS //
 
//------------------------------------------------------------------------------
Transform findBodyTransformInAnotherBody(const State& state, 
                                         const MobilizedBody& inBodyA) const
{
    const Transform& X_GA = inBodyA.getBodyTransform(state);
    const Transform& X_GB = this->getBodyTransform(state);
 
    return ~X_GA*X_GB; // X_AB=X_AG*X_GB
}
 
Rotation findBodyRotationInAnotherBody(const State& state, 
                                       const MobilizedBody& inBodyA) const
{
    const Rotation& R_GA = inBodyA.getBodyRotation(state);
    const Rotation& R_GB = this->getBodyRotation(state);
 
    return ~R_GA*R_GB; // R_AB=R_AG*R_GB
}
 
Vec3 findBodyOriginLocationInAnotherBody
   (const State& state, const MobilizedBody& toBodyA) const {   
    return toBodyA.findStationAtGroundPoint(state,
                                            getBodyOriginLocation(state)); 
}
 
SpatialVec findBodyVelocityInAnotherBody
   (const State& state, const MobilizedBody& inBodyA) const
{
    const SpatialVec& V_GB   = this->getBodyVelocity(state);
    const SpatialVec& V_GA   = inBodyA.getBodyVelocity(state);
    // angular velocity of B in A, exp in G
    const Vec3        w_AB_G = V_GB[0]-V_GA[0];             //  3 flops 
 
    // Angular vel. was easy, but for linear vel. we have to add in an wXr term.
 
    const Transform&  X_GB       = getBodyTransform(state);
    const Transform&  X_GA       = inBodyA.getBodyTransform(state);
    // vector from Ao to Bo, exp in G
    const Vec3        p_AB_G     = X_GB.p() - X_GA.p();     //  3 flops
    // d/dt p taken in G
    const Vec3        p_AB_G_dot = V_GB[1]  - V_GA[1];      //  3 flops
 
    // d/dt p taken in A, exp in G 
    const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G;      // 12 flops
 
    // We're done, but answer is expressed in Ground. Reexpress in A and return.
    return ~X_GA.R()*SpatialVec(w_AB_G, v_AB_G);            // 30 flops
}
 
Vec3 findBodyAngularVelocityInAnotherBody(const State& state,
                                          const MobilizedBody& inBodyA) const 
{
    const Vec3& w_GB   = getBodyAngularVelocity(state);
    const Vec3& w_GA   = inBodyA.getBodyAngularVelocity(state);
    // angular velocity of B in A, exp in G
    const Vec3  w_AB_G = w_GB-w_GA;                                 //  3 flops
 
    // Now reexpress in A.
    return inBodyA.expressGroundVectorInBodyFrame(state, w_AB_G);   // 15 flops
}
 
Vec3 findBodyOriginVelocityInAnotherBody(const State& state,
                                         const MobilizedBody& inBodyA) const
{
    // Doesn't save much to special case this one.
    return findBodyVelocityInAnotherBody(state,inBodyA)[1];
}
 
SpatialVec findBodyAccelerationInAnotherBody(const State& state,
                                             const MobilizedBody& inBodyA) const
{
    const Transform&  X_GA = inBodyA.getBodyTransform(state);
    const SpatialVec& V_GA = inBodyA.getBodyVelocity(state);
    const SpatialVec& A_GA = inBodyA.getBodyAcceleration(state);
    const Transform&  X_GB = this->getBodyTransform(state);
    const SpatialVec& V_GB = this->getBodyVelocity(state);
    const SpatialVec& A_GB = this->getBodyAcceleration(state);
 
    return findRelativeAcceleration(X_GA, V_GA, A_GA,
                                    X_GB, V_GB, A_GB);
}
 
Vec3 findBodyAngularAccelerationInAnotherBody
   (const State& state, const MobilizedBody& inBodyA) const
{
    const Rotation& R_GA = inBodyA.getBodyRotation(state);
    const Vec3&     w_GA = inBodyA.getBodyAngularVelocity(state);
    const Vec3&     w_GB = this->getBodyAngularVelocity(state);
    const Vec3&     b_GA = inBodyA.getBodyAngularAcceleration(state);
    const Vec3&     b_GB = this->getBodyAngularAcceleration(state);
 
    // relative ang. vel. of B in A, exp. in G
    const Vec3 w_AB_G     = w_GB - w_GA; // 3 flops
    const Vec3 w_AB_G_dot = b_GB - b_GA; // d/dt of w_AB_G taken in G; 3 flops
 
    // We have the derivative in G; change it to derivative in A by adding 
    // in contribution caused by motion of G in A, that is w_AG X w_AB_G. 
    // (Note that w_AG=-w_GA.)
    const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G; // ang. accel. of B in A
                                                    // 12 flops
 
    return ~R_GA * b_AB_G; // taken in A, expressed in A; 15 flops
}
 
Vec3 findBodyOriginAccelerationInAnotherBody(const State& state, 
                                             const MobilizedBody& inBodyA) const
{
    // Not much to be saved by trying to optimize this since the linear part
    // is the most expensive to calculate.
    return findBodyAccelerationInAnotherBody(state,inBodyA)[1];
}
 
SpatialVec findMobilizerReactionOnBodyAtMInGround(const State& state) const;
 
SpatialVec findMobilizerReactionOnBodyAtOriginInGround
   (const State& state) const;
 
SpatialVec findMobilizerReactionOnParentAtFInGround(const State& state) const;
 
SpatialVec findMobilizerReactionOnParentAtOriginInGround
   (const State& state) const;
 
Vec3 findStationLocationInGround
   (const State& state, const Vec3& stationOnB) const {
    return getBodyTransform(state) * stationOnB;
}
 
 
Vec3 findStationLocationInAnotherBody
   (const State& state, const Vec3& stationOnB, const MobilizedBody& toBodyA) 
    const 
{
    return toBodyA.findStationAtGroundPoint(state, 
                                findStationLocationInGround(state,stationOnB));
}
 
Vec3 findStationVelocityInGround
   (const State& state, const Vec3& stationOnB) const {
    const Vec3& w = getBodyAngularVelocity(state); // in G
    const Vec3& v = getBodyOriginVelocity(state);  // in G
    const Vec3  r = expressVectorInGroundFrame(state,stationOnB);   // 15 flops
    return v + w % r;                                               // 12 flops
}
 
 
Vec3 findStationVelocityInAnotherBody(const State&         state, 
                                      const Vec3&          stationOnBodyB,//p_BS
                                      const MobilizedBody& inBodyA) const
{
    const SpatialVec V_AB = 
        findBodyVelocityInAnotherBody(state, inBodyA); //51 flops
    // Bo->S rexpressed in A but not shifted to Ao
    const Vec3 p_BS_A = 
        expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA); //30
    return V_AB[1] + (V_AB[0] % p_BS_A);                              //12 flops
}
 
      
Vec3 findStationAccelerationInGround
   (const State& state, const Vec3& stationOnB) const {
    const Vec3& w = getBodyAngularVelocity(state);     // in G
    const Vec3& b = getBodyAngularAcceleration(state); // in G
    const Vec3& a = getBodyOriginAcceleration(state);  // in G
 
    const Vec3  r = expressVectorInGroundFrame(state,stationOnB); // 15 flops
    return a + b % r + w % (w % r);                               // 33 flops
}
 
Vec3 findStationAccelerationInAnotherBody(const State&         state,
                                          const Vec3&          stationOnBodyB, 
                                          const MobilizedBody& inBodyA) const 
{
    const Vec3       w_AB = 
        findBodyAngularVelocityInAnotherBody(state,inBodyA); // 18 flops
    const SpatialVec A_AB = 
        findBodyAccelerationInAnotherBody(state,inBodyA);    // 105 flops
    // Bo->S rexpressed in A but not shifted to Ao
    const Vec3       p_BS_A = 
        expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA); // 30
 
    return A_AB[1] + (A_AB[0] % p_BS_A) + w_AB % (w_AB % p_BS_A);    // 33 flops
}
 
void findStationLocationAndVelocityInGround
   (const State& state, const Vec3& locationOnB,
    Vec3& locationOnGround, Vec3& velocityInGround) const
{
    const Vec3& p_GB   = getBodyOriginLocation(state);
    const Vec3  p_BS_G = expressVectorInGroundFrame(state,locationOnB);//15flops
    locationOnGround = p_GB + p_BS_G;                                  // 3flops
 
    const Vec3& w_GB = getBodyAngularVelocity(state);
    const Vec3& v_GB = getBodyOriginVelocity(state);
    velocityInGround = v_GB + w_GB % p_BS_G;                         // 12 flops
}
 
 
void findStationLocationVelocityAndAccelerationInGround
   (const State& state, const Vec3& locationOnB,
    Vec3& locationOnGround, Vec3& velocityInGround, Vec3& accelerationInGround) 
    const
{
    const Rotation&  R_GB = getBodyRotation(state);
    const Vec3&      p_GB = getBodyOriginLocation(state);
 
    // re-express station vector p_BS in G
    const Vec3 r = R_GB*locationOnB;  // 15 flops
    locationOnGround  = p_GB + r;     //  3 flops
 
    const Vec3& w = getBodyAngularVelocity(state);      // in G
    const Vec3& v = getBodyOriginVelocity(state);       // in G
    const Vec3& b = getBodyAngularAcceleration(state);  // in G
    const Vec3& a = getBodyOriginAcceleration(state);   // in G
 
    const Vec3 wXr = w % r; // "whipping" velocity w X r due to ang vel; 9 flops
    velocityInGround     = v + wXr;                 // v + w X r (3 flops)
    accelerationInGround = a + b % r + w % wXr;     // 24 flops
}
 
Vec3 findMassCenterLocationInGround(const State& state) const {
    return findStationLocationInGround(state,getBodyMassCenterStation(state));
}
 
Vec3 findMassCenterLocationInAnotherBody
   (const State& state, const MobilizedBody& toBodyA) const {
    return findStationLocationInAnotherBody(state,
                                    getBodyMassCenterStation(state),toBodyA);
}
 
Vec3 findStationAtGroundPoint
   (const State& state, const Vec3& locationInG) const {
    return ~getBodyTransform(state) * locationInG;
}
 
Vec3 findStationAtAnotherBodyStation
   (const State& state, const MobilizedBody& fromBodyA, 
    const Vec3& stationOnA) const {
    return fromBodyA.findStationLocationInAnotherBody(state, stationOnA, *this);
}
 
Vec3 findStationAtAnotherBodyOrigin
   (const State& state, const MobilizedBody& fromBodyA) const {
    return findStationAtGroundPoint(state,
                                    fromBodyA.getBodyOriginLocation(state));
}
 
Vec3 findStationAtAnotherBodyMassCenter
   (const State& state, const MobilizedBody& fromBodyA) const {
    return fromBodyA.findStationLocationInAnotherBody(state,
                                        getBodyMassCenterStation(state),*this);
}
 
Transform findFrameTransformInGround
    (const State& state, const Transform& frameOnB) const {
    return getBodyTransform(state) * frameOnB;
}
 
SpatialVec findFrameVelocityInGround
   (const State& state, const Transform& frameOnB) const {
    return SpatialVec(getBodyAngularVelocity(state),
                      findStationVelocityInGround(state,frameOnB.p()));
}
 
SpatialVec findFrameAccelerationInGround
    (const State& state, const Transform& frameOnB) const {
    return SpatialVec(getBodyAngularAcceleration(state),
                      findStationAccelerationInGround(state,frameOnB.p()));
}
 
Vec3 expressVectorInGroundFrame
   (const State& state, const Vec3& vectorInB) const {
    return getBodyRotation(state)*vectorInB;
}
 
Vec3 expressGroundVectorInBodyFrame
   (const State& state, const Vec3& vectorInG) const {
    return ~getBodyRotation(state)*vectorInG;
}
 
Vec3 expressVectorInAnotherBodyFrame
   (const State& state, const Vec3& vectorInB, 
    const MobilizedBody& inBodyA) const
{
    return inBodyA.expressGroundVectorInBodyFrame(state, 
                                expressVectorInGroundFrame(state,vectorInB));
}
 
MassProperties expressMassPropertiesInGroundFrame(const State& state) const {
    const MassProperties& M_Bo_B = getBodyMassProperties(state);
    const Rotation&       R_GB   = getBodyRotation(state);
    return M_Bo_B.reexpress(~R_GB);
}
 
MassProperties expressMassPropertiesInAnotherBodyFrame
   (const State& state, const MobilizedBody& inBodyA) const {
    const MassProperties& M_Bo_B = getBodyMassProperties(state);
    const Rotation        R_AB   = findBodyRotationInAnotherBody(state,inBodyA);
    return M_Bo_B.reexpress(~R_AB);
}
 
// End of Basic Operators.
//------------------------------------------------------------------------------
SpatialMat calcBodySpatialInertiaMatrixInGround(const State& state) const
{
    if (isGround())
        return SpatialMat(Mat33(Infinity)); // sets diagonals to Inf
 
    const MassProperties& mp   = getBodyMassProperties(state);
    const Rotation&       R_GB = getBodyRotation(state);
        // re-express in Ground without shifting, convert to spatial mat.
    return mp.reexpress(~R_GB).toSpatialMat();
}
 
 
Inertia calcBodyCentralInertia(const State& state, 
                                MobilizedBodyIndex objectBodyB) const
{
    return getBodyMassProperties(state).calcCentralInertia();
}
 
Inertia calcBodyInertiaAboutAnotherBodyStation
   (const State& state, const MobilizedBody& inBodyA, 
    const Vec3& aboutLocationOnBodyA) const
{
    // get B's mass props MB, measured about Bo, exp. in B
    const MassProperties& MB_Bo_B = getBodyMassProperties(state);
 
    // Calculate the vector from the body B origin (current "about" point) to 
    // the new "about" point PA, expressed in B.
    const Vec3 p_Bo_PA = 
        findStationAtAnotherBodyStation(state, inBodyA, aboutLocationOnBodyA);
 
    // Now shift the "about" point for body B's inertia IB to PA, but still 
    // expressed in B.
    const Inertia IB_PA_B = MB_Bo_B.calcShiftedInertia(p_Bo_PA);
        
    // Finally reexpress the inertia in the A frame.
    const Rotation R_BA    = 
        inBodyA.findBodyRotationInAnotherBody(state, *this);
    const Inertia  IB_PA_A = IB_PA_B.reexpress(R_BA);
    return IB_PA_A;
}
 
 
SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State& state) {
    const MassProperties M_Bo_G = expressMassPropertiesInGroundFrame(state);
    const SpatialVec&    V_GB   = getBodyVelocity(state);
    return M_Bo_G.toSpatialMat() * V_GB;
}
 
SpatialVec calcBodyMomentumAboutBodyMassCenterInGround
   (const State& state) const {
    const MassProperties& M_Bo_B = getBodyMassProperties(state);
    const Rotation&       R_GB   = getBodyRotation(state);
 
    // Given a central inertia matrix I, angular velocity w, and mass center 
    // velocity v, the central angular momentum is Iw and linear momentum is mv.
    const Inertia I_Bc_B = M_Bo_B.calcCentralInertia();
    const Inertia I_Bc_G = I_Bc_B.reexpress(~R_GB);
    const Real  mb    = M_Bo_B.getMass();
    const Vec3& w_GB  = getBodyAngularVelocity(state);
    Vec3        v_GBc = 
        findStationVelocityInGround(state, M_Bo_B.getMassCenter());
 
    return SpatialVec( I_Bc_G*w_GB, mb*v_GBc );
}
 
Real calcStationToStationDistance(const State&         state,
                                  const Vec3&          locationOnBodyB,
                                  const MobilizedBody& bodyA,
                                  const Vec3&          locationOnBodyA) const
{
    if (isSameMobilizedBody(bodyA))
        return (locationOnBodyA-locationOnBodyB).norm();
 
    const Vec3 r_Go_PB = 
        this->findStationLocationInGround(state,locationOnBodyB);
    const Vec3 r_Go_PA = 
        bodyA.findStationLocationInGround(state,locationOnBodyA);
    return (r_Go_PA - r_Go_PB).norm();
}
 
Real calcStationToStationDistanceTimeDerivative
   (const State&         state,
    const Vec3&          locationOnBodyB,
    const MobilizedBody& bodyA,
    const Vec3&          locationOnBodyA) const
{
    if (isSameMobilizedBody(bodyA))
        return 0;
 
    Vec3 rB, rA, vB, vA;
    this->findStationLocationAndVelocityInGround(state,locationOnBodyB,rB,vB);
    bodyA.findStationLocationAndVelocityInGround(state,locationOnBodyA,rA,vA);
    const Vec3 r = rA-rB, v = vA-vB;
    const Real d = r.norm();
 
    // When the points are coincident, the rate of change of distance is just 
    // their relative speed. Otherwise, it is the speed along the direction of 
    // separation. 
    if (d==0) return v.norm();
    else return dot(v, r/d);
}
 
 
Real calcStationToStationDistance2ndTimeDerivative
   (const State&         state,
    const Vec3&          locationOnBodyB,
    const MobilizedBody& bodyA,
    const Vec3&          locationOnBodyA) const
{
    if (isSameMobilizedBody(bodyA))
        return 0;
 
    Vec3 rB, rA, vB, vA, aB, aA;
    this->findStationLocationVelocityAndAccelerationInGround
                                            (state,locationOnBodyB,rB,vB,aB);
    bodyA.findStationLocationVelocityAndAccelerationInGround
                                            (state,locationOnBodyA,rA,vA,aA);
 
    const Vec3 r = rA-rB, v = vA-vB, a = aA-aB;
    const Real d = r.norm();
        
    // This method is the time derivative of 
    // calcFixedPointToPointDistanceTimeDerivative(), so it must follow the same
    // two cases. That is, when the points are coincident the change in 
    // separation rate is the time derivative of the speed |v|, otherwise it is 
    // the time derivative of the speed along the separation vector.
 
    if (d==0) {
        // Return d/dt |v|. This has two cases: if |v| is zero, the rate of 
        // change of speed is just the points' relative acceleration magnitude. 
        // Otherwise, it is the acceleration in the direction of the current 
        // relative velocity vector.
        const Real s = v.norm(); // speed
        if (s==0) return a.norm();
        else return dot(a, v/s);
    }
 
    // Points are separated.
    const Vec3 u = r/d; // u is separation direction (a unit vector from B to A) 
    const Vec3 vp = v - dot(v,u)*u; // velocity perp. to separation direction
    return dot(a,u) + dot(vp,v)/d;
}
 
 
Vec3 calcBodyMovingPointVelocityInBody
   (const State&         state,
    const Vec3&          locationOnBodyB, 
    const Vec3&          velocityOnBodyB,
    const MobilizedBody& inBodyA) const
{
    SimTK_ASSERT_ALWAYS(!"unimplemented method", 
                        "MobilizedBody::calcBodyMovingPointVelocityInBody()"
                        " is not yet implemented -- any volunteers?");
    return Vec3::getNaN();
}
 
 
Vec3 calcBodyMovingPointAccelerationInBody
   (const State&         state, 
    const Vec3&          locationOnBodyB, 
    const Vec3&          velocityOnBodyB, 
    const Vec3&          accelerationOnBodyB,
    const MobilizedBody& inBodyA) const
{
    SimTK_ASSERT_ALWAYS(!"unimplemented method", 
                        "MobilizedBody::calcBodyMovingPointAccelerationInBody()"
                        " is not yet implemented -- any volunteers?");
    return Vec3::getNaN();
}
 
Real calcMovingPointToPointDistanceTimeDerivative
   (const State&         state,
    const Vec3&          locationOnBodyB,
    const Vec3&          velocityOnBodyB,
    const MobilizedBody& bodyA,
    const Vec3&          locationOnBodyA,
    const Vec3&          velocityOnBodyA) const
{
    SimTK_ASSERT_ALWAYS(!"unimplemented method", 
                "MobilizedBody::calcMovingPointToPointDistanceTimeDerivative()"
                " is not yet implemented -- any volunteers?");
    return NaN;
}
 
Real calcMovingPointToPointDistance2ndTimeDerivative
   (const State&         state,
    const Vec3&          locationOnBodyB,
    const Vec3&          velocityOnBodyB,
    const Vec3&          accelerationOnBodyB,
    const MobilizedBody& bodyA,
    const Vec3&          locationOnBodyA,
    const Vec3&          velocityOnBodyA,
    const Vec3&          accelerationOnBodyA) const
{
    SimTK_ASSERT_ALWAYS(!"unimplemented method", 
            "MobilizedBody::calcMovingPointToPointDistance2ndTimeDerivative()"
            " is not yet implemented -- any volunteers?");
    return NaN;
}
 
 
// End of High Level Operators.
        // CONSTRUCTION METHODS //
 
MobilizedBody() {}
 
explicit MobilizedBody(MobilizedBodyImpl* r);
 
//------------------------------------------------------------------------------
const Body& getBody() const;
 
Body& updBody();
 
MobilizedBody& setBody(const Body&);
 
int addBodyDecoration(const Transform&          X_BD, 
                      const DecorativeGeometry& geometry) {
    return updBody().addDecoration(X_BD, geometry);
}
int addBodyDecoration(const DecorativeGeometry& geometry) {
    return updBody().addDecoration(geometry);
}
 
int addOutboardDecoration(const Transform&          X_MD, 
                          const DecorativeGeometry& geometry);
int getNumOutboardDecorations() const;
const DecorativeGeometry& getOutboardDecoration(int i) const;
DecorativeGeometry& updOutboardDecoration(int i);
 
int addInboardDecoration(const Transform&          X_FD, 
                         const DecorativeGeometry& geometry);
int getNumInboardDecorations() const;
const DecorativeGeometry& getInboardDecoration(int i) const;
DecorativeGeometry& updInboardDecoration(int i);
 
MobilizedBody& setDefaultMassProperties(const MassProperties& m) {
    updBody().setDefaultRigidBodyMassProperties(m); // might not be allowed
    return *this;
}
 
const MassProperties& getDefaultMassProperties() const {
     // every body type can do this
    return getBody().getDefaultRigidBodyMassProperties();
}
 
void adoptMotion(Motion& ownerHandle);
 
void clearMotion();
 
bool hasMotion() const;
 
const Motion& getMotion() const;
 
MobilizedBody& setDefaultInboardFrame (const Transform& X_PF);
MobilizedBody& setDefaultOutboardFrame(const Transform& X_BM);
 
const Transform& getDefaultInboardFrame()  const; // X_PF
const Transform& getDefaultOutboardFrame() const; // X_BM
 
operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}
 
MobilizedBodyIndex getMobilizedBodyIndex()  const;
 
const MobilizedBody& getParentMobilizedBody() const;
 
const MobilizedBody& getBaseMobilizedBody()   const;
 
const SimbodyMatterSubsystem& getMatterSubsystem() const;
SimbodyMatterSubsystem& updMatterSubsystem();
 
bool isInSubsystem() const;
 
bool isInSameSubsystem(const MobilizedBody& mobod) const;
 
bool isSameMobilizedBody(const MobilizedBody& mobod) const;
 
bool isGround() const;
 
int getLevelInMultibodyTree() const;
    
MobilizedBody& cloneForNewParent(MobilizedBody& parent) const;
 
 
    // Utility operators //
 
Real getOneFromQPartition
   (const State& state, int which, const Vector& qlike) const;
 
Real& updOneFromQPartition
   (const State& state, int which, Vector& qlike) const;
 
Real getOneFromUPartition
   (const State& state, int which, const Vector& ulike) const;
 
Real& updOneFromUPartition(const State& state, int which, Vector& ulike) const;
 
void applyOneMobilityForce(const State& state, int which, Real f, 
                           Vector& mobilityForces) const
{
    updOneFromUPartition(state,which,mobilityForces) += f;
}
 
void convertQForceToUForce(const State&                        state,
                           const Array_<Real,MobilizerQIndex>& fq,
                           Array_<Real,MobilizerUIndex>&       fu) const;
 
void applyBodyForce(const State& state, const SpatialVec& spatialForceInG, 
                    Vector_<SpatialVec>& bodyForcesInG) const;
 
void applyBodyTorque(const State& state, const Vec3& torqueInG, 
                     Vector_<SpatialVec>& bodyForcesInG) const;
 
void applyForceToBodyPoint
   (const State& state, const Vec3& pointInB, const Vec3& forceInG,
    Vector_<SpatialVec>& bodyForcesInG) const;
 
// End of Construction and Misc Methods.
    // BUILT IN MOBILIZER DECLARATIONS //
 
//------------------------------------------------------------------------------
// These are the built-in MobilizedBody types. Each of these has a known 
// number of coordinates and speeds (at least a default number) so
// can define routines which return and accept specific-size arguments, e.g.
// Real (for 1-dof mobilizer) and Vec5 (for 5-dof mobilizer). Here is the
// conventional interface that each built-in should provide. The base type
// provides similar routines but using variable-sized or "one at a time"
// arguments. (Vec<1> here will actually be a Real; assume the built-in
// MobilizedBody class is "BuiltIn")
//
//    BuiltIn&       setDefaultQ(const Vec<nq>&);
//    const Vec<nq>& getDefaultQ() const;
//
//    const Vec<nq>& getQ[Dot[Dot]](const State&) const;
//    const Vec<nu>& getU[Dot](const State&) const;
//
//    void setQ(State&, const Vec<nq>&) const;
//    void setU(State&, const Vec<nu>&) const;
//
//    const Vec<nq>& getMyPartQ(const State&, const Vector& qlike) const;
//    const Vec<nu>& getMyPartU(const State&, const Vector& ulike) const;
//   
//    Vec<nq>& updMyPartQ(const State&, Vector& qlike) const;
//    Vec<nu>& updMyPartU(const State&, Vector& ulike) const;
//
// Each built in mobilized body type is declared in its own header
// file using naming convention MobilizedBody_Pin.h, for example. All the
// built-in headers are collected in MobilizedBody_BuiltIns.h; you should
// include new ones there also.
 
 
class Pin;              
typedef Pin Torsion;  
typedef Pin Revolute; 
 
class Universal;
class Cylinder;
class Weld;
 
class Slider;           
typedef Slider Prismatic; 
 
class Translation;      
typedef Translation Cartesian;       
typedef Translation CartesianCoords; 
 
class BendStretch;      
typedef BendStretch PolarCoords; 
 
class SphericalCoords;
class LineOrientation;
 
class Planar;
class Gimbal;
class Bushing;
 
class Ball;             
typedef Ball Orientation;   
typedef Ball Spherical;     
 
class Free;
class FreeLine;
class Screw;
class Ellipsoid;
class Custom;
class Ground;
class FunctionBased;
    
// Internal use only.
class PinImpl;
class SliderImpl;
class UniversalImpl;
class CylinderImpl;
class BendStretchImpl;
class PlanarImpl;
class GimbalImpl;
class BushingImpl;
class BallImpl;
class TranslationImpl;
class SphericalCoordsImpl;
class FreeImpl;
class LineOrientationImpl;
class FreeLineImpl;
class WeldImpl;
class ScrewImpl;
class EllipsoidImpl;
class CustomImpl;
class GroundImpl;
class FunctionBasedImpl;
};
 
} // namespace SimTK
 
#endif // SimTK_SIMBODY_MOBILIZED_BODY_H_