MobilizedBody.h

Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_MOBILIZED_BODY_H_
00002 #define SimTK_SIMBODY_MOBILIZED_BODY_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                      SimTK Core: SimTK Simbody(tm)                         *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK Core biosimulation toolkit originating from      *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org.               *
00011  *                                                                            *
00012  * Portions copyright (c) 2007-8 Stanford University and the Authors.         *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors: Paul Mitiguy, Peter Eastman                                  *
00015  *                                                                            *
00016  * Permission is hereby granted, free of charge, to any person obtaining a    *
00017  * copy of this software and associated documentation files (the "Software"), *
00018  * to deal in the Software without restriction, including without limitation  *
00019  * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
00020  * and/or sell copies of the Software, and to permit persons to whom the      *
00021  * Software is furnished to do so, subject to the following conditions:       *
00022  *                                                                            *
00023  * The above copyright notice and this permission notice shall be included in *
00024  * all copies or substantial portions of the Software.                        *
00025  *                                                                            *
00026  * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
00027  * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
00028  * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
00029  * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
00030  * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
00031  * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
00032  * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
00033  * -------------------------------------------------------------------------- */
00034 
00046 #include "SimTKmath.h"
00047 #include "simbody/internal/common.h"
00048 #include "simbody/internal/Body.h"
00049 
00050 #include <cassert>
00051 #include <vector>
00052 
00053 namespace SimTK {
00054 
00055 class SimbodyMatterSubsystem;
00056 class MobilizedBody;
00057 class MobilizedBodyImpl;
00058 
00059 // We only want the template instantiation to occur once. This symbol is defined in the SimTK core
00060 // compilation unit that instantiates the mobilized body class but should not be defined any other time.
00061 #ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
00062     extern template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
00063 #endif
00064 
00112 class SimTK_SIMBODY_EXPORT MobilizedBody : public PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true> {
00113 public:
00114 
00120     enum Direction {
00121         Forward = 0,
00122         Reverse = 1
00123     };
00124 
00126         // STATE ACCESS METHODS //
00128 
00132 
00133 
00146     const Transform& getBodyTransform(const State&) const; // X_GB
00147 
00156     const Rotation& getBodyRotation(const State& s) const {
00157         return getBodyTransform(s).R();
00158     }
00163     const Vec3& getBodyOriginLocation(const State& s) const {
00164         return getBodyTransform(s).p();
00165     }
00166 
00170     const Transform& getMobilizerTransform(const State&) const; // X_FM
00171 
00177     const SpatialVec& getBodyVelocity(const State&) const;          // V_GB
00178 
00182     const Vec3& getBodyAngularVelocity(const State& s) const {      // w_GB
00183         return getBodyVelocity(s)[0]; 
00184     }
00190     const Vec3& getBodyOriginVelocity(const State& s) const {       // v_GB
00191         return getBodyVelocity(s)[1];
00192     }
00193 
00199     const SpatialVec& getMobilizerVelocity(const State&) const;     // V_FM
00200 
00206     const SpatialVec& getBodyAcceleration(const State& s) const;    // A_GB
00207 
00211     const Vec3& getBodyAngularAcceleration(const State& s) const {  // b_GB
00212         return getBodyAcceleration(s)[0]; 
00213     }
00214 
00219     const Vec3& getBodyOriginAcceleration(const State& s) const {   // a_GB
00220         return getBodyAcceleration(s)[1];
00221     }
00222 
00228     const SpatialVec& getMobilizerAcceleration(const State&) const { // A_FM
00229         SimTK_ASSERT_ALWAYS(!"unimplemented method", 
00230             "MobilizedBody::getMobilizerAcceleration() is not yet implemented -- any volunteers?");
00231         return *(new SpatialVec());
00232     }
00233 
00236     const MassProperties& getBodyMassProperties(const State&) const;
00237 
00239     Real getBodyMass(const State& s) const {
00240         return getBodyMassProperties(s).getMass();
00241     }
00242 
00246     const Vec3& getBodyMassCenterStation(const State& s) const {
00247         return getBodyMassProperties(s).getMassCenter();
00248     }
00249 
00253     const Inertia& getBodyInertiaAboutBodyOrigin(const State& s) const {
00254         return getBodyMassProperties(s).getInertia();
00255     }
00256 
00261     const Transform& getInboardFrame (const State&) const;  // X_PF
00266     const Transform& getOutboardFrame(const State&) const;  // X_BM
00267 
00270     void setInboardFrame (State&, const Transform& X_PF) const;
00273     void setOutboardFrame(State&, const Transform& X_BM) const;
00274 
00275     // End of State Access - Bodies
00277 
00281 
00282 
00283 
00284     int getNumQ(const State&) const;
00287     int getNumU(const State&) const;
00288 
00292     Real getOneQ(const State&, int which) const;
00293 
00297     Real getOneU(const State&, int which) const;
00298 
00302     Vector getQAsVector(const State&) const;
00306     Vector getUAsVector(const State&) const;
00307 
00311     Real getOneQDot   (const State&, int which) const;
00315     Vector getQDotAsVector(const State&) const;
00316 
00320     Real getOneUDot   (const State&, int which) const;
00324     Real getOneQDotDot(const State&, int which) const;
00328     Vector getUDotAsVector   (const State&) const;
00332     Vector getQDotDotAsVector(const State&) const;
00333 
00337     void setOneQ(State&, int which, Real v) const;
00341     void setOneU(State&, int which, Real v) const;
00342 
00345     void setQFromVector(State& s, const Vector& v) const;
00348     void setUFromVector(State& s, const Vector& v) const;
00349 
00385 
00386     void setQToFitTransform      (State&, const Transform& X_FM) const;
00391     void setQToFitRotation       (State&, const Rotation&  R_FM) const;
00397     void setQToFitTranslation    (State&, const Vec3&      p_FM) const;
00398 
00405     void setUToFitVelocity          (State&, const SpatialVec& V_FM) const;
00412     void setUToFitAngularVelocity   (State&, const Vec3&       w_FM) const;
00419     void setUToFitLinearVelocity    (State&, const Vec3&       v_FM) const;
00420 
00423     SpatialVec getHCol(const State& s, UIndex ux) const;
00424 
00425     // End of State Access Methods.
00427 
00429         // BASIC OPERATORS //
00431 
00433 
00456 
00458 
00464     Transform findBodyTransformInAnotherBody(const State& s, 
00465                                              const MobilizedBody& inBodyA) const
00466     {
00467         const Transform& X_GA = inBodyA.getBodyTransform(s);
00468         const Transform& X_GB = this->getBodyTransform(s);
00469 
00470         return ~X_GA*X_GB; // X_AB=X_AG*X_GB
00471     }
00472 
00478     Rotation findBodyRotationInAnotherBody(const State& s, 
00479                                             const MobilizedBody& inBodyA) const
00480     {
00481         const Rotation& R_GA = inBodyA.getBodyRotation(s);
00482         const Rotation& R_GB = this->getBodyRotation(s);
00483 
00484         return ~R_GA*R_GB; // R_AB=R_AG*R_GB
00485     }
00486 
00492     Vec3 findBodyOriginLocationInAnotherBody(const State& s, const MobilizedBody& toBodyA) const {
00493         return toBodyA.findStationAtGroundPoint(s,getBodyOriginLocation(s));
00494     }
00495 
00501     SpatialVec findBodyVelocityInAnotherBody(const State& s,
00502                                              const MobilizedBody& inBodyA) const
00503     {
00504         const SpatialVec& V_GB   = this->getBodyVelocity(s);
00505         const SpatialVec& V_GA   = inBodyA.getBodyVelocity(s);
00506         const Vec3        w_AB_G = V_GB[0]-V_GA[0]; // angular velocity of B in A, exp in G   ( 3 flops)
00507 
00508         // Angular velocity was easy, but for linear velocity we have to add in an wXr term.
00509         const Transform&  X_GB       = getBodyTransform(s);
00510         const Transform&  X_GA       = inBodyA.getBodyTransform(s);
00511         const Vec3        p_AB_G     = X_GB.p() - X_GA.p(); // vector from OA to OB, exp in G ( 3 flops)
00512         const Vec3        p_AB_G_dot = V_GB[1]  - V_GA[1];  // d/dt p taken in G              ( 3 flops)
00513 
00514         const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G;  // d/dt p taken in A, exp in G    (12 flops)
00515 
00516         // We're done, but the answer is expressed in Ground. Reexpress in A and return.
00517         return ~X_GA.R()*SpatialVec(w_AB_G, v_AB_G);        //                                (30 flops)
00518     }
00519 
00524     Vec3 findBodyAngularVelocityInAnotherBody(const State& s,
00525                                        const MobilizedBody& inBodyA) const 
00526     {
00527         const Vec3& w_GB   = getBodyAngularVelocity(s);
00528         const Vec3& w_GA   = inBodyA.getBodyAngularVelocity(s);
00529         const Vec3  w_AB_G = w_GB-w_GA; // angular velocity of B in A, exp in G ( 3 flops)
00530 
00531         // Now reexpress in A.
00532         return inBodyA.expressGroundVectorInBodyFrame(s, w_AB_G); //            (15 flops)
00533     }
00534 
00539     Vec3 findBodyOriginVelocityInAnotherBody(const State& s,
00540                                       const MobilizedBody& inBodyA) const
00541     {
00542         // Doesn't save much to special case this one.
00543         return findBodyVelocityInAnotherBody(s,inBodyA)[1];
00544     }
00545 
00550     SpatialVec findBodyAccelerationInAnotherBody(const State& s,
00551                                                  const MobilizedBody& inBodyA) const
00552     {
00553         const Vec3&       p_GB = this->getBodyOriginLocation(s);
00554         const Transform&  X_GA = inBodyA.getBodyTransform(s);
00555         const SpatialVec& V_GB = this->getBodyVelocity(s);
00556         const SpatialVec& V_GA = inBodyA.getBodyVelocity(s);
00557         const SpatialVec& A_GB = this->getBodyAcceleration(s);
00558         const SpatialVec& A_GA = inBodyA.getBodyAcceleration(s);
00559         const Vec3&       p_GA = X_GA.p();
00560         const Vec3&       w_GA = V_GA[0];
00561         const Vec3&       w_GB = V_GB[0];
00562         const Vec3&       b_GA = A_GA[0];
00563         const Vec3&       b_GB = A_GB[0];
00564 
00565         const Vec3 p_AB_G        = p_GB     - p_GA;         // vector from OA to OB, in G   ( 3 flops)
00566         const Vec3 p_AB_G_dot    = V_GB[1]  - V_GA[1];      // d/dt p taken in G            ( 3 flops)
00567         const Vec3 p_AB_G_dotdot = A_GB[1]  - A_GA[1];      // d^2/dt^2 taken in G          ( 3 flops)
00568 
00569         const Vec3 w_AB_G     = w_GB - w_GA;                // relative ang. vel. of B in A, exp. in G (3 flops)
00570         const Vec3 v_AB_G     = p_AB_G_dot - w_GA % p_AB_G; // d/dt p taken in A, exp in G  (12 flops)
00571 
00572         const Vec3 w_AB_G_dot = b_GB - b_GA;                // d/dt of w_AB_G taken in G    ( 3 flops)
00573         const Vec3 v_AB_G_dot = p_AB_G_dotdot - (b_GA % p_AB_G + w_GA % p_AB_G_dot); // d/dt v_AB_G taken in G
00574                                                                                      //     (24 flops)
00575 
00576         // We have the derivative in G; change it to derivative in A by adding in contribution caused
00577         // by motion of G in A, that is w_AG X w_AB_G. (Note that w_AG=-w_GA.)
00578         const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G; // ang. accel. of B in A            (12 flops)
00579         const Vec3 a_AB_G = v_AB_G_dot - w_GA % v_AB_G; // taken in A, exp. in G            (12 flops)
00580 
00581         return ~X_GA.R() * SpatialVec(b_AB_G, a_AB_G); // taken in A, expressed in A        (30 flops)
00582     }
00583 
00588     Vec3 findBodyAngularAccelerationInAnotherBody(const State& s,
00589                                                   const MobilizedBody& inBodyA) const
00590     {
00591         const Rotation& R_GA = inBodyA.getBodyRotation(s);
00592         const Vec3&     w_GA = inBodyA.getBodyAngularVelocity(s);
00593         const Vec3&     w_GB = this->getBodyAngularVelocity(s);
00594         const Vec3&     b_GA = inBodyA.getBodyAngularAcceleration(s);
00595         const Vec3&     b_GB = this->getBodyAngularAcceleration(s);
00596 
00597         const Vec3 w_AB_G     = w_GB - w_GA;                // relative ang. vel. of B in A, exp. in G (3 flops)
00598         const Vec3 w_AB_G_dot = b_GB - b_GA;                // d/dt of w_AB_G taken in G    ( 3 flops)
00599 
00600         // We have the derivative in G; change it to derivative in A by adding in contribution caused
00601         // by motion of G in A, that is w_AG X w_AB_G. (Note that w_AG=-w_GA.)
00602         const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G; // ang. accel. of B in A            (12 flops)
00603 
00604         return ~R_GA * b_AB_G; // taken in A, expressed in A                                (15 flops)
00605     }
00606 
00611     Vec3 findBodyOriginAccelerationInAnotherBody(const State& s, 
00612                                           const MobilizedBody& inBodyA) const
00613     {
00614         // Not much to be saved by trying to optimize this since the linear part
00615         // is the most expensive to calculate.
00616         return findBodyAccelerationInAnotherBody(s,inBodyA)[1];
00617     }
00618 
00624     Vec3 findStationLocationInGround(const State& s, const Vec3& stationOnB) const {
00625         return getBodyTransform(s) * stationOnB;
00626     }
00627 
00628 
00637     Vec3 findStationLocationInAnotherBody(const State& s, const Vec3& stationOnB, 
00638                                const MobilizedBody& toBodyA) const
00639     {
00640         return toBodyA.findStationAtGroundPoint(s, findStationLocationInGround(s,stationOnB));
00641     }
00642 
00646     Vec3 findStationVelocityInGround(const State& s, const Vec3& stationOnB) const {
00647         const Vec3& w = getBodyAngularVelocity(s); // in G
00648         const Vec3& v = getBodyOriginVelocity(s);  // in G
00649         const Vec3  r = expressVectorInGroundFrame(s,stationOnB); // 15 flops
00650         return v + w % r;                                         // 12 flops
00651     }
00652 
00653 
00658     Vec3 findStationVelocityInAnotherBody(const State& s, 
00659                                           const Vec3&          stationOnBodyB, // p_BS
00660                                           const MobilizedBody& inBodyA) const
00661     {
00662         const SpatialVec V_AB   = findBodyVelocityInAnotherBody(s,inBodyA); // (51 flops)
00663          // OB->S rexpressed in A but not shifted to OA
00664         const Vec3       p_BS_A = expressVectorInAnotherBodyFrame(s, stationOnBodyB, inBodyA);
00665                                                                             // (30 flops)
00666         return V_AB[1] + (V_AB[0] % p_BS_A);                                // (12 flops)
00667     }
00668 
00669       
00673     Vec3 findStationAccelerationInGround(const State& s, const Vec3& stationOnB) const {
00674         const Vec3& w = getBodyAngularVelocity(s);     // in G
00675         const Vec3& b = getBodyAngularAcceleration(s); // in G
00676         const Vec3& a = getBodyOriginAcceleration(s);  // in G
00677 
00678         const Vec3  r = expressVectorInGroundFrame(s,stationOnB); // 15 flops
00679         return a + b % r + w % (w % r);                           // 33 flops
00680     }
00681 
00686     Vec3 findStationAccelerationInAnotherBody(const State& s,
00687                                               const Vec3&          stationOnBodyB, 
00688                                               const MobilizedBody& inBodyA) const 
00689     {
00690         const Vec3       w_AB = findBodyAngularVelocityInAnotherBody(s,inBodyA);  // ( 18 flops)
00691         const SpatialVec A_AB = findBodyAccelerationInAnotherBody(s,inBodyA);     // (105 flops)
00692          // OB->S rexpressed in A but not shifted to OA
00693         const Vec3       p_BS_A = expressVectorInAnotherBodyFrame(s, stationOnBodyB, inBodyA);
00694                                                                                   // ( 30 flops)
00695 
00696         return A_AB[1] + (A_AB[0] % p_BS_A) + w_AB % (w_AB % p_BS_A);             // ( 33 flops)
00697     }
00698 
00702     void findStationLocationAndVelocityInGround(const State& s, const Vec3& locationOnB,
00703                                                 Vec3& locationOnGround, Vec3& velocityInGround) const
00704     {
00705         const Vec3& p_GB   = getBodyOriginLocation(s);
00706         const Vec3  p_BS_G = expressVectorInGroundFrame(s,locationOnB); // 15 flops
00707         locationOnGround = p_GB + p_BS_G;                               //  3 flops
00708 
00709         const Vec3& w_GB = getBodyAngularVelocity(s);
00710         const Vec3& v_GB = getBodyOriginVelocity(s);
00711         velocityInGround = v_GB + w_GB % p_BS_G;                        // 12 flops
00712     }
00713 
00714 
00718     void findStationLocationVelocityAndAccelerationInGround
00719        (const State& s, const Vec3& locationOnB,
00720         Vec3& locationOnGround, Vec3& velocityInGround, Vec3& accelerationInGround) const
00721     {
00722         const Rotation&  R_GB = getBodyRotation(s);
00723         const Vec3&      p_GB = getBodyOriginLocation(s);
00724 
00725         const Vec3 r = R_GB*locationOnB; // re-express station vector p_BS in G (15 flops)
00726         locationOnGround  = p_GB + r;    // 3 flops
00727 
00728         const Vec3& w = getBodyAngularVelocity(s);      // in G
00729         const Vec3& v = getBodyOriginVelocity(s);       // in G
00730         const Vec3& b = getBodyAngularAcceleration(s);  // in G
00731         const Vec3& a = getBodyOriginAcceleration(s);   // in G
00732 
00733         const Vec3 wXr = w % r; // "whipping" velocity w X r due to angular velocity (9 flops)
00734         velocityInGround     = v + wXr;                 // v + w X r (3 flops)
00735         accelerationInGround = a + b % r + w % wXr;     // 24 flops
00736     }
00737 
00740     Vec3 findMassCenterLocationInGround(const State& s) const {
00741         return findStationLocationInGround(s,getBodyMassCenterStation(s));
00742     }
00743 
00747     Vec3 findMassCenterLocationInAnotherBody(const State& s, const MobilizedBody& toBodyA) const {
00748         return findStationLocationInAnotherBody(s,getBodyMassCenterStation(s),toBodyA);
00749     }
00750 
00756     Vec3 findStationAtGroundPoint(const State& s, const Vec3& locationInG) const {
00757         return ~getBodyTransform(s) * locationInG;
00758     }
00759 
00765     Vec3 findStationAtAnotherBodyStation(const State& s, const MobilizedBody& fromBodyA, 
00766                                 const Vec3& stationOnA) const {
00767         return fromBodyA.findStationLocationInAnotherBody(s, stationOnA, *this);
00768     }
00769 
00773     Vec3 findStationAtAnotherBodyOrigin(const State& s, const MobilizedBody& fromBodyA) const {
00774         return findStationAtGroundPoint(s,fromBodyA.getBodyOriginLocation(s));
00775     }
00776 
00780     Vec3 findStationAtAnotherBodyMassCenter(const State& s, const MobilizedBody& fromBodyA) const {
00781         return fromBodyA.findStationLocationInAnotherBody(s,getBodyMassCenterStation(s),*this);
00782     }
00783 
00787     Vec3 expressVectorInGroundFrame(const State& s, const Vec3& vectorInB) const {
00788         return getBodyRotation(s)*vectorInB;
00789     }
00790 
00794     Vec3 expressGroundVectorInBodyFrame(const State& s, const Vec3& vectorInG) const {
00795         return ~getBodyRotation(s)*vectorInG;
00796     }
00797 
00803     Vec3 expressVectorInAnotherBodyFrame(const State& s, const Vec3& vectorInB,
00804                                          const MobilizedBody& inBodyA) const
00805     {
00806         return inBodyA.expressGroundVectorInBodyFrame(s, expressVectorInGroundFrame(s,vectorInB));
00807     }
00808 
00812     MassProperties expressMassPropertiesInGroundFrame(const State& s) {
00813             const MassProperties& M_OB_B = getBodyMassProperties(s);
00814             const Rotation&       R_GB   = getBodyRotation(s);
00815             return M_OB_B.reexpress(~R_GB);
00816     }
00817 
00821     MassProperties expressMassPropertiesInAnotherBodyFrame(const State& s, const MobilizedBody& inBodyA) {
00822             const MassProperties& M_OB_B = getBodyMassProperties(s);
00823             const Rotation        R_AB   = findBodyRotationInAnotherBody(s,inBodyA);
00824             return M_OB_B.reexpress(~R_AB);
00825     }
00826 
00827     // End of Basic Operators.
00829 
00834 
00835 
00837 
00857     SpatialMat calcBodySpatialInertiaMatrixInGround(const State& s) const
00858     {
00859         if (isGround())
00860             return SpatialMat(Mat33(Infinity)); // sets diagonals to Inf
00861 
00862         const MassProperties& mp   = getBodyMassProperties(s);
00863         const Rotation&       R_GB = getBodyRotation(s);
00864          // re-express in Ground without shifting, convert to spatial mat.
00865         return mp.reexpress(~R_GB).toSpatialMat();
00866     }
00867 
00873     Inertia calcBodyCentralInertia(const State& s, 
00874                                    MobilizedBodyIndex objectBodyB) const
00875     {
00876         return getBodyMassProperties(s).calcCentralInertia();
00877     }
00878 
00882     Inertia calcBodyInertiaAboutAnotherBodyStation(const State& s,
00883                                                    const MobilizedBody& inBodyA, 
00884                                                    const Vec3&          aboutLocationOnBodyA) const
00885     {
00886         // get B's mass props MB, measured about OB, exp. in B
00887         const MassProperties& MB_OB_B = getBodyMassProperties(s);
00888 
00889         // Calculate the vector from the body B origin (current "about" point) to the new "about" point PA,
00890         // expressed in B.
00891         const Vec3 r_OB_PA = findStationAtAnotherBodyStation(s, inBodyA, aboutLocationOnBodyA);
00892 
00893         // Now shift the "about" point for body B's inertia IB to PA, but still expressed in B.
00894         const Inertia IB_PA_B = MB_OB_B.calcShiftedInertia(r_OB_PA);
00895         
00896         // Finally reexpress the inertia in the A frame.
00897         const Rotation R_BA    = inBodyA.findBodyRotationInAnotherBody(s, *this);
00898         const Inertia  IB_PA_A = IB_PA_B.reexpress(R_BA);
00899         return IB_PA_A;
00900     }
00901 
00902 
00905     SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State& s) {
00906         const MassProperties M_OB_G = expressMassPropertiesInGroundFrame(s);
00907         const SpatialVec&    V_GB   = getBodyVelocity(s);
00908         return M_OB_G.toSpatialMat() * V_GB;
00909     }
00910 
00913     SpatialVec calcBodyMomentumAboutBodyMassCenterInGround(const State& s) const {
00914         const MassProperties& M_OB_B = getBodyMassProperties(s);
00915         const Rotation&       R_GB   = getBodyRotation(s);
00916 
00917         // Given a central inertia matrix I, angular velocity w, and mass center velocity v,
00918         // the central angular momentum is Iw and linear momentum is mv.
00919         const Inertia I_CB_B = M_OB_B.calcCentralInertia();
00920         const Inertia I_CB_G = I_CB_B.reexpress(~R_GB);
00921         const Real    mb     = M_OB_B.getMass();
00922         const Vec3&   w_GB   = getBodyAngularVelocity(s);
00923         Vec3          v_G_CB = findStationVelocityInGround(s, M_OB_B.getMassCenter());
00924 
00925         return SpatialVec( I_CB_G*w_GB, mb*v_G_CB );
00926     }
00927 
00931     Real calcStationToStationDistance(const State& s,
00932                                       const Vec3&          locationOnBodyB,
00933                                       const MobilizedBody& bodyA,
00934                                       const Vec3&          locationOnBodyA) const
00935     {
00936         if (isSameMobilizedBody(bodyA))
00937             return (locationOnBodyA-locationOnBodyB).norm();
00938 
00939         const Vec3 r_OG_PB = this->findStationLocationInGround(s,locationOnBodyB);
00940         const Vec3 r_OG_PA = bodyA.findStationLocationInGround(s,locationOnBodyA);
00941         return (r_OG_PA - r_OG_PB).norm();
00942     }
00943 
00948     Real calcStationToStationDistanceTimeDerivative(const State& s,
00949                                                     const Vec3&          locationOnBodyB,
00950                                                     const MobilizedBody& bodyA,
00951                                                     const Vec3&          locationOnBodyA) const
00952     {
00953         if (isSameMobilizedBody(bodyA))
00954             return 0;
00955 
00956         Vec3 rB, rA, vB, vA;
00957         this->findStationLocationAndVelocityInGround(s,locationOnBodyB,rB,vB);
00958         bodyA.findStationLocationAndVelocityInGround(s,locationOnBodyA,rA,vA);
00959         const Vec3 r = rA-rB, v = vA-vB;
00960         const Real d = r.norm();
00961 
00962         // When the points are coincident, the rate of change of distance is just their relative speed.
00963         // Otherwise, it is the speed along the direction of separation. 
00964         if (d==0) return v.norm();
00965         else return dot(v, r/d);
00966     }
00967 
00968 
00973     Real calcStationToStationDistance2ndTimeDerivative(const State& s,
00974                                                        const Vec3&          locationOnBodyB,
00975                                                        const MobilizedBody& bodyA,
00976                                                        const Vec3&          locationOnBodyA) const
00977     {
00978         if (isSameMobilizedBody(bodyA))
00979             return 0;
00980 
00981         Vec3 rB, rA, vB, vA, aB, aA;
00982         this->findStationLocationVelocityAndAccelerationInGround(s,locationOnBodyB,rB,vB,aB);
00983         bodyA.findStationLocationVelocityAndAccelerationInGround(s,locationOnBodyA,rA,vA,aA);
00984 
00985         const Vec3 r = rA-rB, v = vA-vB, a = aA-aB;
00986         const Real d = r.norm();
00987         
00988         // This method is the time derivative of calcFixedPointToPointDistanceTimeDerivative(), so it
00989         // must follow the same two cases. That is, when the points are coincident the change in 
00990         // separation rate is the time derivative of the speed |v|, otherwise it is the time
00991         // derivative of the speed along the separation vector.
00992 
00993         if (d==0) {
00994             // Return d/dt |v|. This has two cases: if |v| is zero, the rate of change of speed is
00995             // just the points' relative acceleration magnitude. Otherwise, it is the acceleration
00996             // in the direction of the current relative velocity vector.
00997             const Real s = v.norm(); // speed
00998             if (s==0) return a.norm();
00999             else return dot(a, v/s);
01000         }
01001 
01002         // Points are separated.
01003         const Vec3 u = r/d;             // u is the separation direction (a unit vector from B to A) 
01004         const Vec3 vp = v - dot(v,u)*u; // velocity perpendicular to separation direction
01005         return dot(a,u) + dot(vp,v)/d;
01006     }
01007 
01008 
01011     Vec3 calcBodyMovingPointVelocityInBody(const State& s,
01012                                            const Vec3& locationOnBodyB, 
01013                                            const Vec3& velocityOnBodyB,
01014                                            const MobilizedBody& inBodyA) const
01015     {
01016         SimTK_ASSERT_ALWAYS(!"unimplemented method", 
01017             "MobilizedBody::calcBodyMovingPointVelocityInBody() is not yet implemented -- any volunteers?");
01018         return Vec3::getNaN();
01019     }
01020 
01021 
01024     Vec3 calcBodyMovingPointAccelerationInBody(const State& s, 
01025                                                const Vec3&          locationOnBodyB, 
01026                                                const Vec3&          velocityOnBodyB, 
01027                                                const Vec3&          accelerationOnBodyB,
01028                                                const MobilizedBody& inBodyA) const
01029     {
01030         SimTK_ASSERT_ALWAYS(!"unimplemented method", 
01031             "MobilizedBody::calcBodyMovingPointAccelerationInBody() is not yet implemented -- any volunteers?");
01032         return Vec3::getNaN();
01033     }
01034 
01041     Real calcMovingPointToPointDistanceTimeDerivative(const State& s,
01042                                                       const Vec3&          locationOnBodyB,
01043                                                       const Vec3&          velocityOnBodyB,
01044                                                       const MobilizedBody& bodyA,
01045                                                       const Vec3&          locationOnBodyA,
01046                                                       const Vec3&          velocityOnBodyA) const
01047     {
01048         SimTK_ASSERT_ALWAYS(!"unimplemented method", 
01049             "MobilizedBody::calcMovingPointToPointDistanceTimeDerivative() is not yet implemented -- any volunteers?");
01050         return NaN;
01051     }
01052 
01059     Real calcMovingPointToPointDistance2ndTimeDerivative(const State& s,
01060                                                          const Vec3&          locationOnBodyB,
01061                                                          const Vec3&          velocityOnBodyB,
01062                                                          const Vec3&          accelerationOnBodyB,
01063                                                          const MobilizedBody& bodyA,
01064                                                          const Vec3&          locationOnBodyA,
01065                                                          const Vec3&          velocityOnBodyA,
01066                                                          const Vec3&          accelerationOnBodyA) const
01067     {
01068         SimTK_ASSERT_ALWAYS(!"unimplemented method", 
01069             "MobilizedBody::calcMovingPointToPointDistance2ndTimeDerivative() is not yet implemented -- any volunteers?");
01070         return NaN;
01071     }
01072 
01073 
01074     // End of High Level Operators.
01076 
01077 
01079             // CONSTRUCTION METHODS //
01081 
01085     MobilizedBody();
01086 
01088     explicit MobilizedBody(MobilizedBodyImpl* r);
01089 
01094 
01095 
01101     MobilizedBody& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01102         (void)updBody().addDecoration(X_BD,g);
01103         return *this;
01104     }
01105 
01109     MobilizedBody& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry&);
01110 
01114     MobilizedBody& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry&);
01115 
01117     const Body& getBody() const;
01121     Body& updBody();
01122 
01128     MobilizedBody& setBody(const Body&);
01129 
01136     MobilizedBody& setDefaultMassProperties(const MassProperties& m) {
01137         updBody().setDefaultRigidBodyMassProperties(m); // might not be allowed
01138         return *this;
01139     }
01140 
01142     const MassProperties& getDefaultMassProperties() const {
01143         return getBody().getDefaultRigidBodyMassProperties(); // every body type can do this
01144     }
01145 
01151     MobilizedBody& setDefaultInboardFrame (const Transform& X_PF);
01157     MobilizedBody& setDefaultOutboardFrame(const Transform& X_BM);
01158 
01163     const Transform& getDefaultInboardFrame()  const; // X_PF
01167     const Transform& getDefaultOutboardFrame() const; // X_BM
01168 
01171     operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}
01172 
01175     MobilizedBodyIndex     getMobilizedBodyIndex()  const;
01176 
01179     const MobilizedBody&   getParentMobilizedBody() const;
01180 
01184     const MobilizedBody&   getBaseMobilizedBody()   const;
01185 
01188     const SimbodyMatterSubsystem& getMatterSubsystem()      const;
01191     SimbodyMatterSubsystem&       updMatterSubsystem();
01192 
01194     bool isInSubsystem() const;
01195 
01199     bool isInSameSubsystem(const MobilizedBody&) const;
01200 
01205     bool isSameMobilizedBody(const MobilizedBody& mBody) const;
01206 
01209     bool isGround() const;
01210 
01215     int getLevelInMultibodyTree() const;
01216     
01219     MobilizedBody& cloneForNewParent(MobilizedBody& parent) const;
01220 
01221         // Utility operators //
01222 
01226     Real  getOneFromQPartition(const State&, int which, const Vector& qlike) const;
01227 
01232     Real& updOneFromQPartition(const State&, int which, Vector& qlike) const;
01233 
01237     Real  getOneFromUPartition(const State&, int which, const Vector& ulike) const;
01238 
01243     Real& updOneFromUPartition(const State&, int which, Vector& ulike) const;
01244 
01250     void applyOneMobilityForce(const State& s, int which, Real f, 
01251                                Vector& mobilityForces) const
01252     {
01253         updOneFromUPartition(s,which,mobilityForces) += f;
01254     }
01255 
01262     void applyBodyForce(const State& s, const SpatialVec& spatialForceInG, 
01263                         Vector_<SpatialVec>& bodyForcesInG) const;
01264 
01270     void applyBodyTorque(const State& s, const Vec3& torqueInG, 
01271                          Vector_<SpatialVec>& bodyForcesInG) const;
01272 
01282     void applyForceToBodyPoint(const State& s, const Vec3& pointInB, const Vec3& forceInG,
01283                                Vector_<SpatialVec>& bodyForcesInG) const;
01284 
01285     // End of Construction and Misc Methods.
01287 
01289         // BUILT IN MOBILIZER DECLARATIONS //
01291 
01292     // These are the built-in MobilizedBody types. Types on the same line are
01293     // synonymous. Each of these has a known number of coordinates and speeds 
01294     // (at least a default number) so
01295     // can define routines which return and accept specific-size arguments, e.g.
01296     // Real (for 1-dof mobilizer) and Vec5 (for 5-dof mobilizer). Here is the
01297     // conventional interface that each built-in should provide. The base type
01298     // provides similar routines but using variable-sized or "one at a time"
01299     // arguments. (Vec<1> here will actually be a Real; assume the built-in
01300     // MobilizedBody class is "BuiltIn")
01301     //
01302     //    BuiltIn&       setDefaultQ(const Vec<nq>&);
01303     //    const Vec<nq>& getDefaultQ() const;
01304     //
01305     //    const Vec<nq>& getQ[Dot[Dot]](const State&) const;
01306     //    const Vec<nu>& getU[Dot](const State&) const;
01307     //
01308     //    void setQ(State&, const Vec<nq>&) const;
01309     //    void setU(State&, const Vec<nu>&) const;
01310     //
01311     //    const Vec<nq>& getMyPartQ(const State&, const Vector& qlike) const;
01312     //    const Vec<nu>& getMyPartU(const State&, const Vector& ulike) const;
01313     //   
01314     //    Vec<nq>& updMyPartQ(const State&, Vector& qlike) const;
01315     //    Vec<nu>& updMyPartU(const State&, Vector& ulike) const;
01316     //      
01317 
01318 
01319     class Pin;              typedef Pin             Torsion;
01320     class Universal;
01321     class Cylinder;
01322 
01323     class Weld;
01324     class Slider;           typedef Slider          Prismatic;
01325     class Translation2D;    typedef Translation2D   Cartesian2D, CartesianCoords2D;
01326     class Translation;      typedef Translation     Cartesian, CartesianCoords;
01327     class BendStretch;      typedef BendStretch     PolarCoords;
01328     class TorsionStretch;   typedef TorsionStretch  ConicalCoords2D;
01329     class SphericalCoords;
01330     class CylindricalCoords;
01331     class LineOrientation;
01332 
01333 
01334     class Planar;
01335     class Gimbal;
01336     class Ball;             typedef Ball            Orientation, Spherical;
01337     class Free;
01338     class FreeLine;
01339     class Screw;
01340     class Ellipsoid;
01341     class Custom;
01342     class Ground;
01343     class FunctionBased;
01344     
01345     class PinImpl;
01346     class SliderImpl;
01347     class UniversalImpl;
01348     class CylinderImpl;
01349     class BendStretchImpl;
01350     class TorsionStretchImpl;
01351     class PlanarImpl;
01352     class GimbalImpl;
01353     class BallImpl;
01354     class TranslationImpl;
01355     class SphericalCoordsImpl;
01356     class FreeImpl;
01357     class LineOrientationImpl;
01358     class FreeLineImpl;
01359     class WeldImpl;
01360     class ScrewImpl;
01361     class EllipsoidImpl;
01362     class CustomImpl;
01363     class GroundImpl;
01364     class FunctionBasedImpl;
01365 
01366 };
01367 
01371 class SimTK_SIMBODY_EXPORT MobilizedBody::Pin : public MobilizedBody {
01372 public:
01373         // SPECIALIZED INTERFACE FOR PIN MOBILIZER
01374 
01375     // "Angle" is just a nicer name for a pin joint's lone generalized coordinate q.
01376     Pin& setDefaultAngle(Real angleInRadians) {return setDefaultQ(angleInRadians);}
01377     Real getDefaultAngle() const              {return getDefaultQ();}
01378 
01379         // Friendly, mobilizer-specific access to generalized coordinates and speeds.
01380 
01381     void setAngle(State& s, Real angleInRadians) {setQ(s, angleInRadians);}
01382     Real getAngle(const State& s) const {return getQ(s);}
01383 
01384     void setRate(State& s, Real rateInRadiansPerTime) {setU(s, rateInRadiansPerTime);}
01385     Real getRate(const State& s) const {return getU(s);}
01386 
01387     // Mobility forces are "u-like", that is, one per dof.
01388     Real getAppliedPinTorque(const State& s, const Vector& mobilityForces) const {
01389         return getMyPartU(s,mobilityForces);
01390     }
01391     void applyPinTorque(const State& s, Real torque, Vector& mobilityForces) const {
01392         updMyPartU(s,mobilityForces) += torque;
01393     }
01394 
01395         // STANDARDIZED MOBILIZED BODY INTERFACE
01396 
01397         // required constructors
01398     explicit Pin(Direction=Forward);
01399     Pin(MobilizedBody& parent, const Body&, Direction=Forward);
01400     Pin(MobilizedBody& parent, const Transform& inbFrame,
01401         const Body&,           const Transform& outbFrame,
01402         Direction=Forward);
01403 
01404         // access to generalized coordinates q and generalized speeds u
01405     Pin& setDefaultQ(Real);
01406     Real getDefaultQ() const;
01407 
01408     Real getQ(const State&) const;
01409     Real getQDot(const State&) const;
01410     Real getQDotDot(const State&) const;
01411     Real getU(const State&) const;
01412     Real getUDot(const State&) const;
01413 
01414     void setQ(State&, Real) const;
01415     void setU(State&, Real) const;
01416 
01417     Real getMyPartQ(const State&, const Vector& qlike) const;
01418     Real getMyPartU(const State&, const Vector& ulike) const;
01419    
01420     Real& updMyPartQ(const State&, Vector& qlike) const;
01421     Real& updMyPartU(const State&, Vector& ulike) const;
01422 
01423         // specialize return type for convenience
01424     Pin& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
01425       { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
01426     Pin& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g)
01427       { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
01428     Pin& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
01429       { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
01430     Pin& setDefaultInboardFrame(const Transform& X_PF)
01431       { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
01432     Pin& setDefaultOutboardFrame(const Transform& X_BM)
01433       { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
01434     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Pin, PinImpl, MobilizedBody);
01435 };
01436 
01440 class SimTK_SIMBODY_EXPORT MobilizedBody::Slider : public MobilizedBody {
01441 public:
01442         // SPECIALIZED INTERFACE FOR SLIDER MOBILIZER
01443 
01444     // "Length" is just a nicer name for a sliding joint's lone generalized coordinate q.
01445     Slider& setDefaultLength(Real length) {return setDefaultQ(length);}
01446     Real getDefaultLength() const         {return getDefaultQ();}
01447 
01448         // Friendly, mobilizer-specific access to generalized coordinates and speeds.
01449 
01450     void setLength(State& s, Real length) {setQ(s, length);}
01451     Real getLength(const State& s) const {return getQ(s);}
01452 
01453     void setRate(State& s, Real rateInLengthPerTime) {setU(s, rateInLengthPerTime);}
01454     Real getRate(const State& s) const {return getU(s);}
01455 
01456     // Mobility forces are "u-like", that is, one per dof.
01457     Real getAppliedForce(const State& s, const Vector& mobilityForces) const {
01458         return getMyPartU(s,mobilityForces);
01459     }
01460     void applyForce(const State& s, Real force, Vector& mobilityForces) const {
01461         updMyPartU(s,mobilityForces) += force;
01462     }
01463 
01464         // STANDARDIZED MOBILIZED BODY INTERFACE
01465 
01466         // required constructors
01467     explicit Slider(Direction=Forward);
01468     Slider(MobilizedBody& parent, const Body&, Direction=Forward);
01469     Slider(MobilizedBody& parent, const Transform& inbFrame,
01470            const Body&,           const Transform& outbFrame, Direction=Forward);
01471 
01472         // access to generalized coordinates q and generalized speeds u
01473     Slider& setDefaultQ(Real);
01474     Real getDefaultQ() const;
01475 
01476     Real getQ(const State&) const;
01477     Real getQDot(const State&) const;
01478     Real getQDotDot(const State&) const;
01479     Real getU(const State&) const;
01480     Real getUDot(const State&) const;
01481 
01482     void setQ(State&, Real) const;
01483     void setU(State&, Real) const;
01484 
01485     Real getMyPartQ(const State&, const Vector& qlike) const;
01486     Real getMyPartU(const State&, const Vector& ulike) const;
01487    
01488     Real& updMyPartQ(const State&, Vector& qlike) const;
01489     Real& updMyPartU(const State&, Vector& ulike) const;
01490 
01491         // specialize return type for convenience
01492     Slider& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
01493       { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
01494     Slider& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g)
01495       { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
01496     Slider& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
01497       { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
01498     Slider& setDefaultInboardFrame(const Transform& X_PF)
01499       { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
01500     Slider& setDefaultOutboardFrame(const Transform& X_BM)
01501       { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
01502     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Slider, SliderImpl, MobilizedBody);
01503 };
01504 
01510 class SimTK_SIMBODY_EXPORT MobilizedBody::Screw : public MobilizedBody {
01511 public:
01512     explicit Screw(Real pitch, Direction=Forward);
01513 
01516     Screw(MobilizedBody& parent, const Body&, Real pitch, Direction=Forward);
01517 
01520     Screw(MobilizedBody& parent, const Transform& inbFrame,
01521          const Body&,           const Transform& outbFrame,
01522          Real pitch, Direction=Forward);
01523 
01524     Screw& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01525         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01526     }
01527     Screw& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
01528         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01529     }
01530     Screw& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01531         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01532     }
01533 
01534     Screw& setDefaultInboardFrame(const Transform& X_PF) {
01535         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01536     }
01537 
01538     Screw& setDefaultOutboardFrame(const Transform& X_BM) {
01539         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01540     }
01541 
01542     Screw& setDefaultPitch(Real pitch);
01543     Real   getDefaultPitch() const;
01544 
01545     Screw& setDefaultQ(Real);
01546     Real   getDefaultQ() const;
01547 
01548     Real getQ(const State&) const;
01549     Real getQDot(const State&) const;
01550     Real getQDotDot(const State&) const;
01551     Real getU(const State&) const;
01552     Real getUDot(const State&) const;
01553 
01554     void setQ(State&, Real) const;
01555     void setU(State&, Real) const;
01556 
01557     Real getMyPartQ(const State&, const Vector& qlike) const;
01558     Real getMyPartU(const State&, const Vector& ulike) const;
01559    
01560     Real& updMyPartQ(const State&, Vector& qlike) const;
01561     Real& updMyPartU(const State&, Vector& ulike) const;
01562     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Screw, ScrewImpl, MobilizedBody);
01563 };
01564 
01568 class SimTK_SIMBODY_EXPORT MobilizedBody::Universal : public MobilizedBody {
01569 public:
01570     explicit Universal(Direction=Forward);
01571 
01574     Universal(MobilizedBody& parent, const Body&, Direction=Forward);
01575 
01578     Universal(MobilizedBody& parent, const Transform& inbFrame,
01579               const Body&,           const Transform& outbFrame, Direction=Forward);
01580 
01581     Universal& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01582         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01583     }
01584     Universal& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
01585         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01586     }
01587     Universal& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01588         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01589     }
01590 
01591     Universal& setDefaultInboardFrame(const Transform& X_PF) {
01592         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01593     }
01594 
01595     Universal& setDefaultOutboardFrame(const Transform& X_BM) {
01596         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01597     }
01598     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Universal, UniversalImpl, MobilizedBody);
01599 };
01600 
01603 class SimTK_SIMBODY_EXPORT MobilizedBody::Cylinder : public MobilizedBody {
01604 public:
01605     explicit Cylinder(Direction=Forward);
01606 
01609     Cylinder(MobilizedBody& parent, const Body&, Direction=Forward);
01610 
01613     Cylinder(MobilizedBody& parent, const Transform& inbFrame,
01614              const Body&,           const Transform& outbFrame, Direction=Forward);
01615 
01616     Cylinder& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01617         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01618     }
01619     Cylinder& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
01620         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01621     }
01622     Cylinder& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01623         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01624     }
01625 
01626     Cylinder& setDefaultInboardFrame(const Transform& X_PF) {
01627         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01628     }
01629 
01630     Cylinder& setDefaultOutboardFrame(const Transform& X_BM) {
01631         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01632     }
01633     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Cylinder, CylinderImpl, MobilizedBody);
01634 };
01635 
01644 class SimTK_SIMBODY_EXPORT MobilizedBody::BendStretch : public MobilizedBody {
01645 public:
01646     explicit BendStretch(Direction=Forward);
01647 
01650     BendStretch(MobilizedBody& parent, const Body&, Direction=Forward);
01651 
01654     BendStretch(MobilizedBody& parent, const Transform& inbFrame,
01655                 const Body&,           const Transform& outbFrame, Direction=Forward);
01656 
01657     BendStretch& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01658         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01659     }
01660     BendStretch& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
01661         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01662     }
01663     BendStretch& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01664         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01665     }
01666 
01667     BendStretch& setDefaultInboardFrame(const Transform& X_PF) {
01668         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01669     }
01670 
01671     BendStretch& setDefaultOutboardFrame(const Transform& X_BM) {
01672         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01673     }
01674     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(BendStretch, BendStretchImpl, MobilizedBody);
01675 };
01676 
01681 class SimTK_SIMBODY_EXPORT MobilizedBody::Planar : public MobilizedBody {
01682 public:
01683     explicit Planar(Direction=Forward);
01684 
01687     Planar(MobilizedBody& parent, const Body&, Direction=Forward);
01688 
01691     Planar(MobilizedBody& parent, const Transform& inbFrame,
01692            const Body&,           const Transform& outbFrame,
01693            Direction=Forward);
01694 
01695     Planar& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01696         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01697     }
01698     Planar& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
01699         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01700     }
01701     Planar& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01702         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01703     }
01704 
01705     Planar& setDefaultInboardFrame(const Transform& X_PF) {
01706         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01707     }
01708 
01709     Planar& setDefaultOutboardFrame(const Transform& X_BM) {
01710         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01711     }
01712 
01713     // Friendly, mobilizer-specific access to coordinates and speeds.
01714     Planar& setDefaultAngle(Real a) {
01715         Vec3 q = getDefaultQ(); q[0] = a; setDefaultQ(q);
01716         return *this;
01717     }
01718     Planar& setDefaultTranslation(const Vec2& r) {
01719         Vec3 q = getDefaultQ(); q.updSubVec<2>(1) = r; setDefaultQ(q);
01720         return *this;
01721     }
01722 
01723     Real getDefaultAngle() const {return getDefaultQ()[0];}
01724     const Vec2& getDefaultTranslation() const {return getDefaultQ().getSubVec<2>(1);}
01725 
01726     void setAngle      (State& s, Real        a) {setOneQ(s,0,a);}
01727     void setTranslation(State& s, const Vec2& r) {setOneQ(s,1,r[0]); setOneQ(s,2,r[1]);}
01728 
01729     Real getAngle(const State& s) const {return getQ(s)[0];}
01730     const Vec2& getTranslation(const State& s) const {return getQ(s).getSubVec<2>(1);}
01731 
01732     // Generic default state Topology methods.
01733     const Vec3& getDefaultQ() const;
01734     Planar& setDefaultQ(const Vec3& q);
01735 
01736     const Vec3& getQ(const State&) const;
01737     const Vec3& getQDot(const State&) const;
01738     const Vec3& getQDotDot(const State&) const;
01739     const Vec3& getU(const State&) const;
01740     const Vec3& getUDot(const State&) const;
01741 
01742     void setQ(State&, const Vec3&) const;
01743     void setU(State&, const Vec3&) const;
01744 
01745     const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
01746     const Vec3& getMyPartU(const State&, const Vector& ulike) const;
01747    
01748     Vec3& updMyPartQ(const State&, Vector& qlike) const;
01749     Vec3& updMyPartU(const State&, Vector& ulike) const;
01750     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Planar, PlanarImpl, MobilizedBody);
01751 };
01752 
01777 class SimTK_SIMBODY_EXPORT MobilizedBody::SphericalCoords : public MobilizedBody {
01778 public:
01779     explicit SphericalCoords(Direction=Forward);
01780 
01783     SphericalCoords(MobilizedBody& parent, const Body&, Direction=Forward);
01784 
01789     SphericalCoords(MobilizedBody& parent, const Transform& inbFrame,
01790                     const Body&,           const Transform& outbFrame,
01791                     Direction=Forward);
01792 
01794     SphericalCoords(MobilizedBody& parent,      const Transform& inbFrame,
01795                     const Body&,                const Transform& outbFrame,
01796                     Real azimuthOffset,         bool azimuthNegated,
01797                     Real zenithOffset,          bool zenithNegated,
01798                     CoordinateAxis radialAxis,  bool radialNegated,
01799                     Direction=Forward);
01800 
01801     SphericalCoords& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01802         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01803     }
01804     SphericalCoords& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
01805         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01806     }
01807     SphericalCoords& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01808         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01809     }
01810 
01811     SphericalCoords& setDefaultInboardFrame(const Transform& X_PF) {
01812         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01813     }
01814 
01815     SphericalCoords& setDefaultOutboardFrame(const Transform& X_BM) {
01816         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01817     }
01818 
01819     // Friendly, mobilizer-specific access to coordinates and speeds.
01820     SphericalCoords& setDefaultAngles(const Vec2& a) {
01821         Vec3 q = getDefaultQ(); q.updSubVec<2>(0) = a; setDefaultQ(q);
01822         return *this;
01823     }
01824     SphericalCoords& setDefaultRadius(Real r) {
01825         Vec3 q = getDefaultQ(); q[2] = r; setDefaultQ(q);
01826         return *this;
01827     }
01828     SphericalCoords& setRadialAxis(CoordinateAxis);
01829     SphericalCoords& setNegateAzimuth(bool);
01830     SphericalCoords& setNegateZenith(bool);
01831     SphericalCoords& setNegateRadial(bool);
01832 
01833     const Vec2&    getDefaultAngles()      const {return getDefaultQ().getSubVec<2>(0);}
01834     Real           getDefaultTranslation() const {return getDefaultQ()[2];}
01835     
01836     CoordinateAxis getRadialAxis()    const;
01837     bool           isAzimuthNegated() const;
01838     bool           isZenithNegated()  const;
01839     bool           isRadialNegated()  const;
01840 
01841     void setAngles(State& s, const Vec2& a) {setOneQ(s,0,a[0]); setOneQ(s,1,a[1]);}
01842     void setRadius(State& s, Real        r) {setOneQ(s,2,r);}
01843 
01844     const Vec2& getAngles(const State& s) const {return getQ(s).getSubVec<2>(0);}
01845     Real        getRadius(const State& s) const {return getQ(s)[2];}
01846 
01847     // Generic default state Topology methods.
01848     const Vec3& getDefaultQ() const;
01849     SphericalCoords& setDefaultQ(const Vec3& q);
01850 
01851     const Vec3& getQ(const State&) const;
01852     const Vec3& getQDot(const State&) const;
01853     const Vec3& getQDotDot(const State&) const;
01854     const Vec3& getU(const State&) const;
01855     const Vec3& getUDot(const State&) const;
01856 
01857     void setQ(State&, const Vec3&) const;
01858     void setU(State&, const Vec3&) const;
01859 
01860     const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
01861     const Vec3& getMyPartU(const State&, const Vector& ulike) const;
01862    
01863     Vec3& updMyPartQ(const State&, Vector& qlike) const;
01864     Vec3& updMyPartU(const State&, Vector& ulike) const;
01865 
01866     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(SphericalCoords, SphericalCoordsImpl, MobilizedBody);
01867 };
01868 
01872 class SimTK_SIMBODY_EXPORT MobilizedBody::Gimbal : public MobilizedBody {
01873 public:
01874     explicit Gimbal(Direction=Forward);
01875 
01878     Gimbal(MobilizedBody& parent, const Body&, Direction=Forward);
01879 
01882     Gimbal(MobilizedBody& parent, const Transform& inbFrame,
01883            const Body&,           const Transform& outbFrame, Direction=Forward);
01884 
01885     Gimbal& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01886         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01887     }
01888     Gimbal& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
01889         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01890     }
01891     Gimbal& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01892         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01893     }
01894 
01895     Gimbal& setDefaultInboardFrame(const Transform& X_PF) {
01896         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01897     }
01898 
01899     Gimbal& setDefaultOutboardFrame(const Transform& X_BM) {
01900         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01901     }
01902 
01903     // This is just a nicer name for the generalized coordinate.
01904     Gimbal& setDefaultRotation(const Rotation& R_FM) {
01905         return setDefaultQ(R_FM.convertRotationToBodyFixedXYZ());
01906     }
01907     Rotation getDefaultRotation() const {
01908         const Vec3& q = getDefaultQ();
01909         return Rotation(BodyRotationSequence,
01910             q[0], XAxis, q[1], YAxis, q[2], ZAxis);
01911     }
01912 
01913     // This is used only for visualization.
01914     Gimbal& setDefaultRadius(Real r);
01915     Real getDefaultRadius() const;
01916 
01917     // Generic default state Topology methods.
01918     const Vec3& getDefaultQ() const; // X,Y,Z body-fixed Euler angles
01919     Gimbal& setDefaultQ(const Vec3& q);
01920 
01921     const Vec3& getQ(const State&) const;
01922     const Vec3& getQDot(const State&) const;
01923     const Vec3& getQDotDot(const State&) const;
01924     const Vec3& getU(const State&) const;
01925     const Vec3& getUDot(const State&) const;
01926 
01927     void setQ(State&, const Vec3&) const;
01928     void setU(State&, const Vec3&) const;
01929 
01930     const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
01931     const Vec3& getMyPartU(const State&, const Vector& ulike) const;
01932    
01933     Vec3& updMyPartQ(const State&, Vector& qlike) const;
01934     Vec3& updMyPartU(const State&, Vector& ulike) const;
01935     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Gimbal, GimbalImpl, MobilizedBody);
01936 };
01937 
01942 class SimTK_SIMBODY_EXPORT MobilizedBody::Ball : public MobilizedBody {
01943 public:
01944     explicit Ball(Direction=Forward);
01945 
01948     Ball(MobilizedBody& parent, const Body&, Direction=Forward);
01949 
01952     Ball(MobilizedBody& parent, const Transform& inbFrame,
01953          const Body&,           const Transform& outbFrame, Direction=Forward);
01954 
01955     Ball& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01956         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01957     }
01958     Ball& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
01959         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01960     }
01961     Ball& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01962         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01963     }
01964 
01965     Ball& setDefaultInboardFrame(const Transform& X_PF) {
01966         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01967     }
01968 
01969     Ball& setDefaultOutboardFrame(const Transform& X_BM) {
01970         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01971     }
01972 
01973     // This is just a nicer name for the generalized coordinate.
01974     Ball& setDefaultRotation(const Rotation& R_FM) {
01975         return setDefaultQ(R_FM.convertRotationToQuaternion());
01976     }
01977     Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
01978 
01979     // This is used only for visualization.
01980     Ball& setDefaultRadius(Real r);
01981     Real getDefaultRadius() const;
01982 
01983     // Generic default state Topology methods.
01984     const Quaternion& getDefaultQ() const;
01985     Ball& setDefaultQ(const Quaternion& q);
01986 
01987     const Vec4& getQ(const State&) const;
01988     const Vec4& getQDot(const State&) const;
01989     const Vec4& getQDotDot(const State&) const;
01990     const Vec3& getU(const State&) const;
01991     const Vec3& getUDot(const State&) const;
01992 
01993     void setQ(State&, const Vec4&) const;
01994     void setU(State&, const Vec3&) const;
01995 
01996     const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
01997     const Vec3& getMyPartU(const State&, const Vector& ulike) const;
01998    
01999     Vec4& updMyPartQ(const State&, Vector& qlike) const;
02000     Vec3& updMyPartU(const State&, Vector& ulike) const;
02001     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ball, BallImpl, MobilizedBody);
02002 };
02003 
02008 class SimTK_SIMBODY_EXPORT MobilizedBody::Ellipsoid : public MobilizedBody {
02009 public:
02012     explicit Ellipsoid(Direction=Forward); // not very useful until radii are set, but has some defaults
02013 
02016     Ellipsoid(MobilizedBody& parent, const Body&, Direction=Forward);
02017 
02020     Ellipsoid(MobilizedBody& parent, const Transform& inbFrame,
02021               const Body&,           const Transform& outbFrame,
02022               Direction=Forward);
02023 
02027     Ellipsoid(MobilizedBody& parent, const Transform& inbFrame,
02028               const Body&,           const Transform& outbFrame,
02029               const Vec3& radii,
02030               Direction=Forward);
02031 
02032     Ellipsoid& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02033         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02034     }
02035     Ellipsoid& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
02036         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02037     }
02038     Ellipsoid& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02039         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02040     }
02041 
02042     Ellipsoid& setDefaultInboardFrame(const Transform& X_PF) {
02043         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02044     }
02045 
02046     Ellipsoid& setDefaultOutboardFrame(const Transform& X_BM) {
02047         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02048     }
02049 
02050     // This is just a nicer name for the generalized coordinate.
02051     Ellipsoid& setDefaultRotation(const Rotation& R_FM) {
02052         return setDefaultQ(R_FM.convertRotationToQuaternion());
02053     }
02054     Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
02055 
02056     Ellipsoid& setDefaultRadii(const Vec3& r);
02057     const Vec3& getDefaultRadii() const;
02058 
02059     // Generic default state Topology methods.
02060     const Quaternion& getDefaultQ() const;
02061     Quaternion& updDefaultQ();
02062     Ellipsoid& setDefaultQ(const Quaternion& q) {updDefaultQ()=q; return *this;}
02063 
02064     const Vec4& getQ(const State&) const;
02065     const Vec4& getQDot(const State&) const;
02066     const Vec4& getQDotDot(const State&) const;
02067     const Vec3& getU(const State&) const;
02068     const Vec3& getUDot(const State&) const;
02069 
02070     void setQ(State&, const Vec4&) const;
02071     void setU(State&, const Vec3&) const;
02072 
02073     const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
02074     const Vec3& getMyPartU(const State&, const Vector& ulike) const;
02075    
02076     Vec4& updMyPartQ(const State&, Vector& qlike) const;
02077     Vec3& updMyPartU(const State&, Vector& ulike) const;
02078 
02079     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ellipsoid, EllipsoidImpl, MobilizedBody);
02080 };
02081 
02084 class SimTK_SIMBODY_EXPORT MobilizedBody::Translation : public MobilizedBody {
02085 public:
02086     explicit Translation(Direction=Forward);
02087 
02090     Translation(MobilizedBody& parent, const Body&, Direction=Forward);
02091 
02094     Translation(MobilizedBody& parent, const Transform& inbFrame,
02095                 const Body&,           const Transform& outbFrame, Direction=Forward);
02096 
02097     Translation& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02098         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02099     }
02100     Translation& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
02101         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02102     }
02103     Translation& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02104         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02105     }
02106 
02107     Translation& setDefaultInboardFrame(const Transform& X_PF) {
02108         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02109     }
02110 
02111     Translation& setDefaultOutboardFrame(const Transform& X_BM) {
02112         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02113     }
02114 
02115     // This is just a nicer name for setting this mobilizer's generalized coordinates,
02116     // which together constitute the vector from the F frame's origin to the M
02117     // frame's origin, expressed in F.
02118 
02119     // Set the topological default values for the initial q's.
02120     Translation& setDefaultTranslation(const Vec3& p_FM) {
02121         return setDefaultQ(p_FM);
02122     }
02123 
02124     // Get the topological default values for the initial q's.
02125     const Vec3& getDefaultTranslation() const {
02126         return getDefaultQ();
02127     }
02128 
02129     // Set the current value of q's in the given State. Note that this is
02130     // the *cross-mobilizer* translation, not location in the Ground frame.
02131     void setMobilizerTranslation(State& s, const Vec3& p_FM) const {
02132         setQ(s,p_FM);
02133     }
02134 
02135     // Get the current value of the q's for this mobilizer from the given State.
02136     const Vec3& getMobilizerTranslation(const State& s) const {
02137         return getQ(s);
02138     }
02139 
02140 
02141     // Set the current value of u's in the given State. Note that this is
02142     // the *cross-mobilizer* velocity v_FM, not velocity in the Ground frame.
02143     void setMobilizerVelocity(State& s, const Vec3& v_FM) const {
02144         setU(s,v_FM);
02145     }
02146 
02147     // Get the current value of the u's for this mobilizer from the given State.
02148     const Vec3& getMobilizerVelocity(const State& s) const {
02149         return getU(s);
02150     }
02151 
02152     // Get the value of the udot's for this mobilizer from the given State.
02153     const Vec3& getMobilizerAcceleration(const State& s) const {
02154         return getUDot(s);
02155     }
02156 
02157     // Generic default state Topology methods.
02158     const Vec3& getDefaultQ() const;
02159     Translation& setDefaultQ(const Vec3& q);
02160 
02161     const Vec3& getQ(const State&) const;
02162     const Vec3& getQDot(const State&) const;
02163     const Vec3& getQDotDot(const State&) const;
02164     const Vec3& getU(const State&) const;
02165     const Vec3& getUDot(const State&) const;
02166 
02167     void setQ(State&, const Vec3&) const;
02168     void setU(State&, const Vec3&) const;
02169 
02170     const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
02171     const Vec3& getMyPartU(const State&, const Vector& ulike) const;
02172    
02173     Vec3& updMyPartQ(const State&, Vector& qlike) const;
02174     Vec3& updMyPartU(const State&, Vector& ulike) const;
02175     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Translation, TranslationImpl, MobilizedBody);
02176 };
02177 
02184 class SimTK_SIMBODY_EXPORT MobilizedBody::Free : public MobilizedBody {
02185 public:
02186     explicit Free(Direction=Forward);
02187 
02190     Free(MobilizedBody& parent, const Body&, Direction=Forward);
02191 
02194     Free(MobilizedBody& parent, const Transform& inbFrame,
02195          const Body&,           const Transform& outbFrame,
02196          Direction=Forward);
02197 
02198     Free& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02199         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02200     }
02201     Free& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
02202         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02203     }
02204     Free& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02205         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02206     }
02207 
02208     Free& setDefaultInboardFrame(const Transform& X_PF) {
02209         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02210     }
02211 
02212     Free& setDefaultOutboardFrame(const Transform& X_BM) {
02213         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02214     }
02215 
02216     // Leaves rotation unchanged.
02217     Free& setDefaultTranslation(const Vec3&);
02218 
02219     // Leaves translation unchanged. The internal representation is a quaternion
02220     // so we guarantee that the stored value is numerically identical to the
02221     // supplied one.
02222     Free& setDefaultQuaternion(const Quaternion&);
02223 
02224     // Leaves translation unchanged. The Rotation matrix will be converted to
02225     // a quaternion for storage.
02226     Free& setDefaultRotation(const Rotation&);
02227     // Sets both translation and rotation. The Rotation part of the Transform 
02228     // will be converted to a quaternion for storage.
02229     Free& setDefaultTransform(const Transform&);
02230 
02231     // These return references to the stored default values.
02232     const Vec3& getDefaultTranslation() const;
02233     const Quaternion& getDefaultQuaternion() const;
02234 
02235     // These next two are derived from the stored values.
02236     Rotation getDefaultRotation() const {
02237         return Rotation(getDefaultQuaternion());
02238     }
02239     Transform getDefaultTransform() const {
02240         return Transform(Rotation(getDefaultQuaternion()), getDefaultTranslation());
02241     }
02242 
02243     // Generic default state Topology methods.
02244 
02245     // Returns (Vec4,Vec3) where the Vec4 is a normalized quaternion.
02246     const Vec7& getDefaultQ() const;
02247 
02248     // Interprets the supplied q as (Vec4,Vec3) where the Vec4 is a possibly
02249     // unnormalized quaternion. The quaternion will be normalized before it is
02250     // stored here, so you may not get back exactly the value supplied here if
02251     // you call getDefaultQ().
02252     Free& setDefaultQ(const Vec7& q);
02253 
02254     // Note that there is no guarantee that the quaternion part of the returned Q is normalized.
02255     const Vec7& getQ(const State&) const;
02256     const Vec7& getQDot(const State&) const;
02257     const Vec7& getQDotDot(const State&) const;
02258 
02259     const Vec6& getU(const State&) const;
02260     const Vec6& getUDot(const State&) const;
02261 
02262     // The Q's in the state are set exactly as supplied without normalization.
02263     void setQ(State&, const Vec7&) const;
02264     void setU(State&, const Vec6&) const;
02265 
02266     const Vec7& getMyPartQ(const State&, const Vector& qlike) const;
02267     const Vec6& getMyPartU(const State&, const Vector& ulike) const;
02268    
02269     Vec7& updMyPartQ(const State&, Vector& qlike) const;
02270     Vec6& updMyPartU(const State&, Vector& ulike) const;
02271     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Free, FreeImpl, MobilizedBody);
02272 };
02273 
02274 
02275 // These are special "ball" and "free" joints designed to allow arbitrary orientations
02276 // for "linear" bodies, such as a CO2 molecule consisting only of point masses arranged
02277 // along a straight line. Such bodies have no inertia about the line and cause singularities
02278 // in the equations of motion if attached to Orientation or Free mobilizers. Instead, use the
02279 // LineOrientation and LineFree moblizers, making sure that the inertialess direction is
02280 // along the outboard body's z axis (that is, Mz). These mobilizers introduce only two
02281 // mobilities (generalized speeds u), being incapable of representing non-zero angular
02282 // velocity of M in F about Mz. The generalized speeds are in fact the wx and wy 
02283 // components of w_FM_M, that is, the x and y components of the angular velocity of M
02284 // in F *expressed in M*. However, at least three generalized coordinates (q's)
02285 // are required to represent the orientation. By default we use four quaternions for
02286 // unconditional stability. Alternatively, you can request a 1-2-3 body fixed 
02287 // Euler angle sequence (that is, about x, then new y, then new z) which will
02288 // suffer a singularity when the y rotation is 90 degrees since that aligns the
02289 // first rotation axis (x) with the last (z) which is the inertialess direction.
02290 
02296 class SimTK_SIMBODY_EXPORT MobilizedBody::LineOrientation : public MobilizedBody {
02297 public:
02298     explicit LineOrientation(Direction=Forward);
02299 
02302     LineOrientation(MobilizedBody& parent, const Body&, Direction=Forward);
02303 
02306     LineOrientation(MobilizedBody& parent, const Transform& inbFrame,
02307                     const Body&,           const Transform& outbFrame, Direction=Forward);
02308 
02309     LineOrientation& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02310         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02311     }
02312     LineOrientation& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
02313         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02314     }
02315     LineOrientation& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02316         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02317     }
02318 
02319     LineOrientation& setDefaultInboardFrame(const Transform& X_PF) {
02320         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02321     }
02322 
02323     LineOrientation& setDefaultOutboardFrame(const Transform& X_BM) {
02324         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02325     }
02326 
02327     // This is just a nicer name for the generalized coordinate.
02328     LineOrientation& setDefaultRotation(const Rotation& R_FM) {
02329         return setDefaultQ(R_FM.convertRotationToQuaternion());
02330     }
02331     Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
02332 
02333     // Generic default state Topology methods.
02334     const Quaternion& getDefaultQ() const;
02335     LineOrientation& setDefaultQ(const Quaternion& q);
02336 
02337     const Vec4& getQ(const State&) const;
02338     const Vec4& getQDot(const State&) const;
02339     const Vec4& getQDotDot(const State&) const;
02340     const Vec2& getU(const State&) const;
02341     const Vec2& getUDot(const State&) const;
02342 
02343     void setQ(State&, const Vec4&) const;
02344     void setU(State&, const Vec2&) const;
02345 
02346     const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
02347     const Vec2& getMyPartU(const State&, const Vector& ulike) const;
02348    
02349     Vec4& updMyPartQ(const State&, Vector& qlike) const;
02350     Vec2& updMyPartU(const State&, Vector& ulike) const;
02351     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(LineOrientation, LineOrientationImpl, MobilizedBody);
02352 };
02353 
02358 class SimTK_SIMBODY_EXPORT MobilizedBody::FreeLine : public MobilizedBody {
02359 public:
02360     explicit FreeLine(Direction=Forward);
02361 
02364     FreeLine(MobilizedBody& parent, const Body&, Direction=Forward);
02365 
02368     FreeLine(MobilizedBody& parent, const Transform& inbFrame,
02369              const Body&,           const Transform& outbFrame, Direction=Forward);
02370 
02371     FreeLine& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02372         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02373     }
02374     FreeLine& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
02375         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02376     }
02377     FreeLine& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02378         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02379     }
02380 
02381     FreeLine& setDefaultInboardFrame(const Transform& X_PF) {
02382         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02383     }
02384 
02385     FreeLine& setDefaultOutboardFrame(const Transform& X_BM) {
02386         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02387     }
02388     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(FreeLine, FreeLineImpl, MobilizedBody);
02389 };
02390 
02393 class SimTK_SIMBODY_EXPORT MobilizedBody::Weld : public MobilizedBody {
02394 public:
02397     Weld();
02398 
02401     Weld(MobilizedBody& parent, const Body&);
02402 
02405     Weld(MobilizedBody& parent, const Transform& inbFrame,
02406          const Body&,           const Transform& outbFrame);
02407 
02408     Weld& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02409         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02410     }
02411     Weld& addOutboardDecoration(const Transform& X_MD,  const DecorativeGeometry& g) {
02412         (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02413     }
02414     Weld& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02415         (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02416     }
02417 
02418     Weld& setDefaultInboardFrame(const Transform& X_PF) {
02419         (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02420     }
02421 
02422     Weld& setDefaultOutboardFrame(const Transform& X_BM) {
02423         (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02424     }
02425     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Weld, WeldImpl, MobilizedBody);
02426 };
02427 
02428 
02432 class SimTK_SIMBODY_EXPORT MobilizedBody::Ground : public MobilizedBody {
02433 public:
02435     Ground();
02436     Ground& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02437         (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02438     }
02439     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ground, GroundImpl, MobilizedBody);
02440 };
02441 
02442 
02474 class SimTK_SIMBODY_EXPORT MobilizedBody::Custom : public MobilizedBody {
02475 public:
02476     class Implementation;
02477     class ImplementationImpl;
02478 
02479     /* Create a Custom MobilizedBody.
02480      * 
02481      * @param parent         the MobilizedBody's parent body
02482      * @param implementation the object which implements the custom mobilized body.  The MobilizedBody::Custom takes over
02483      *                       ownership of the implementation object, and deletes it when the MobilizedBody itself
02484      *                       is deleted.
02485      * @param body           describes this MobilizedBody's physical properties
02486      * @param direction      whether you want the coordinates defined as though parent & child were swapped
02487      */
02488     Custom(MobilizedBody& parent, Implementation* implementation, const Body& body, Direction direction=Forward);
02489     /* Create a Custom MobilizedBody.
02490      * 
02491      * @param parent         the MobilizedBody's parent body
02492      * @param implementation the object which implements the custom mobilized body.  The MobilizedBody::Custom takes over
02493      *                       ownership of the implementation object, and deletes it when the MobilizedBody itself
02494      *                       is deleted.
02495      * @param inbFrame       the MobilizedBody's inboard reference frame
02496      * @param body           describes this MobilizedBody's physical properties
02497      * @param outbFrame      the MobilizedBody's outboard reference frame
02498      * @param direction      whether you want the coordinates defined as though parent & child were swapped
02499      */
02500     Custom(MobilizedBody& parent, Implementation* implementation, 
02501            const Transform& inbFrame, const Body& body, const Transform& outbFrame,
02502            Direction direction=Forward);
02503     SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, MobilizedBody);
02504 protected:
02505     const Implementation& getImplementation() const;
02506     Implementation&       updImplementation();
02507 };
02508 
02509 // We only want the template instantiation to occur once. This symbol is defined in the SimTK core
02510 // compilation unit that defines the MobilizedBody class but should not be defined any other time.
02511 // BE SURE TO DEAL WITH THIS IN MobilizedBody_Instantiation.cpp.
02512 #ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
02513     extern template class PIMPLHandle<MobilizedBody::Custom::Implementation, MobilizedBody::Custom::ImplementationImpl>;
02514 #endif
02515 
02516 
02517 class SimTK_SIMBODY_EXPORT MobilizedBody::Custom::Implementation 
02518   : public PIMPLHandle<Implementation,ImplementationImpl> 
02519 {
02520 public:
02521     // No default constructor because you have to supply at least the SimbodyMatterSubsystem
02522     // to which this MobilizedBody belongs.
02523 
02525     virtual ~Implementation();
02526 
02531     virtual Implementation* clone() const = 0;
02532 
02556     Implementation(SimbodyMatterSubsystem&, int nu, int nq, int nAngles=0);
02557 
02561     Vector getQ(const State& s) const;
02562     
02564     Vector getU(const State& s) const;
02565 
02569     Vector getQDot(const State& s) const;
02570 
02572     Vector getUDot(const State& s) const;
02573     
02577     Vector getQDotDot(const State& s) const;
02578 
02583     Transform getMobilizerTransform(const State& s) const;
02584 
02591     SpatialVec getMobilizerVelocity(const State& s) const;
02592 
02601     bool getUseEulerAngles(const State& s) const;
02602 
02609     void invalidateTopologyCache() const;
02610 
02615 
02616 
02621     virtual Transform calcMobilizerTransformFromQ(const State& s, int nq, const Real* q) const = 0;
02622 
02623 
02640     virtual SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const = 0;
02641 
02650     virtual void multiplyByHTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const = 0;
02651 
02661     virtual SpatialVec multiplyByHDotMatrix(const State& s, int nu, const Real* u) const = 0;
02662 
02671     virtual void multiplyByHDotTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const = 0;
02672 
02683     virtual void multiplyByN(const State& s, bool transposeMatrix, 
02684                              int nIn, const Real* in, int nOut, Real* out) const;
02685 
02696     virtual void multiplyByNInv(const State& s, bool transposeMatrix, 
02697                                 int nIn, const Real* in, int nOut, Real* out) const;
02698 
02709     virtual void multiplyByNDot(const State& s, bool transposeMatrix, 
02710                                 int nIn, const Real* in, int nOut, Real* out) const;
02711 
02712         // Methods for setting Mobilizer initial conditions. Note -- I've stripped this
02713         // down to the two basic routines but the built-ins have 8 so that you can 
02714         // specify only rotations or translations. I'm not sure that's needed here and
02715         // I suppose you could add more routines later if needed.
02716         // Eventually it might be nice to provide default implementation here that would
02717         // use a root finder to attempt to solve these initial condition problems. For
02718         // most joints there is a much more direct way to do it, and sometimes there
02719         // are behavioral choices to make, which is why it is nice to have mobilizer-specific
02720         // overrides for these.
02721 
02730     virtual void setQToFitTransform(const State&, const Transform& X_FM, int nq, Real* q) const;
02731 
02743     virtual void setUToFitVelocity(const State&, const SpatialVec& V_FM, int nu, Real* u) const;
02744 
02751     virtual void calcDecorativeGeometryAndAppend
02752        (const State& s, Stage stage, std::vector<DecorativeGeometry>& geom) const
02753     {
02754     }
02756 
02757 
02766 
02768 
02769 
02770 
02771 
02772 
02773 
02774 
02775 
02776     virtual void realizeTopology(State&) const { }
02777 
02786     virtual void realizeModel(State&) const { }
02787 
02791     virtual void realizeInstance(const State&) const { }
02792 
02797     virtual void realizeTime(const State&) const { }
02798 
02805     virtual void realizePosition(const State&) const { }
02806 
02813     virtual void realizeVelocity(const State&) const { }
02814 
02821     virtual void realizeDynamics(const State&) const { }
02822 
02829     virtual void realizeAcceleration(const State&) const { }
02830 
02836     virtual void realizeReport(const State&) const { }
02838 
02839     friend class MobilizedBody::CustomImpl;
02840 };
02841 
02853 class SimTK_SIMBODY_EXPORT MobilizedBody::FunctionBased : public MobilizedBody::Custom {
02854 public:
02855     /* Create a FunctionBased MobilizedBody.
02856      * 
02857      * @param parent         the MobilizedBody's parent body
02858      * @param body           describes this MobilizedBody's physical properties
02859      * @param nmobilities    the number of generalized coordinates belonging to this MobilizedBody
02860      * @param functions      the Functions describing how the body moves based on its generalized coordinates.
02861      *                       This must be of length 6.  The elements correspond to, in order, x rotation, y rotation, z rotation,
02862      *                       x translation, y translation, and z translation.  The MobilizedBody takes over ownership of the functions,
02863      *                       and automatically deletes them when the MobilizedBody is deleted.
02864      * @param coordIndices   the indices of the generalized coordinates that are inputs to each function.  For example, if coordIndices[2] = {0, 1},
02865      *                       that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
02866      * @param direction      whether you want the coordinates defined as though parent & child were swapped
02867      */
02868     FunctionBased(MobilizedBody& parent, const Body& body, 
02869                   int nmobilities, const std::vector<const Function*>& functions,
02870                   const std::vector<std::vector<int> >& coordIndices,
02871                   Direction direction=Forward);
02872     /* Create a FunctionBased MobilizedBody.
02873      * 
02874      * @param parent         the MobilizedBody's parent body
02875      * @param inbFrame       the default inboard frame
02876      * @param body           describes this MobilizedBody's physical properties
02877      * @param outbFrame      the default outboard frame
02878      * @param nmobilities    the number of generalized coordinates belonging to this MobilizedBody
02879      * @param functions      the Functions describing how the body moves based on its generalized coordinates.
02880      *                       This must be of length 6.  The elements correspond to, in order, x rotation, y rotation, z rotation,
02881      *                       x translation, y translation, and z translation.  The MobilizedBody takes over ownership of the functions,
02882      *                       and automatically deletes them when the MobilizedBody is deleted.
02883      * @param coordIndices   the indices of the generalized coordinates that are inputs to each function.  For example, if coordIndices[2] = {0, 1},
02884      *                       that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
02885      * @param direction      whether you want the coordinates defined as though parent & child were swapped
02886      */
02887     FunctionBased(MobilizedBody& parent, const Transform& inbFrame, 
02888                   const Body& body, const Transform& outbFrame, 
02889                   int nmobilities, const std::vector<const Function*>& functions,
02890                   const std::vector<std::vector<int> >& coordIndices,
02891                   Direction direction=Forward);
02892     /* Create a FunctionBased MobilizedBody.
02893      * 
02894      * @param parent         the MobilizedBody's parent body
02895      * @param body           describes this MobilizedBody's physical properties
02896      * @param nmobilities    the number of generalized coordinates belonging to this MobilizedBody
02897      * @param functions      the Functions describing how the body moves based on its generalized coordinates.
02898      *                       This must be of length 6.  The elements correspond to, in order, x rotation, y rotation, z rotation,
02899      *                       x translation, y translation, and z translation.  The MobilizedBody takes over ownership of the functions,
02900      *                       and automatically deletes them when the MobilizedBody is deleted.
02901      * @param coordIndices   the indices of the generalized coordinates that are inputs to each function.  For example, if coordIndices[2] = {0, 1},
02902      *                       that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
02903      * @param axes           the axes directions (as Vec3's) for each spatial coordinate, which each function describes, and is therefore length 6.
02904      *                       First 3 and last 3 axes must be linearly independent, otherwise there will be redundant speeds for the same motion.
02905      * @param direction      whether you want the coordinates defined as though parent & child were swapped
02906      */
02907     FunctionBased(MobilizedBody& parent, const Body& body, 
02908                   int nmobilities, const std::vector<const Function*>& functions,
02909                   const std::vector<std::vector<int> >& coordIndices, const std::vector<Vec3>& axes,
02910                   Direction direction=Forward);
02911     /* Create a FunctionBased MobilizedBody.
02912      * 
02913      * @param parent         the MobilizedBody's parent body
02914      * @param inbFrame       the default inboard frame
02915      * @param body           describes this MobilizedBody's physical properties
02916      * @param outbFrame      the default outboard frame
02917      * @param nmobilities    the number of generalized coordinates belonging to this MobilizedBody
02918      * @param functions      the Functions describing how the body moves based on its generalized coordinates.
02919      *                       This must be of length 6.  The elements correspond to, in order, x rotation, y rotation, z rotation,
02920      *                       x translation, y translation, and z translation.  The MobilizedBody takes over ownership of the functions,
02921      *                       and automatically deletes them when the MobilizedBody is deleted.
02922      * @param coordIndices   the indices of the generalized coordinates that are inputs to each function.  For example, if coordIndices[2] = {0, 1},
02923      *                       that means that functions[2] takes two input arguments, and q[0] and q[1] respectively should be passed as those arguments.
02924      * @param axes           the axes directions (as Vec3's) for each spatial coordinate, which each function describes, and is therefore length 6.
02925      *                       First 3 and last 3 axes must be linearly independent, otherwise there will be redundant speeds for the same motion.
02926      * @param direction      whether you want the coordinates defined as though parent & child were swapped
02927      */
02928     FunctionBased(MobilizedBody& parent, const Transform& inbFrame, 
02929                   const Body& body, const Transform& outbFrame, 
02930                   int nmobilities, const std::vector<const Function*>& functions,
02931                   const std::vector<std::vector<int> >& coordIndices, const std::vector<Vec3>& axes,
02932                   Direction direction=Forward);
02933 };
02934 
02935 } // namespace SimTK
02936 
02937 #endif // SimTK_SIMBODY_MOBILIZED_BODY_H_
02938 
02939 
02940 

Generated by  doxygen 1.6.2