Simbody
3.5
|
00001 #ifndef SimTK_SIMBODY_CONSTRAINT_H_ 00002 #define SimTK_SIMBODY_CONSTRAINT_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) 2007-14 Stanford University and the Authors. * 00013 * Authors: Michael Sherman * 00014 * Contributors: * 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 00033 #include "SimTKmath.h" 00034 #include "simbody/internal/common.h" 00035 00036 #include <cassert> 00037 00038 namespace SimTK { 00039 00040 class SimbodyMatterSubsystem; 00041 class SimbodyMatterSubtree; 00042 class MobilizedBody; 00043 class Constraint; 00044 class ConstraintImpl; 00045 00046 // We only want the template instantiation to occur once. This symbol is 00047 // defined in the SimTK core compilation unit that defines the Constraint 00048 // class but should not be defined any other time. 00049 #ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT 00050 extern template class PIMPLHandle<Constraint, ConstraintImpl, true>; 00051 #endif 00052 00054 // CONSTRAINT BASE CLASS // 00056 00066 class SimTK_SIMBODY_EXPORT Constraint 00067 : public PIMPLHandle<Constraint, ConstraintImpl, true> { 00068 public: 00071 Constraint() { } 00074 explicit Constraint(ConstraintImpl* r) : HandleBase(r) { } 00075 00079 void disable(State&) const; 00080 00087 void enable(State&) const; 00090 bool isDisabled(const State&) const; 00094 bool isDisabledByDefault() const; 00095 00100 void setDisabledByDefault(bool shouldBeDisabled); 00101 00104 operator ConstraintIndex() const {return getConstraintIndex();} 00105 00111 const SimbodyMatterSubsystem& getMatterSubsystem() const; 00112 00118 SimbodyMatterSubsystem& updMatterSubsystem(); 00119 00126 ConstraintIndex getConstraintIndex() const; 00127 00129 bool isInSubsystem() const; 00133 bool isInSameSubsystem(const MobilizedBody& mobod) const; 00134 00135 // TOPOLOGY STAGE (i.e., post-construction) // 00136 00142 int getNumConstrainedBodies() const; 00143 00148 const MobilizedBody& getMobilizedBodyFromConstrainedBody 00149 (ConstrainedBodyIndex consBodyIx) const; 00150 00155 const MobilizedBody& getAncestorMobilizedBody() const; 00156 00163 int getNumConstrainedMobilizers() const; 00164 00169 const MobilizedBody& getMobilizedBodyFromConstrainedMobilizer 00170 (ConstrainedMobilizerIndex consMobilizerIx) const; 00171 00174 const SimbodyMatterSubtree& getSubtree() const; 00175 00176 // MODEL STAGE // 00177 // nothing in base class currently 00178 00179 // INSTANCE STAGE // 00180 00185 int getNumConstrainedQ(const State&, ConstrainedMobilizerIndex) const; 00190 int getNumConstrainedU(const State&, ConstrainedMobilizerIndex) const; 00191 00198 ConstrainedUIndex getConstrainedUIndex 00199 (const State&, ConstrainedMobilizerIndex, MobilizerUIndex which) const; 00206 ConstrainedQIndex getConstrainedQIndex 00207 (const State&, ConstrainedMobilizerIndex, MobilizerQIndex which) const; 00208 00211 int getNumConstrainedQ(const State&) const; 00212 00217 int getNumConstrainedU(const State&) const; 00218 00221 QIndex getQIndexOfConstrainedQ(const State& state, 00222 ConstrainedQIndex consQIndex) const; 00225 UIndex getUIndexOfConstrainedU(const State& state, 00226 ConstrainedUIndex consUIndex) const; 00227 00241 void getNumConstraintEquationsInUse(const State& state, 00242 int& mp, int& mv, int& ma) const; 00243 00266 void getIndexOfMultipliersInUse(const State& state, 00267 MultiplierIndex& px0, 00268 MultiplierIndex& vx0, 00269 MultiplierIndex& ax0) const; 00270 00292 void setMyPartInConstraintSpaceVector(const State& state, 00293 const Vector& myPart, 00294 Vector& constraintSpace) const; 00295 00313 void getMyPartFromConstraintSpaceVector(const State& state, 00314 const Vector& constraintSpace, 00315 Vector& myPart) const; 00316 00317 // POSITION STAGE // 00320 Vector getPositionErrorsAsVector(const State&) const; // mp of these 00321 Vector calcPositionErrorFromQ(const State&, const Vector& q) const; 00322 00323 // Matrix P = partial(perr_dot)/partial(u). (just the holonomic constraints) 00324 Matrix calcPositionConstraintMatrixP(const State&) const; // mp X nu 00325 Matrix calcPositionConstraintMatrixPt(const State&) const; // nu X mp 00326 00327 // Matrix PNInv = partial(perr)/partial(q) = P*N^-1 00328 Matrix calcPositionConstraintMatrixPNInv(const State&) const; // mp X nq 00329 00345 void calcConstraintForcesFromMultipliers(const State&, 00346 const Vector& lambda, // mp+mv+ma of these 00347 Vector_<SpatialVec>& bodyForcesInA, // numConstrainedBodies 00348 Vector& mobilityForces) const; // numConstrainedU 00349 00350 // VELOCITY STAGE // 00353 Vector getVelocityErrorsAsVector(const State&) const; // mp+mv of these 00354 Vector calcVelocityErrorFromU(const State&, // mp+mv of these 00355 const Vector& u) const; // numParticipatingU u's 00356 00357 // Matrix V = partial(verr)/partial(u) for just the non-holonomic 00358 // constraints. 00359 Matrix calcVelocityConstraintMatrixV(const State&) const; // mv X nu 00360 Matrix calcVelocityConstraintMatrixVt(const State&) const; // nu X mv 00361 00362 // DYNAMICS STAGE // 00363 // nothing in base class currently 00364 00365 // ACCELERATION STAGE // 00369 Vector getAccelerationErrorsAsVector(const State&) const; // mp+mv+ma of these 00370 Vector calcAccelerationErrorFromUDot(const State&, // mp+mv+ma of these 00371 const Vector& udot) const; // numParticipatingU udot's 00372 00376 Vector getMultipliersAsVector(const State&) const; // mp+mv+ma of these 00377 00390 void getConstraintForcesAsVectors 00391 (const State& state, 00392 Vector_<SpatialVec>& bodyForcesInG, // numConstrainedBodies 00393 Vector& mobilityForces) const; // numConstrainedU 00394 00397 Vector_<SpatialVec> getConstrainedBodyForcesAsVector(const State& state) const { 00398 Vector_<SpatialVec> bodyForcesInG; 00399 Vector mobilityForces; 00400 getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces); 00401 return bodyForcesInG; 00402 } 00406 Vector getConstrainedMobilityForcesAsVector(const State& state) const { 00407 Vector_<SpatialVec> bodyForcesInG; 00408 Vector mobilityForces; 00409 getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces); 00410 return mobilityForces; 00411 } 00412 00436 Real calcPower(const State& state) const; 00437 00438 // Matrix A = partial(aerr)/partial(udot) for just the acceleration-only 00439 // constraints. 00440 Matrix calcAccelerationConstraintMatrixA(const State&) const; // ma X nu 00441 Matrix calcAccelerationConstraintMatrixAt(const State&) const; // nu X ma 00442 00447 void setIsConditional(bool isConditional); 00449 bool isConditional() const; 00450 00451 //------------------------------------------------------------------------------ 00452 // These are the built-in Constraint types, and some synonyms. Each built in 00453 // Constraint type is declared in its own header file using naming convention 00454 // Constraint_Rod.h, for example. All the built-in headers are collected in 00455 // Constraint_BuiltIns.h; you should include new ones there also. 00456 class Rod; 00457 typedef Rod ConstantDistance; 00458 00459 class Ball; 00460 typedef Ball CoincidentPoints; 00461 typedef Ball Spherical; 00462 00463 class Weld; 00464 typedef Weld CoincidentFrames; 00465 00466 class PointInPlane; // translations perpendicular to plane normal only 00467 class PointOnLine; // translations along a line only 00468 class ConstantAngle; // prevent rotation about common normal of two vectors 00469 class ConstantOrientation; // allows any translation but no rotation 00470 class NoSlip1D; // same velocity at a point along a direction 00471 class ConstantCoordinate; // prescribe generalized coordinate value 00472 class ConstantSpeed; // prescribe generalized speed value 00473 class ConstantAcceleration; // prescribe generalized acceleration value 00474 class Custom; 00475 class CoordinateCoupler; 00476 class SpeedCoupler; 00477 class PrescribedMotion; 00478 class PointOnPlaneContact; 00479 class SphereOnPlaneContact; // ball in contact with plane (sliding or rolling) 00480 class SphereOnSphereContact; // ball in contact with ball (sliding or rolling) 00481 class LineOnLineContact; // edge/edge contact 00482 00483 // Internal use only. 00484 class RodImpl; 00485 class BallImpl; 00486 class WeldImpl; 00487 class PointInPlaneImpl; 00488 class PointOnLineImpl; 00489 class ConstantAngleImpl; 00490 class ConstantOrientationImpl; 00491 class NoSlip1DImpl; 00492 class ConstantCoordinateImpl; 00493 class ConstantSpeedImpl; 00494 class ConstantAccelerationImpl; 00495 class CustomImpl; 00496 class CoordinateCouplerImpl; 00497 class SpeedCouplerImpl; 00498 class PrescribedMotionImpl; 00499 class PointOnPlaneContactImpl; 00500 class SphereOnPlaneContactImpl; 00501 class SphereOnSphereContactImpl; 00502 class LineOnLineContactImpl; 00503 00504 }; 00505 00507 // POINT ON LINE CONSTRAINT // 00509 00520 class SimTK_SIMBODY_EXPORT Constraint::PointOnLine : public Constraint { 00521 public: 00522 // no default constructor 00523 PointOnLine(MobilizedBody& lineBody_B, const UnitVec3& defaultLineDirection_B, const Vec3& defaultPointOnLine_B, 00524 MobilizedBody& followerBody_F, const Vec3& defaultFollowerPoint_F); 00525 00527 PointOnLine() {} 00528 00529 // These affect only generated decorative geometry for visualization; 00530 // the line is really infinite in extent and the 00531 // point is really of zero radius. 00532 PointOnLine& setLineDisplayHalfLength(Real); 00533 PointOnLine& setPointDisplayRadius(Real); 00534 Real getLineDisplayHalfLength() const; 00535 Real getPointDisplayRadius() const; 00536 00537 // Defaults for Instance variables. 00538 PointOnLine& setDefaultLineDirection(const UnitVec3&); 00539 PointOnLine& setDefaultPointOnLine(const Vec3&); 00540 PointOnLine& setDefaultFollowerPoint(const Vec3&); 00541 00542 // Stage::Topology 00543 MobilizedBodyIndex getLineMobilizedBodyIndex() const; 00544 MobilizedBodyIndex getFollowerMobilizedBodyIndex() const; 00545 00546 const UnitVec3& getDefaultLineDirection() const; 00547 const Vec3& getDefaultPointOnLine() const; 00548 const Vec3& getDefaultFollowerPoint() const; 00549 00550 // Stage::Instance 00551 const UnitVec3& getLineDirection(const State&) const; 00552 const Vec3& getPointOnLine(const State&) const; 00553 const Vec3& getFollowerPoint(const State&) const; 00554 00555 // Stage::Position, Velocity 00556 Vec2 getPositionErrors(const State&) const; 00557 Vec2 getVelocityErrors(const State&) const; 00558 00559 // Stage::Acceleration 00560 Vec2 getAccelerationErrors(const State&) const; 00561 Vec2 getMultipliers(const State&) const; 00562 const Vec2& getForceOnFollowerPoint(const State&) const; // in normal direction 00563 // hide from Doxygen 00565 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 00566 (PointOnLine, PointOnLineImpl, Constraint); 00568 }; 00569 00571 // CONSTANT ANGLE CONSTRAINT // 00573 00602 class SimTK_SIMBODY_EXPORT Constraint::ConstantAngle : public Constraint { 00603 public: 00604 // no default constructor 00605 ConstantAngle(MobilizedBody& baseBody_B, const UnitVec3& defaultAxis_B, 00606 MobilizedBody& followerBody_F, const UnitVec3& defaultAxis_F, 00607 Real angle = Pi/2); 00608 00610 ConstantAngle() {} 00611 00612 // These affect only generated decorative geometry for visualization. 00613 ConstantAngle& setAxisDisplayLength(Real); 00614 ConstantAngle& setAxisDisplayWidth(Real); 00615 Real getAxisDisplayLength() const; 00616 Real getAxisDisplayWidth() const; 00617 00618 // Defaults for Instance variables. 00619 ConstantAngle& setDefaultBaseAxis(const UnitVec3&); 00620 ConstantAngle& setDefaultFollowerAxis(const UnitVec3&); 00621 ConstantAngle& setDefaultAngle(Real); 00622 00623 // Stage::Topology 00624 MobilizedBodyIndex getBaseMobilizedBodyIndex() const; 00625 MobilizedBodyIndex getFollowerMobilizedBodyIndex() const; 00626 00627 const UnitVec3& getDefaultBaseAxis() const; 00628 const UnitVec3& getDefaultFollowerAxis() const; 00629 Real getDefaultAngle() const; 00630 00631 // Stage::Instance 00632 const UnitVec3& getBaseAxis(const State&) const; 00633 const UnitVec3& getFollowerAxis(const State&) const; 00634 Real getAngle(const State&) const; 00635 00636 // Stage::Position, Velocity 00637 Real getPositionError(const State&) const; 00638 Real getVelocityError(const State&) const; 00639 00640 // Stage::Acceleration 00641 Real getAccelerationError(const State&) const; 00642 Real getMultiplier(const State&) const; 00643 Real getTorqueOnFollowerBody(const State&) const; // about f X b 00644 // hide from Doxygen 00646 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ConstantAngle, ConstantAngleImpl, Constraint); 00648 }; 00649 00650 00652 // CONSTANT ORIENTATION CONSTRAINT // 00654 00671 class SimTK_SIMBODY_EXPORT Constraint::ConstantOrientation : public Constraint 00672 { 00673 public: 00674 // no default constructor 00675 ConstantOrientation(MobilizedBody& baseBody_B, const Rotation& defaultRB, 00676 MobilizedBody& followerBody_F, const Rotation& defaultRF); 00677 00679 ConstantOrientation() {} 00680 00681 //TODO: default visualization geometry? 00682 00683 // Defaults for Instance variables. 00684 ConstantOrientation& setDefaultBaseRotation(const Rotation&); 00685 ConstantOrientation& setDefaultFollowerRotation(const Rotation&); 00686 00687 // Stage::Topology 00688 MobilizedBodyIndex getBaseMobilizedBodyIndex() const; 00689 MobilizedBodyIndex getFollowerMobilizedBodyIndex() const; 00690 00691 const Rotation& getDefaultBaseRotation() const; 00692 const Rotation& getDefaultFollowerRotation() const; 00693 00694 // Stage::Instance 00695 const Rotation& getBaseRotation(const State&) const; 00696 const Rotation& getFollowerRotation(const State&) const; 00697 00698 // Stage::Position, Velocity 00699 Vec3 getPositionErrors(const State&) const; 00700 Vec3 getVelocityErrors(const State&) const; 00701 00702 // Stage::Acceleration 00703 Vec3 getAccelerationErrors(const State&) const; 00704 Vec3 getMultipliers(const State&) const; 00705 Vec3 getTorqueOnFollowerBody(const State&) const; 00706 // hide from Doxygen 00708 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 00709 (ConstantOrientation, ConstantOrientationImpl, Constraint); 00711 }; 00712 00714 // NO SLIP 1D CONSTRAINT // 00716 00728 class SimTK_SIMBODY_EXPORT Constraint::NoSlip1D : public Constraint { 00729 public: 00730 // no default constructor 00731 00738 NoSlip1D(MobilizedBody& caseBodyC, const Vec3& P_C, const UnitVec3& n_C, 00739 MobilizedBody& movingBody0, MobilizedBody& movingBody1); 00740 00742 NoSlip1D() {} 00743 00748 void setContactPoint(State& state, const Vec3& point_C) const; 00753 void setDirection(State& state, const UnitVec3& direction_C) const; 00754 00757 const Vec3& getContactPoint(const State& state) const; 00760 const UnitVec3& getDirection(const State& state) const; 00761 00762 // These affect only generated decorative geometry for visualization; 00763 // the plane is really infinite in extent with zero depth and the 00764 // point is really of zero radius. 00765 00768 NoSlip1D& setDirectionDisplayLength(Real); 00771 NoSlip1D& setPointDisplayRadius(Real); 00774 Real getDirectionDisplayLength() const; 00777 Real getPointDisplayRadius() const; 00778 00779 // Defaults for Instance variables. 00780 00783 NoSlip1D& setDefaultContactPoint(const Vec3&); 00786 NoSlip1D& setDefaultDirection(const UnitVec3&); 00787 00788 00789 // Stage::Topology 00790 00793 MobilizedBodyIndex getCaseMobilizedBodyIndex() const; 00796 MobilizedBodyIndex getMovingBodyMobilizedBodyIndex(int which) const; 00797 00800 const UnitVec3& getDefaultDirection() const; 00803 const Vec3& getDefaultContactPoint() const; 00804 00805 00806 // Stage::Position, Velocity 00807 // no position error 00808 00812 Real getVelocityError(const State& state) const; 00813 00814 // Stage::Acceleration 00815 00820 Real getAccelerationError(const State&) const; 00821 00828 Real getMultiplier(const State&) const; 00829 00833 Real getForceAtContactPoint(const State&) const; 00834 // hide from Doxygen 00836 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(NoSlip1D, NoSlip1DImpl, Constraint); 00838 }; 00839 00841 // CONSTANT COORDINATE // 00843 00858 class SimTK_SIMBODY_EXPORT Constraint::ConstantCoordinate : public Constraint { 00859 public: 00863 ConstantCoordinate(MobilizedBody& mobilizer, MobilizerQIndex whichQ, 00864 Real defaultPosition); 00865 00871 ConstantCoordinate(MobilizedBody& mobilizer, Real defaultPosition); 00872 00875 ConstantCoordinate() {} 00876 00880 MobilizedBodyIndex getMobilizedBodyIndex() const; 00881 00884 MobilizerQIndex getWhichQ() const; 00885 00890 Real getDefaultPosition() const; 00891 00897 ConstantCoordinate& setDefaultPosition(Real position); 00898 00903 void setPosition(State& state, Real position) const; 00904 00908 Real getPosition(const State& state) const; 00909 00915 Real getPositionError(const State& state) const; 00916 00921 Real getVelocityError(const State& state) const; 00922 00928 Real getAccelerationError(const State& state) const; 00929 00935 Real getMultiplier(const State& state) const; 00936 // hide from Doxygen 00938 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 00939 (ConstantCoordinate, ConstantCoordinateImpl, Constraint); 00941 }; 00942 00944 // CONSTANT SPEED // 00946 00959 class SimTK_SIMBODY_EXPORT Constraint::ConstantSpeed : public Constraint { 00960 public: 00963 ConstantSpeed(MobilizedBody& mobilizer, MobilizerUIndex whichU, 00964 Real defaultSpeed); 00967 ConstantSpeed(MobilizedBody& mobilizer, Real defaultSpeed); 00968 00971 ConstantSpeed() {} 00972 00976 MobilizedBodyIndex getMobilizedBodyIndex() const; 00977 00980 MobilizerUIndex getWhichU() const; 00981 00986 Real getDefaultSpeed() const; 00987 00993 ConstantSpeed& setDefaultSpeed(Real speed); 00994 00999 void setSpeed(State& state, Real speed) const; 01000 01004 Real getSpeed(const State& state) const; 01005 01006 // no position error 01007 01011 Real getVelocityError(const State& state) const; 01012 01016 Real getAccelerationError(const State& state) const; 01017 01023 Real getMultiplier(const State& state) const; 01024 // hide from Doxygen 01026 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 01027 (ConstantSpeed, ConstantSpeedImpl, Constraint); 01029 }; 01030 01032 // CONSTANT ACCELERATION // 01034 01047 class SimTK_SIMBODY_EXPORT Constraint::ConstantAcceleration : public Constraint 01048 { 01049 public: 01052 ConstantAcceleration(MobilizedBody& mobilizer, MobilizerUIndex whichU, 01053 Real defaultAcceleration); 01054 01057 ConstantAcceleration(MobilizedBody& mobilizer, 01058 Real defaultAcceleration); 01059 01062 ConstantAcceleration() {} 01063 01067 MobilizedBodyIndex getMobilizedBodyIndex() const; 01068 01072 MobilizerUIndex getWhichU() const; 01073 01078 Real getDefaultAcceleration() const; 01079 01085 ConstantAcceleration& setDefaultAcceleration(Real accel); 01086 01091 void setAcceleration(State& state, Real accel) const; 01092 01096 Real getAcceleration(const State& state) const; 01097 01098 // no position or velocity error 01099 01103 Real getAccelerationError(const State&) const; 01104 01110 Real getMultiplier(const State&) const; 01111 // hide from Doxygen 01113 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 01114 (ConstantAcceleration, ConstantAccelerationImpl, Constraint); 01116 }; 01117 01118 //============================================================================== 01119 // CUSTOM 01120 //============================================================================== 01153 class SimTK_SIMBODY_EXPORT Constraint::Custom : public Constraint { 01154 public: 01155 class Implementation; 01156 class ImplementationImpl; 01157 01165 explicit Custom(Implementation* implementation); 01166 01167 01169 Custom() {} 01170 01171 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, Constraint); 01172 protected: 01173 const Implementation& getImplementation() const; 01174 Implementation& updImplementation(); 01175 }; 01176 01177 //============================================================================== 01178 // CUSTOM::IMPLEMENTATION 01179 //============================================================================== 01180 01181 // We only want the template instantiation to occur once. This symbol is 01182 // defined in the SimTK core compilation unit that defines the Constraint 01183 // class but should not be defined any other time. 01184 #ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT 01185 extern template class PIMPLHandle<Constraint::Custom::Implementation, 01186 Constraint::Custom::ImplementationImpl>; 01187 #endif 01188 01191 class SimTK_SIMBODY_EXPORT Constraint::Custom::Implementation 01192 : public PIMPLHandle<Implementation,ImplementationImpl> { 01193 public: 01194 // No default constructor because you have to supply at least the 01195 // SimbodyMatterSubsystem to which this Constraint belongs. 01196 01199 virtual ~Implementation() { } 01200 01205 virtual Implementation* clone() const = 0; 01206 01210 Implementation(SimbodyMatterSubsystem&, int mp, int mv, int ma); 01211 01216 explicit Implementation(SimbodyMatterSubsystem&); 01217 01219 const SimbodyMatterSubsystem& getMatterSubsystem() const; 01220 01221 // Topological information// 01222 01229 void invalidateTopologyCache() const; 01230 01235 Implementation& setDefaultNumConstraintEquations(int mp, int mv, int ma); 01236 01241 Implementation& setDisabledByDefault(bool shouldBeDisabled); 01242 01248 ConstrainedBodyIndex addConstrainedBody(const MobilizedBody&); 01249 01256 ConstrainedMobilizerIndex addConstrainedMobilizer(const MobilizedBody&); 01257 01262 MobilizedBodyIndex 01263 getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex) const; 01264 01269 MobilizedBodyIndex 01270 getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex) const; 01271 01272 01302 Real getOneQ(const State& state, 01303 const Array_<Real,ConstrainedQIndex>& constrainedQ, 01304 ConstrainedMobilizerIndex mobilizer, 01305 MobilizerQIndex whichQ) const; 01306 01311 Real getOneQFromState(const State& state, 01312 ConstrainedMobilizerIndex mobilizer, 01313 MobilizerQIndex whichQ) const; 01314 01335 Real getOneQDot(const State& state, 01336 const Array_<Real,ConstrainedQIndex>& constrainedQDot, 01337 ConstrainedMobilizerIndex mobilizer, 01338 MobilizerQIndex whichQ) const; 01339 01345 Real getOneQDotFromState(const State& state, 01346 ConstrainedMobilizerIndex mobilizer, 01347 MobilizerQIndex whichQ) const; 01348 01349 01373 Real getOneQDotDot(const State& state, 01374 const Array_<Real,ConstrainedQIndex>& constrainedQDotDot, 01375 ConstrainedMobilizerIndex mobilizer, 01376 MobilizerQIndex whichQ) const; 01377 01395 Real getOneU(const State& state, 01396 const Array_<Real,ConstrainedUIndex>& constrainedU, 01397 ConstrainedMobilizerIndex mobilizer, 01398 MobilizerUIndex whichU) const; 01399 01407 Real getOneUFromState(const State& state, 01408 ConstrainedMobilizerIndex mobilizer, 01409 MobilizerUIndex whichU) const; 01410 01433 Real getOneUDot(const State& state, 01434 const Array_<Real,ConstrainedUIndex>& constrainedUDot, 01435 ConstrainedMobilizerIndex mobilizer, 01436 MobilizerUIndex whichU) const; 01437 01446 void addInOneMobilityForce 01447 (const State& state, 01448 ConstrainedMobilizerIndex mobilizer, 01449 MobilizerUIndex whichU, 01450 Real fu, 01451 Array_<Real,ConstrainedUIndex>& mobilityForces) const; 01452 01468 void addInOneQForce 01469 (const State& state, 01470 ConstrainedMobilizerIndex mobilizer, 01471 MobilizerQIndex whichQ, 01472 Real fq, 01473 Array_<Real,ConstrainedQIndex>& qForces) const; 01492 const Transform& getBodyTransform 01493 (const Array_<Transform,ConstrainedBodyIndex>& allX_AB, 01494 ConstrainedBodyIndex bodyB) const; 01497 const Rotation& getBodyRotation 01498 (const Array_<Transform,ConstrainedBodyIndex>& allX_AB, 01499 ConstrainedBodyIndex bodyB) const 01500 { return getBodyTransform(allX_AB,bodyB).R(); } 01504 const Vec3& getBodyOriginLocation 01505 (const Array_<Transform,ConstrainedBodyIndex>& allX_AB, 01506 ConstrainedBodyIndex bodyB) const 01507 { return getBodyTransform(allX_AB,bodyB).p(); } 01508 01514 const Transform& getBodyTransformFromState 01515 (const State& state, ConstrainedBodyIndex B) const; // X_AB 01518 const Rotation& getBodyRotationFromState 01519 (const State& state, ConstrainedBodyIndex bodyB) const 01520 { return getBodyTransformFromState(state,bodyB).R(); } 01524 const Vec3& getBodyOriginLocationFromState 01525 (const State& state, ConstrainedBodyIndex bodyB) const 01526 { return getBodyTransformFromState(state,bodyB).p(); } 01527 01531 const SpatialVec& getBodyVelocity 01532 (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB, 01533 ConstrainedBodyIndex bodyB) const; 01536 const Vec3& getBodyAngularVelocity 01537 (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB, 01538 ConstrainedBodyIndex bodyB) const 01539 { return getBodyVelocity(allV_AB,bodyB)[0]; } 01542 const Vec3& getBodyOriginVelocity 01543 (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB, 01544 ConstrainedBodyIndex bodyB) const 01545 { return getBodyVelocity(allV_AB,bodyB)[1]; } 01546 01552 const SpatialVec& getBodyVelocityFromState 01553 (const State& state, ConstrainedBodyIndex bodyB) const; // V_AB 01556 const Vec3& getBodyAngularVelocityFromState 01557 (const State& state, ConstrainedBodyIndex bodyB) const 01558 { return getBodyVelocityFromState(state,bodyB)[0]; } 01561 const Vec3& getBodyOriginVelocityFromState 01562 (const State& state, ConstrainedBodyIndex bodyB) const 01563 { return getBodyVelocityFromState(state,bodyB)[1]; } 01564 01570 const SpatialVec& getBodyAcceleration 01571 (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB, 01572 ConstrainedBodyIndex bodyB) const; 01575 const Vec3& getBodyAngularAcceleration 01576 (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB, 01577 ConstrainedBodyIndex bodyB) const 01578 { return getBodyAcceleration(allA_AB,bodyB)[0]; } 01581 const Vec3& getBodyOriginAcceleration 01582 (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB, 01583 ConstrainedBodyIndex bodyB) const 01584 { return getBodyAcceleration(allA_AB,bodyB)[1]; } 01585 01586 // Calculate location, velocity, and acceleration for a given station. 01587 01594 Vec3 findStationLocation 01595 (const Array_<Transform, ConstrainedBodyIndex>& allX_AB, 01596 ConstrainedBodyIndex bodyB, 01597 const Vec3& p_BS) const 01598 { 01599 const Transform& X_AB = allX_AB[bodyB]; 01600 return X_AB * p_BS; // re-measure and re-express 01601 } 01605 Vec3 findStationLocationFromState 01606 (const State& state, 01607 ConstrainedBodyIndex bodyB, 01608 const Vec3& p_BS) const 01609 { 01610 const Transform& X_AB = getBodyTransformFromState(state,bodyB); 01611 return X_AB * p_BS; // re-measure and re-express 01612 } 01613 01620 Vec3 findStationVelocity 01621 (const State& state, 01622 const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB, 01623 ConstrainedBodyIndex bodyB, 01624 const Vec3& p_BS) const 01625 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao 01626 const Rotation& R_AB = getBodyRotationFromState(state,bodyB); 01627 const Vec3 p_BS_A = R_AB * p_BS; 01628 const SpatialVec& V_AB = allV_AB[bodyB]; 01629 return V_AB[1] + (V_AB[0] % p_BS_A); // v + w X r 01630 } 01634 Vec3 findStationVelocityFromState 01635 (const State& state, 01636 ConstrainedBodyIndex bodyB, 01637 const Vec3& p_BS) const 01638 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao 01639 const Rotation& R_AB = getBodyRotationFromState(state,bodyB); 01640 const Vec3 p_BS_A = R_AB * p_BS; 01641 const SpatialVec& V_AB = getBodyVelocityFromState(state,bodyB); 01642 return V_AB[1] + (V_AB[0] % p_BS_A); // v + w X r 01643 } 01644 01654 Vec3 findStationAcceleration 01655 (const State& state, 01656 const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB, 01657 ConstrainedBodyIndex bodyB, 01658 const Vec3& p_BS) const 01659 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao 01660 const Rotation& R_AB = getBodyRotationFromState(state,bodyB); 01661 const Vec3 p_BS_A = R_AB * p_BS; 01662 const Vec3& w_AB = getBodyAngularVelocityFromState(state,bodyB); 01663 const SpatialVec& A_AB = allA_AB[bodyB]; 01664 01665 // Result is a + b X r + w X (w X r). 01666 // ("b" is angular acceleration; w is angular velocity). 01667 const Vec3 a_AS = A_AB[1] + (A_AB[0] % p_BS_A) 01668 + w_AB % (w_AB % p_BS_A); // % is not associative 01669 return a_AS; 01670 } 01671 01672 // Utilities for applying constraint forces to ConstrainedBodies. 01673 01678 void addInStationForce 01679 (const State& state, 01680 ConstrainedBodyIndex bodyB, 01681 const Vec3& p_BS, 01682 const Vec3& forceInA, 01683 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const; 01684 01687 void addInBodyTorque 01688 (const State& state, 01689 ConstrainedBodyIndex bodyB, 01690 const Vec3& torqueInA, 01691 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const; 01700 void getMultipliers(const State& state, 01701 Array_<Real>& multipliers) const; 01704 protected: 01725 virtual void realizeTopology(State&) const { } 01726 01737 virtual void realizeModel(State&) const { } 01738 01744 virtual void realizeInstance(const State&) const { } 01745 01751 virtual void realizeTime(const State&) const { } 01752 01759 virtual void realizePosition(const State&) const { } 01760 01767 virtual void realizeVelocity(const State&) const { } 01768 01774 virtual void realizeDynamics(const State&) const { } 01775 01784 virtual void realizeAcceleration(const State&) const { } 01785 01791 virtual void realizeReport(const State&) const { } 01805 virtual void calcPositionErrors 01806 (const State& state, // Stage::Time 01807 const Array_<Transform,ConstrainedBodyIndex>& X_AB, 01808 const Array_<Real, ConstrainedQIndex>& constrainedQ, 01809 Array_<Real>& perr) // mp of these 01810 const; 01811 01823 virtual void calcPositionDotErrors 01824 (const State& state, // Stage::Position 01825 const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB, 01826 const Array_<Real, ConstrainedQIndex>& constrainedQDot, 01827 Array_<Real>& pverr) // mp of these 01828 const; 01829 01842 virtual void calcPositionDotDotErrors 01843 (const State& state, // Stage::Velocity 01844 const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB, 01845 const Array_<Real, ConstrainedQIndex>& constrainedQDotDot, 01846 Array_<Real>& paerr) // mp of these 01847 const; 01848 01867 virtual void addInPositionConstraintForces 01868 (const State& state, 01869 const Array_<Real>& multipliers, 01870 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA, 01871 Array_<Real, ConstrainedQIndex>& qForces) const; 01889 virtual void calcVelocityErrors 01890 (const State& state, // Stage::Position 01891 const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB, 01892 const Array_<Real, ConstrainedUIndex>& constrainedU, 01893 Array_<Real>& verr) // mv of these 01894 const; 01895 01906 virtual void calcVelocityDotErrors 01907 (const State& state, // Stage::Velocity 01908 const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB, 01909 const Array_<Real, ConstrainedUIndex>& constrainedUDot, 01910 Array_<Real>& vaerr) // mv of these 01911 const; 01912 01931 virtual void addInVelocityConstraintForces 01932 (const State& state, 01933 const Array_<Real>& multipliers, 01934 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA, 01935 Array_<Real, ConstrainedUIndex>& mobilityForces) const; 01956 virtual void calcAccelerationErrors 01957 (const State& state, // Stage::Velocity 01958 const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB, 01959 const Array_<Real, ConstrainedUIndex>& constrainedUDot, 01960 Array_<Real>& aerr) // ma of these 01961 const; 01962 01980 virtual void addInAccelerationConstraintForces 01981 (const State& state, 01982 const Array_<Real>& multipliers, 01983 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA, 01984 Array_<Real, ConstrainedUIndex>& mobilityForces) const; 01994 virtual void calcDecorativeGeometryAndAppend 01995 (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const 01996 { 01997 } 01998 01999 friend class Constraint::CustomImpl; 02000 }; 02001 02002 02003 02004 //============================================================================== 02005 // COORDINATE COUPLER 02006 //============================================================================== 02015 class SimTK_SIMBODY_EXPORT Constraint::CoordinateCoupler 02016 : public Constraint::Custom { 02017 public: 02038 CoordinateCoupler(SimbodyMatterSubsystem& matter, 02039 const Function* function, 02040 const Array_<MobilizedBodyIndex>& coordMobod, 02041 const Array_<MobilizerQIndex>& coordQIndex); 02042 02044 CoordinateCoupler(SimbodyMatterSubsystem& matter, const Function* function, 02045 const std::vector<MobilizedBodyIndex>& coordBody, 02046 const std::vector<MobilizerQIndex>& coordIndex) 02047 { // Invoke the above constructor with converted arguments. 02048 new (this) CoordinateCoupler(matter,function, 02049 ArrayViewConst_<MobilizedBodyIndex>(coordBody), 02050 ArrayViewConst_<MobilizerQIndex>(coordIndex)); 02051 } 02052 02054 CoordinateCoupler() {} 02055 }; 02056 02057 02058 02059 //============================================================================== 02060 // SPEED COUPLER 02061 //============================================================================== 02072 class SimTK_SIMBODY_EXPORT Constraint::SpeedCoupler : public Constraint::Custom { 02073 public: 02092 SpeedCoupler(SimbodyMatterSubsystem& matter, 02093 const Function* function, 02094 const Array_<MobilizedBodyIndex>& speedBody, 02095 const Array_<MobilizerUIndex>& speedIndex); 02096 02098 SpeedCoupler(SimbodyMatterSubsystem& matter, 02099 const Function* function, 02100 const std::vector<MobilizedBodyIndex>& speedBody, 02101 const std::vector<MobilizerUIndex>& speedIndex) 02102 { // Invoke above constructor with converted arguments. 02103 new (this) SpeedCoupler(matter, function, 02104 ArrayViewConst_<MobilizedBodyIndex>(speedBody), 02105 ArrayViewConst_<MobilizerUIndex>(speedIndex)); 02106 } 02107 02136 SpeedCoupler(SimbodyMatterSubsystem& matter, 02137 const Function* function, 02138 const Array_<MobilizedBodyIndex>& speedBody, 02139 const Array_<MobilizerUIndex>& speedIndex, 02140 const Array_<MobilizedBodyIndex>& coordBody, 02141 const Array_<MobilizerQIndex>& coordIndex); 02142 02144 SpeedCoupler(SimbodyMatterSubsystem& matter, 02145 const Function* function, 02146 const std::vector<MobilizedBodyIndex>& speedBody, 02147 const std::vector<MobilizerUIndex>& speedIndex, 02148 const std::vector<MobilizedBodyIndex>& coordBody, 02149 const std::vector<MobilizerQIndex>& coordIndex) 02150 { // Invoke above constructor with converted arguments. 02151 new (this) SpeedCoupler(matter, function, 02152 ArrayViewConst_<MobilizedBodyIndex>(speedBody), 02153 ArrayViewConst_<MobilizerUIndex>(speedIndex), 02154 ArrayViewConst_<MobilizedBodyIndex>(coordBody), 02155 ArrayViewConst_<MobilizerQIndex>(coordIndex)); 02156 } 02157 02159 SpeedCoupler() {} 02160 }; 02161 02162 02163 02164 //============================================================================== 02165 // PRESCRIBED MOTION 02166 //============================================================================== 02172 class SimTK_SIMBODY_EXPORT Constraint::PrescribedMotion 02173 : public Constraint::Custom { 02174 public: 02190 PrescribedMotion(SimbodyMatterSubsystem& matter, 02191 const Function* function, 02192 MobilizedBodyIndex coordBody, 02193 MobilizerQIndex coordIndex); 02194 02195 02197 PrescribedMotion() {} 02198 }; 02199 02200 02201 02202 } // namespace SimTK 02203 02204 #endif // SimTK_SIMBODY_CONSTRAINT_H_ 02205 02206 02207