Simbody  3.5
Constraint.h
Go to the documentation of this file.
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 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines