Simbody  3.8
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 
112 
119 
127 
129 bool isInSubsystem() const;
133 bool isInSameSubsystem(const MobilizedBody& mobod) const;
134 
135  // TOPOLOGY STAGE (i.e., post-construction) //
136 
143 
149  (ConstrainedBodyIndex consBodyIx) const;
150 
156 
164 
170  (ConstrainedMobilizerIndex consMobilizerIx) const;
171 
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 
222  ConstrainedQIndex consQIndex) const;
226  ConstrainedUIndex consUIndex) const;
227 
242  int& mp, int& mv, int& ma) const;
243 
267  MultiplierIndex& px0,
268  MultiplierIndex& vx0,
269  MultiplierIndex& ax0) const;
270 
293  const Vector& myPart,
294  Vector& constraintSpace) const;
295 
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
329 
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 
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.
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;
458 
459 class Ball;
461 typedef Ball Spherical;
462 
463 class Weld;
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.
536 
537  // Defaults for Instance variables.
541 
542  // Stage::Topology
545 
547  const Vec3& getDefaultPointOnLine() 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
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.
617 
618  // Defaults for Instance variables.
622 
623  // Stage::Topology
626 
627  const UnitVec3& getDefaultBaseAxis() 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
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.
686 
687  // Stage::Topology
690 
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
704  Vec3 getMultipliers(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 
778 
779  // Defaults for Instance variables.
780 
787 
788 
789  // Stage::Topology
790 
797 
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 
821 
828  Real getMultiplier(const State&) const;
829 
834  // hide from Doxygen
838 };
839 
841  // CONSTANT COORDINATE //
843 
859 public:
864  Real defaultPosition);
865 
871  ConstantCoordinate(MobilizedBody& mobilizer, Real defaultPosition);
872 
876 
881 
885 
891 
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:
964  Real defaultSpeed);
967  ConstantSpeed(MobilizedBody& mobilizer, Real defaultSpeed);
968 
972 
977 
981 
987 
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 
1058  Real defaultAcceleration);
1059 
1063 
1068 
1073 
1079 
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 
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:
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 
1220 
1221  // Topological information//
1222 
1230 
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 
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 
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 
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 
1447  (const State& state,
1448  ConstrainedMobilizerIndex mobilizer,
1449  MobilizerUIndex whichU,
1450  Real fu,
1451  Array_<Real,ConstrainedUIndex>& mobilityForces) const;
1452 
1469  (const State& state,
1470  ConstrainedMobilizerIndex mobilizer,
1471  MobilizerQIndex whichQ,
1472  Real fq,
1473  Array_<Real,ConstrainedQIndex>& qForces) const;
1493  (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
1494  ConstrainedBodyIndex bodyB) const;
1498  (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
1499  ConstrainedBodyIndex bodyB) const
1500 { return getBodyTransform(allX_AB,bodyB).R(); }
1505  (const Array_<Transform,ConstrainedBodyIndex>& allX_AB,
1506  ConstrainedBodyIndex bodyB) const
1507 { return getBodyTransform(allX_AB,bodyB).p(); }
1508 
1515  (const State& state, ConstrainedBodyIndex B) const; // X_AB
1519  (const State& state, ConstrainedBodyIndex bodyB) const
1520 { return getBodyTransformFromState(state,bodyB).R(); }
1525  (const State& state, ConstrainedBodyIndex bodyB) const
1526 { return getBodyTransformFromState(state,bodyB).p(); }
1527 
1532  (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
1533  ConstrainedBodyIndex bodyB) const;
1537  (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
1538  ConstrainedBodyIndex bodyB) const
1539 { return getBodyVelocity(allV_AB,bodyB)[0]; }
1543  (const Array_<SpatialVec,ConstrainedBodyIndex>& allV_AB,
1544  ConstrainedBodyIndex bodyB) const
1545 { return getBodyVelocity(allV_AB,bodyB)[1]; }
1546 
1553  (const State& state, ConstrainedBodyIndex bodyB) const; // V_AB
1557  (const State& state, ConstrainedBodyIndex bodyB) const
1558 { return getBodyVelocityFromState(state,bodyB)[0]; }
1562  (const State& state, ConstrainedBodyIndex bodyB) const
1563 { return getBodyVelocityFromState(state,bodyB)[1]; }
1564 
1571  (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
1572  ConstrainedBodyIndex bodyB) const;
1576  (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
1577  ConstrainedBodyIndex bodyB) const
1578 { return getBodyAcceleration(allA_AB,bodyB)[0]; }
1582  (const Array_<SpatialVec,ConstrainedBodyIndex>& allA_AB,
1583  ConstrainedBodyIndex bodyB) const
1584 { return getBodyAcceleration(allA_AB,bodyB)[1]; }
1585 
1586  // Calculate location, velocity, and acceleration for a given station.
1587 
1595  (const Array_<Transform, ConstrainedBodyIndex>& allX_AB,
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 }
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 
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 }
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 
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 
1679  (const State& state,
1680  ConstrainedBodyIndex bodyB,
1681  const Vec3& p_BS,
1682  const Vec3& forceInA,
1683  Array_<SpatialVec,ConstrainedBodyIndex>& bodyForcesInA) const;
1684 
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 
1824  (const State& state, // Stage::Position
1826  const Array_<Real, ConstrainedQIndex>& constrainedQDot,
1827  Array_<Real>& pverr) // mp of these
1828  const;
1829 
1843  (const State& state, // Stage::Velocity
1845  const Array_<Real, ConstrainedQIndex>& constrainedQDotDot,
1846  Array_<Real>& paerr) // mp of these
1847  const;
1848 
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 
1907  (const State& state, // Stage::Velocity
1909  const Array_<Real, ConstrainedUIndex>& constrainedUDot,
1910  Array_<Real>& vaerr) // mv of these
1911  const;
1912 
1932  (const State& state,
1933  const Array_<Real>& multipliers,
1935  Array_<Real, ConstrainedUIndex>& mobilityForces) const;
1957  (const State& state, // Stage::Velocity
1959  const Array_<Real, ConstrainedUIndex>& constrainedUDot,
1960  Array_<Real>& aerr) // ma of these
1961  const;
1962 
1981  (const State& state,
1982  const Array_<Real>& multipliers,
1984  Array_<Real, ConstrainedUIndex>& mobilityForces) const;
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 
#define SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(DERIVED, DERIVED_IMPL, PARENT)
Definition: PrivateImplementation.h:343
Every Simbody header and source file should include this header before any other Simbody header.
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:324
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
Enforce that a fixed station on one body remains coincident with a fixed station on a second body,...
Definition: Constraint_Ball.h:57
Constrain a single mobility to have a particular acceleration.
Definition: Constraint.h:1048
MobilizedBodyIndex getMobilizedBodyIndex() const
Return the index of the mobilized body to which this constant acceleration constraint is being applie...
void setAcceleration(State &state, Real accel) const
Override the default acceleration with this one whose value is stored in the given State.
ConstantAcceleration(MobilizedBody &mobilizer, MobilizerUIndex whichU, Real defaultAcceleration)
Construct a constant acceleration constraint on a particular mobility of the given mobilizer.
ConstantAcceleration(MobilizedBody &mobilizer, Real defaultAcceleration)
Construct a constant acceleration constraint on the mobility of the given mobilizer,...
MobilizerUIndex getWhichU() const
Return the particular mobility whose generalized acceleration is controlled by this ConstantAccelerat...
ConstantAcceleration()
Default constructor creates an empty handle you can use to reference any existing ConstantAcceleratio...
Definition: Constraint.h:1062
Real getDefaultAcceleration() const
Return the default value for the acceleration to be enforced.
ConstantAcceleration & setDefaultAcceleration(Real accel)
Change the default value for the acceleration to be enforced by this constraint.
Real getMultiplier(const State &) const
Get the value of the Lagrange multipler generated to satisfy this constraint.
Real getAccelerationError(const State &) const
Return the amount by which the accelerations in the given State fail to satify this constraint.
Real getAcceleration(const State &state) const
Get the current value of the acceleration set point from the indicated State.
This constraint consists of a single constraint equation that enforces that a unit vector v1 fixed to...
Definition: Constraint.h:602
ConstantAngle(MobilizedBody &baseBody_B, const UnitVec3 &defaultAxis_B, MobilizedBody &followerBody_F, const UnitVec3 &defaultAxis_F, Real angle=Pi/2)
Real getAngle(const State &) const
Real getTorqueOnFollowerBody(const State &) const
ConstantAngle & setDefaultBaseAxis(const UnitVec3 &)
ConstantAngle & setDefaultAngle(Real)
ConstantAngle & setAxisDisplayLength(Real)
const UnitVec3 & getDefaultBaseAxis() const
MobilizedBodyIndex getBaseMobilizedBodyIndex() const
ConstantAngle & setDefaultFollowerAxis(const UnitVec3 &)
Real getMultiplier(const State &) const
MobilizedBodyIndex getFollowerMobilizedBodyIndex() const
Real getPositionError(const State &) const
ConstantAngle & setAxisDisplayWidth(Real)
Real getVelocityError(const State &) const
ConstantAngle()
Default constructor creates an empty handle.
Definition: Constraint.h:610
const UnitVec3 & getBaseAxis(const State &) const
const UnitVec3 & getFollowerAxis(const State &) const
const UnitVec3 & getDefaultFollowerAxis() const
Real getAccelerationError(const State &) const
Constrain a single mobilizer coordinate q to have a particular value.
Definition: Constraint.h:858
void setPosition(State &state, Real position) const
Override the default position with this one whose value is stored in the given State.
ConstantCoordinate(MobilizedBody &mobilizer, MobilizerQIndex whichQ, Real defaultPosition)
Construct a constant coordinate constraint on a particular generalized coordinate q of the given mobi...
ConstantCoordinate(MobilizedBody &mobilizer, Real defaultPosition)
Construct a constant coordinate constraint on the generalized coordinate q of the given mobilizer,...
Real getAccelerationError(const State &state) const
Return the amount by which the accelerations in the given State fail to satify the second time deriva...
MobilizedBodyIndex getMobilizedBodyIndex() const
Return the index of the mobilized body to which this constant coordinate constraint is being applied ...
MobilizerQIndex getWhichQ() const
Return the particular coordinate whose position is controlled by this ConstantCoordinate constraint.
ConstantCoordinate & setDefaultPosition(Real position)
Change the default value for the position to be enforced by this constraint.
Real getPositionError(const State &state) const
Return the amount by which the given State fails to satisfy this ConstantCoordinate constraint.
Real getDefaultPosition() const
Return the default value for the position to be enforced.
Real getMultiplier(const State &state) const
Get the value of the Lagrange multiplier generated to satisfy this constraint.
Real getPosition(const State &state) const
Get the current value of the position set point from the indicated State.
Real getVelocityError(const State &state) const
Return the amount by which the given State fails to satisfy the time derivative of this ConstantCoord...
ConstantCoordinate()
Default constructor creates an empty handle you can use to reference any existing ConstantCoordinate ...
Definition: Constraint.h:875
Three constraint equations.
Definition: Constraint.h:672
const Rotation & getDefaultBaseRotation() const
const Rotation & getDefaultFollowerRotation() const
Vec3 getPositionErrors(const State &) const
Vec3 getVelocityErrors(const State &) const
const Rotation & getFollowerRotation(const State &) const
MobilizedBodyIndex getBaseMobilizedBodyIndex() const
ConstantOrientation(MobilizedBody &baseBody_B, const Rotation &defaultRB, MobilizedBody &followerBody_F, const Rotation &defaultRF)
MobilizedBodyIndex getFollowerMobilizedBodyIndex() const
const Rotation & getBaseRotation(const State &) const
ConstantOrientation()
Default constructor creates an empty handle.
Definition: Constraint.h:679
Vec3 getAccelerationErrors(const State &) const
ConstantOrientation & setDefaultFollowerRotation(const Rotation &)
ConstantOrientation & setDefaultBaseRotation(const Rotation &)
Vec3 getTorqueOnFollowerBody(const State &) const
Vec3 getMultipliers(const State &) const
Constrain a single mobility to have a particular speed.
Definition: Constraint.h:959
Real getAccelerationError(const State &state) const
Return the amount by which the accelerations in the given State fail to satify the time derivative of...
MobilizedBodyIndex getMobilizedBodyIndex() const
Return the index of the mobilized body to which this constant speed constraint is being applied (to o...
Real getSpeed(const State &state) const
Get the current value of the speed set point from the indicated State.
Real getDefaultSpeed() const
Return the default value for the speed to be enforced.
MobilizerUIndex getWhichU() const
Return the particular mobility whose generalized speed is controlled by this ConstantSpeed constraint...
ConstantSpeed & setDefaultSpeed(Real speed)
Change the default value for the speed to be enforced by this constraint.
ConstantSpeed(MobilizedBody &mobilizer, MobilizerUIndex whichU, Real defaultSpeed)
Construct a constant speed constraint on a particular mobility of the given mobilizer.
void setSpeed(State &state, Real speed) const
Override the default speed with this one whose value is stored in the given State.
ConstantSpeed()
Default constructor creates an empty handle you can use to reference any existing ConstantSpeed Const...
Definition: Constraint.h:971
Real getVelocityError(const State &state) const
Return the amount by which the given State fails to satisfy this ConstantSpeed constraint.
ConstantSpeed(MobilizedBody &mobilizer, Real defaultSpeed)
Construct a constant speed constraint on the mobility of the given mobilizer, assuming there is only ...
Real getMultiplier(const State &state) const
Get the value of the Lagrange multiplier generated to satisfy this constraint.
This is a Constraint that uses a Function object to define a single holonomic (position) constraint e...
Definition: Constraint.h:2016
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
CoordinateCoupler(SimbodyMatterSubsystem &matter, const Function *function, const Array_< MobilizedBodyIndex > &coordMobod, const Array_< MobilizerQIndex > &coordQIndex)
Create a CoordinateCoupler.
CoordinateCoupler()
Default constructor creates an empty handle.
Definition: Constraint.h:2054
This is the abstract base class for the implementation of custom constraints. See Constraint::Custom ...
Definition: Constraint.h:1192
const Vec3 & getBodyAngularAcceleration(const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyAcceleration() that returns just the angular acceleration vecto...
Definition: Constraint.h:1576
void getMultipliers(const State &state, Array_< Real > &multipliers) const
Given a state as passed to your realizeAcceleration() implementation, obtain the multipliers that Sim...
virtual void addInPositionConstraintForces(const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedQIndex > &qForces) const
From the mp supplied Lagrange multipliers provided in multipliers, calculate the forces produced by t...
void invalidateTopologyCache() const
Call this if you want to make sure that the next realizeTopology() call does something.
const Vec3 & getBodyAngularVelocity(const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyVelocity() that returns just the angular velocity vector w_AB.
Definition: Constraint.h:1537
const Rotation & getBodyRotationFromState(const State &state, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyTransformFromState() that returns just the orientation as the R...
Definition: Constraint.h:1519
void addInOneMobilityForce(const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU, Real fu, Array_< Real, ConstrainedUIndex > &mobilityForces) const
Apply a scalar generalized (mobility-space) force fu to a particular mobility of one of this Constrai...
virtual void calcAccelerationErrors(const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedUIndex > &constrainedUDot, Array_< Real > &aerr) const
Calculate the ma acceleration-constraint errors due to the specification of an acceleration-only cons...
Vec3 findStationLocation(const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const
Calculate the position p_AS in the Ancestor frame of a station S of a Constrained Body B,...
Definition: Constraint.h:1595
virtual void realizeAcceleration(const State &) const
The Matter Subsystem's realizeAcceleration() method will call this method after any MobilizedBody Acc...
Definition: Constraint.h:1784
const Vec3 & getBodyOriginAcceleration(const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyAcceleration() that returns just the linear acceleration vector...
Definition: Constraint.h:1582
Real getOneUFromState(const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const
Same as the getOneU() method but for use in velocity- or acceleration- level methods to which no expl...
virtual void calcVelocityErrors(const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &V_AB, const Array_< Real, ConstrainedUIndex > &constrainedU, Array_< Real > &verr) const
Calculate the mv velocity-constraint errors due to the velocity-level specification of a nonholonomic...
virtual void realizeReport(const State &) const
The Matter Subsystem's realizeReport() method will call this method after any MobilizedBody Report-st...
Definition: Constraint.h:1791
Real getOneUDot(const State &state, const Array_< Real, ConstrainedUIndex > &constrainedUDot, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const
Use this method in your calcVelocityDotErrors() and calcAccelerationErrors() implementations to extra...
virtual void addInAccelerationConstraintForces(const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedUIndex > &mobilityForces) const
From the ma supplied Lagrange multipliers provided in multipliers, calculate the forces produced by t...
Real getOneQDot(const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQDot, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
Use this method in your calcPositionDotErrors() implementation to extract the value of a particular g...
Vec3 findStationVelocity(const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const
Calculate the velocity v_AS in the Ancestor frame of a station S of a Constrained Body B,...
Definition: Constraint.h:1621
virtual void realizeDynamics(const State &) const
The Matter Subsystem's realizeDynamics() method will call this method after any MobilizedBody Dynamic...
Definition: Constraint.h:1774
Vec3 findStationVelocityFromState(const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const
Same as findStationVelocity() but for when you have to get the velocity information from the state ra...
Definition: Constraint.h:1635
virtual void realizeInstance(const State &) const
The Matter Subsystem's realizeInstance() method will call this method after all MobilizedBody Instanc...
Definition: Constraint.h:1744
Implementation & setDefaultNumConstraintEquations(int mp, int mv, int ma)
This is an alternate way to set the default number of equations to be generated if you didn't specify...
MobilizedBodyIndex getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex) const
Map a constrained body for this constraint to the mobilized body to which it corresponds in the matte...
virtual void realizePosition(const State &) const
The Matter Subsystem's realizePosition() method will call this method after any MobilizedBody Positio...
Definition: Constraint.h:1759
virtual void calcPositionErrors(const State &state, const Array_< Transform, ConstrainedBodyIndex > &X_AB, const Array_< Real, ConstrainedQIndex > &constrainedQ, Array_< Real > &perr) const
Calculate the mp position-constraint errors due to the position-level specification of a holonomic co...
ConstrainedMobilizerIndex addConstrainedMobilizer(const MobilizedBody &)
Call this during construction phase to add a mobilizer to the topological structure of this Constrain...
Implementation(SimbodyMatterSubsystem &, int mp, int mv, int ma)
This Implementation base class constructor sets the topological defaults for the number of position l...
Implementation & setDisabledByDefault(bool shouldBeDisabled)
Normally Constraints are enabled when defined and can be disabled later.
virtual void realizeTime(const State &) const
The Matter Subsystem's realizeTime() method will call this method after any MobilizedBody Time-stage ...
Definition: Constraint.h:1751
void addInStationForce(const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS, const Vec3 &forceInA, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA) const
Apply an Ancestor-frame force to a B-frame station S given by the position vector p_BS (or more expli...
Real getOneQDotDot(const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQDotDot, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
Use this method in your calcPositionDotDotErrors() implementation to extract the value of a particula...
void addInBodyTorque(const State &state, ConstrainedBodyIndex bodyB, const Vec3 &torqueInA, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA) const
Apply an Ancestor-frame torque to body B, adding to the appropriate bodyForcesInA entry for this Cons...
const Vec3 & getBodyOriginVelocityFromState(const State &state, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyVelocityFromState() that returns just the linear velocity vecto...
Definition: Constraint.h:1562
virtual void calcDecorativeGeometryAndAppend(const State &s, Stage stage, Array_< DecorativeGeometry > &geom) const
Implement this optional method if you would like your constraint to generate any suggestions for geom...
Definition: Constraint.h:1995
virtual void realizeVelocity(const State &) const
The Matter Subsystem's realizeVelocity() method will call this method after any MobilizedBody Velocit...
Definition: Constraint.h:1767
virtual void realizeModel(State &) const
The Matter Subsystem's realizeModel() method will call this method after all MobilizedBody Model-stag...
Definition: Constraint.h:1737
const Vec3 & getBodyOriginLocation(const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyTransform() that returns just the location part of B's pose in ...
Definition: Constraint.h:1505
const SimbodyMatterSubsystem & getMatterSubsystem() const
Return a reference to the matter subsystem containing this constraint.
virtual void calcPositionDotDotErrors(const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedQIndex > &constrainedQDotDot, Array_< Real > &paerr) const
Calculate the mp errors arising from the second time derivative of the position-level holonomic const...
Real getOneU(const State &state, const Array_< Real, ConstrainedUIndex > &constrainedU, ConstrainedMobilizerIndex mobilizer, MobilizerUIndex whichU) const
Use this method in your calcVelocityErrors() implementation to extract the value of a particular gene...
virtual void calcVelocityDotErrors(const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &A_AB, const Array_< Real, ConstrainedUIndex > &constrainedUDot, Array_< Real > &vaerr) const
Calculate the mv errors arising from the first time derivative of the velocity-level specification of...
const Transform & getBodyTransform(const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const
Extract from the allX_AB argument the spatial transform X_AB giving the pose (orientation and locatio...
Real getOneQFromState(const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
Same as the getOneQ() method but for use in methods to which no explicit "constrained q" argument is ...
const SpatialVec & getBodyVelocityFromState(const State &state, ConstrainedBodyIndex bodyB) const
Extract from the State cache the spatial velocity V_AB giving the angular and linear velocity of a Co...
virtual ~Implementation()
Destructor is virtual so derived classes get a chance to clean up if necessary.
Definition: Constraint.h:1199
Vec3 findStationLocationFromState(const State &state, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const
Same as findStationLocation() but for when you have to get the position information from the state ra...
Definition: Constraint.h:1606
const Vec3 & getBodyOriginVelocity(const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyVelocity() that returns just the linear velocity vector v_AB.
Definition: Constraint.h:1543
virtual void addInVelocityConstraintForces(const State &state, const Array_< Real > &multipliers, Array_< SpatialVec, ConstrainedBodyIndex > &bodyForcesInA, Array_< Real, ConstrainedUIndex > &mobilityForces) const
From the mv supplied Lagrange multipliers provided in multipliers, calculate the forces produced by t...
const SpatialVec & getBodyVelocity(const Array_< SpatialVec, ConstrainedBodyIndex > &allV_AB, ConstrainedBodyIndex bodyB) const
Extract from the allV_AB argument the spatial velocity V_AB giving the angular and linear velocity of...
void addInOneQForce(const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ, Real fq, Array_< Real, ConstrainedQIndex > &qForces) const
For use with holonomic (position) constraints, this method allows generalized forces to be applied in...
Real getOneQDotFromState(const State &state, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
Same as the getOneQDot() method above but for use in velocity- or acceleration-level methods to which...
Real getOneQ(const State &state, const Array_< Real, ConstrainedQIndex > &constrainedQ, ConstrainedMobilizerIndex mobilizer, MobilizerQIndex whichQ) const
Use this method in your calcPositionErrors() implementation to extract the value of a particular gene...
const Vec3 & getBodyOriginLocationFromState(const State &state, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyTransformFromState() that returns just the location part of B's...
Definition: Constraint.h:1525
ConstrainedBodyIndex addConstrainedBody(const MobilizedBody &)
Call this during construction phase to add a body to the topological structure of this Constraint.
const SpatialVec & getBodyAcceleration(const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB) const
Extract from the allA_AB argument the spatial acceleration A_AB giving the angular and linear acceler...
Vec3 findStationAcceleration(const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &allA_AB, ConstrainedBodyIndex bodyB, const Vec3 &p_BS) const
Calculate the acceleration a_AS in the Ancestor frame of a station S of a Constrained Body B,...
Definition: Constraint.h:1655
Implementation(SimbodyMatterSubsystem &)
The default constructor for the Implementation base class sets the number of generated equations to z...
virtual void realizeTopology(State &) const
The Matter Subsystem's realizeTopology() method will call this method after all MobilizedBody topolog...
Definition: Constraint.h:1725
virtual void calcPositionDotErrors(const State &state, const Array_< SpatialVec, ConstrainedBodyIndex > &V_AB, const Array_< Real, ConstrainedQIndex > &constrainedQDot, Array_< Real > &pverr) const
Calculate the mp velocity errors arising from the first time derivative of the position-level holonom...
const Vec3 & getBodyAngularVelocityFromState(const State &state, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyVelocityFromState() that returns just the angular velocity vect...
Definition: Constraint.h:1557
MobilizedBodyIndex getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex) const
Map a constrained mobilizer for this constraint to the mobilized body to which it corresponds in the ...
virtual Implementation * clone() const =0
This method should produce a deep copy identical to the concrete derived Implementation object underl...
const Rotation & getBodyRotation(const Array_< Transform, ConstrainedBodyIndex > &allX_AB, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyTransform() that returns just the orientation as the Rotation m...
Definition: Constraint.h:1498
const Transform & getBodyTransformFromState(const State &state, ConstrainedBodyIndex B) const
Extract from the State cache the spatial transform X_AB giving the pose (orientation and location) of...
The handle class Constraint::Custom (dataless) and its companion class Constraint::Custom::Implementa...
Definition: Constraint.h:1153
Custom()
Default constructor creates an empty handle.
Definition: Constraint.h:1169
Implementation & updImplementation()
const Implementation & getImplementation() const
Custom(Implementation *implementation)
Create a Custom Constraint.
SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, Constraint)
This constraint represents a bilateral connection between an edge on one body and a non-parallel edge...
Definition: Constraint_LineOnLineContact.h:143
One non-holonomic constraint equation.
Definition: Constraint.h:728
MobilizedBodyIndex getCaseMobilizedBodyIndex() const
Get the mobilized body index of the Case body that was set during construction.
void setDirection(State &state, const UnitVec3 &direction_C) const
Change the no-slip direction along which this Constraint acts.
NoSlip1D()
Default constructor creates an empty handle.
Definition: Constraint.h:742
NoSlip1D & setDirectionDisplayLength(Real)
For visualization only, set the length of the line used to show the no-slip direction.
const UnitVec3 & getDefaultDirection() const
Obtain the default value for the no-slip direction, expressed in the Case body frame.
Real getMultiplier(const State &) const
Get the Lagrange multiplier for this constraint equation, using configuration, velocity,...
const UnitVec3 & getDirection(const State &state) const
Return from the given state the no-slip direction, in the Case body frame.
Real getAccelerationError(const State &) const
Get the acceleration error for this constraint equation, using configuration, velocity,...
NoSlip1D & setDefaultDirection(const UnitVec3 &)
Change the default no-slip direction; this is the initial value for for the actual direction and is a...
NoSlip1D(MobilizedBody &caseBodyC, const Vec3 &P_C, const UnitVec3 &n_C, MobilizedBody &movingBody0, MobilizedBody &movingBody1)
Define the up to three bodies involved in this constraint: the two "moving" bodies and a Case body,...
Real getPointDisplayRadius() const
Return the current value of the radius for visualization of the contact point.
const Vec3 & getDefaultContactPoint() const
Obtain the default value for the contact point, in the Case body frame.
Real getVelocityError(const State &state) const
Get the velocity error for this constraint equation, using configuration and velocity information fro...
NoSlip1D & setDefaultContactPoint(const Vec3 &)
Change the default contact point; this is the initial value for for the actual contact point and is a...
const Vec3 & getContactPoint(const State &state) const
Return from the given state the contact point, in the Case body frame.
NoSlip1D & setPointDisplayRadius(Real)
For visualization only, set the radius of the sphere used to show the contact point location.
Real getForceAtContactPoint(const State &) const
Determine the constraint force currently being generated by this constraint.
Real getDirectionDisplayLength() const
Return the current value of the visualization line length for the no-slip direction.
void setContactPoint(State &state, const Vec3 &point_C) const
Change the contact point at which this Constraint acts.
MobilizedBodyIndex getMovingBodyMobilizedBodyIndex(int which) const
Get the mobilized body index of moving body 0 or moving body 1 that was set during construction.
One constraint equation.
Definition: Constraint_PointInPlane.h:47
Two constraint equations.
Definition: Constraint.h:520
PointOnLine & setLineDisplayHalfLength(Real)
const Vec3 & getDefaultFollowerPoint() const
PointOnLine & setDefaultFollowerPoint(const Vec3 &)
const Vec2 & getForceOnFollowerPoint(const State &) const
PointOnLine & setDefaultPointOnLine(const Vec3 &)
const Vec3 & getFollowerPoint(const State &) const
Vec2 getPositionErrors(const State &) const
Vec2 getVelocityErrors(const State &) const
PointOnLine & setPointDisplayRadius(Real)
MobilizedBodyIndex getLineMobilizedBodyIndex() const
const Vec3 & getPointOnLine(const State &) const
PointOnLine()
Default constructor creates an empty handle.
Definition: Constraint.h:527
Vec2 getAccelerationErrors(const State &) const
Vec2 getMultipliers(const State &) const
const UnitVec3 & getLineDirection(const State &) const
const Vec3 & getDefaultPointOnLine() const
PointOnLine(MobilizedBody &lineBody_B, const UnitVec3 &defaultLineDirection_B, const Vec3 &defaultPointOnLine_B, MobilizedBody &followerBody_F, const Vec3 &defaultFollowerPoint_F)
PointOnLine & setDefaultLineDirection(const UnitVec3 &)
const UnitVec3 & getDefaultLineDirection() const
MobilizedBodyIndex getFollowerMobilizedBodyIndex() const
(Advanced) This is the underlying constraint for unilateral contact with friction but must be combine...
Definition: Constraint_PointOnPlaneContact.h:88
This is a Constraint that uses a Function to prescribe the behavior of a single generalized coordinat...
Definition: Constraint.h:2173
PrescribedMotion(SimbodyMatterSubsystem &matter, const Function *function, MobilizedBodyIndex coordBody, MobilizerQIndex coordIndex)
Create a PrescribedMotion constraint.
PrescribedMotion()
Default constructor creates an empty handle.
Definition: Constraint.h:2197
This constraint consists of one constraint equation that enforces a constant distance between a point...
Definition: Constraint_Rod.h:52
This is a Constraint that uses a Function object to define a nonholonomic (velocity) constraint.
Definition: Constraint.h:2072
SpeedCoupler(SimbodyMatterSubsystem &matter, const Function *function, const Array_< MobilizedBodyIndex > &speedBody, const Array_< MobilizerUIndex > &speedIndex, const Array_< MobilizedBodyIndex > &coordBody, const Array_< MobilizerQIndex > &coordIndex)
Create a SpeedCoupler.
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
SpeedCoupler(SimbodyMatterSubsystem &matter, const Function *function, const Array_< MobilizedBodyIndex > &speedBody, const Array_< MobilizerUIndex > &speedIndex)
Create a SpeedCoupler.
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()
Default constructor creates an empty handle.
Definition: Constraint.h:2159
This constraint represents a bilateral connection between a sphere on one body and a plane on another...
Definition: Constraint_SphereOnPlaneContact.h:87
This constraint represents a bilateral connection between a sphere on one body and a sphere on anothe...
Definition: Constraint_SphereOnSphereContact.h:103
Six constraint equations.
Definition: Constraint_Weld.h:60
This is the base class for all Constraint classes, which is just a handle for the underlying hidden i...
Definition: Constraint.h:67
void getMyPartFromConstraintSpaceVector(const State &state, const Vector &constraintSpace, Vector &myPart) const
Get the part of a complete constraint-space vector that belongs to this constraint.
Vector calcVelocityErrorFromU(const State &, const Vector &u) const
void setDisabledByDefault(bool shouldBeDisabled)
Normally Constraints are enabled when defined and can be disabled later.
Real calcPower(const State &state) const
Calculate the power being applied by this Constraint to the system.
Matrix calcVelocityConstraintMatrixV(const State &) const
int getNumConstrainedU(const State &) const
Return the sum of the number of mobilities u associated with each of the constrained mobilizers.
int getNumConstrainedQ(const State &, ConstrainedMobilizerIndex) const
Return the number of constrainable generalized coordinates q associated with a particular constrained...
Matrix calcAccelerationConstraintMatrixAt(const State &) const
Vector calcPositionErrorFromQ(const State &, const Vector &q) const
Constraint()
Default constructor creates an empty Constraint handle that can be used to reference any Constraint.
Definition: Constraint.h:71
void disable(State &) const
Disable this Constraint, effectively removing it from the system.
const MobilizedBody & getMobilizedBodyFromConstrainedBody(ConstrainedBodyIndex consBodyIx) const
Return a const reference to the actual MobilizedBody corresponding to one of the Constrained Bodies i...
void getIndexOfMultipliersInUse(const State &state, MultiplierIndex &px0, MultiplierIndex &vx0, MultiplierIndex &ax0) const
Return the start of the blocks of multipliers (or acceleration errors) assigned to this Constraint.
Matrix calcPositionConstraintMatrixP(const State &) const
void calcConstraintForcesFromMultipliers(const State &, const Vector &lambda, Vector_< SpatialVec > &bodyForcesInA, Vector &mobilityForces) const
This operator calculates this constraint's body and mobility forces given the complete set of multipl...
UIndex getUIndexOfConstrainedU(const State &state, ConstrainedUIndex consUIndex) const
Map one of this Constraint's constrained U's (or mobilities) to the corresponding index within the ma...
Vector getPositionErrorsAsVector(const State &) const
Get a Vector containing the position errors.
int getNumConstrainedBodies() const
Return the number of unique bodies directly restricted by this constraint.
bool isConditional() const
(Advanced) Get the value of the isConditional flag.
Matrix calcVelocityConstraintMatrixVt(const State &) const
ConstraintIndex getConstraintIndex() const
Get the ConstraintIndex that was assigned to this Constraint when it was added to the matter subsyste...
Ball Spherical
Synonym for Ball constraint.
Definition: Constraint.h:461
void getNumConstraintEquationsInUse(const State &state, int &mp, int &mv, int &ma) const
Find out how many holonomic (position), nonholonomic (velocity), and acceleration-only constraint equ...
Vector getAccelerationErrorsAsVector(const State &) const
Get a Vector containing the acceleration errors.
Matrix calcPositionConstraintMatrixPt(const State &) const
Vector getConstrainedMobilityForcesAsVector(const State &state) const
For convenience, returns constrained mobility forces as the function return.
Definition: Constraint.h:406
QIndex getQIndexOfConstrainedQ(const State &state, ConstrainedQIndex consQIndex) const
Map one of this Constraint's constrained q's to the corresponding index within the matter subsystem's...
const SimbodyMatterSubsystem & getMatterSubsystem() const
Get a const reference to the matter subsystem that contains this Constraint.
Vector calcAccelerationErrorFromUDot(const State &, const Vector &udot) const
ConstrainedUIndex getConstrainedUIndex(const State &, ConstrainedMobilizerIndex, MobilizerUIndex which) const
Return the index into the constrained mobilities u array corresponding to a particular mobility of th...
Ball CoincidentPoints
Synonym for Ball constraint.
Definition: Constraint.h:459
Vector getMultipliersAsVector(const State &) const
Get a Vector containing the Lagrange multipliers.
const MobilizedBody & getAncestorMobilizedBody() const
Return a const reference to the actual MobilizedBody which is serving as the Ancestor body for the co...
Vector_< SpatialVec > getConstrainedBodyForcesAsVector(const State &state) const
For convenience, returns constrained body forces as the function return.
Definition: Constraint.h:397
SimbodyMatterSubsystem & updMatterSubsystem()
Assuming you have writable access to this Constraint, get a writable reference to the containing matt...
bool isDisabled(const State &) const
Test whether this constraint is currently disabled in the supplied State.
Constraint(ConstraintImpl *r)
For internal use: construct a new Constraint handle referencing a particular implementation object.
Definition: Constraint.h:74
bool isDisabledByDefault() const
Test whether this Constraint is disabled by default in which case it must be explicitly enabled befor...
void setIsConditional(bool isConditional)
(Advanced) Mark this constraint as one that is only conditionally active.
Matrix calcPositionConstraintMatrixPNInv(const State &) const
void enable(State &) const
Enable this Constraint, without necessarily satisfying it.
int getNumConstrainedQ(const State &) const
Return the sum of the number of coordinates q associated with each of the constrained mobilizers.
Weld CoincidentFrames
Definition: Constraint.h:463
int getNumConstrainedMobilizers() const
Return the number of unique mobilizers directly restricted by this Constraint.
int getNumConstrainedU(const State &, ConstrainedMobilizerIndex) const
Return the number of constrainable mobilities u associated with a particular constrained mobilizer.
bool isInSubsystem() const
Test whether this Constraint is contained within a matter subsystem.
Rod ConstantDistance
Synonym for Rod constraint.
Definition: Constraint.h:456
const MobilizedBody & getMobilizedBodyFromConstrainedMobilizer(ConstrainedMobilizerIndex consMobilizerIx) const
Return a const reference to the actual MobilizedBody corresponding to one of the Constrained Mobilize...
bool isInSameSubsystem(const MobilizedBody &mobod) const
Test whether the supplied MobilizedBody is in the same matter subsystem as this Constraint.
Matrix calcAccelerationConstraintMatrixA(const State &) const
ConstrainedQIndex getConstrainedQIndex(const State &, ConstrainedMobilizerIndex, MobilizerQIndex which) const
Return the index into the constrained coordinates q array corresponding to a particular coordinate of...
void getConstraintForcesAsVectors(const State &state, Vector_< SpatialVec > &bodyForcesInG, Vector &mobilityForces) const
Given a State realized through Acceleration stage, return the forces that were applied to the system ...
void setMyPartInConstraintSpaceVector(const State &state, const Vector &myPart, Vector &constraintSpace) const
Set the part of a complete constraint-space vector that belongs to this constraint.
Vector getVelocityErrorsAsVector(const State &) const
Get a Vector containing the velocity errors.
const SimbodyMatterSubtree & getSubtree() const
Return a subtree object indicating which parts of the multibody tree are potentially affected by this...
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:169
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
Unique integer type for Subsystem-local multiplier indexing.
Unique integer type for Subsystem-local q indexing.
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
A SimbodyMatterSubtree is a view of a connected subgraph of the tree of mobilized bodies in a Simbody...
Definition: SimbodyMatterSubtree.h:109
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:66
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
Unique integer type for Subsystem-local u indexing.
const Real Pi
Real(pi)
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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:607