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>;
149 (ConstrainedBodyIndex consBodyIx)
const;
170 (ConstrainedMobilizerIndex consMobilizerIx)
const;
222 ConstrainedQIndex consQIndex)
const;
226 ConstrainedUIndex consUIndex)
const;
242 int& mp,
int& mv,
int& ma)
const;
294 Vector& constraintSpace)
const;
314 const Vector& constraintSpace,
348 Vector& mobilityForces)
const;
371 const Vector& udot)
const;
393 Vector& mobilityForces)
const;
400 getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
401 return bodyForcesInG;
409 getConstraintForcesAsVectors(state,bodyForcesInG,mobilityForces);
410 return mobilityForces;
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;
864 Real defaultPosition);
1053 Real defaultAcceleration);
1058 Real defaultAcceleration);
1156 class ImplementationImpl;
1184 #ifndef SimTK_SIMBODY_DEFINING_CONSTRAINT
1186 Constraint::Custom::ImplementationImpl>;
1192 :
public PIMPLHandle<Implementation,ImplementationImpl> {
1304 ConstrainedMobilizerIndex mobilizer,
1312 ConstrainedMobilizerIndex mobilizer,
1337 ConstrainedMobilizerIndex mobilizer,
1346 ConstrainedMobilizerIndex mobilizer,
1375 ConstrainedMobilizerIndex mobilizer,
1397 ConstrainedMobilizerIndex mobilizer,
1408 ConstrainedMobilizerIndex mobilizer,
1435 ConstrainedMobilizerIndex mobilizer,
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(); }
1506 ConstrainedBodyIndex bodyB)
const
1507 {
return getBodyTransform(allX_AB,bodyB).p(); }
1515 (
const State& state, ConstrainedBodyIndex B)
const;
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(); }
1533 ConstrainedBodyIndex bodyB)
const;
1538 ConstrainedBodyIndex bodyB)
const
1539 {
return getBodyVelocity(allV_AB,bodyB)[0]; }
1544 ConstrainedBodyIndex bodyB)
const
1545 {
return getBodyVelocity(allV_AB,bodyB)[1]; }
1553 (
const State& state, ConstrainedBodyIndex bodyB)
const;
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]; }
1572 ConstrainedBodyIndex bodyB)
const;
1577 ConstrainedBodyIndex bodyB)
const
1578 {
return getBodyAcceleration(allA_AB,bodyB)[0]; }
1583 ConstrainedBodyIndex bodyB)
const
1584 {
return getBodyAcceleration(allA_AB,bodyB)[1]; }
1596 ConstrainedBodyIndex bodyB,
1597 const Vec3& p_BS)
const
1606 (
const State& state,
1607 ConstrainedBodyIndex bodyB,
1608 const Vec3& p_BS)
const
1610 const Transform& X_AB = getBodyTransformFromState(state,bodyB);
1621 (
const State& state,
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);
1635 (
const State& state,
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);
1655 (
const State& state,
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);
1679 (
const State& state,
1680 ConstrainedBodyIndex bodyB,
1682 const Vec3& forceInA,
1688 (
const State& state,
1689 ConstrainedBodyIndex bodyB,
1690 const Vec3& torqueInA,
1806 (
const State& state,
1824 (
const State& state,
1843 (
const State& state,
1868 (
const State& state,
1890 (
const State& state,
1907 (
const State& state,
1932 (
const State& state,
1957 (
const State& state,
1981 (
const State& state,
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)
#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 getDefaultAngle() const
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
Real getAxisDisplayLength() 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
Real getAxisDisplayWidth() 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)
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
Real getLineDisplayHalfLength() const
PointOnLine()
Default constructor creates an empty handle.
Definition: Constraint.h:527
Vec2 getAccelerationErrors(const State &) const
Vec2 getMultipliers(const State &) const
Real getPointDisplayRadius() 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
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
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.
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