SimbodyMatterSubsystem.h

Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
00002 #define SimTK_SIMBODY_MATTER_SUBSYSTEM_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) 2006-9 Stanford University and the Authors.         *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors: Paul Mitiguy                                                 *
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 
00035 #include "SimTKcommon.h"
00036 #include "simbody/internal/common.h"
00037 #include "simbody/internal/MobilizedBody.h"
00038 
00039 #include <cassert>
00040 #include <vector>
00041 #include <iostream>
00042 
00043 class SimbodyMatterSubsystemRep;
00044 
00045 namespace SimTK {
00046 
00047 class MobilizedBody;
00048 class MultibodySystem;
00049 class Constraint;
00050 
00122 class SimTK_SIMBODY_EXPORT SimbodyMatterSubsystem : public Subsystem {
00123 public:
00125     SimbodyMatterSubsystem();
00126     explicit SimbodyMatterSubsystem(MultibodySystem&);
00127 
00128     class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
00129     class SubtreeResults;
00130 
00131     // These are the same as the compiler defaults but are handy to
00132     // have around explicitly for debugging.
00133     ~SimbodyMatterSubsystem() {
00134     }
00135     SimbodyMatterSubsystem(const SimbodyMatterSubsystem& ss) : Subsystem(ss) {
00136     }
00137     SimbodyMatterSubsystem& operator=(const SimbodyMatterSubsystem& ss) {
00138         Subsystem::operator=(ss);
00139         return *this;
00140     }
00141     
00143     bool getShowDefaultGeometry() const;
00144 
00146     void setShowDefaultGeometry(bool show);
00147 
00148 
00150         // PAUL'S FRIENDLY INTERFACE //
00152 
00157     Real calcSystemMass(const State& s) const;
00158 
00159 
00165     Vec3 calcSystemMassCenterLocationInGround(const State& s) const;
00166 
00167 
00173     MassProperties calcSystemMassPropertiesInGround(const State& s) const;
00174 
00180     Inertia calcSystemCentralInertiaInGround(const State& s) const;
00181 
00187     Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;
00188 
00194     Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;
00195 
00202     SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;
00203 
00211     SpatialVec calcSystemCentralMomentum(const State& s) const;
00212 
00214         // CONSTRUCTION //
00216 
00217     // Attach new matter using the indicated parent body as the reference
00218     // frame, with the mobilizer and mass properties provided by 'child'.
00219     // We take over ownership of child's representation from the given
00220     // handle, leaving that handle as a reference to our new matter object.
00221     // It is an error if the given handle wasn't the owner of the
00222     // matter representation.
00223     MobilizedBodyIndex   adoptMobilizedBody(MobilizedBodyIndex parent, MobilizedBody& child);
00224     const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;
00225     MobilizedBody&       updMobilizedBody(MobilizedBodyIndex);
00226 
00227 
00228     // Note: topology is not marked invalid upon returning a writable reference
00229     // here; that will be done only if a non-const method of the returned MobilizedBody
00230     // is called. That means it is OK to use Ground() to satisfy a const argument;
00231     // it won't have an "invalidate topology" side effect.
00232     const MobilizedBody::Ground& getGround() const;
00233     MobilizedBody::Ground&       updGround();
00234     MobilizedBody::Ground&       Ground() {return updGround();}
00235 
00236     ConstraintIndex   adoptConstraint(Constraint&);
00237     const Constraint& getConstraint(ConstraintIndex) const;
00238     Constraint&       updConstraint(ConstraintIndex);
00239 
00241         // OPERATORS //
00243 
00244         // Operators make use of the State but do not write their results back
00245         // into the State, not even into the State cache.
00246 
00271     void calcAcceleration(const State&,
00272         const Vector&              mobilityForces,
00273         const Vector_<SpatialVec>& bodyForces,
00274         Vector&                    udot,
00275         Vector_<SpatialVec>&       A_GB) const;
00276 
00291     void calcAccelerationIgnoringConstraints(const State&,
00292         const Vector&              mobilityForces,
00293         const Vector_<SpatialVec>& bodyForces,
00294         Vector&                    udot,
00295         Vector_<SpatialVec>&       A_GB) const;
00296 
00308     void calcMInverseV(const State&,
00309         const Vector&        v,
00310         Vector&              MinvV) const;
00311 
00331     void calcResidualForce
00332        (const State&               state,
00333         const Vector&              appliedMobilityForces,
00334         const Vector_<SpatialVec>& appliedBodyForces,
00335         const Vector&              knownUdot,
00336         const Vector&              knownMultipliers,
00337         Vector&                    residualMobilityForces) const;
00338 
00387     void calcResidualForceIgnoringConstraints
00388        (const State&               state,
00389         const Vector&              appliedMobilityForces,
00390         const Vector_<SpatialVec>& appliedBodyForces,
00391         const Vector&              knownUdot,
00392         Vector&                    residualMobilityForces) const;
00393 
00401     void calcMV(const State&, const Vector& v, Vector& MV) const;
00402 
00409     void calcCompositeBodyInertias(const State&,
00410         Vector_<SpatialMat>& R) const;
00411 
00418     void calcAccConstraintErr(const State&,
00419         const Vector&   knownUdot,
00420         Vector&         constraintErr) const;
00421 
00426     void calcGV(const State&,
00427         const Vector&   v,
00428         Vector&         Gv) const;
00429 
00435     void calcGtV(const State&,
00436         const Vector&   v,
00437         Vector&         GtV) const;
00438 
00439 
00452     void calcM(const State&, Matrix& M) const;
00453 
00467     void calcMInv(const State&, Matrix& MInv) const;
00468 
00479     void calcG(const State&, Matrix& G) const;
00480 
00491     void calcGt(const State&, Matrix& Gt) const;
00492 
00502     void calcConstraintForcesFromMultipliers
00503       (const State& s, const Vector& multipliers,
00504        Vector_<SpatialVec>& bodyForcesInG,
00505        Vector&              mobilityForces) const;
00506 
00515     void calcMobilizerReactionForces(const State& s, Vector_<SpatialVec>& forces) const;
00516 
00518     void calcSpatialKinematicsFromInternal(const State&,
00519         const Vector&        v,
00520         Vector_<SpatialVec>& Jv) const;
00521 
00523     void calcInternalGradientFromSpatial(const State&,
00524         const Vector_<SpatialVec>& dEdR,
00525         Vector&                    dEdQ) const; // really Qbar
00526 
00528     Real calcKineticEnergy(const State&) const;
00529 
00535     void calcTreeEquivalentMobilityForces(const State&, 
00536         const Vector_<SpatialVec>& bodyForces,
00537         Vector&                    mobilityForces) const;
00538 
00539 
00540 
00542     void calcQDot(const State& s,
00543         const Vector& u,
00544         Vector&       qdot) const;
00545 
00547     void calcQDotDot(const State& s,
00548         const Vector& udot,
00549         Vector&       qdotdot) const;
00550 
00557     void multiplyByN(const State& s, bool transpose, const Vector& in, Vector& out) const;
00558 
00565     void multiplyByNInv(const State& s, bool transpose, const Vector& in, Vector& out) const;
00566 
00567 
00568     // These are available after realizeTopology().
00569 
00578     int getNumBodies() const;
00579 
00582     int getNumConstraints() const;
00583 
00585     int getNumParticles() const;
00586 
00589     int getNumMobilities() const;
00590 
00593     int getTotalQAlloc() const;
00594 
00597     int getTotalMultAlloc() const;
00598 
00604     void setUseEulerAngles(State&, bool) const;
00605     bool getUseEulerAngles  (const State&) const;
00606 
00607     int  getNumQuaternionsInUse(const State&) const;
00608 
00609     void setMobilizerIsPrescribed(State&, MobilizedBodyIndex, bool) const;
00610     bool isMobilizerPrescribed  (const State&, MobilizedBodyIndex) const;
00611     bool isUsingQuaternion(const State&, MobilizedBodyIndex) const;
00612     QuaternionPoolIndex getQuaternionPoolIndex(const State&, MobilizedBodyIndex) const;
00613     AnglePoolIndex      getAnglePoolIndex(const State&, MobilizedBodyIndex) const;
00614     void setConstraintIsDisabled(State&, ConstraintIndex constraint, bool) const;
00615     bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
00616     
00619     void convertToEulerAngles(const State& inputState, State& outputState) const;
00620     
00623     void convertToQuaternions(const State& inputState, State& outputState) const; 
00624 
00625 
00626         // PARTICLES
00627         // TODO: not currently implemented. Use a point mass with a Cartesian (translation)
00628         // mobilizer to Ground instead. The idea here would be to special-case particles
00629         // to make them faster; there would be no additional functionality.
00630 
00631     // The generalized coordinates for a particle are always the three measure numbers
00632     // (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized
00633     // speeds are always the three corresponding measure numbers of the particle's
00634     // Ground-relative Cartesian velocity. The generalized applied forces are
00635     // always the three measure numbers of a Ground-relative force vector.
00636     const Vector_<Vec3>& getAllParticleLocations    (const State&) const;
00637     const Vector_<Vec3>& getAllParticleVelocities   (const State&) const;
00638 
00639     const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
00640         return getAllParticleLocations(s)[p];
00641     }
00642     const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
00643         return getAllParticleVelocities(s)[p];
00644     }
00645 
00646     Vector& updAllParticleMasses(State& s) const;
00647 
00648     void setAllParticleMasses(State& s, const Vector& masses) const {
00649         updAllParticleMasses(s) = masses;
00650     }
00651 
00652     // Note that particle generalized coordinates, speeds, and applied forces
00653     // are defined to be the particle Cartesian locations, velocities, and
00654     // applied force vectors, so can be set directly at Stage::Model or higher.
00655 
00656     // These are the only routines that must be provided by the concrete MatterSubsystem.
00657     Vector_<Vec3>& updAllParticleLocations(State&)     const;
00658     Vector_<Vec3>& updAllParticleVelocities(State&)    const;
00659 
00660     // The following inline routines are provided by the generic MatterSubsystem class
00661     // for convenience.
00662 
00663     Vec3& updParticleLocation(State& s, ParticleIndex p) const {
00664         return updAllParticleLocations(s)[p];
00665     }
00666     Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
00667         return updAllParticleVelocities(s)[p];
00668     }
00669 
00670     void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
00671         updAllParticleLocations(s)[p] = r;
00672     }
00673     void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
00674         updAllParticleVelocities(s)[p] = v;
00675     }
00676 
00677     void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {
00678         updAllParticleLocations(s) = r;
00679     }
00680     void setAllParticleVelocities(State& s, const Vector_<Vec3>& v) const {
00681         updAllParticleVelocities(s) = v;
00682     }
00683 
00685     const Vector& getAllParticleMasses(const State&) const;
00686 
00687     const Vector_<Vec3>& getAllParticleAccelerations(const State&) const;
00688 
00689     const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
00690         return getAllParticleAccelerations(s)[p];
00691     }
00692 
00693         // POSITION STAGE realizations //
00694 
00703     void realizeCompositeBodyInertias(const State&) const;
00704 
00713     void realizeArticulatedBodyInertias(const State&) const;
00714 
00715 
00716         // POSITION STAGE responses //
00717  
00725     const SpatialMat& getCompositeBodyInertia(const State&, MobilizedBodyIndex) const;
00726 
00735     const SpatialMat& getArticulatedBodyInertia(const State&, MobilizedBodyIndex) const;
00736 
00737         // POSITION STAGE operators //
00738 
00742     void addInStationForce(const State&, MobilizedBodyIndex bodyB, const Vec3& stationOnB, 
00743                            const Vec3& forceInG, Vector_<SpatialVec>& bodyForcesInG) const;
00744 
00747     void addInBodyTorque(const State&, MobilizedBodyIndex, const Vec3& torqueInG, 
00748                          Vector_<SpatialVec>& bodyForcesInG) const;
00749 
00752     void addInMobilityForce(const State&, MobilizedBodyIndex, MobilizerUIndex which, Real f,
00753                             Vector& mobilityForces) const;
00754 
00755         // POSITION STAGE solvers //
00756 
00762     bool projectQConstraints(State& s, Real consAccuracy, const Vector& yWeights,
00763                              const Vector& ooTols, Vector& yErrest, System::ProjectOptions) const;
00764 
00765         // VELOCITY STAGE responses //
00766 
00767     // Cross joint
00768     const SpatialVec& getCoriolisAcceleration(const State&, MobilizedBodyIndex) const;
00769 
00770     // Including parent
00771     const SpatialVec& getTotalCoriolisAcceleration(const State&, MobilizedBodyIndex) const;
00772 
00773     const SpatialVec& getGyroscopicForce(const State&, MobilizedBodyIndex) const;
00774     const SpatialVec& getCentrifugalForces(const State&, MobilizedBodyIndex) const;
00775 
00776         // VELOCITY STAGE operators //
00777 
00778         // VELOCITY STAGE solvers //
00779 
00785     bool projectUConstraints(State& s, Real consAccuracy, const Vector& yWeights,
00786                              const Vector& ooTols, Vector& yErrest, System::ProjectOptions) const;
00787 
00788         // ACCELERATION STAGE reponses
00789 
00790 
00791         // Bookkeeping
00792     SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem);
00793     const SimbodyMatterSubsystemRep& getRep() const;
00794     SimbodyMatterSubsystemRep&       updRep();
00795 
00796 private:
00797     // OBSOLETE; TODO: remove in SimTK 2.0
00798     void multiplyByQMatrix(const State& s, bool transposeMatrix, const Vector& in, Vector& out) const
00799     {   multiplyByN(s,transposeMatrix,in,out);}
00800     // OBSOLETE; TODO: remove in SimTK 2.0
00801     void multiplyByQMatrixInverse(const State& s, bool transposeMatrix, const Vector& in, Vector& out) const
00802     {   multiplyByNInv(s,transposeMatrix,in,out);}
00803     // OBSOLETE; TODO: remove in SimTK 2.0
00804     int getNBodies() const {return getNumBodies();}
00805     // OBSOLETE; TODO: remove in SimTK 2.0
00806     int getNConstraints() const {return getNumConstraints();}
00807     // OBSOLETE; TODO: remove in SimTK 2.0
00808     int getNMobilities() const {return getNumMobilities();}
00809     // OBSOLETE; TODO: remove in SimTK 2.0
00810     int getNParticles() const {return getNumParticles();}
00811 
00812 };
00813 
00814 SimTK_SIMBODY_EXPORT std::ostream& 
00815 operator<<(std::ostream&, const SimbodyMatterSubsystem&);
00816 
00817 
00818 } // namespace SimTK
00819 
00820 #endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_

Generated by  doxygen 1.6.2