00001 #ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
00002 #define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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;
00129 class SubtreeResults;
00130
00131
00132
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
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
00216
00217
00218
00219
00220
00221
00222
00223 MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent, MobilizedBody& child);
00224 const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;
00225 MobilizedBody& updMobilizedBody(MobilizedBodyIndex);
00226
00227
00228
00229
00230
00231
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
00243
00244
00245
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;
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
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
00627
00628
00629
00630
00631
00632
00633
00634
00635
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
00653
00654
00655
00656
00657 Vector_<Vec3>& updAllParticleLocations(State&) const;
00658 Vector_<Vec3>& updAllParticleVelocities(State&) const;
00659
00660
00661
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
00694
00703 void realizeCompositeBodyInertias(const State&) const;
00704
00713 void realizeArticulatedBodyInertias(const State&) const;
00714
00715
00716
00717
00725 const SpatialMat& getCompositeBodyInertia(const State&, MobilizedBodyIndex) const;
00726
00735 const SpatialMat& getArticulatedBodyInertia(const State&, MobilizedBodyIndex) const;
00736
00737
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
00756
00762 bool projectQConstraints(State& s, Real consAccuracy, const Vector& yWeights,
00763 const Vector& ooTols, Vector& yErrest, System::ProjectOptions) const;
00764
00765
00766
00767
00768 const SpatialVec& getCoriolisAcceleration(const State&, MobilizedBodyIndex) const;
00769
00770
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
00777
00778
00779
00785 bool projectUConstraints(State& s, Real consAccuracy, const Vector& yWeights,
00786 const Vector& ooTols, Vector& yErrest, System::ProjectOptions) const;
00787
00788
00789
00790
00791
00792 SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem);
00793 const SimbodyMatterSubsystemRep& getRep() const;
00794 SimbodyMatterSubsystemRep& updRep();
00795
00796 private:
00797
00798 void multiplyByQMatrix(const State& s, bool transposeMatrix, const Vector& in, Vector& out) const
00799 { multiplyByN(s,transposeMatrix,in,out);}
00800
00801 void multiplyByQMatrixInverse(const State& s, bool transposeMatrix, const Vector& in, Vector& out) const
00802 { multiplyByNInv(s,transposeMatrix,in,out);}
00803
00804 int getNBodies() const {return getNumBodies();}
00805
00806 int getNConstraints() const {return getNumConstraints();}
00807
00808 int getNMobilities() const {return getNumMobilities();}
00809
00810 int getNParticles() const {return getNumParticles();}
00811
00812 };
00813
00814 SimTK_SIMBODY_EXPORT std::ostream&
00815 operator<<(std::ostream&, const SimbodyMatterSubsystem&);
00816
00817
00818 }
00819
00820 #endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_