Simbody  3.5
Constraint.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_CONSTRAINT_H_
2 #define SimTK_SIMBODY_CONSTRAINT_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm) *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2007-14 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
33 #include "SimTKmath.h"
35 
36 #include <cassert>
37 
38 namespace SimTK {
39 
40 class SimbodyMatterSubsystem;
41 class SimbodyMatterSubtree;
42 class MobilizedBody;
43 class Constraint;
44 class ConstraintImpl;
45 
46 // We only want the template instantiation to occur once. This symbol is
47 // defined in the SimTK core compilation unit that defines the Constraint
48 // class but should not be defined any other time.
49 #ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT
50  extern template class PIMPLHandle<Constraint, ConstraintImpl, true>;
51 #endif
52 
54  // CONSTRAINT BASE CLASS //
56 
68 public:
74 explicit Constraint(ConstraintImpl* r) : HandleBase(r) { }
75 
79 void disable(State&) const;
80 
87 void enable(State&) const;
90 bool isDisabled(const State&) const;
94 bool isDisabledByDefault() const;
95 
100 void setDisabledByDefault(bool shouldBeDisabled);
101 
104 operator ConstraintIndex() const {return getConstraintIndex();}
105 
111 const SimbodyMatterSubsystem& getMatterSubsystem() const;
112 
118 SimbodyMatterSubsystem& updMatterSubsystem();
119 
126 ConstraintIndex getConstraintIndex() const;
127 
129 bool isInSubsystem() const;
133 bool isInSameSubsystem(const MobilizedBody& mobod) const;
134 
135  // TOPOLOGY STAGE (i.e., post-construction) //
136 
142 int getNumConstrainedBodies() const;
143 
148 const MobilizedBody& getMobilizedBodyFromConstrainedBody
149  (ConstrainedBodyIndex consBodyIx) const;
150 
155 const MobilizedBody& getAncestorMobilizedBody() const;
156 
163 int getNumConstrainedMobilizers() const;
164 
169 const MobilizedBody& getMobilizedBodyFromConstrainedMobilizer
170  (ConstrainedMobilizerIndex consMobilizerIx) const;
171 
174 const SimbodyMatterSubtree& getSubtree() const;
175 
176  // MODEL STAGE //
177 // nothing in base class currently
178 
179  // INSTANCE STAGE //
180 
185 int getNumConstrainedQ(const State&, ConstrainedMobilizerIndex) const;
190 int getNumConstrainedU(const State&, ConstrainedMobilizerIndex) const;
191 
198 ConstrainedUIndex getConstrainedUIndex
199  (const State&, ConstrainedMobilizerIndex, MobilizerUIndex which) const;
206 ConstrainedQIndex getConstrainedQIndex
207  (const State&, ConstrainedMobilizerIndex, MobilizerQIndex which) const;
208 
211 int getNumConstrainedQ(const State&) const;
212 
217 int getNumConstrainedU(const State&) const;
218 
221 QIndex getQIndexOfConstrainedQ(const State& state,
222  ConstrainedQIndex consQIndex) const;
225 UIndex getUIndexOfConstrainedU(const State& state,
226  ConstrainedUIndex consUIndex) const;
227 
241 void getNumConstraintEquationsInUse(const State& state,
242  int& mp, int& mv, int& ma) const;
243 
266 void getIndexOfMultipliersInUse(const State& state,
267  MultiplierIndex& px0,
268  MultiplierIndex& vx0,
269  MultiplierIndex& ax0) const;
270 
292 void setMyPartInConstraintSpaceVector(const State& state,
293  const Vector& myPart,
294  Vector& constraintSpace) const;
295 
313 void getMyPartFromConstraintSpaceVector(const State& state,
314  const Vector& constraintSpace,
315  Vector& myPart) const;
316 
317  // POSITION STAGE //
320 Vector getPositionErrorsAsVector(const State&) const; // mp of these
321 Vector calcPositionErrorFromQ(const State&, const Vector& q) const;
322 
323 // Matrix P = partial(perr_dot)/partial(u). (just the holonomic constraints)
324 Matrix calcPositionConstraintMatrixP(const State&) const; // mp X nu
325 Matrix calcPositionConstraintMatrixPt(const State&) const; // nu X mp
326 
327 // Matrix PNInv = partial(perr)/partial(q) = P*N^-1
328 Matrix calcPositionConstraintMatrixPNInv(const State&) const; // mp X nq
329 
345 void calcConstraintForcesFromMultipliers(const State&,
346  const Vector& lambda, // mp+mv+ma of these
347  Vector_<SpatialVec>& bodyForcesInA, // numConstrainedBodies
348  Vector& mobilityForces) const; // numConstrainedU
349 
350  // VELOCITY STAGE //
353 Vector getVelocityErrorsAsVector(const State&) const; // mp+mv of these
354 Vector calcVelocityErrorFromU(const State&, // mp+mv of these
355  const Vector& u) const; // numParticipatingU u's
356 
357 // Matrix V = partial(verr)/partial(u) for just the non-holonomic
358 // constraints.
359 Matrix calcVelocityConstraintMatrixV(const State&) const; // mv X nu
360 Matrix calcVelocityConstraintMatrixVt(const State&) const; // nu X mv
361 
362  // DYNAMICS STAGE //
363 // nothing in base class currently
364 
365  // ACCELERATION STAGE //
369 Vector getAccelerationErrorsAsVector(const State&) const; // mp+mv+ma of these
370 Vector calcAccelerationErrorFromUDot(const State&, // mp+mv+ma of these
371  const Vector& udot) const; // numParticipatingU udot's
372 
376 Vector getMultipliersAsVector(const State&) const; // mp+mv+ma of these
377 
390 void getConstraintForcesAsVectors
391  (const State& state,
392  Vector_<SpatialVec>& bodyForcesInG, // numConstrainedBodies
393  Vector& mobilityForces) const; // numConstrainedU
394 
398  Vector_<SpatialVec> bodyForcesInG;
399  Vector mobilityForces;
400  getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
401  return bodyForcesInG;
402 }
407  Vector_<SpatialVec> bodyForcesInG;
408  Vector mobilityForces;
409  getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
410  return mobilityForces;
411 }
412 
436 Real calcPower(const State& state) const;
437 
438 // Matrix A = partial(aerr)/partial(udot) for just the acceleration-only
439 // constraints.
440 Matrix calcAccelerationConstraintMatrixA(const State&) const; // ma X nu
441 Matrix calcAccelerationConstraintMatrixAt(const State&) const; // nu X ma
442 
447 void setIsConditional(bool isConditional);
449 bool isConditional() const;
450 
451 //------------------------------------------------------------------------------
452 // These are the built-in Constraint types, and some synonyms. Each built in
453 // Constraint type is declared in its own header file using naming convention
454 // Constraint_Rod.h, for example. All the built-in headers are collected in
455 // Constraint_BuiltIns.h; you should include new ones there also.
456 class Rod;
457 typedef Rod ConstantDistance;
458 
459 class Ball;
460 typedef Ball CoincidentPoints;
461 typedef Ball Spherical;
462 
463 class Weld;
464 typedef Weld CoincidentFrames;
465 
466 class PointInPlane; // translations perpendicular to plane normal only
467 class PointOnLine; // translations along a line only
468 class ConstantAngle; // prevent rotation about common normal of two vectors
469 class ConstantOrientation; // allows any translation but no rotation
470 class NoSlip1D; // same velocity at a point along a direction
471 class ConstantCoordinate; // prescribe generalized coordinate value
472 class ConstantSpeed; // prescribe generalized speed value
473 class ConstantAcceleration; // prescribe generalized acceleration value
474 class Custom;
475 class CoordinateCoupler;
476 class SpeedCoupler;
477 class PrescribedMotion;
478 class PointOnPlaneContact;
479 class SphereOnPlaneContact; // ball in contact with plane (sliding or rolling)
480 class SphereOnSphereContact; // ball in contact with ball (sliding or rolling)
481 class LineOnLineContact; // edge/edge contact
482 
483 // Internal use only.
484 class RodImpl;
485 class BallImpl;
486 class WeldImpl;
487 class PointInPlaneImpl;
488 class PointOnLineImpl;
489 class ConstantAngleImpl;
490 class ConstantOrientationImpl;
491 class NoSlip1DImpl;
492 class ConstantCoordinateImpl;
493 class ConstantSpeedImpl;
494 class ConstantAccelerationImpl;
495 class CustomImpl;
496 class CoordinateCouplerImpl;
497 class SpeedCouplerImpl;
498 class PrescribedMotionImpl;
499 class PointOnPlaneContactImpl;
500 class SphereOnPlaneContactImpl;
501 class SphereOnSphereContactImpl;
502 class LineOnLineContactImpl;
503 
504 };
505 
507  // POINT ON LINE CONSTRAINT //
509 
521 public:
522  // no default constructor
523  PointOnLine(MobilizedBody& lineBody_B, const UnitVec3& defaultLineDirection_B, const Vec3& defaultPointOnLine_B,
524  MobilizedBody& followerBody_F, const Vec3& defaultFollowerPoint_F);
525 
528 
529  // These affect only generated decorative geometry for visualization;
530  // the line is really infinite in extent and the
531  // point is really of zero radius.
532  PointOnLine& setLineDisplayHalfLength(Real);
533  PointOnLine& setPointDisplayRadius(Real);
534  Real getLineDisplayHalfLength() const;
535  Real getPointDisplayRadius() const;
536 
537  // Defaults for Instance variables.
538  PointOnLine& setDefaultLineDirection(const UnitVec3&);
539  PointOnLine& setDefaultPointOnLine(const Vec3&);
540  PointOnLine& setDefaultFollowerPoint(const Vec3&);
541 
542  // Stage::Topology
543  MobilizedBodyIndex getLineMobilizedBodyIndex() const;
544  MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;
545 
546  const UnitVec3& getDefaultLineDirection() const;
547  const Vec3& getDefaultPointOnLine() const;
548  const Vec3& getDefaultFollowerPoint() const;
549 
550  // Stage::Instance
551  const UnitVec3& getLineDirection(const State&) const;
552  const Vec3& getPointOnLine(const State&) const;
553  const Vec3& getFollowerPoint(const State&) const;
554 
555  // Stage::Position, Velocity
556  Vec2 getPositionErrors(const State&) const;
557  Vec2 getVelocityErrors(const State&) const;
558 
559  // Stage::Acceleration
560  Vec2 getAccelerationErrors(const State&) const;
561  Vec2 getMultipliers(const State&) const;
562  const Vec2& getForceOnFollowerPoint(const State&) const; // in normal direction
563  // hide from Doxygen
566  (PointOnLine, PointOnLineImpl, Constraint);
568 };
569 
571  // CONSTANT ANGLE CONSTRAINT //
573 
603 public:
604  // no default constructor
605  ConstantAngle(MobilizedBody& baseBody_B, const UnitVec3& defaultAxis_B,
606  MobilizedBody& followerBody_F, const UnitVec3& defaultAxis_F,
607  Real angle = Pi/2);
608 
611 
612  // These affect only generated decorative geometry for visualization.
613  ConstantAngle& setAxisDisplayLength(Real);
614  ConstantAngle& setAxisDisplayWidth(Real);
615  Real getAxisDisplayLength() const;
616  Real getAxisDisplayWidth() const;
617 
618  // Defaults for Instance variables.
619  ConstantAngle& setDefaultBaseAxis(const UnitVec3&);
620  ConstantAngle& setDefaultFollowerAxis(const UnitVec3&);
621  ConstantAngle& setDefaultAngle(Real);
622 
623  // Stage::Topology
624  MobilizedBodyIndex getBaseMobilizedBodyIndex() const;
625  MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;
626 
627  const UnitVec3& getDefaultBaseAxis() const;
628  const UnitVec3& getDefaultFollowerAxis() const;
629  Real getDefaultAngle() const;
630 
631  // Stage::Instance
632  const UnitVec3& getBaseAxis(const State&) const;
633  const UnitVec3& getFollowerAxis(const State&) const;
634  Real getAngle(const State&) const;
635 
636  // Stage::Position, Velocity
637  Real getPositionError(const State&) const;
638  Real getVelocityError(const State&) const;
639 
640  // Stage::Acceleration
641  Real getAccelerationError(const State&) const;
642  Real getMultiplier(const State&) const;
643  Real getTorqueOnFollowerBody(const State&) const; // about f X b
644  // hide from Doxygen
648 };
649 
650 
652  // CONSTANT ORIENTATION CONSTRAINT //
654 
672 {
673 public:
674  // no default constructor
675  ConstantOrientation(MobilizedBody& baseBody_B, const Rotation& defaultRB,
676  MobilizedBody& followerBody_F, const Rotation& defaultRF);
677 
680 
681  //TODO: default visualization geometry?
682 
683  // Defaults for Instance variables.
684  ConstantOrientation& setDefaultBaseRotation(const Rotation&);
685  ConstantOrientation& setDefaultFollowerRotation(const Rotation&);
686 
687  // Stage::Topology
688  MobilizedBodyIndex getBaseMobilizedBodyIndex() const;
689  MobilizedBodyIndex getFollowerMobilizedBodyIndex() const;
690 
691  const Rotation& getDefaultBaseRotation() const;
692  const Rotation& getDefaultFollowerRotation() const;
693 
694  // Stage::Instance
695  const Rotation& getBaseRotation(const State&) const;
696  const Rotation& getFollowerRotation(const State&) const;
697 
698  // Stage::Position, Velocity
699  Vec3 getPositionErrors(const State&) const;
700  Vec3 getVelocityErrors(const State&) const;
701 
702  // Stage::Acceleration
703  Vec3 getAccelerationErrors(const State&) const;
704  Vec3 getMultipliers(const State&) const;
705  Vec3 getTorqueOnFollowerBody(const State&) const;
706  // hide from Doxygen
709  (ConstantOrientation, ConstantOrientationImpl, Constraint);
711 };
712 
714  // NO SLIP 1D CONSTRAINT //
716 
729 public:
730  // no default constructor
731 
738  NoSlip1D(MobilizedBody& caseBodyC, const Vec3& P_C, const UnitVec3& n_C,
739  MobilizedBody& movingBody0, MobilizedBody& movingBody1);
740 
742  NoSlip1D() {}
743 
748  void setContactPoint(State& state, const Vec3& point_C) const;
753  void setDirection(State& state, const UnitVec3& direction_C) const;
754 
757  const Vec3& getContactPoint(const State& state) const;
760  const UnitVec3& getDirection(const State& state) const;
761 
762  // These affect only generated decorative geometry for visualization;
763  // the plane is really infinite in extent with zero depth and the
764  // point is really of zero radius.
765 
768  NoSlip1D& setDirectionDisplayLength(Real);
771  NoSlip1D& setPointDisplayRadius(Real);
774  Real getDirectionDisplayLength() const;
777  Real getPointDisplayRadius() const;
778 
779  // Defaults for Instance variables.
780 
783  NoSlip1D& setDefaultContactPoint(const Vec3&);
786  NoSlip1D& setDefaultDirection(const UnitVec3&);
787 
788 
789  // Stage::Topology
790 
793  MobilizedBodyIndex getCaseMobilizedBodyIndex() const;
796  MobilizedBodyIndex getMovingBodyMobilizedBodyIndex(int which) const;
797 
800  const UnitVec3& getDefaultDirection() const;
803  const Vec3& getDefaultContactPoint() const;
804 
805 
806  // Stage::Position, Velocity
807  // no position error
808 
812  Real getVelocityError(const State& state) const;
813 
814  // Stage::Acceleration
815 
820  Real getAccelerationError(const State&) const;
821 
828  Real getMultiplier(const State&) const;
829 
833  Real getForceAtContactPoint(const State&) const;
834  // hide from Doxygen
838 };
839 
841  // CONSTANT COORDINATE //
843 
859 public:
864  Real defaultPosition);
865 
871  ConstantCoordinate(MobilizedBody& mobilizer, Real defaultPosition);
872 
876 
880  MobilizedBodyIndex getMobilizedBodyIndex() const;
881 
884  MobilizerQIndex getWhichQ() const;
885 
890  Real getDefaultPosition() const;
891 
897  ConstantCoordinate& setDefaultPosition(Real position);
898 
903  void setPosition(State& state, Real position) const;
904 
908  Real getPosition(const State& state) const;
909 
915  Real getPositionError(const State& state) const;
916 
921  Real getVelocityError(const State& state) const;
922 
928  Real getAccelerationError(const State& state) const;
929 
935  Real getMultiplier(const State& state) const;
936  // hide from Doxygen
939  (ConstantCoordinate, ConstantCoordinateImpl, Constraint);
941 };
942 
944  // CONSTANT SPEED //
946 
960 public:
963  ConstantSpeed(MobilizedBody& mobilizer, MobilizerUIndex whichU,
964  Real defaultSpeed);
967  ConstantSpeed(MobilizedBody& mobilizer, Real defaultSpeed);
968 
972 
976  MobilizedBodyIndex getMobilizedBodyIndex() const;
977 
980  MobilizerUIndex getWhichU() const;
981 
986  Real getDefaultSpeed() const;
987 
993  ConstantSpeed& setDefaultSpeed(Real speed);
994 
999  void setSpeed(State& state, Real speed) const;
1000 
1004  Real getSpeed(const State& state) const;
1005 
1006  // no position error
1007 
1011  Real getVelocityError(const State& state) const;
1012 
1016  Real getAccelerationError(const State& state) const;
1017 
1023  Real getMultiplier(const State& state) const;
1024  // hide from Doxygen
1027  (ConstantSpeed, ConstantSpeedImpl, Constraint);
1029 };
1030 
1032  // CONSTANT ACCELERATION //
1034 
1048 {
1049 public:
1053  Real defaultAcceleration);
1054 
1057  ConstantAcceleration(MobilizedBody& mobilizer,
1058  Real defaultAcceleration);
1059 
1063 
1067  MobilizedBodyIndex getMobilizedBodyIndex() const;
1068 
1072  MobilizerUIndex getWhichU() const;
1073 
1078  Real getDefaultAcceleration() const;
1079 
1085  ConstantAcceleration& setDefaultAcceleration(Real accel);
1086 
1091  void setAcceleration(State& state, Real accel) const;
1092 
1096  Real getAcceleration(const State& state) const;
1097 
1098  // no position or velocity error
1099 
1103  Real getAccelerationError(const State&) const;
1104 
1110  Real getMultiplier(const State&) const;
1111  // hide from Doxygen
1114  (ConstantAcceleration, ConstantAccelerationImpl, Constraint);
1116 };
1117 
1118 //==============================================================================
1119 // CUSTOM
1120 //==============================================================================
1154 public:
1155  class Implementation;
1156  class ImplementationImpl;
1157 
1165  explicit Custom(Implementation* implementation);
1166 
1167 
1169  Custom() {}
1170 
1172 protected:
1173  const Implementation& getImplementation() const;
1174  Implementation& updImplementation();
1175 };
1176 
1177 //==============================================================================
1178 // CUSTOM::IMPLEMENTATION
1179 //==============================================================================
1180 
1181 // We only want the template instantiation to occur once. This symbol is
1182 // defined in the SimTK core compilation unit that defines the Constraint
1183 // class but should not be defined any other time.
1184 #ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT
1185  extern template class PIMPLHandle<Constraint::Custom::Implementation,
1186  Constraint::Custom::ImplementationImpl>;
1187 #endif
1188 
1192 : public PIMPLHandle<Implementation,ImplementationImpl> {
1193 public:
1194 // No default constructor because you have to supply at least the
1195 // SimbodyMatterSubsystem to which this Constraint belongs.
1196 
1199 virtual ~Implementation() { }
1200 
1205 virtual Implementation* clone() const = 0;
1206 
1210 Implementation(SimbodyMatterSubsystem&, int mp, int mv, int ma);
1211 
1217 
1219 const SimbodyMatterSubsystem& getMatterSubsystem() const;
1220 
1221  // Topological information//
1222 
1229 void invalidateTopologyCache() const;
1230 
1235 Implementation& setDefaultNumConstraintEquations(int mp, int mv, int ma);
1236 
1241 Implementation& setDisabledByDefault(bool shouldBeDisabled);
1242 
1248 ConstrainedBodyIndex addConstrainedBody(const MobilizedBody&);
1249 
1256 ConstrainedMobilizerIndex addConstrainedMobilizer(const MobilizedBody&);
1257 
1263 getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex) const;
1264 
1270 getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex) const;
1271 
1272 
1302 Real getOneQ(const State& state,
1303  const Array_<Real,ConstrainedQIndex>& constrainedQ,
1304  ConstrainedMobilizerIndex mobilizer,
1305  MobilizerQIndex whichQ) const;
1306 
1311 Real getOneQFromState(const State& state,
1312  ConstrainedMobilizerIndex mobilizer,
1313  MobilizerQIndex whichQ) const;
1314 
1335 Real getOneQDot(const State& state,
1336  const Array_<Real,ConstrainedQIndex>& constrainedQDot,
1337  ConstrainedMobilizerIndex mobilizer,
1338  MobilizerQIndex whichQ) const;
1339 
1345 Real getOneQDotFromState(const State& state,
1346  ConstrainedMobilizerIndex mobilizer,
1347  MobilizerQIndex whichQ) const;
1348 
1349 
1373 Real getOneQDotDot(const State& state,
1374  const Array_<Real,ConstrainedQIndex>& constrainedQDotDot,
1375  ConstrainedMobilizerIndex mobilizer,
1376  MobilizerQIndex whichQ) const;
1377 
1395 Real getOneU(const State& state,
1396  const Array_<Real,ConstrainedUIndex>& constrainedU,
1397  ConstrainedMobilizerIndex mobilizer,
1398  MobilizerUIndex whichU) const;
1399 
1407 Real getOneUFromState(const State& state,
1408  ConstrainedMobilizerIndex mobilizer,
1409  MobilizerUIndex whichU) const;
1410 
1433 Real getOneUDot(const State& state,
1434  const Array_<Real,ConstrainedUIndex>& constrainedUDot,
1435  ConstrainedMobilizerIndex mobilizer,
1436  MobilizerUIndex whichU) const;
1437 
1446 void addInOneMobilityForce
1447  (const State& state,
1448  ConstrainedMobilizerIndex mobilizer,
1449  MobilizerUIndex whichU,
1450  Real fu,
1451  Array_<Real,ConstrainedUIndex>& mobilityForces) const;
1452 
1468 void addInOneQForce
1469  (const State& state,
1470  ConstrainedMobilizerIndex mobilizer,
1471  MobilizerQIndex whichQ,
1472  Real fq,
1473  Array_<Real,ConstrainedQIndex>& qForces) const;
1492 const Transform& getBodyTransform
1493  (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
1494  ConstrainedBodyIndex bodyB) const;
1497 const Rotation& getBodyRotation
1499  ConstrainedBodyIndex bodyB) const
1500 { return getBodyTransform(allX_AB,bodyB).R(); }
1504 const Vec3& getBodyOriginLocation
1506  ConstrainedBodyIndex bodyB) const
1507 { return getBodyTransform(allX_AB,bodyB).p(); }
1508 
1514 const Transform& getBodyTransformFromState
1515  (const State& state, ConstrainedBodyIndex B) const; // X_AB
1518 const Rotation& getBodyRotationFromState
1519  (const State& state, ConstrainedBodyIndex bodyB) const
1520 { return getBodyTransformFromState(state,bodyB).R(); }
1524 const Vec3& getBodyOriginLocationFromState
1525  (const State& state, ConstrainedBodyIndex bodyB) const
1526 { return getBodyTransformFromState(state,bodyB).p(); }
1527 
1531 const SpatialVec& getBodyVelocity
1532  (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
1533  ConstrainedBodyIndex bodyB) const;
1536 const Vec3& getBodyAngularVelocity
1538  ConstrainedBodyIndex bodyB) const
1539 { return getBodyVelocity(allV_AB,bodyB)[0]; }
1542 const Vec3& getBodyOriginVelocity
1544  ConstrainedBodyIndex bodyB) const
1545 { return getBodyVelocity(allV_AB,bodyB)[1]; }
1546 
1552 const SpatialVec& getBodyVelocityFromState
1553  (const State& state, ConstrainedBodyIndex bodyB) const; // V_AB
1556 const Vec3& getBodyAngularVelocityFromState
1557  (const State& state, ConstrainedBodyIndex bodyB) const
1558 { return getBodyVelocityFromState(state,bodyB)[0]; }
1561 const Vec3& getBodyOriginVelocityFromState
1562  (const State& state, ConstrainedBodyIndex bodyB) const
1563 { return getBodyVelocityFromState(state,bodyB)[1]; }
1564 
1570 const SpatialVec& getBodyAcceleration
1571  (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
1572  ConstrainedBodyIndex bodyB) const;
1575 const Vec3& getBodyAngularAcceleration
1577  ConstrainedBodyIndex bodyB) const
1578 { return getBodyAcceleration(allA_AB,bodyB)[0]; }
1581 const Vec3& getBodyOriginAcceleration
1583  ConstrainedBodyIndex bodyB) const
1584 { return getBodyAcceleration(allA_AB,bodyB)[1]; }
1585 
1586  // Calculate location, velocity, and acceleration for a given station.
1587 
1594 Vec3 findStationLocation
1596  ConstrainedBodyIndex bodyB,
1597  const Vec3& p_BS) const
1598 {
1599  const Transform& X_AB = allX_AB[bodyB];
1600  return X_AB * p_BS; // re-measure and re-express
1601 }
1605 Vec3 findStationLocationFromState
1606  (const State& state,
1607  ConstrainedBodyIndex bodyB,
1608  const Vec3& p_BS) const
1609 {
1610  const Transform& X_AB = getBodyTransformFromState(state,bodyB);
1611  return X_AB * p_BS; // re-measure and re-express
1612 }
1613 
1620 Vec3 findStationVelocity
1621  (const State& state,
1623  ConstrainedBodyIndex bodyB,
1624  const Vec3& p_BS) const
1625 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao
1626  const Rotation& R_AB = getBodyRotationFromState(state,bodyB);
1627  const Vec3 p_BS_A = R_AB * p_BS;
1628  const SpatialVec& V_AB = allV_AB[bodyB];
1629  return V_AB[1] + (V_AB[0] % p_BS_A); // v + w X r
1630 }
1634 Vec3 findStationVelocityFromState
1635  (const State& state,
1636  ConstrainedBodyIndex bodyB,
1637  const Vec3& p_BS) const
1638 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao
1639  const Rotation& R_AB = getBodyRotationFromState(state,bodyB);
1640  const Vec3 p_BS_A = R_AB * p_BS;
1641  const SpatialVec& V_AB = getBodyVelocityFromState(state,bodyB);
1642  return V_AB[1] + (V_AB[0] % p_BS_A); // v + w X r
1643 }
1644 
1654 Vec3 findStationAcceleration
1655  (const State& state,
1657  ConstrainedBodyIndex bodyB,
1658  const Vec3& p_BS) const
1659 { // p_BS_A is p_BS rexpressed in A but not shifted to Ao
1660  const Rotation& R_AB = getBodyRotationFromState(state,bodyB);
1661  const Vec3 p_BS_A = R_AB * p_BS;
1662  const Vec3& w_AB = getBodyAngularVelocityFromState(state,bodyB);
1663  const SpatialVec& A_AB = allA_AB[bodyB];
1664 
1665  // Result is a + b X r + w X (w X r).
1666  // ("b" is angular acceleration; w is angular velocity).
1667  const Vec3 a_AS = A_AB[1] + (A_AB[0] % p_BS_A)
1668  + w_AB % (w_AB % p_BS_A); // % is not associative
1669  return a_AS;
1670 }
1671 
1672  // Utilities for applying constraint forces to ConstrainedBodies.
1673 
1678 void addInStationForce
1679  (const State& state,
1680  ConstrainedBodyIndex bodyB,
1681  const Vec3& p_BS,
1682  const Vec3& forceInA,
1683  Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const;
1684 
1687 void addInBodyTorque
1688  (const State& state,
1689  ConstrainedBodyIndex bodyB,
1690  const Vec3& torqueInA,
1691  Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const;
1700 void getMultipliers(const State& state,
1701  Array_<Real>& multipliers) const;
1704 protected:
1725 virtual void realizeTopology(State&) const { }
1726 
1737 virtual void realizeModel(State&) const { }
1738 
1744 virtual void realizeInstance(const State&) const { }
1745 
1751 virtual void realizeTime(const State&) const { }
1752 
1759 virtual void realizePosition(const State&) const { }
1760 
1767 virtual void realizeVelocity(const State&) const { }
1768 
1774 virtual void realizeDynamics(const State&) const { }
1775 
1784 virtual void realizeAcceleration(const State&) const { }
1785 
1791 virtual void realizeReport(const State&) const { }
1805 virtual void calcPositionErrors
1806  (const State& state, // Stage::Time
1808  const Array_<Real, ConstrainedQIndex>& constrainedQ,
1809  Array_<Real>& perr) // mp of these
1810  const;
1811 
1823 virtual void calcPositionDotErrors
1824  (const State& state, // Stage::Position
1826  const Array_<Real, ConstrainedQIndex>& constrainedQDot,
1827  Array_<Real>& pverr) // mp of these
1828  const;
1829 
1842 virtual void calcPositionDotDotErrors
1843  (const State& state, // Stage::Velocity
1845  const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
1846  Array_<Real>& paerr) // mp of these
1847  const;
1848 
1867 virtual void addInPositionConstraintForces
1868  (const State& state,
1869  const Array_<Real>& multipliers,
1871  Array_<Real, ConstrainedQIndex>& qForces) const;
1889 virtual void calcVelocityErrors
1890  (const State& state, // Stage::Position
1892  const Array_<Real, ConstrainedUIndex>& constrainedU,
1893  Array_<Real>& verr) // mv of these
1894  const;
1895 
1906 virtual void calcVelocityDotErrors
1907  (const State& state, // Stage::Velocity
1909  const Array_<Real, ConstrainedUIndex>& constrainedUDot,
1910  Array_<Real>& vaerr) // mv of these
1911  const;
1912 
1931 virtual void addInVelocityConstraintForces
1932  (const State& state,
1933  const Array_<Real>& multipliers,
1935  Array_<Real, ConstrainedUIndex>& mobilityForces) const;
1956 virtual void calcAccelerationErrors
1957  (const State& state, // Stage::Velocity
1959  const Array_<Real, ConstrainedUIndex>& constrainedUDot,
1960  Array_<Real>& aerr) // ma of these
1961  const;
1962 
1980 virtual void addInAccelerationConstraintForces
1981  (const State& state,
1982  const Array_<Real>& multipliers,
1984  Array_<Real, ConstrainedUIndex>& mobilityForces) const;
1994 virtual void calcDecorativeGeometryAndAppend
1995  (const State& s, Stage stage, Array_<DecorativeGeometry>& geom) const
1996 {
1997 }
1998 
1999 friend class Constraint::CustomImpl;
2000 };
2001 
2002 
2003 
2004 //==============================================================================
2005 // COORDINATE COUPLER
2006 //==============================================================================
2016 : public Constraint::Custom {
2017 public:
2039  const Function* function,
2040  const Array_<MobilizedBodyIndex>& coordMobod,
2041  const Array_<MobilizerQIndex>& coordQIndex);
2042 
2045  const std::vector<MobilizedBodyIndex>& coordBody,
2046  const std::vector<MobilizerQIndex>& coordIndex)
2047  { // Invoke the above constructor with converted arguments.
2048  new (this) CoordinateCoupler(matter,function,
2050  ArrayViewConst_<MobilizerQIndex>(coordIndex));
2051  }
2052 
2055 };
2056 
2057 
2058 
2059 //==============================================================================
2060 // SPEED COUPLER
2061 //==============================================================================
2073 public:
2093  const Function* function,
2094  const Array_<MobilizedBodyIndex>& speedBody,
2095  const Array_<MobilizerUIndex>& speedIndex);
2096 
2099  const Function* function,
2100  const std::vector<MobilizedBodyIndex>& speedBody,
2101  const std::vector<MobilizerUIndex>& speedIndex)
2102  { // Invoke above constructor with converted arguments.
2103  new (this) SpeedCoupler(matter, function,
2105  ArrayViewConst_<MobilizerUIndex>(speedIndex));
2106  }
2107 
2137  const Function* function,
2138  const Array_<MobilizedBodyIndex>& speedBody,
2139  const Array_<MobilizerUIndex>& speedIndex,
2140  const Array_<MobilizedBodyIndex>& coordBody,
2141  const Array_<MobilizerQIndex>& coordIndex);
2142 
2145  const Function* function,
2146  const std::vector<MobilizedBodyIndex>& speedBody,
2147  const std::vector<MobilizerUIndex>& speedIndex,
2148  const std::vector<MobilizedBodyIndex>& coordBody,
2149  const std::vector<MobilizerQIndex>& coordIndex)
2150  { // Invoke above constructor with converted arguments.
2151  new (this) SpeedCoupler(matter, function,
2155  ArrayViewConst_<MobilizerQIndex>(coordIndex));
2156  }
2157 
2160 };
2161 
2162 
2163 
2164 //==============================================================================
2165 // PRESCRIBED MOTION
2166 //==============================================================================
2173 : public Constraint::Custom {
2174 public:
2191  const Function* function,
2192  MobilizedBodyIndex coordBody,
2193  MobilizerQIndex coordIndex);
2194 
2195 
2198 };
2199 
2200 
2201 
2202 } // namespace SimTK
2203 
2204 #endif // SimTK_SIMBODY_CONSTRAINT_H_
2205 
2206 
2207 
virtual void realizeDynamics(const State &) const
The Matter Subsystem&#39;s realizeDynamics() method will call this method after any MobilizedBody Dynamic...
Definition: Constraint.h:1774
This constraint represents a bilateral connection between an edge on one body and a non-parallel edge...
Definition: Constraint_LineOnLineContact.h:142
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
Three constraint equations.
Definition: Constraint.h:671
Unique integer type for Subsystem-local u indexing.
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
ConstantOrientation()
Default constructor creates an empty handle.
Definition: Constraint.h:679
This is a Constraint that uses a Function to prescribe the behavior of a single generalized coordinat...
Definition: Constraint.h:2172
ConstantAcceleration()
Default constructor creates an empty handle you can use to reference any existing ConstantAcceleratio...
Definition: Constraint.h:1062
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:50
A SimbodyMatterSubtree is a view of a connected subgraph of the tree of mobilized bodies in a Simbody...
Definition: SimbodyMatterSubtree.h:109
NoSlip1D()
Default constructor creates an empty handle.
Definition: Constraint.h:742
SpeedCoupler()
Default constructor creates an empty handle.
Definition: Constraint.h:2159
PrescribedMotion()
Default constructor creates an empty handle.
Definition: Constraint.h:2197
Constrain a single mobility to have a particular speed.
Definition: Constraint.h:959
Every Simbody header and source file should include this header before any other Simbody header...
This constraint represents a bilateral connection between a sphere on one body and a sphere on anothe...
Definition: Constraint_SphereOnSphereContact.h:102
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:593
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
ConstantAngle()
Default constructor creates an empty handle.
Definition: Constraint.h:610
Constrain a single mobilizer coordinate q to have a particular value.
Definition: Constraint.h:858
(Advanced) This is the underlying constraint for unilateral contact with friction but must be combine...
Definition: Constraint_PointOnPlaneContact.h:87
virtual void realizeModel(State &) const
The Matter Subsystem&#39;s realizeModel() method will call this method after all MobilizedBody Model-stag...
Definition: Constraint.h:1737
This constraint represents a bilateral connection between a sphere on one body and a plane on another...
Definition: Constraint_SphereOnPlaneContact.h:86
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
Six constraint equations.
Definition: Constraint_Weld.h:60
#define SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(DERIVED, DERIVED_IMPL, PARENT)
Definition: PrivateImplementation.h:343
virtual ~Implementation()
Destructor is virtual so derived classes get a chance to clean up if necessary.
Definition: Constraint.h:1199
Constraint()
Default constructor creates an empty Constraint handle that can be used to reference any Constraint...
Definition: Constraint.h:71
This is a Constraint that uses a Function object to define a nonholonomic (velocity) constraint...
Definition: Constraint.h:2072
One constraint equation.
Definition: Constraint_PointInPlane.h:47
virtual void realizeTime(const State &) const
The Matter Subsystem&#39;s realizeTime() method will call this method after any MobilizedBody Time-stage ...
Definition: Constraint.h:1751
Unique integer type for Subsystem-local q indexing.
This is a Constraint that uses a Function object to define a single holonomic (position) constraint e...
Definition: Constraint.h:2015
Two constraint equations.
Definition: Constraint.h:520
virtual void realizeVelocity(const State &) const
The Matter Subsystem&#39;s realizeVelocity() method will call this method after any MobilizedBody Velocit...
Definition: Constraint.h:1767
CoordinateCoupler(SimbodyMatterSubsystem &matter, const Function *function, const std::vector< MobilizedBodyIndex > &coordBody, const std::vector< MobilizerQIndex > &coordIndex)
For compatibility with std::vector; no copying is done.
Definition: Constraint.h:2044
const Real Pi
Real(pi)
const Vec< 3, P > & p() const
Return a read-only reference to our translation vector p_BF.
Definition: Transform.h:239
Ball Spherical
Synonym for Ball constraint.
Definition: Constraint.h:461
This constraint consists of one constraint equation that enforces a constant distance between a point...
Definition: Constraint_Rod.h:52
Ball CoincidentPoints
Synonym for Ball constraint.
Definition: Constraint.h:459
Enforce that a fixed station on one body remains coincident with a fixed station on a second body...
Definition: Constraint_Ball.h:57
PointOnLine()
Default constructor creates an empty handle.
Definition: Constraint.h:527
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:48
Weld CoincidentFrames
Definition: Constraint.h:463
ConstantCoordinate()
Default constructor creates an empty handle you can use to reference any existing ConstantCoordinate ...
Definition: Constraint.h:875
virtual void realizeInstance(const State &) const
The Matter Subsystem&#39;s realizeInstance() method will call this method after all MobilizedBody Instanc...
Definition: Constraint.h:1744
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
Vector_< SpatialVec > getConstrainedBodyForcesAsVector(const State &state) const
For convenience, returns constrained body forces as the function return.
Definition: Constraint.h:397
A MobilizedBody is Simbody&#39;s fundamental body-and-joint object used to parameterize a system&#39;s motion...
Definition: MobilizedBody.h:167
virtual void realizeTopology(State &) const
The Matter Subsystem&#39;s realizeTopology() method will call this method after all MobilizedBody topolog...
Definition: Constraint.h:1725
This is the base class for all Constraint classes, which is just a handle for the underlying hidden i...
Definition: Constraint.h:66
The handle class Constraint::Custom (dataless) and its companion class Constraint::Custom::Implementa...
Definition: Constraint.h:1153
Constraint(ConstraintImpl *r)
For internal use: construct a new Constraint handle referencing a particular implementation object...
Definition: Constraint.h:74
ConstantSpeed()
Default constructor creates an empty handle you can use to reference any existing ConstantSpeed Const...
Definition: Constraint.h:971
Custom()
Default constructor creates an empty handle.
Definition: Constraint.h:1169
This is the abstract base class for the implementation of custom constraints. See Constraint::Custom ...
Definition: Constraint.h:1191
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
Rod ConstantDistance
Synonym for Rod constraint.
Definition: Constraint.h:456
This constraint consists of a single constraint equation that enforces that a unit vector v1 fixed to...
Definition: Constraint.h:602
virtual void realizeAcceleration(const State &) const
The Matter Subsystem&#39;s realizeAcceleration() method will call this method after any MobilizedBody Acc...
Definition: Constraint.h:1784
Unique integer type for Subsystem-local multiplier indexing.
virtual void realizeReport(const State &) const
The Matter Subsystem&#39;s realizeReport() method will call this method after any MobilizedBody Report-st...
Definition: Constraint.h:1791
const Rotation_< P > & R() const
Return a read-only reference to the contained rotation R_BF.
Definition: Transform.h:215
Constrain a single mobility to have a particular acceleration.
Definition: Constraint.h:1047
One non-holonomic constraint equation.
Definition: Constraint.h:728
virtual void realizePosition(const State &) const
The Matter Subsystem&#39;s realizePosition() method will call this method after any MobilizedBody Positio...
Definition: Constraint.h:1759
Vector getConstrainedMobilityForcesAsVector(const State &state) const
For convenience, returns constrained mobility forces as the function return.
Definition: Constraint.h:406
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
SpeedCoupler(SimbodyMatterSubsystem &matter, const Function *function, const std::vector< MobilizedBodyIndex > &speedBody, const std::vector< MobilizerUIndex > &speedIndex, const std::vector< MobilizedBodyIndex > &coordBody, const std::vector< MobilizerQIndex > &coordIndex)
For compatibility with std::vector; no copying is done.
Definition: Constraint.h:2144
SpeedCoupler(SimbodyMatterSubsystem &matter, const Function *function, const std::vector< MobilizedBodyIndex > &speedBody, const std::vector< MobilizerUIndex > &speedIndex)
For compatibility with std::vector; no copying is done.
Definition: Constraint.h:2098
CoordinateCoupler()
Default constructor creates an empty handle.
Definition: Constraint.h:2054