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_