Simbody
3.5
|
00001 #ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_ 00002 #define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * Simbody(tm) * 00006 * -------------------------------------------------------------------------- * 00007 * This is part of the SimTK 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/home/simbody. * 00011 * * 00012 * Portions copyright (c) 2006-12 Stanford University and the Authors. * 00013 * Authors: Michael Sherman * 00014 * Contributors: Paul Mitiguy * 00015 * * 00016 * Licensed under the Apache License, Version 2.0 (the "License"); you may * 00017 * not use this file except in compliance with the License. You may obtain a * 00018 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. * 00019 * * 00020 * Unless required by applicable law or agreed to in writing, software * 00021 * distributed under the License is distributed on an "AS IS" BASIS, * 00022 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. * 00023 * See the License for the specific language governing permissions and * 00024 * limitations under the License. * 00025 * -------------------------------------------------------------------------- */ 00026 00027 #include "SimTKcommon.h" 00028 #include "simbody/internal/common.h" 00029 #include "simbody/internal/MobilizedBody.h" 00030 00031 #include <cassert> 00032 #include <vector> 00033 #include <iostream> 00034 00035 class SimbodyMatterSubsystemRep; 00036 00037 namespace SimTK { 00038 00039 class MobilizedBody; 00040 class MultibodySystem; 00041 class Constraint; 00042 00043 class UnilateralContact; 00044 class StateLimitedFriction; 00045 00133 class SimTK_SIMBODY_EXPORT SimbodyMatterSubsystem : public Subsystem { 00134 public: 00135 00136 //============================================================================== 00152 explicit SimbodyMatterSubsystem(MultibodySystem&); 00156 SimbodyMatterSubsystem(); 00161 ~SimbodyMatterSubsystem() {} // invokes ~Subsystem() 00162 00167 const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const; 00168 00173 MobilizedBody& updMobilizedBody(MobilizedBodyIndex); 00174 00177 const MobilizedBody::Ground& getGround() const; 00181 MobilizedBody::Ground& updGround(); 00188 MobilizedBody::Ground& Ground() {return updGround();} 00189 00193 const Constraint& getConstraint(ConstraintIndex) const; 00194 00198 Constraint& updConstraint(ConstraintIndex); 00199 00203 void setShowDefaultGeometry(bool show); 00206 bool getShowDefaultGeometry() const; 00207 00218 int getNumBodies() const; 00219 00223 int getNumConstraints() const; 00224 00227 int getNumMobilities() const; 00228 00231 int getTotalQAlloc() const; 00232 00236 int getTotalMultAlloc() const; 00237 00238 00251 MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent, 00252 MobilizedBody& child); 00253 00264 ConstraintIndex adoptConstraint(Constraint&); 00265 00267 UnilateralContactIndex adoptUnilateralContact(UnilateralContact*); 00268 int getNumUnilateralContacts() const; 00269 const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const; 00270 UnilateralContact& updUnilateralContact(UnilateralContactIndex); 00272 StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction*); 00273 int getNumStateLimitedFrictions() const; 00274 const StateLimitedFriction& 00275 getStateLimitedFriction(StateLimitedFrictionIndex) const; 00276 StateLimitedFriction& 00277 updStateLimitedFriction(StateLimitedFrictionIndex); 00278 00280 SimbodyMatterSubsystem(const SimbodyMatterSubsystem& ss) : Subsystem(ss) {} 00282 SimbodyMatterSubsystem& operator=(const SimbodyMatterSubsystem& ss) 00283 { Subsystem::operator=(ss); return *this; } 00284 00285 00286 //============================================================================== 00301 void setUseEulerAngles(State& state, bool useEulerAngles) const; 00302 00305 bool getUseEulerAngles (const State& state) const; 00306 00311 int getNumQuaternionsInUse(const State& state) const; 00312 00317 bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const; 00323 QuaternionPoolIndex getQuaternionPoolIndex(const State& state, 00324 MobilizedBodyIndex mobodIx) const; 00325 00332 void setConstraintIsDisabled(State& state, 00333 ConstraintIndex constraintIx, 00334 bool shouldDisableConstraint) const; 00335 00339 bool isConstraintDisabled(const State&, ConstraintIndex constraint) const; 00340 00347 void convertToEulerAngles(const State& inputState, State& outputState) const; 00348 00355 void convertToQuaternions(const State& inputState, State& outputState) const; 00356 00364 void normalizeQuaternions(State& state) const; 00365 00369 //============================================================================== 00383 Real calcSystemMass(const State& s) const; 00384 00389 Vec3 calcSystemMassCenterLocationInGround(const State& s) const; 00390 00395 MassProperties calcSystemMassPropertiesInGround(const State& s) const; 00396 00401 Inertia calcSystemCentralInertiaInGround(const State& s) const; 00402 00407 Vec3 calcSystemMassCenterVelocityInGround(const State& s) const; 00408 00413 Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const; 00414 00421 SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const; 00422 00430 SpatialVec calcSystemCentralMomentum(const State& s) const; 00431 00436 Real calcKineticEnergy(const State& state) const; 00439 //============================================================================== 00552 void multiplyBySystemJacobian( const State& state, 00553 const Vector& u, 00554 Vector_<SpatialVec>& Ju) const; 00555 00583 void calcBiasForSystemJacobian(const State& state, 00584 Vector_<SpatialVec>& JDotu) const; 00585 00586 00590 void calcBiasForSystemJacobian(const State& state, 00591 Vector& JDotu) const; 00592 00644 void multiplyBySystemJacobianTranspose( const State& state, 00645 const Vector_<SpatialVec>& F_G, 00646 Vector& f) const; 00647 00648 00681 void calcSystemJacobian(const State& state, 00682 Matrix_<SpatialVec>& J_G) const; // nb X nu 00683 00688 void calcSystemJacobian(const State& state, 00689 Matrix& J_G) const; // 6 nb X nu 00690 00691 00737 void multiplyByStationJacobian(const State& state, 00738 const Array_<MobilizedBodyIndex>& onBodyB, 00739 const Array_<Vec3>& stationPInB, 00740 const Vector& u, 00741 Vector_<Vec3>& JSu) const; 00742 00745 Vec3 multiplyByStationJacobian(const State& state, 00746 MobilizedBodyIndex onBodyB, 00747 const Vec3& stationPInB, 00748 const Vector& u) const 00749 { 00750 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 00751 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1); 00752 Vector_<Vec3> JSu(1); 00753 multiplyByStationJacobian(state, bodies, stations, u, JSu); 00754 return JSu[0]; 00755 } 00756 00757 00770 void multiplyByStationJacobianTranspose 00771 (const State& state, 00772 const Array_<MobilizedBodyIndex>& onBodyB, 00773 const Array_<Vec3>& stationPInB, 00774 const Vector_<Vec3>& f_GP, 00775 Vector& f) const; 00776 00778 void multiplyByStationJacobianTranspose 00779 (const State& state, 00780 MobilizedBodyIndex onBodyB, 00781 const Vec3& stationPInB, 00782 const Vec3& f_GP, 00783 Vector& f) const 00784 { 00785 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 00786 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1); 00787 Vector_<Vec3> forces(1, f_GP); 00788 multiplyByStationJacobianTranspose(state, bodies, stations, forces, f); 00789 } 00790 00831 void calcStationJacobian(const State& state, 00832 const Array_<MobilizedBodyIndex>& onBodyB, 00833 const Array_<Vec3>& stationPInB, 00834 Matrix_<Vec3>& JS) const; 00835 00837 void calcStationJacobian(const State& state, 00838 MobilizedBodyIndex onBodyB, 00839 const Vec3& stationPInB, 00840 RowVector_<Vec3>& JS) const 00841 { 00842 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 00843 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1); 00844 calcStationJacobian(state, bodies, stations, JS); 00845 } 00846 00850 void calcStationJacobian(const State& state, 00851 const Array_<MobilizedBodyIndex>& onBodyB, 00852 const Array_<Vec3>& stationPInB, 00853 Matrix& JS) const; 00854 00856 void calcStationJacobian(const State& state, 00857 MobilizedBodyIndex onBodyB, 00858 const Vec3& stationPInB, 00859 Matrix& JS) const 00860 { 00861 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 00862 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1); 00863 calcStationJacobian(state, bodies, stations, JS); 00864 } 00865 00866 00903 void calcBiasForStationJacobian(const State& state, 00904 const Array_<MobilizedBodyIndex>& onBodyB, 00905 const Array_<Vec3>& stationPInB, 00906 Vector_<Vec3>& JSDotu) const; 00907 00911 void calcBiasForStationJacobian(const State& state, 00912 const Array_<MobilizedBodyIndex>& onBodyB, 00913 const Array_<Vec3>& stationPInB, 00914 Vector& JSDotu) const; 00915 00918 Vec3 calcBiasForStationJacobian(const State& state, 00919 MobilizedBodyIndex onBodyB, 00920 const Vec3& stationPInB) const 00921 { 00922 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 00923 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1); 00924 Vector_<Vec3> JSDotu(1); 00925 calcBiasForStationJacobian(state, bodies, stations, JSDotu); 00926 return JSDotu[0]; 00927 } 00928 00929 00986 void multiplyByFrameJacobian 00987 (const State& state, 00988 const Array_<MobilizedBodyIndex>& onBodyB, 00989 const Array_<Vec3>& originAoInB, 00990 const Vector& u, 00991 Vector_<SpatialVec>& JFu) const; 00992 00996 SpatialVec multiplyByFrameJacobian( const State& state, 00997 MobilizedBodyIndex onBodyB, 00998 const Vec3& originAoInB, 00999 const Vector& u) const 01000 { 01001 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 01002 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1); 01003 Vector_<SpatialVec> JFu(1); 01004 multiplyByFrameJacobian(state, bodies, stations, u, JFu); 01005 return JFu[0]; 01006 } 01007 01008 01051 void multiplyByFrameJacobianTranspose 01052 (const State& state, 01053 const Array_<MobilizedBodyIndex>& onBodyB, 01054 const Array_<Vec3>& originAoInB, 01055 const Vector_<SpatialVec>& F_GAo, 01056 Vector& f) const; 01057 01060 void multiplyByFrameJacobianTranspose( const State& state, 01061 MobilizedBodyIndex onBodyB, 01062 const Vec3& originAoInB, 01063 const SpatialVec& F_GAo, 01064 Vector& f) const 01065 { 01066 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 01067 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1); 01068 const Vector_<SpatialVec> forces(1, F_GAo); 01069 multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f); 01070 } 01071 01072 01073 01118 void calcFrameJacobian(const State& state, 01119 const Array_<MobilizedBodyIndex>& onBodyB, 01120 const Array_<Vec3>& originAoInB, 01121 Matrix_<SpatialVec>& JF) const; 01122 01125 void calcFrameJacobian(const State& state, 01126 MobilizedBodyIndex onBodyB, 01127 const Vec3& originAoInB, 01128 RowVector_<SpatialVec>& JF) const 01129 { 01130 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 01131 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1); 01132 calcFrameJacobian(state, bodies, stations, JF); 01133 } 01134 01138 void calcFrameJacobian(const State& state, 01139 const Array_<MobilizedBodyIndex>& onBodyB, 01140 const Array_<Vec3>& originAoInB, 01141 Matrix& JF) const; 01142 01145 void calcFrameJacobian(const State& state, 01146 MobilizedBodyIndex onBodyB, 01147 const Vec3& originAoInB, 01148 Matrix& JF) const 01149 { 01150 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 01151 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1); 01152 calcFrameJacobian(state, bodies, stations, JF); 01153 } 01154 01197 void calcBiasForFrameJacobian 01198 (const State& state, 01199 const Array_<MobilizedBodyIndex>& onBodyB, 01200 const Array_<Vec3>& originAoInB, 01201 Vector_<SpatialVec>& JFDotu) const; 01202 01206 void calcBiasForFrameJacobian(const State& state, 01207 const Array_<MobilizedBodyIndex>& onBodyB, 01208 const Array_<Vec3>& originAoInB, 01209 Vector& JFDotu) const; 01210 01214 SpatialVec calcBiasForFrameJacobian(const State& state, 01215 MobilizedBodyIndex onBodyB, 01216 const Vec3& originAoInB) const 01217 { 01218 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1); 01219 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1); 01220 Vector_<SpatialVec> JFDotu(1); 01221 calcBiasForFrameJacobian(state, bodies, stations, JFDotu); 01222 return JFDotu[0]; 01223 } 01224 01227 //============================================================================== 01260 void multiplyByM(const State& state, const Vector& a, Vector& Ma) const; 01261 01341 void multiplyByMInv(const State& state, 01342 const Vector& v, 01343 Vector& MinvV) const; 01344 01354 void calcM(const State&, Matrix& M) const; 01355 01369 void calcMInv(const State&, Matrix& MInv) const; 01370 01415 void calcProjectedMInv(const State& s, 01416 Matrix& GMInvGt) const; 01417 01462 void solveForConstraintImpulses(const State& state, 01463 const Vector& deltaV, 01464 Vector& impulse) const; 01465 01466 01490 void multiplyByG(const State& state, 01491 const Vector& ulike, 01492 Vector& Gulike) const { 01493 Vector bias; 01494 calcBiasForMultiplyByG(state, bias); 01495 multiplyByG(state, ulike, bias, Gulike); 01496 } 01497 01498 01502 void multiplyByG(const State& state, 01503 const Vector& ulike, 01504 const Vector& bias, 01505 Vector& Gulike) const; 01506 01532 void calcBiasForMultiplyByG(const State& state, 01533 Vector& bias) const; 01534 01548 void calcG(const State& state, Matrix& G) const; 01549 01550 01585 void calcBiasForAccelerationConstraints(const State& state, 01586 Vector& bias) const; 01587 01588 01622 void multiplyByGTranspose(const State& state, 01623 const Vector& lambda, 01624 Vector& f) const; 01625 01637 void calcGTranspose(const State&, Matrix& Gt) const; 01638 01639 01690 void multiplyByPq(const State& state, 01691 const Vector& qlike, 01692 Vector& PqXqlike) const { 01693 Vector biasp; 01694 calcBiasForMultiplyByPq(state, biasp); 01695 multiplyByPq(state, qlike, biasp, PqXqlike); 01696 } 01697 01698 01702 void multiplyByPq(const State& state, 01703 const Vector& qlike, 01704 const Vector& biasp, 01705 Vector& PqXqlike) const; 01706 01723 void calcBiasForMultiplyByPq(const State& state, 01724 Vector& biasp) const; 01725 01753 void calcPq(const State& state, Matrix& Pq) const; 01754 01755 01788 void multiplyByPqTranspose(const State& state, 01789 const Vector& lambdap, 01790 Vector& f) const; 01791 01819 void calcPqTranspose(const State& state, Matrix& Pqt) const; 01820 01837 void calcP(const State& state, Matrix& P) const; 01838 01842 void calcPt(const State& state, Matrix& Pt) const; 01843 01844 01853 void multiplyByN(const State& s, bool transpose, 01854 const Vector& in, Vector& out) const; 01855 01864 void multiplyByNInv(const State& s, bool transpose, 01865 const Vector& in, Vector& out) const; 01866 01876 void multiplyByNDot(const State& s, bool transpose, 01877 const Vector& in, Vector& out) const; 01878 01882 //============================================================================== 01943 void calcAcceleration 01944 (const State& state, 01945 const Vector& appliedMobilityForces, 01946 const Vector_<SpatialVec>& appliedBodyForces, 01947 Vector& udot, // output only; no prescribed motions 01948 Vector_<SpatialVec>& A_GB) const; 01949 01973 void calcAccelerationIgnoringConstraints 01974 (const State& state, 01975 const Vector& appliedMobilityForces, 01976 const Vector_<SpatialVec>& appliedBodyForces, 01977 Vector& udot, 01978 Vector_<SpatialVec>& A_GB) const; 01979 01980 01981 02036 void calcResidualForceIgnoringConstraints 02037 (const State& state, 02038 const Vector& appliedMobilityForces, 02039 const Vector_<SpatialVec>& appliedBodyForces, 02040 const Vector& knownUdot, 02041 Vector& residualMobilityForces) const; 02042 02043 02106 void calcResidualForce 02107 (const State& state, 02108 const Vector& appliedMobilityForces, 02109 const Vector_<SpatialVec>& appliedBodyForces, 02110 const Vector& knownUdot, 02111 const Vector& knownLambda, 02112 Vector& residualMobilityForces) const; 02113 02114 02125 void calcCompositeBodyInertias(const State& state, 02126 Array_<SpatialInertia,MobilizedBodyIndex>& R) const; 02127 02128 02129 02167 void calcBodyAccelerationFromUDot(const State& state, 02168 const Vector& knownUDot, 02169 Vector_<SpatialVec>& A_GB) const; 02170 02171 02194 void calcConstraintForcesFromMultipliers 02195 (const State& state, 02196 const Vector& multipliers, 02197 Vector_<SpatialVec>& bodyForcesInG, 02198 Vector& mobilityForces) const; 02199 02282 void calcMobilizerReactionForces 02283 (const State& state, 02284 Vector_<SpatialVec>& forcesAtMInG) const; 02285 02292 const Vector& getMotionMultipliers(const State& state) const; 02293 02307 Vector calcMotionErrors(const State& state, const Stage& stage) const; 02308 02314 void findMotionForces 02315 (const State& state, 02316 Vector& mobilityForces) const; 02317 02324 const Vector& getConstraintMultipliers(const State& state) const; 02325 02330 void findConstraintForces 02331 (const State& state, 02332 Vector_<SpatialVec>& bodyForcesInG, 02333 Vector& mobilityForces) const; 02334 02350 Real calcMotionPower(const State& state) const; 02351 02378 Real calcConstraintPower(const State& state) const; 02379 02385 void calcTreeEquivalentMobilityForces(const State&, 02386 const Vector_<SpatialVec>& bodyForces, 02387 Vector& mobilityForces) const; 02388 02389 02394 void calcQDot(const State& s, 02395 const Vector& u, 02396 Vector& qdot) const; 02397 02403 void calcQDotDot(const State& s, 02404 const Vector& udot, 02405 Vector& qdotdot) const; 02406 02417 void addInStationForce(const State& state, 02418 MobilizedBodyIndex bodyB, 02419 const Vec3& stationOnB, 02420 const Vec3& forceInG, 02421 Vector_<SpatialVec>& bodyForcesInG) const; 02422 02429 void addInBodyTorque(const State& state, 02430 MobilizedBodyIndex mobodIx, 02431 const Vec3& torqueInG, 02432 Vector_<SpatialVec>& bodyForcesInG) const; 02433 02442 void addInMobilityForce(const State& state, 02443 MobilizedBodyIndex mobodIx, 02444 MobilizerUIndex which, 02445 Real f, 02446 Vector& mobilityForces) const; 02451 //============================================================================== 02466 // POSITION STAGE realizations // 02467 02476 void realizeCompositeBodyInertias(const State&) const; 02477 02487 void realizeArticulatedBodyInertias(const State&) const; 02488 02489 02490 // INSTANCE STAGE responses // 02491 02492 const Array_<QIndex>& getFreeQIndex(const State& state) const; 02493 const Array_<UIndex>& getFreeUIndex(const State& state) const; 02494 const Array_<UIndex>& getFreeUDotIndex(const State& state) const; 02495 const Array_<UIndex>& getKnownUDotIndex(const State& state) const; 02496 void packFreeQ 02497 (const State& s, const Vector& allQ, Vector& packedFreeQ) const; 02498 void unpackFreeQ 02499 (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const; 02500 void packFreeU 02501 (const State& s, const Vector& allU, Vector& packedFreeU) const; 02502 void unpackFreeU 02503 (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const; 02504 02505 02506 // POSITION STAGE responses // 02507 02517 const SpatialInertia& 02518 getCompositeBodyInertia(const State& state, MobilizedBodyIndex mbx) const; 02519 02530 const ArticulatedInertia& 02531 getArticulatedBodyInertia(const State& state, MobilizedBodyIndex mbx) const; 02532 02533 02534 // VELOCITY STAGE responses // 02535 02540 const SpatialVec& 02541 getGyroscopicForce(const State& state, MobilizedBodyIndex mbx) const; 02542 02548 const SpatialVec& 02549 getMobilizerCoriolisAcceleration(const State& state, 02550 MobilizedBodyIndex mbx) const; 02551 02560 const SpatialVec& 02561 getTotalCoriolisAcceleration(const State& state, MobilizedBodyIndex mbx) const; 02562 02563 02564 // DYNAMICS STAGE responses // 02565 02572 const SpatialVec& 02573 getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const; 02574 02583 const SpatialVec& 02584 getTotalCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const; 02589 //============================================================================== 02620 void calcMobilizerReactionForcesUsingFreebodyMethod 02621 (const State& state, 02622 Vector_<SpatialVec>& forcesAtMInG) const; 02623 02627 void invalidateCompositeBodyInertias(const State& state) const; 02628 02634 void invalidateArticulatedBodyInertias(const State& state) const; 02638 //============================================================================== 02649 02650 int getNumParticles() const; 02651 02652 // The generalized coordinates for a particle are always the three measure numbers 02653 // (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized 02654 // speeds are always the three corresponding measure numbers of the particle's 02655 // Ground-relative Cartesian velocity. The generalized applied forces are 02656 // always the three measure numbers of a Ground-relative force vector. 02657 const Vector_<Vec3>& getAllParticleLocations (const State&) const; 02658 const Vector_<Vec3>& getAllParticleVelocities (const State&) const; 02659 02660 const Vec3& getParticleLocation(const State& s, ParticleIndex p) const { 02661 return getAllParticleLocations(s)[p]; 02662 } 02663 const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const { 02664 return getAllParticleVelocities(s)[p]; 02665 } 02666 02667 Vector& updAllParticleMasses(State& s) const; 02668 02669 void setAllParticleMasses(State& s, const Vector& masses) const { 02670 updAllParticleMasses(s) = masses; 02671 } 02672 02673 // Note that particle generalized coordinates, speeds, and applied forces 02674 // are defined to be the particle Cartesian locations, velocities, and 02675 // applied force vectors, so can be set directly at Stage::Model or higher. 02676 02677 // These are the only routines that must be provided by the concrete MatterSubsystem. 02678 Vector_<Vec3>& updAllParticleLocations(State&) const; 02679 Vector_<Vec3>& updAllParticleVelocities(State&) const; 02680 02681 // The following inline routines are provided by the generic MatterSubsystem 02682 // class for convenience. 02683 02684 Vec3& updParticleLocation(State& s, ParticleIndex p) const { 02685 return updAllParticleLocations(s)[p]; 02686 } 02687 Vec3& updParticleVelocity(State& s, ParticleIndex p) const { 02688 return updAllParticleVelocities(s)[p]; 02689 } 02690 02691 void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const { 02692 updAllParticleLocations(s)[p] = r; 02693 } 02694 void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const { 02695 updAllParticleVelocities(s)[p] = v; 02696 } 02697 02698 void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const { 02699 updAllParticleLocations(s) = r; 02700 } 02701 void setAllParticleVelocities(State& s, const Vector_<Vec3>& v) const { 02702 updAllParticleVelocities(s) = v; 02703 } 02704 02705 const Vector& getAllParticleMasses(const State&) const; 02706 02707 const Vector_<Vec3>& getAllParticleAccelerations(const State&) const; 02708 02709 const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const { 02710 return getAllParticleAccelerations(s)[p]; 02711 } 02714 //============================================================================== 02723 private: 02726 void calcSpatialKinematicsFromInternal(const State& state, 02727 const Vector& u, 02728 Vector_<SpatialVec>& Ju) const 02729 { multiplyBySystemJacobian(state,u,Ju); } 02730 02733 void calcInternalGradientFromSpatial(const State& state, 02734 const Vector_<SpatialVec>& F_G, 02735 Vector& f) const 02736 { multiplyBySystemJacobianTranspose(state, F_G, f); } 02737 02740 void calcMV(const State& state, const Vector& v, Vector& MV) const 02741 { multiplyByM(state,v,MV); } 02742 02745 void calcMInverseV(const State& state, 02746 const Vector& v, 02747 Vector& MinvV) const 02748 { multiplyByMInv(state,v,MinvV); } 02749 02752 void calcPNInv(const State& state, Matrix& PNInv) const; 02753 02756 void calcGt(const State&, Matrix& Gt) const; 02757 02758 02762 //============================================================================== 02763 // Bookkeeping methods and internal types -- hide from Doxygen 02765 public: 02766 class Subtree; // used for working with a connected subgraph of the MobilizedBody tree 02767 class SubtreeResults; 02768 02769 02770 SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem); 02771 const SimbodyMatterSubsystemRep& getRep() const; 02772 SimbodyMatterSubsystemRep& updRep(); 02775 private: 02776 }; 02777 02781 SimTK_SIMBODY_EXPORT std::ostream& 02782 operator<<(std::ostream&, const SimbodyMatterSubsystem&); 02783 02784 02785 } // namespace SimTK 02786 02787 #endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_