|
Simbody
3.4 (development)
|
|
Simbody
3.4 (development)
|
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-12 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 00443 00444 // These are the built-in Constraint types. Types on the same line are 00445 // synonymous. 00446 class Rod; typedef Rod ConstantDistance; 00447 class Ball; typedef Ball CoincidentPoints; typedef Ball Spherical; 00448 class Weld; typedef Weld CoincidentFrames; 00449 class PointInPlane; // translations perpendicular to plane normal only 00450 class PointOnLine; // translations along a line only 00451 class ConstantAngle; // prevent rotation about common normal of two vectors 00452 class ConstantOrientation; // allows any translation but no rotation 00453 class NoSlip1D; // same velocity at a point along a direction 00454 class BallRollingOnPlane; // ball in contact and rolling w/o slip against plane 00455 class ConstantSpeed; // prescribe generalized speed value 00456 class ConstantAcceleration; // prescribe generalized acceleration value 00457 class Custom; 00458 class CoordinateCoupler; 00459 class SpeedCoupler; 00460 class PrescribedMotion; 00461 00462 class RodImpl; 00463 class BallImpl; 00464 class WeldImpl; 00465 class PointInPlaneImpl; 00466 class PointOnLineImpl; 00467 class ConstantAngleImpl; 00468 class ConstantOrientationImpl; 00469 class NoSlip1DImpl; 00470 class BallRollingOnPlaneImpl; 00471 class ConstantSpeedImpl; 00472 class ConstantAccelerationImpl; 00473 class CustomImpl; 00474 class CoordinateCouplerImpl; 00475 class SpeedCouplerImpl; 00476 class PrescribedMotionImpl; 00477 }; 00478 00480 // ROD (CONSTANT DISTANCE) CONSTRAINT // 00482 00498 class SimTK_SIMBODY_EXPORT Constraint::Rod : public Constraint { 00499 public: 00500 // no default constructor 00501 Rod(MobilizedBody& body1, MobilizedBody& body2, 00502 Real defaultLength=1); 00503 Rod(MobilizedBody& body1, const Vec3& defaultPoint1, 00504 MobilizedBody& body2, const Vec3& defaultPoint2, 00505 Real defaultLength=1); 00506 00508 Rod() {} 00509 00510 // Defaults for Instance variables. 00511 Rod& setDefaultPointOnBody1(const Vec3&); 00512 Rod& setDefaultPointOnBody2(const Vec3&); 00513 Rod& setDefaultRodLength(Real); 00514 00515 // Stage::Topology 00516 MobilizedBodyIndex getBody1MobilizedBodyIndex() const; 00517 MobilizedBodyIndex getBody2MobilizedBodyIndex() const; 00518 const Vec3& getDefaultPointOnBody1() const; 00519 const Vec3& getDefaultPointOnBody2() const; 00520 Real getDefaultRodLength() const; 00521 00522 // Stage::Instance 00523 const Vec3& getPointOnBody1(const State&) const; 00524 const Vec3& getPointOnBody2(const State&) const; 00525 Real getRodLength (const State&) const; 00526 00527 // Stage::Position, Velocity, Acceleration 00528 Real getPositionError(const State&) const; 00529 Real getVelocityError(const State&) const; 00530 00531 // Stage::Acceleration 00532 Real getAccelerationError(const State&) const; 00533 Real getMultiplier(const State&) const; 00534 Real getRodTension(const State&) const; // negative means compression 00535 // hide from Doxygen 00537 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Rod, RodImpl, Constraint); 00539 }; 00540 00542 // POINT IN PLANE CONSTRAINT // 00544 00555 class SimTK_SIMBODY_EXPORT Constraint::PointInPlane : public Constraint { 00556 public: 00557 // no default constructor 00558 PointInPlane(MobilizedBody& planeBody_B, const UnitVec3& defaultPlaneNormal_B, Real defaultHeight, 00559 MobilizedBody& followerBody_F, const Vec3& defaultFollowerPoint_F); 00560 00562 PointInPlane() {} 00563 00564 // These affect only generated decorative geometry for visualization; 00565 // the plane is really infinite in extent with zero depth and the 00566 // point is really of zero radius. 00567 PointInPlane& setPlaneDisplayHalfWidth(Real); 00568 PointInPlane& setPointDisplayRadius(Real); 00569 Real getPlaneDisplayHalfWidth() const; 00570 Real getPointDisplayRadius() const; 00571 00572 // Defaults for Instance variables. 00573 PointInPlane& setDefaultPlaneNormal(const UnitVec3&); 00574 PointInPlane& setDefaultPlaneHeight(Real); 00575 PointInPlane& setDefaultFollowerPoint(const Vec3&); 00576 00577 // Stage::Topology 00578 MobilizedBodyIndex getPlaneMobilizedBodyIndex() const; 00579 MobilizedBodyIndex getFollowerMobilizedBodyIndex() const; 00580 00581 const UnitVec3& getDefaultPlaneNormal() const; 00582 Real getDefaultPlaneHeight() const; 00583 const Vec3& getDefaultFollowerPoint() const; 00584 00585 // Stage::Instance 00586 const UnitVec3& getPlaneNormal(const State&) const; 00587 Real getPlaneHeight(const State&) const; 00588 const Vec3& getFollowerPoint(const State&) const; 00589 00590 // Stage::Position, Velocity 00591 Real getPositionError(const State&) const; 00592 Real getVelocityError(const State&) const; 00593 00594 // Stage::Acceleration 00595 Real getAccelerationError(const State&) const; 00596 Real getMultiplier(const State&) const; 00597 Real getForceOnFollowerPoint(const State&) const; // in normal direction 00598 // hide from Doxygen 00600 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 00601 (PointInPlane, PointInPlaneImpl, Constraint); 00603 }; 00604 00606 // POINT ON LINE CONSTRAINT // 00608 00619 class SimTK_SIMBODY_EXPORT Constraint::PointOnLine : public Constraint { 00620 public: 00621 // no default constructor 00622 PointOnLine(MobilizedBody& lineBody_B, const UnitVec3& defaultLineDirection_B, const Vec3& defaultPointOnLine_B, 00623 MobilizedBody& followerBody_F, const Vec3& defaultFollowerPoint_F); 00624 00626 PointOnLine() {} 00627 00628 // These affect only generated decorative geometry for visualization; 00629 // the line is really infinite in extent and the 00630 // point is really of zero radius. 00631 PointOnLine& setLineDisplayHalfLength(Real); 00632 PointOnLine& setPointDisplayRadius(Real); 00633 Real getLineDisplayHalfLength() const; 00634 Real getPointDisplayRadius() const; 00635 00636 // Defaults for Instance variables. 00637 PointOnLine& setDefaultLineDirection(const UnitVec3&); 00638 PointOnLine& setDefaultPointOnLine(const Vec3&); 00639 PointOnLine& setDefaultFollowerPoint(const Vec3&); 00640 00641 // Stage::Topology 00642 MobilizedBodyIndex getLineMobilizedBodyIndex() const; 00643 MobilizedBodyIndex getFollowerMobilizedBodyIndex() const; 00644 00645 const UnitVec3& getDefaultLineDirection() const; 00646 const Vec3& getDefaultPointOnLine() const; 00647 const Vec3& getDefaultFollowerPoint() const; 00648 00649 // Stage::Instance 00650 const UnitVec3& getLineDirection(const State&) const; 00651 const Vec3& getPointOnLine(const State&) const; 00652 const Vec3& getFollowerPoint(const State&) const; 00653 00654 // Stage::Position, Velocity 00655 Vec2 getPositionErrors(const State&) const; 00656 Vec2 getVelocityErrors(const State&) const; 00657 00658 // Stage::Acceleration 00659 Vec2 getAccelerationErrors(const State&) const; 00660 Vec2 getMultipliers(const State&) const; 00661 const Vec2& getForceOnFollowerPoint(const State&) const; // in normal direction 00662 // hide from Doxygen 00664 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 00665 (PointOnLine, PointOnLineImpl, Constraint); 00667 }; 00668 00670 // CONSTANT ANGLE CONSTRAINT // 00672 00701 class SimTK_SIMBODY_EXPORT Constraint::ConstantAngle : public Constraint { 00702 public: 00703 // no default constructor 00704 ConstantAngle(MobilizedBody& baseBody_B, const UnitVec3& defaultAxis_B, 00705 MobilizedBody& followerBody_F, const UnitVec3& defaultAxis_F, 00706 Real angle = Pi/2); 00707 00709 ConstantAngle() {} 00710 00711 // These affect only generated decorative geometry for visualization. 00712 ConstantAngle& setAxisDisplayLength(Real); 00713 ConstantAngle& setAxisDisplayWidth(Real); 00714 Real getAxisDisplayLength() const; 00715 Real getAxisDisplayWidth() const; 00716 00717 // Defaults for Instance variables. 00718 ConstantAngle& setDefaultBaseAxis(const UnitVec3&); 00719 ConstantAngle& setDefaultFollowerAxis(const UnitVec3&); 00720 ConstantAngle& setDefaultAngle(Real); 00721 00722 // Stage::Topology 00723 MobilizedBodyIndex getBaseMobilizedBodyIndex() const; 00724 MobilizedBodyIndex getFollowerMobilizedBodyIndex() const; 00725 00726 const UnitVec3& getDefaultBaseAxis() const; 00727 const UnitVec3& getDefaultFollowerAxis() const; 00728 Real getDefaultAngle() const; 00729 00730 // Stage::Instance 00731 const UnitVec3& getBaseAxis(const State&) const; 00732 const UnitVec3& getFollowerAxis(const State&) const; 00733 Real getAngle(const State&) const; 00734 00735 // Stage::Position, Velocity 00736 Real getPositionError(const State&) const; 00737 Real getVelocityError(const State&) const; 00738 00739 // Stage::Acceleration 00740 Real getAccelerationError(const State&) const; 00741 Real getMultiplier(const State&) const; 00742 Real getTorqueOnFollowerBody(const State&) const; // about f X b 00743 // hide from Doxygen 00745 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(ConstantAngle, ConstantAngleImpl, Constraint); 00747 }; 00748 00750 // BALL (COINCIDENT POINTS) CONSTRAINT // 00752 00772 class SimTK_SIMBODY_EXPORT Constraint::Ball : public Constraint { 00773 public: 00777 Ball(MobilizedBody& body1, MobilizedBody& body2); 00781 Ball(MobilizedBody& body1, const Vec3& defaultPoint1, 00782 MobilizedBody& body2, const Vec3& defaultPoint2); 00783 00785 Ball() {} 00786 00791 void setPointOnBody1(State& state, const Vec3& point_B1) const; 00796 void setPointOnBody2(State& state, const Vec3& point_B2) const; 00797 00800 const Vec3& getPointOnBody1(const State& state) const; 00803 const Vec3& getPointOnBody2(const State& state) const; 00804 00809 Ball& setDefaultPointOnBody1(const Vec3& defaultPoint_B1); 00814 Ball& setDefaultPointOnBody2(const Vec3& defaultPoint_B2); 00815 00820 const Vec3& getDefaultPointOnBody1() const; 00825 const Vec3& getDefaultPointOnBody2() const; 00826 00827 00830 Ball& setDefaultRadius(Real r); 00833 Real getDefaultRadius() const; 00834 00836 MobilizedBodyIndex getBody1MobilizedBodyIndex() const; 00838 MobilizedBodyIndex getBody2MobilizedBodyIndex() const; 00839 00840 00846 Vec3 getPositionErrors(const State& state) const; 00847 00854 Vec3 getVelocityErrors(const State& state) const; 00855 00864 Vec3 getAccelerationErrors(const State&) const; 00865 00869 Vec3 getBallReactionForceOnBody1(const State&) const; 00873 Vec3 getBallReactionForceOnBody2(const State&) const; 00874 00882 Vec3 getMultipliers(const State& state) const; 00883 // hide from Doxygen 00885 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ball, BallImpl, Constraint); 00887 }; 00888 00890 // CONSTANT ORIENTATION CONSTRAINT // 00892 00909 class SimTK_SIMBODY_EXPORT Constraint::ConstantOrientation : public Constraint 00910 { 00911 public: 00912 // no default constructor 00913 ConstantOrientation(MobilizedBody& baseBody_B, const Rotation& defaultRB, 00914 MobilizedBody& followerBody_F, const Rotation& defaultRF); 00915 00917 ConstantOrientation() {} 00918 00919 //TODO: default visualization geometry? 00920 00921 // Defaults for Instance variables. 00922 ConstantOrientation& setDefaultBaseRotation(const Rotation&); 00923 ConstantOrientation& setDefaultFollowerRotation(const Rotation&); 00924 00925 // Stage::Topology 00926 MobilizedBodyIndex getBaseMobilizedBodyIndex() const; 00927 MobilizedBodyIndex getFollowerMobilizedBodyIndex() const; 00928 00929 const Rotation& getDefaultBaseRotation() const; 00930 const Rotation& getDefaultFollowerRotation() const; 00931 00932 // Stage::Instance 00933 const Rotation& getBaseRotation(const State&) const; 00934 const Rotation& getFollowerRotation(const State&) const; 00935 00936 // Stage::Position, Velocity 00937 Vec3 getPositionErrors(const State&) const; 00938 Vec3 getVelocityErrors(const State&) const; 00939 00940 // Stage::Acceleration 00941 Vec3 getAccelerationErrors(const State&) const; 00942 Vec3 getMultipliers(const State&) const; 00943 Vec3 getTorqueOnFollowerBody(const State&) const; 00944 // hide from Doxygen 00946 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 00947 (ConstantOrientation, ConstantOrientationImpl, Constraint); 00949 }; 00950 00952 // WELD (COINCIDENT FRAMES) CONSTRAINT // 00954 00978 class SimTK_SIMBODY_EXPORT Constraint::Weld : public Constraint { 00979 public: 00980 // no default constructor 00981 00984 Weld(MobilizedBody& body1, MobilizedBody& body2); 00985 00990 Weld(MobilizedBody& body1, const Transform& frame1, 00991 MobilizedBody& body2, const Transform& frame2); 00992 00994 Weld() {} 00995 00996 // Control over generated decorative geometry. 00997 01001 Weld& setAxisDisplayLength(Real r); 01002 01006 Real getAxisDisplayLength() const; 01007 01008 // Defaults for Instance variables. 01009 01013 Weld& setDefaultFrameOnBody1(const Transform&); 01014 01016 const Transform& getDefaultFrameOnBody1() const; 01017 01021 Weld& setDefaultFrameOnBody2(const Transform&); 01022 01024 const Transform& getDefaultFrameOnBody2() const; 01025 01026 01027 // Stage::Topology 01028 01030 MobilizedBodyIndex getBody1MobilizedBodyIndex() const; 01031 01033 MobilizedBodyIndex getBody2MobilizedBodyIndex() const; 01034 01035 01036 // Stage::Instance 01037 const Transform& getFrameOnBody1(const State&) const; 01038 const Transform& getFrameOnBody2(const State&) const; 01039 01040 // Stage::Position, Velocity, Acceleration 01041 Vec6 getPositionErrors(const State&) const; 01042 Vec6 getVelocityErrors(const State&) const; 01043 01044 // Stage::Acceleration 01045 Vec6 getAccelerationErrors(const State&) const; 01046 Vec6 getMultipliers(const State&) const; 01047 01048 // Forces are reported expressed in the body frame of the indicated body. 01049 const SpatialVec& getWeldReactionOnBody1(const State&) const; 01050 const SpatialVec& getWeldReactionOnBody2(const State&) const; 01051 // hide from Doxygen 01053 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Weld, WeldImpl, Constraint); 01055 }; 01056 01058 // NO SLIP 1D CONSTRAINT // 01060 01072 class SimTK_SIMBODY_EXPORT Constraint::NoSlip1D : public Constraint { 01073 public: 01074 // no default constructor 01075 01082 NoSlip1D(MobilizedBody& caseBodyC, const Vec3& P_C, const UnitVec3& n_C, 01083 MobilizedBody& movingBody0, MobilizedBody& movingBody1); 01084 01086 NoSlip1D() {} 01087 01092 void setContactPoint(State& state, const Vec3& point_C) const; 01097 void setDirection(State& state, const UnitVec3& direction_C) const; 01098 01101 const Vec3& getContactPoint(const State& state) const; 01104 const UnitVec3& getDirection(const State& state) const; 01105 01106 // These affect only generated decorative geometry for visualization; 01107 // the plane is really infinite in extent with zero depth and the 01108 // point is really of zero radius. 01109 01112 NoSlip1D& setDirectionDisplayLength(Real); 01115 NoSlip1D& setPointDisplayRadius(Real); 01118 Real getDirectionDisplayLength() const; 01121 Real getPointDisplayRadius() const; 01122 01123 // Defaults for Instance variables. 01124 01127 NoSlip1D& setDefaultContactPoint(const Vec3&); 01130 NoSlip1D& setDefaultDirection(const UnitVec3&); 01131 01132 01133 // Stage::Topology 01134 01137 MobilizedBodyIndex getCaseMobilizedBodyIndex() const; 01140 MobilizedBodyIndex getMovingBodyMobilizedBodyIndex(int which) const; 01141 01144 const UnitVec3& getDefaultDirection() const; 01147 const Vec3& getDefaultContactPoint() const; 01148 01149 01150 // Stage::Position, Velocity 01151 // no position error 01152 01156 Real getVelocityError(const State& state) const; 01157 01158 // Stage::Acceleration 01159 01164 Real getAccelerationError(const State&) const; 01165 01172 Real getMultiplier(const State&) const; 01173 01177 Real getForceAtContactPoint(const State&) const; 01178 // hide from Doxygen 01180 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(NoSlip1D, NoSlip1DImpl, Constraint); 01182 }; 01183 01185 // BALL ROLLING ON PLANE CONSTRAINT // 01187 01222 class SimTK_SIMBODY_EXPORT Constraint::BallRollingOnPlane : public Constraint { 01223 public: 01224 // no default constructor 01227 BallRollingOnPlane(MobilizedBody& planeBody_P, 01228 const UnitVec3& defaultPlaneNormal_P, 01229 Real defaultPlaneHeight, 01230 MobilizedBody& ballBody_B, 01231 const Vec3& defaultBallCenter_B, 01232 Real defaultBallRadius); 01233 01235 BallRollingOnPlane() {} 01236 01237 // These affect only generated decorative geometry for visualization; 01238 // the plane is really infinite in extent with zero depth. 01239 BallRollingOnPlane& setPlaneDisplayHalfWidth(Real); 01240 Real getPlaneDisplayHalfWidth() const; 01241 01242 // Defaults for Instance variables. 01243 BallRollingOnPlane& setDefaultPlaneNormal(const UnitVec3&); 01244 BallRollingOnPlane& setDefaultPlaneHeight(Real); 01245 BallRollingOnPlane& setDefaultBallCenter(const Vec3&); 01246 BallRollingOnPlane& setDefaultBallRadius(Real); 01247 01248 // Stage::Topology 01249 MobilizedBodyIndex getPlaneMobilizedBodyIndex() const; 01250 MobilizedBodyIndex getBallMobilizedBodyIndex() const; 01251 01252 const UnitVec3& getDefaultPlaneNormal() const; 01253 Real getDefaultPlaneHeight() const; 01254 const Vec3& getDefaultBallCenter() const; 01255 Real getDefaultBallRadius() const; 01256 01257 // Stage::Instance 01258 const UnitVec3& getPlaneNormal(const State&) const; 01259 Real getPlaneHeight(const State&) const; 01260 const Vec3& getBallCenter(const State&) const; 01261 Real getBallRadius(const State&) const; 01262 01263 // Stage::Position, Velocity 01264 Real getPositionError(const State&) const; 01265 Vec3 getVelocityError(const State&) const; 01266 01267 // Stage::Acceleration 01268 Vec3 getAccelerationError(const State&) const; 01269 Vec3 getMultipliers(const State&) const; 01270 01274 Real getNormalForce(const State&) const; 01277 Vec2 getFrictionForceOnBallInPlaneFrame(const State&) const; 01278 // Don't let doxygen see this 01280 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(BallRollingOnPlane, BallRollingOnPlaneImpl, Constraint); 01282 }; 01283 01284 01286 // CONSTANT SPEED // 01288 01296 class SimTK_SIMBODY_EXPORT Constraint::ConstantSpeed : public Constraint { 01297 public: 01298 // no default constructor 01301 ConstantSpeed(MobilizedBody& mobilizer, MobilizerUIndex whichU, 01302 Real defaultSpeed); 01305 ConstantSpeed(MobilizedBody& mobilizer, Real defaultSpeed); 01306 01308 ConstantSpeed() {} 01309 01313 MobilizedBodyIndex getMobilizedBodyIndex() const; 01316 MobilizerUIndex getWhichU() const; 01321 Real getDefaultSpeed() const; 01327 ConstantSpeed& setDefaultSpeed(Real speed); 01328 01333 void setSpeed(State& state, Real speed) const; 01337 Real getSpeed(const State& state) const; 01338 01339 // no position error 01340 01344 Real getVelocityError(const State& state) const; 01345 01346 // Stage::Acceleration 01350 Real getAccelerationError(const State& state) const; 01356 Real getMultiplier(const State&) const; 01357 // hide from Doxygen 01359 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 01360 (ConstantSpeed, ConstantSpeedImpl, Constraint); 01362 }; 01363 01365 // CONSTANT ACCELERATION // 01367 01375 class SimTK_SIMBODY_EXPORT Constraint::ConstantAcceleration : public Constraint 01376 { 01377 public: 01378 // no default constructor 01381 ConstantAcceleration(MobilizedBody& mobilizer, MobilizerUIndex whichU, 01382 Real defaultAcceleration); 01385 ConstantAcceleration(MobilizedBody& mobilizer, 01386 Real defaultAcceleration); 01387 01389 ConstantAcceleration() {} 01390 01394 MobilizedBodyIndex getMobilizedBodyIndex() const; 01398 MobilizerUIndex getWhichU() const; 01403 Real getDefaultAcceleration() const; 01409 ConstantAcceleration& setDefaultAcceleration(Real accel); 01410 01415 void setAcceleration(State& state, Real accel) const; 01419 Real getAcceleration(const State& state) const; 01420 01421 // no position or velocity error 01422 01423 // Stage::Acceleration 01427 Real getAccelerationError(const State&) const; 01433 Real getMultiplier(const State&) const; 01434 // hide from Doxygen 01436 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS 01437 (ConstantAcceleration, ConstantAccelerationImpl, Constraint); 01439 }; 01440 01441 //============================================================================== 01442 // CUSTOM 01443 //============================================================================== 01476 class SimTK_SIMBODY_EXPORT Constraint::Custom : public Constraint { 01477 public: 01478 class Implementation; 01479 class ImplementationImpl; 01480 01488 explicit Custom(Implementation* implementation); 01489 01490 01492 Custom() {} 01493 01494 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, Constraint); 01495 protected: 01496 const Implementation& getImplementation() const; 01497 Implementation& updImplementation(); 01498 }; 01499 01500 //============================================================================== 01501 // CUSTOM::IMPLEMENTATION 01502 //============================================================================== 01503 01504 // We only want the template instantiation to occur once. This symbol is 01505 // defined in the SimTK core compilation unit that defines the Constraint 01506 // class but should not be defined any other time. 01507 #ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT 01508 extern template class PIMPLHandle<Constraint::Custom::Implementation, 01509 Constraint::Custom::ImplementationImpl>; 01510 #endif 01511 01514 class SimTK_SIMBODY_EXPORT Constraint::Custom::Implementation 01515 : public PIMPLHandle<Implementation,ImplementationImpl> { 01516 public: 01517 // No default constructor because you have to supply at least the 01518 // SimbodyMatterSubsystem to which this Constraint belongs. 01519 01522 virtual ~Implementation() { } 01523 01528 virtual Implementation* clone() const = 0; 01529 01533 Implementation(SimbodyMatterSubsystem&, int mp, int mv, int ma); 01534 01539 explicit Implementation(SimbodyMatterSubsystem&); 01540 01542 const SimbodyMatterSubsystem& getMatterSubsystem() const; 01543 01544 // Topological information// 01545 01552 void invalidateTopologyCache() const; 01553 01558 Implementation& setDefaultNumConstraintEquations(int mp, int mv, int ma); 01559 01564 Implementation& setDisabledByDefault(bool shouldBeDisabled); 01565 01571 ConstrainedBodyIndex addConstrainedBody(const MobilizedBody&); 01572 01579 ConstrainedMobilizerIndex addConstrainedMobilizer(const MobilizedBody&); 01580 01585 MobilizedBodyIndex 01586 getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex) const; 01587 01592 MobilizedBodyIndex 01593 getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex) const; 01594 01595 01625 Real getOneQ(const State& state, 01626 const Array_<Real,ConstrainedQIndex>& constrainedQ, 01627 ConstrainedMobilizerIndex mobilizer, 01628 MobilizerQIndex whichQ) const; 01629 01634 Real getOneQFromState(const State& state, 01635 ConstrainedMobilizerIndex mobilizer, 01636 MobilizerQIndex whichQ) const; 01637 01658 Real getOneQDot(const State& state, 01659 const Array_<Real,ConstrainedQIndex>& constrainedQDot, 01660 ConstrainedMobilizerIndex mobilizer, 01661 MobilizerQIndex whichQ) const; 01662 01668 Real getOneQDotFromState(const State& state, 01669 ConstrainedMobilizerIndex mobilizer, 01670 MobilizerQIndex whichQ) const; 01671 01672 01696 Real getOneQDotDot(const State& state, 01697 const Array_<Real,ConstrainedQIndex>& constrainedQDotDot, 01698 ConstrainedMobilizerIndex mobilizer, 01699 MobilizerQIndex whichQ) const; 01700 01718 Real getOneU(const State& state, 01719 const Array_<Real,ConstrainedUIndex>& constrainedU, 01720 ConstrainedMobilizerIndex mobilizer, 01721 MobilizerUIndex whichU) const; 01722 01730 Real getOneUFromState(const State& state, 01731 ConstrainedMobilizerIndex mobilizer, 01732 MobilizerUIndex whichU) const; 01733 01756 Real getOneUDot(const State& state, 01757 const Array_<Real,ConstrainedUIndex>& constrainedUDot, 01758 ConstrainedMobilizerIndex mobilizer, 01759 MobilizerUIndex whichU) const; 01760 01769 void addInOneMobilityForce 01770 (const State& state, 01771 ConstrainedMobilizerIndex mobilizer, 01772 MobilizerUIndex whichU, 01773 Real fu, 01774 Array_<Real,ConstrainedUIndex>& mobilityForces) const; 01775 01791 void addInOneQForce 01792 (const State& state, 01793 ConstrainedMobilizerIndex mobilizer, 01794 MobilizerQIndex whichQ, 01795 Real fq, 01796 Array_<Real,ConstrainedQIndex>& qForces) const; 01815 const Transform& getBodyTransform 01816 (const Array_<Transform,ConstrainedBodyIndex>& allX_AB, 01817 ConstrainedBodyIndex bodyB) const; 01820 const Rotation& getBodyRotation 01821 (const Array_<Transform,ConstrainedBodyIndex>& allX_AB, 01822 ConstrainedBodyIndex bodyB) const 01823 { return getBodyTransform(allX_AB,bodyB).R(); } 01827 const Vec3& getBodyOriginLocation 01828 (const Array_<Transform,ConstrainedBodyIndex>& allX_AB, 01829 ConstrainedBodyIndex bodyB) const 01830 { return getBodyTransform(allX_AB,bodyB).p(); } 01831 01837 const Transform& getBodyTransformFromState 01838 (const State& state, ConstrainedBodyIndex B) const; // X_AB 01841 const Rotation& getBodyRotationFromState 01842 (const State& state, ConstrainedBodyIndex bodyB) const 01843 { return getBodyTransformFromState(state,bodyB).R(); } 01847 const Vec3& getBodyOriginLocationFromState 01848 (const State& state, ConstrainedBodyIndex bodyB) const 01849 { return getBodyTransformFromState(state,bodyB).p(); } 01850 01854 const SpatialVec& getBodyVelocity 01855 (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB, 01856 ConstrainedBodyIndex bodyB) const; 01859 const Vec3& getBodyAngularVelocity 01860 (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB, 01861 ConstrainedBodyIndex bodyB) const 01862 { return getBodyVelocity(allV_AB,bodyB)[0]; } 01865 const Vec3& getBodyOriginVelocity 01866 (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB, 01867 ConstrainedBodyIndex bodyB) const 01868 { return getBodyVelocity(allV_AB,bodyB)[1]; } 01869 01875 const SpatialVec& getBodyVelocityFromState 01876 (const State& state, ConstrainedBodyIndex bodyB) const; // V_AB 01879 const Vec3& getBodyAngularVelocityFromState 01880 (const State& state, ConstrainedBodyIndex bodyB) const 01881 { return getBodyVelocityFromState(state,bodyB)[0]; } 01884 const Vec3& getBodyOriginVelocityFromState 01885 (const State& state, ConstrainedBodyIndex bodyB) const 01886 { return getBodyVelocityFromState(state,bodyB)[1]; } 01887 01893 const SpatialVec& getBodyAcceleration 01894 (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB, 01895 ConstrainedBodyIndex bodyB) const; 01898 const Vec3& getBodyAngularAcceleration 01899 (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB, 01900 ConstrainedBodyIndex bodyB) const 01901 { return getBodyAcceleration(allA_AB,bodyB)[0]; } 01904 const Vec3& getBodyOriginAcceleration 01905 (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB, 01906 ConstrainedBodyIndex bodyB) const 01907 { return getBodyAcceleration(allA_AB,bodyB)[1]; } 01908 01909 // Calculate location, velocity, and acceleration for a given station. 01910 01917 Vec3 findStationLocation 01918 (const Array_<Transform, ConstrainedBodyIndex>& allX_AB, 01919 ConstrainedBodyIndex bodyB, 01920 const Vec3& p_BS) const 01921 { 01922 const Transform& X_AB = allX_AB[bodyB]; 01923 return X_AB * p_BS; // re-measure and re-express 01924 } 01928 Vec3 findStationLocationFromState 01929 (const State& state, 01930 ConstrainedBodyIndex bodyB, 01931 const Vec3& p_BS) const 01932 { 01933 const Transform& X_AB = getBodyTransformFromState(state,bodyB); 01934 return X_AB * p_BS; // re-measure and re-express 01935 } 01936 01943 Vec3 findStationVelocity 01944 (const State& state, 01945 const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB, 01946 ConstrainedBodyIndex bodyB, 01947 const Vec3& p_BS) const 01948 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao 01949 const Rotation& R_AB = getBodyRotationFromState(state,bodyB); 01950 const Vec3 p_BS_A = R_AB * p_BS; 01951 const SpatialVec& V_AB = allV_AB[bodyB]; 01952 return V_AB[1] + (V_AB[0] % p_BS_A); // v + w X r 01953 } 01957 Vec3 findStationVelocityFromState 01958 (const State& state, 01959 ConstrainedBodyIndex bodyB, 01960 const Vec3& p_BS) const 01961 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao 01962 const Rotation& R_AB = getBodyRotationFromState(state,bodyB); 01963 const Vec3 p_BS_A = R_AB * p_BS; 01964 const SpatialVec& V_AB = getBodyVelocityFromState(state,bodyB); 01965 return V_AB[1] + (V_AB[0] % p_BS_A); // v + w X r 01966 } 01967 01977 Vec3 findStationAcceleration 01978 (const State& state, 01979 const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB, 01980 ConstrainedBodyIndex bodyB, 01981 const Vec3& p_BS) const 01982 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao 01983 const Rotation& R_AB = getBodyRotationFromState(state,bodyB); 01984 const Vec3 p_BS_A = R_AB * p_BS; 01985 const Vec3& w_AB = getBodyAngularVelocityFromState(state,bodyB); 01986 const SpatialVec& A_AB = allA_AB[bodyB]; 01987 01988 // Result is a + b X r + w X (w X r). 01989 // ("b" is angular acceleration; w is angular velocity). 01990 const Vec3 a_AS = A_AB[1] + (A_AB[0] % p_BS_A) 01991 + w_AB % (w_AB % p_BS_A); // % is not associative 01992 return a_AS; 01993 } 01994 01995 // Utilities for applying constraint forces to ConstrainedBodies. 01996 02001 void addInStationForce 02002 (const State& state, 02003 ConstrainedBodyIndex bodyB, 02004 const Vec3& p_BS, 02005 const Vec3& forceInA, 02006 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const; 02007 02010 void addInBodyTorque 02011 (const State& state, 02012 ConstrainedBodyIndex bodyB, 02013 const Vec3& torqueInA, 02014 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const; 02023 void getMultipliers(const State& state, 02024 Array_<Real>& multipliers) const; 02027 protected: 02048 virtual void realizeTopology(State&) const { } 02049 02060 virtual void realizeModel(State&) const { } 02061 02067 virtual void realizeInstance(const State&) const { } 02068 02074 virtual void realizeTime(const State&) const { } 02075 02082 virtual void realizePosition(const State&) const { } 02083 02090 virtual void realizeVelocity(const State&) const { } 02091 02097 virtual void realizeDynamics(const State&) const { } 02098 02107 virtual void realizeAcceleration(const State&) const { } 02108 02114 virtual void realizeReport(const State&) const { } 02128 virtual void calcPositionErrors 02129 (const State& state, // Stage::Time 02130 const Array_<Transform,ConstrainedBodyIndex>& X_AB, 02131 const Array_<Real, ConstrainedQIndex>& constrainedQ, 02132 Array_<Real>& perr) // mp of these 02133 const; 02134 02146 virtual void calcPositionDotErrors 02147 (const State& state, // Stage::Position 02148 const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB, 02149 const Array_<Real, ConstrainedQIndex>& constrainedQDot, 02150 Array_<Real>& pverr) // mp of these 02151 const; 02152 02165 virtual void calcPositionDotDotErrors 02166 (const State& state, // Stage::Velocity 02167 const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB, 02168 const Array_<Real, ConstrainedQIndex>& constrainedQDotDot, 02169 Array_<Real>& paerr) // mp of these 02170 const; 02171 02190 virtual void addInPositionConstraintForces 02191 (const State& state, 02192 const Array_<Real>& multipliers, 02193 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA, 02194 Array_<Real, ConstrainedQIndex>& qForces) const; 02212 virtual void calcVelocityErrors 02213 (const State& state, // Stage::Position 02214 const Array_<SpatialVec,ConstrainedBodyIndex>& V_AB, 02215 const Array_<Real, ConstrainedUIndex>& constrainedU, 02216 Array_<Real>& verr) // mv of these 02217 const; 02218 02229 virtual void calcVelocityDotErrors 02230 (const State& state, // Stage::Velocity 02231 const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB, 02232 const Array_<Real, ConstrainedUIndex>& constrainedUDot, 02233 Array_<Real>& vaerr) // mv of these 02234 const; 02235 02254 virtual void addInVelocityConstraintForces 02255 (const State& state, 02256 const Array_<Real>& multipliers, 02257 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA, 02258 Array_<Real, ConstrainedUIndex>& mobilityForces) const; 02279 virtual void calcAccelerationErrors 02280 (const State& state, // Stage::Velocity 02281 const Array_<SpatialVec,ConstrainedBodyIndex>& A_AB, 02282 const Array_<Real, ConstrainedUIndex>& constrainedUDot, 02283 Array_<Real>& aerr) // ma of these 02284 const; 02285 02303 virtual void addInAccelerationConstraintForces 02304 (const State& state, 02305 const Array_<Real>& multipliers, 02306 Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA, 02307 Array_<Real, ConstrainedUIndex>& mobilityForces) const; 02317 virtual void calcDecorativeGeometryAndAppend 02318 (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const 02319 { 02320 } 02321 02322 friend class Constraint::CustomImpl; 02323 }; 02324 02325 02326 02327 //============================================================================== 02328 // COORDINATE COUPLER 02329 //============================================================================== 02338 class SimTK_SIMBODY_EXPORT Constraint::CoordinateCoupler 02339 : public Constraint::Custom { 02340 public: 02361 CoordinateCoupler(SimbodyMatterSubsystem& matter, 02362 const Function* function, 02363 const Array_<MobilizedBodyIndex>& coordMobod, 02364 const Array_<MobilizerQIndex>& coordQIndex); 02365 02367 CoordinateCoupler(SimbodyMatterSubsystem& matter, const Function* function, 02368 const std::vector<MobilizedBodyIndex>& coordBody, 02369 const std::vector<MobilizerQIndex>& coordIndex) 02370 { // Invoke the above constructor with converted arguments. 02371 new (this) CoordinateCoupler(matter,function, 02372 ArrayViewConst_<MobilizedBodyIndex>(coordBody), 02373 ArrayViewConst_<MobilizerQIndex>(coordIndex)); 02374 } 02375 02377 CoordinateCoupler() {} 02378 }; 02379 02380 02381 02382 //============================================================================== 02383 // SPEED COUPLER 02384 //============================================================================== 02395 class SimTK_SIMBODY_EXPORT Constraint::SpeedCoupler : public Constraint::Custom { 02396 public: 02415 SpeedCoupler(SimbodyMatterSubsystem& matter, 02416 const Function* function, 02417 const Array_<MobilizedBodyIndex>& speedBody, 02418 const Array_<MobilizerUIndex>& speedIndex); 02419 02421 SpeedCoupler(SimbodyMatterSubsystem& matter, 02422 const Function* function, 02423 const std::vector<MobilizedBodyIndex>& speedBody, 02424 const std::vector<MobilizerUIndex>& speedIndex) 02425 { // Invoke above constructor with converted arguments. 02426 new (this) SpeedCoupler(matter, function, 02427 ArrayViewConst_<MobilizedBodyIndex>(speedBody), 02428 ArrayViewConst_<MobilizerUIndex>(speedIndex)); 02429 } 02430 02459 SpeedCoupler(SimbodyMatterSubsystem& matter, 02460 const Function* function, 02461 const Array_<MobilizedBodyIndex>& speedBody, 02462 const Array_<MobilizerUIndex>& speedIndex, 02463 const Array_<MobilizedBodyIndex>& coordBody, 02464 const Array_<MobilizerQIndex>& coordIndex); 02465 02467 SpeedCoupler(SimbodyMatterSubsystem& matter, 02468 const Function* function, 02469 const std::vector<MobilizedBodyIndex>& speedBody, 02470 const std::vector<MobilizerUIndex>& speedIndex, 02471 const std::vector<MobilizedBodyIndex>& coordBody, 02472 const std::vector<MobilizerQIndex>& coordIndex) 02473 { // Invoke above constructor with converted arguments. 02474 new (this) SpeedCoupler(matter, function, 02475 ArrayViewConst_<MobilizedBodyIndex>(speedBody), 02476 ArrayViewConst_<MobilizerUIndex>(speedIndex), 02477 ArrayViewConst_<MobilizedBodyIndex>(coordBody), 02478 ArrayViewConst_<MobilizerQIndex>(coordIndex)); 02479 } 02480 02482 SpeedCoupler() {} 02483 }; 02484 02485 02486 02487 //============================================================================== 02488 // PRESCRIBED MOTION 02489 //============================================================================== 02495 class SimTK_SIMBODY_EXPORT Constraint::PrescribedMotion 02496 : public Constraint::Custom { 02497 public: 02513 PrescribedMotion(SimbodyMatterSubsystem& matter, 02514 const Function* function, 02515 MobilizedBodyIndex coordBody, 02516 MobilizerQIndex coordIndex); 02517 02518 02520 PrescribedMotion() {} 02521 }; 02522 02523 02524 02525 } // namespace SimTK 02526 02527 #endif // SimTK_SIMBODY_CONSTRAINT_H_ 02528 02529 02530
1.7.6.1 
