1 #ifndef SimTK_SIMBODY_CONSTRAINT_H_ 2 #define SimTK_SIMBODY_CONSTRAINT_H_ 40 class SimbodyMatterSubsystem;
41 class SimbodyMatterSubtree;
49 #ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT 50 extern template class PIMPLHandle<Constraint, ConstraintImpl, true>;
79 void disable(
State&)
const;
87 void enable(
State&)
const;
90 bool isDisabled(
const State&)
const;
94 bool isDisabledByDefault()
const;
100 void setDisabledByDefault(
bool shouldBeDisabled);
129 bool isInSubsystem()
const;
142 int getNumConstrainedBodies()
const;
149 (ConstrainedBodyIndex consBodyIx)
const;
163 int getNumConstrainedMobilizers()
const;
169 const MobilizedBody& getMobilizedBodyFromConstrainedMobilizer
170 (ConstrainedMobilizerIndex consMobilizerIx)
const;
185 int getNumConstrainedQ(
const State&, ConstrainedMobilizerIndex)
const;
190 int getNumConstrainedU(
const State&, ConstrainedMobilizerIndex)
const;
198 ConstrainedUIndex getConstrainedUIndex
206 ConstrainedQIndex getConstrainedQIndex
211 int getNumConstrainedQ(
const State&)
const;
217 int getNumConstrainedU(
const State&)
const;
222 ConstrainedQIndex consQIndex)
const;
226 ConstrainedUIndex consUIndex)
const;
241 void getNumConstraintEquationsInUse(
const State& state,
242 int& mp,
int& mv,
int& ma)
const;
266 void getIndexOfMultipliersInUse(
const State& state,
292 void setMyPartInConstraintSpaceVector(
const State& state,
294 Vector& constraintSpace)
const;
313 void getMyPartFromConstraintSpaceVector(
const State& state,
314 const Vector& constraintSpace,
320 Vector getPositionErrorsAsVector(
const State&)
const;
324 Matrix calcPositionConstraintMatrixP(
const State&)
const;
325 Matrix calcPositionConstraintMatrixPt(
const State&)
const;
328 Matrix calcPositionConstraintMatrixPNInv(
const State&)
const;
345 void calcConstraintForcesFromMultipliers(
const State&,
348 Vector& mobilityForces)
const;
353 Vector getVelocityErrorsAsVector(
const State&)
const;
359 Matrix calcVelocityConstraintMatrixV(
const State&)
const;
360 Matrix calcVelocityConstraintMatrixVt(
const State&)
const;
369 Vector getAccelerationErrorsAsVector(
const State&)
const;
371 const Vector& udot)
const;
390 void getConstraintForcesAsVectors
393 Vector& mobilityForces)
const;
400 getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
401 return bodyForcesInG;
409 getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
410 return mobilityForces;
436 Real calcPower(
const State& state)
const;
440 Matrix calcAccelerationConstraintMatrixA(
const State&)
const;
441 Matrix calcAccelerationConstraintMatrixAt(
const State&)
const;
447 void setIsConditional(
bool isConditional);
449 bool isConditional()
const;
487 class PointInPlaneImpl;
488 class PointOnLineImpl;
489 class ConstantAngleImpl;
490 class ConstantOrientationImpl;
492 class ConstantCoordinateImpl;
493 class ConstantSpeedImpl;
494 class ConstantAccelerationImpl;
496 class CoordinateCouplerImpl;
497 class SpeedCouplerImpl;
498 class PrescribedMotionImpl;
499 class PointOnPlaneContactImpl;
500 class SphereOnPlaneContactImpl;
501 class SphereOnSphereContactImpl;
502 class LineOnLineContactImpl;
534 Real getLineDisplayHalfLength()
const;
535 Real getPointDisplayRadius()
const;
546 const UnitVec3& getDefaultLineDirection()
const;
547 const Vec3& getDefaultPointOnLine()
const;
548 const Vec3& getDefaultFollowerPoint()
const;
552 const Vec3& getPointOnLine(
const State&)
const;
553 const Vec3& getFollowerPoint(
const State&)
const;
556 Vec2 getPositionErrors(
const State&)
const;
557 Vec2 getVelocityErrors(
const State&)
const;
560 Vec2 getAccelerationErrors(
const State&)
const;
562 const Vec2& getForceOnFollowerPoint(
const State&)
const;
615 Real getAxisDisplayLength()
const;
616 Real getAxisDisplayWidth()
const;
627 const UnitVec3& getDefaultBaseAxis()
const;
628 const UnitVec3& getDefaultFollowerAxis()
const;
629 Real getDefaultAngle()
const;
637 Real getPositionError(
const State&)
const;
638 Real getVelocityError(
const State&)
const;
641 Real getAccelerationError(
const State&)
const;
643 Real getTorqueOnFollowerBody(
const State&)
const;
691 const Rotation& getDefaultBaseRotation()
const;
692 const Rotation& getDefaultFollowerRotation()
const;
699 Vec3 getPositionErrors(
const State&)
const;
700 Vec3 getVelocityErrors(
const State&)
const;
703 Vec3 getAccelerationErrors(
const State&)
const;
705 Vec3 getTorqueOnFollowerBody(
const State&)
const;
748 void setContactPoint(
State& state,
const Vec3& point_C)
const;
753 void setDirection(
State& state,
const UnitVec3& direction_C)
const;
757 const Vec3& getContactPoint(
const State& state)
const;
774 Real getDirectionDisplayLength()
const;
777 Real getPointDisplayRadius()
const;
800 const UnitVec3& getDefaultDirection()
const;
803 const Vec3& getDefaultContactPoint()
const;
812 Real getVelocityError(
const State& state)
const;
820 Real getAccelerationError(
const State&)
const;
833 Real getForceAtContactPoint(
const State&)
const;
864 Real defaultPosition);
890 Real getDefaultPosition()
const;
903 void setPosition(
State& state,
Real position)
const;
908 Real getPosition(
const State& state)
const;
915 Real getPositionError(
const State& state)
const;
921 Real getVelocityError(
const State& state)
const;
928 Real getAccelerationError(
const State& state)
const;
935 Real getMultiplier(
const State& state)
const;
986 Real getDefaultSpeed()
const;
999 void setSpeed(
State& state,
Real speed)
const;
1004 Real getSpeed(
const State& state)
const;
1011 Real getVelocityError(
const State& state)
const;
1016 Real getAccelerationError(
const State& state)
const;
1023 Real getMultiplier(
const State& state)
const;
1053 Real defaultAcceleration);
1058 Real defaultAcceleration);
1078 Real getDefaultAcceleration()
const;
1091 void setAcceleration(
State& state,
Real accel)
const;
1096 Real getAcceleration(
const State& state)
const;
1103 Real getAccelerationError(
const State&)
const;
1156 class ImplementationImpl;
1173 const Implementation& getImplementation()
const;
1174 Implementation& updImplementation();
1184 #ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT 1186 Constraint::Custom::ImplementationImpl>;
1192 :
public PIMPLHandle<Implementation,ImplementationImpl> {
1229 void invalidateTopologyCache()
const;
1235 Implementation& setDefaultNumConstraintEquations(
int mp,
int mv,
int ma);
1248 ConstrainedBodyIndex addConstrainedBody(
const MobilizedBody&);
1256 ConstrainedMobilizerIndex addConstrainedMobilizer(
const MobilizedBody&);
1263 getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex)
const;
1270 getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex)
const;
1304 ConstrainedMobilizerIndex mobilizer,
1311 Real getOneQFromState(
const State& state,
1312 ConstrainedMobilizerIndex mobilizer,
1337 ConstrainedMobilizerIndex mobilizer,
1345 Real getOneQDotFromState(
const State& state,
1346 ConstrainedMobilizerIndex mobilizer,
1375 ConstrainedMobilizerIndex mobilizer,
1397 ConstrainedMobilizerIndex mobilizer,
1407 Real getOneUFromState(
const State& state,
1408 ConstrainedMobilizerIndex mobilizer,
1435 ConstrainedMobilizerIndex mobilizer,
1446 void addInOneMobilityForce
1447 (
const State& state,
1448 ConstrainedMobilizerIndex mobilizer,
1469 (
const State& state,
1470 ConstrainedMobilizerIndex mobilizer,
1494 ConstrainedBodyIndex bodyB)
const;
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(); }
1514 const Transform& getBodyTransformFromState
1515 (
const State& state, ConstrainedBodyIndex B)
const;
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(); }
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]; }
1553 (
const State& state, ConstrainedBodyIndex bodyB)
const;
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]; }
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]; }
1594 Vec3 findStationLocation
1596 ConstrainedBodyIndex bodyB,
1597 const Vec3& p_BS)
const 1605 Vec3 findStationLocationFromState
1607 ConstrainedBodyIndex bodyB,
1608 const Vec3& p_BS)
const 1610 const Transform& X_AB = getBodyTransformFromState(state,bodyB);
1620 Vec3 findStationVelocity
1623 ConstrainedBodyIndex bodyB,
1624 const Vec3& p_BS)
const 1626 const Rotation& R_AB = getBodyRotationFromState(state,bodyB);
1627 const Vec3 p_BS_A = R_AB * p_BS;
1629 return V_AB[1] + (V_AB[0] % p_BS_A);
1634 Vec3 findStationVelocityFromState
1636 ConstrainedBodyIndex bodyB,
1637 const Vec3& p_BS)
const 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);
1654 Vec3 findStationAcceleration
1657 ConstrainedBodyIndex bodyB,
1658 const Vec3& p_BS)
const 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);
1667 const Vec3 a_AS = A_AB[1] + (A_AB[0] % p_BS_A)
1668 + w_AB % (w_AB % p_BS_A);
1678 void addInStationForce
1679 (
const State& state,
1680 ConstrainedBodyIndex bodyB,
1682 const Vec3& forceInA,
1687 void addInBodyTorque
1688 (
const State& state,
1689 ConstrainedBodyIndex bodyB,
1690 const Vec3& torqueInA,
1700 void getMultipliers(
const State& state,
1805 virtual void calcPositionErrors
1806 (
const State& state,
1823 virtual void calcPositionDotErrors
1824 (
const State& state,
1842 virtual void calcPositionDotDotErrors
1843 (
const State& state,
1867 virtual void addInPositionConstraintForces
1868 (
const State& state,
1889 virtual void calcVelocityErrors
1890 (
const State& state,
1906 virtual void calcVelocityDotErrors
1907 (
const State& state,
1931 virtual void addInVelocityConstraintForces
1932 (
const State& state,
1956 virtual void calcAccelerationErrors
1957 (
const State& state,
1980 virtual void addInAccelerationConstraintForces
1981 (
const State& state,
1994 virtual void calcDecorativeGeometryAndAppend
1999 friend class Constraint::CustomImpl;
2045 const std::vector<MobilizedBodyIndex>& coordBody,
2046 const std::vector<MobilizerQIndex>& coordIndex)
2100 const std::vector<MobilizedBodyIndex>& speedBody,
2101 const std::vector<MobilizerUIndex>& speedIndex)
2146 const std::vector<MobilizedBodyIndex>& speedBody,
2147 const std::vector<MobilizerUIndex>& speedIndex,
2148 const std::vector<MobilizedBodyIndex>& coordBody,
2149 const std::vector<MobilizerQIndex>& coordIndex)
2204 #endif // SimTK_SIMBODY_CONSTRAINT_H_ virtual void realizeInstance(const State &) const
The Matter Subsystem's realizeInstance() method will call this method after all MobilizedBody Instanc...
Definition: Constraint.h:1744
Vector getConstrainedMobilityForcesAsVector(const State &state) const
For convenience, returns constrained mobility forces as the function return.
Definition: Constraint.h:406
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
virtual void realizeVelocity(const State &) const
The Matter Subsystem's realizeVelocity() method will call this method after any MobilizedBody Velocit...
Definition: Constraint.h:1767
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
virtual void realizeModel(State &) const
The Matter Subsystem's realizeModel() method will call this method after all MobilizedBody Model-stag...
Definition: Constraint.h:1737
ConstantAcceleration()
Default constructor creates an empty handle you can use to reference any existing ConstantAcceleratio...
Definition: Constraint.h:1062
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
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:66
virtual void realizePosition(const State &) const
The Matter Subsystem's realizePosition() method will call this method after any MobilizedBody Positio...
Definition: Constraint.h:1759
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...
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:606
virtual void realizeAcceleration(const State &) const
The Matter Subsystem's realizeAcceleration() method will call this method after any MobilizedBody Acc...
Definition: Constraint.h:1784
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
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
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:53
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
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
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
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:51
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 realizeTopology(State &) const
The Matter Subsystem's realizeTopology() method will call this method after all MobilizedBody topolog...
Definition: Constraint.h:1725
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:168
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
Vector_< SpatialVec > getConstrainedBodyForcesAsVector(const State &state) const
For convenience, returns constrained body forces as the function return.
Definition: Constraint.h:397
Custom()
Default constructor creates an empty handle.
Definition: Constraint.h:1169
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
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
Unique integer type for Subsystem-local multiplier indexing.
Constrain a single mobility to have a particular acceleration.
Definition: Constraint.h:1047
One non-holonomic constraint equation.
Definition: Constraint.h:728
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
virtual void realizeDynamics(const State &) const
The Matter Subsystem's realizeDynamics() method will call this method after any MobilizedBody Dynamic...
Definition: Constraint.h:1774
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