Simbody 3.7
Loading...
Searching...
No Matches
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
38namespace SimTK {
39
40class SimbodyMatterSubsystem;
41class SimbodyMatterSubtree;
42class MobilizedBody;
43class Constraint;
44class 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
68public:
74explicit Constraint(ConstraintImpl* r) : HandleBase(r) { }
75
79void disable(State&) const;
80
87void enable(State&) const;
90bool isDisabled(const State&) const;
95
100void setDisabledByDefault(bool shouldBeDisabled);
101
104operator ConstraintIndex() const {return getConstraintIndex();}
105
112
119
127
129bool isInSubsystem() const;
133bool 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
185int getNumConstrainedQ(const State&, ConstrainedMobilizerIndex) const;
190int getNumConstrainedU(const State&, ConstrainedMobilizerIndex) const;
191
198ConstrainedUIndex getConstrainedUIndex
199 (const State&, ConstrainedMobilizerIndex, MobilizerUIndex which) const;
206ConstrainedQIndex getConstrainedQIndex
207 (const State&, ConstrainedMobilizerIndex, MobilizerQIndex which) const;
208
211int getNumConstrainedQ(const State&) const;
212
217int 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 //
320Vector getPositionErrorsAsVector(const State&) const; // mp of these
322
323// Matrix P = partial(perr_dot)/partial(u). (just the holonomic constraints)
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 //
353Vector getVelocityErrorsAsVector(const State&) const; // mp+mv of these
354Vector 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.
361
362 // DYNAMICS STAGE //
363// nothing in base class currently
364
365 // ACCELERATION STAGE //
369Vector getAccelerationErrorsAsVector(const State&) const; // mp+mv+ma of these
370Vector calcAccelerationErrorFromUDot(const State&, // mp+mv+ma of these
371 const Vector& udot) const; // numParticipatingU udot's
372
376Vector 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
436Real calcPower(const State& state) const;
437
438// Matrix A = partial(aerr)/partial(udot) for just the acceleration-only
439// constraints.
442
447void setIsConditional(bool isConditional);
449bool 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.
456class Rod;
458
459class Ball;
462
463class Weld;
465
466class PointInPlane; // translations perpendicular to plane normal only
467class PointOnLine; // translations along a line only
468class ConstantAngle; // prevent rotation about common normal of two vectors
469class ConstantOrientation; // allows any translation but no rotation
470class NoSlip1D; // same velocity at a point along a direction
471class ConstantCoordinate; // prescribe generalized coordinate value
472class ConstantSpeed; // prescribe generalized speed value
473class ConstantAcceleration; // prescribe generalized acceleration value
474class Custom;
476class SpeedCoupler;
477class PrescribedMotion;
479class SphereOnPlaneContact; // ball in contact with plane (sliding or rolling)
480class SphereOnSphereContact; // ball in contact with ball (sliding or rolling)
481class LineOnLineContact; // edge/edge contact
482
483// Internal use only.
484class RodImpl;
485class BallImpl;
486class WeldImpl;
487class PointInPlaneImpl;
488class PointOnLineImpl;
489class ConstantAngleImpl;
490class ConstantOrientationImpl;
491class NoSlip1DImpl;
492class ConstantCoordinateImpl;
493class ConstantSpeedImpl;
494class ConstantAccelerationImpl;
495class CustomImpl;
496class CoordinateCouplerImpl;
497class SpeedCouplerImpl;
498class PrescribedMotionImpl;
499class PointOnPlaneContactImpl;
500class SphereOnPlaneContactImpl;
501class SphereOnSphereContactImpl;
502class LineOnLineContactImpl;
503
504};
505
507 // POINT ON LINE CONSTRAINT //
509
521public:
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
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
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
603public:
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
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
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{
673public:
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
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
729public:
730 // no default constructor
731
738 NoSlip1D(MobilizedBody& caseBodyC, const Vec3& P_C, const UnitVec3& n_C,
739 MobilizedBody& movingBody0, MobilizedBody& movingBody1);
740
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
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
859public:
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
960public:
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{
1049public:
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//==============================================================================
1154public:
1155 class Implementation;
1156 class ImplementationImpl;
1157
1165 explicit Custom(Implementation* implementation);
1166
1167
1170
1172protected:
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> {
1193public:
1194// No default constructor because you have to supply at least the
1195// SimbodyMatterSubsystem to which this Constraint belongs.
1196
1199virtual ~Implementation() { }
1200
1205virtual Implementation* clone() const = 0;
1206
1210Implementation(SimbodyMatterSubsystem&, int mp, int mv, int ma);
1211
1217
1220
1221 // Topological information//
1222
1230
1236
1241Implementation& setDisabledByDefault(bool shouldBeDisabled);
1242
1248ConstrainedBodyIndex addConstrainedBody(const MobilizedBody&);
1249
1256ConstrainedMobilizerIndex addConstrainedMobilizer(const MobilizedBody&);
1257
1263getMobilizedBodyIndexOfConstrainedBody(ConstrainedBodyIndex) const;
1264
1270getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex) const;
1271
1272
1302Real 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
1335Real 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
1374 const Array_<Real,ConstrainedQIndex>& constrainedQDotDot,
1375 ConstrainedMobilizerIndex mobilizer,
1376 MobilizerQIndex whichQ) const;
1377
1395Real 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
1433Real 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;
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(); }
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
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]; }
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
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]; }
1585
1586 // Calculate location, velocity, and acceleration for a given station.
1587
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;
1700void getMultipliers(const State& state,
1701 Array_<Real>& multipliers) const;
1704protected:
1725virtual void realizeTopology(State&) const { }
1726
1737virtual void realizeModel(State&) const { }
1738
1744virtual void realizeInstance(const State&) const { }
1745
1751virtual void realizeTime(const State&) const { }
1752
1759virtual void realizePosition(const State&) const { }
1760
1767virtual void realizeVelocity(const State&) const { }
1768
1774virtual void realizeDynamics(const State&) const { }
1775
1784virtual void realizeAcceleration(const State&) const { }
1785
1791virtual void realizeReport(const State&) const { }
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;
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
1999friend class Constraint::CustomImpl;
2000};
2001
2002
2003
2004//==============================================================================
2005// COORDINATE COUPLER
2006//==============================================================================
2016: public Constraint::Custom {
2017public:
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,
2051 }
2052
2055};
2056
2057
2058
2059//==============================================================================
2060// SPEED COUPLER
2061//==============================================================================
2073public:
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,
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,
2156 }
2157
2160};
2161
2162
2163
2164//==============================================================================
2165// PRESCRIBED MOTION
2166//==============================================================================
2173: public Constraint::Custom {
2174public:
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
ConstantAcceleration & setDefaultAcceleration(Real accel)
Change the default value for the acceleration to be enforced by this constraint.
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.
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)
const UnitVec3 & getDefaultFollowerAxis() const
Real getAngle(const State &) const
Real getTorqueOnFollowerBody(const State &) const
ConstantAngle & setDefaultBaseAxis(const UnitVec3 &)
ConstantAngle & setAxisDisplayWidth(Real)
ConstantAngle & setAxisDisplayLength(Real)
MobilizedBodyIndex getBaseMobilizedBodyIndex() const
const UnitVec3 & getBaseAxis(const State &) const
Real getMultiplier(const State &) const
MobilizedBodyIndex getFollowerMobilizedBodyIndex() const
Real getPositionError(const State &) const
Real getVelocityError(const State &) const
ConstantAngle()
Default constructor creates an empty handle.
Definition Constraint.h:610
ConstantAngle & setDefaultFollowerAxis(const UnitVec3 &)
const UnitVec3 & getDefaultBaseAxis() const
ConstantAngle & setDefaultAngle(Real)
const UnitVec3 & getFollowerAxis(const State &) 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.
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.
ConstantCoordinate & setDefaultPosition(Real position)
Change the default value for the position to be enforced by this constraint.
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 & getFollowerRotation(const State &) const
const Rotation & getDefaultBaseRotation() const
const Rotation & getBaseRotation(const State &) const
Vec3 getPositionErrors(const State &) const
ConstantOrientation & setDefaultFollowerRotation(const Rotation &)
Vec3 getVelocityErrors(const State &) const
MobilizedBodyIndex getBaseMobilizedBodyIndex() const
ConstantOrientation(MobilizedBody &baseBody_B, const Rotation &defaultRB, MobilizedBody &followerBody_F, const Rotation &defaultRF)
ConstantOrientation & setDefaultBaseRotation(const Rotation &)
MobilizedBodyIndex getFollowerMobilizedBodyIndex() const
ConstantOrientation()
Default constructor creates an empty handle.
Definition Constraint.h:679
const Rotation & getDefaultFollowerRotation() const
Vec3 getAccelerationErrors(const State &) const
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...
ConstantSpeed & setDefaultSpeed(Real speed)
Change the default value for the speed to be enforced by this constraint.
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(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 & getBodyOriginVelocityFromState(const State &state, ConstrainedBodyIndex bodyB) const
Convenient inline interface to getBodyVelocityFromState() that returns just the linear velocity vecto...
Definition Constraint.h:1562
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 & 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 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
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
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...
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 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...
const SimbodyMatterSubsystem & getMatterSubsystem() const
Return a reference to the matter subsystem containing this constraint.
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 Implementation * clone() const =0
This method should produce a deep copy identical to the concrete derived Implementation object underl...
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 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
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...
virtual void realizeInstance(const State &) const
The Matter Subsystem's realizeInstance() method will call this method after all MobilizedBody Instanc...
Definition Constraint.h:1744
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
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...
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
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
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...
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
Implementation & setDisabledByDefault(bool shouldBeDisabled)
Normally Constraints are enabled when defined and can be disabled later.
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...
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
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
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
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 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 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 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
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
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...
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...
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...
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...
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 & 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
ConstrainedBodyIndex addConstrainedBody(const MobilizedBody &)
Call this during construction phase to add a body to the topological structure of this Constraint.
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 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...
MobilizedBodyIndex getMobilizedBodyIndexOfConstrainedMobilizer(ConstrainedMobilizerIndex) const
Map a constrained mobilizer for this constraint to the mobilized body to which it corresponds in the ...
The handle class Constraint::Custom (dataless) and its companion class Constraint::Custom::Implementa...
Definition Constraint.h:1153
const Implementation & getImplementation() const
Custom()
Default constructor creates an empty handle.
Definition Constraint.h:1169
Custom(Implementation *implementation)
Create a Custom Constraint.
Implementation & updImplementation()
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.
const UnitVec3 & getDirection(const State &state) const
Return from the given state the no-slip direction, in the Case body frame.
const UnitVec3 & getDefaultDirection() const
Obtain the default value for the no-slip direction, expressed in the Case body frame.
const Vec3 & getContactPoint(const State &state) const
Return from the given state the contact point, in the Case body frame.
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
Real getMultiplier(const State &) const
Get the Lagrange multiplier for this constraint equation, using configuration, velocity,...
Real getAccelerationError(const State &) const
Get the acceleration error for this constraint equation, using configuration, velocity,...
NoSlip1D & setDefaultContactPoint(const Vec3 &)
Change the default contact point; this is the initial value for for the actual contact point 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.
Real getVelocityError(const State &state) const
Get the velocity error for this constraint equation, using configuration and velocity information fro...
NoSlip1D & setDefaultDirection(const UnitVec3 &)
Change the default no-slip direction; this is the initial value for for the actual direction and is a...
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.
const Vec3 & getDefaultContactPoint() const
Obtain the default value for the contact point, in the Case body frame.
NoSlip1D & setDirectionDisplayLength(Real)
For visualization only, set the length of the line used to show the no-slip direction.
One constraint equation.
Definition Constraint_PointInPlane.h:47
Two constraint equations.
Definition Constraint.h:520
const UnitVec3 & getLineDirection(const State &) const
PointOnLine & setDefaultFollowerPoint(const Vec3 &)
Vec2 getPositionErrors(const State &) const
Vec2 getVelocityErrors(const State &) const
const Vec3 & getFollowerPoint(const State &) const
MobilizedBodyIndex getLineMobilizedBodyIndex() const
const UnitVec3 & getDefaultLineDirection() const
PointOnLine & setLineDisplayHalfLength(Real)
PointOnLine()
Default constructor creates an empty handle.
Definition Constraint.h:527
Vec2 getAccelerationErrors(const State &) const
PointOnLine & setPointDisplayRadius(Real)
Vec2 getMultipliers(const State &) const
const Vec3 & getDefaultFollowerPoint() const
PointOnLine & setDefaultLineDirection(const UnitVec3 &)
PointOnLine(MobilizedBody &lineBody_B, const UnitVec3 &defaultLineDirection_B, const Vec3 &defaultPointOnLine_B, MobilizedBody &followerBody_F, const Vec3 &defaultFollowerPoint_F)
const Vec3 & getPointOnLine(const State &) const
const Vec3 & getDefaultPointOnLine() const
PointOnLine & setDefaultPointOnLine(const Vec3 &)
MobilizedBodyIndex getFollowerMobilizedBodyIndex() const
const Vec2 & getForceOnFollowerPoint(const State &) 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.
SimbodyMatterSubsystem & updMatterSubsystem()
Assuming you have writable access to this Constraint, get a writable reference to the containing matt...
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.
const MobilizedBody & getMobilizedBodyFromConstrainedBody(ConstrainedBodyIndex consBodyIx) const
Return a const reference to the actual MobilizedBody corresponding to one of the Constrained Bodies i...
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...
const SimbodyMatterSubsystem & getMatterSubsystem() const
Get a const reference to the matter subsystem that contains this Constraint.
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...
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:460
Vector getMultipliersAsVector(const State &) const
Get a Vector containing the Lagrange multipliers.
Vector_< SpatialVec > getConstrainedBodyForcesAsVector(const State &state) const
For convenience, returns constrained body forces as the function return.
Definition Constraint.h:397
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.
const MobilizedBody & getMobilizedBodyFromConstrainedMobilizer(ConstrainedMobilizerIndex consMobilizerIx) const
Return a const reference to the actual MobilizedBody corresponding to one of the Constrained Mobilize...
Weld CoincidentFrames
Definition Constraint.h:464
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:457
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 ...
const SimbodyMatterSubtree & getSubtree() const
Return a subtree object indicating which parts of the multibody tree are potentially affected by this...
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 MobilizedBody & getAncestorMobilizedBody() const
Return a const reference to the actual MobilizedBody which is serving as the Ancestor body for the co...
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.
This class provides some infrastructure useful in making SimTK Private Implementation (PIMPL) classes...
Definition PrivateImplementation.h:106
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:606