1 #ifndef SimTK_SIMBODY_MOBILIZED_BODY_H_
2 #define SimTK_SIMBODY_MOBILIZED_BODY_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-13 Stanford University and the Authors. *
13 * Authors: Michael Sherman *
14 * Contributors: Paul Mitiguy, Peter Eastman *
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
27 /** @file
28 This defines the MobilizedBody class, which associates a new body (the
29 "child", "outboard", or "successor" body) with a mobilizer and a reference
30 frame on an existing body (the "parent", "inboard", or "predecessor" body)
31 that is already part of a SimbodyMatterSubsystem.
32
33 MobilizedBody is an abstract base class handle, with concrete classes
34 defined for each kind of mobilizer. There are a set of built-in mobilizers
35 and a generic "Custom" mobilizer (an actual abstract base class) from
36 which advanced users may derive their own mobilizers.
37 **/
38
39 #include "SimTKmath.h"
40 #include "simbody/internal/common.h"
41 #include "simbody/internal/Body.h"
42 #include "simbody/internal/Motion.h"
43
44 #include <cassert>
45
46 namespace SimTK {
47
48 class SimbodyMatterSubsystem;
49 class Motion;
50 class MobilizedBody;
51 class MobilizedBodyImpl;
52
53 // We only want the template instantiation to occur once. This symbol is
54 // defined in the SimTK core compilation unit that instantiates the mobilized
55 // body class but should not be defined any other time.
56 #ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
57 extern template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
58 #endif
59
60 /** %Mobod is the approved abbreviation for MobilizedBody.\ Feel free to
61 use it if you get tired of typing or seeing the full name.
62 @relates SimTK::MobilizedBody **/
63 typedef MobilizedBody Mobod;
64
65
66
67 //==============================================================================
68 // MOBILIZED BODY
69 //==============================================================================
70 /** A %MobilizedBody is Simbody's fundamental body-and-joint object used to
71 parameterize a system's motion by constructing a multibody tree containing
72 each body and its unique *mobilizer* (internal coordinate joint). A
73 %MobilizedBody connects a new body (the "child", "outboard", or "successor"
74 body) with a mobilizer and a reference frame to an existing %MobilizedBody (the
75 "parent", "inboard", or "predecessor" body) that is already part of a
76 SimbodyMatterSubsystem. In the topology of the multibody tree, the parent
77 mobilized body is always closer to Ground ("inboard") than is the child.
78
79 This is the base class for all %MobilizedBody classes, which include a body
80 and a particular kind of mobilizer. Each built-in %MobilizedBody type is a local
81 subclass within %MobilizedBody, so the built-ins have names like
82 MobilizedBody::Pin. All concrete %MobilizedBody classes, including the
83 built-ins, are derived from MobilizedBody. There is a MobilizedBody::Custom
84 class available for defining your own mobilizer types.
85
86 Normally, a %MobilizedBody's motion is unknown and Simbody calculates it using
87 forward dynamics where the motion results from external or internal forces.
88 However, a %MobilizedBody may be locked or controlled by an associated Motion
89 object which defines how it is to move; those are inverse dynamics conditions
90 in which the motion is known but the generalized forces required to produce that
91 motion are to be determined. In general a system may contain a mix of forward-
92 and inverse-dynamics mobilizers, and mobilizers may be switched between forward
93 and inverse dynamics during simulations.
94
95 The %MobilizedBody class supports a large number of methods useful for
96 obtaining and modifying body-specific and mobilizer-specific information and
97 for performing useful computations. These are organized in three sets:
98 - %State Access
99 - Basic Operators
100 - High Level Operators
101
102 <em>%State Access</em> methods simply extract already-calculated data from the
103 State or %State Cache, or set state values. They involve no additional
104 computation, have names beginning with "get" and "upd" (update) and return
105 references to the requested quantities rather than calculated values. We
106 divide these into routines which deal with bodies and routines which deal
107 with mobilizers and mobilities.
108
109 <em>Basic Operators</em> use State Access methods to compute basic quantities
110 which cannot be precomputed, such as the velocity of an arbitrary point,
111 using an inline combination of basic floating point operations which can be
112 reliably determined at compile time. These have names beginning with "find"
113 or a more specific verb, as a reminder that they do not require a great deal
114 of computation.
115
116 <em>High Level Operators</em> combine state access and basic operators with
117 run-time tests to calculate more complex quantities, with more complicated
118 implementations that can exploit special cases at run time. These begin with
119 "calc" (calculate) as a reminder that they may involve substantial run time
120 computation.
121
122 There is also a set of methods used for construction, and miscellaneous
123 utilities.
124
125 <h3>Mobilizer Terminology and Notation</h3>
126
127 Refer to the figure below for the terminology we use when discussing mobilizers
128 and mobilized bodies.
129
130 @image html MobilizerTerminology.png "Terminology and notation for mobilized bodies"
131
132 The figure shows the coordinate frames used in describing the mobility of
133 %MobilizedBody B with respect to its inboard parent body P. Everything blue
134 in the figure is associated with B. The origin point O of each frame is labeled.
135 A new %MobilizedBody with body frame B is added to the multibody tree by
136 choosing a parent body P that is already present in the tree. There are two
137 frames associated with the mobilizer: the "fixed" frame F that is attached to
138 the parent, and the "moving" frame M that is attached to the new body B. Frame
139 F is specified by giving its transform X_PF relative to the P frame. Frame M is
140 specified by giving its transform X_BM relative to the B frame. At run time
141 the transform X_FM between the two mobilizer frames represents translation and
142 rotation of the mobilizer. That motion is parameterized via generalized
143 coordinates q and generalized speeds u, the specific meaning of which is a
144 unique property of each type of mobilizer.
145
146 In the API below, we'll refer to the current ("this") %MobilizedBody as
147 "body B". It is the "object" or "main" body with which we are concerned. Often
148 there will be another body mentioned in the argument list as a target for some
149 conversion. That "another" body will be called "body A". The Ground body is
150 abbreviated "G".
151
152 We use Fo to mean "the origin of frame F", Bc is "the mass center of body B".
153 R_AF is the rotation matrix giving frame F's orientation in frame A, such that a
154 vector v expressed in F is reexpressed in A by v_A = R_AF*v_F. X_AF is the
155 spatial transform giving frame F's orientation and origin location in frame A,
156 such that a point P whose location is measured from F's origin Fo and expressed
157 in F by position vector p_FP (or more explicitly p_FoP) is remeasured from frame
158 A's origin Ao and reexpressed in A via p_AP = X_AF*p_FP, where p_AP==p_AoP.
159
160 <h3>Theory</h3>
161 For the mathematical and computational theory behind Simbody's mobilizers, see
162 - the <a href="https://simtk.org/docman/view.php/47/1536/Seth-2010-ShermanEastmanDelp-MinimalJointFormulationForBiomechanisms-NonlinearDyn-v62-p291.pdf">
163 paper</a> Seth, A.; Sherman, M.A.; Eastman, P.; Delp, S.L. "Minimal formulation
164 of joint motion for biomechanisms" Nonlinear Dynamics 62:291-303 (2010), or
165 - the Simbody Theory Manual.
166 **/
167
168 class SimTK_SIMBODY_EXPORT MobilizedBody
169 : public PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true> {
170 public:
171
172 /** Constructors can take an argument of this type to indicate that the
173 mobilizer is being defined in the reverse direction, meaning from the outboard
174 (child) body to the inboard (parent) body. That means that the mobilizer
175 coordinates and speeds will be defined as though the tree had been built in the
176 opposite direction. It does not actually affect which body is the inboard one
177 and which is the outboard; it just affects the definitions of the generalized
178 coordinates q and speeds u that parameterize the motion of the outboard body
179 with respect to the inboard one. This is a topological setting and can't be
180 changed dynamically. **/
181 enum Direction {
182 Forward = 0, ///< Use default definitions for q and u (inboard to outboard).
183 Reverse = 1 ///< Define q and u in the reverse order (outboard to inboard).
184 };
185
186 //------------------------------------------------------------------------------
187 /**@name Mobilizer locking and unlocking
188
189 Every %MobilizedBody object supports locking and unlocking of the mobilizer it
190 contains. You can lock the mobilizer's position, velocity, or acceleration. In
191 all cases the generalized accelerations udot of a locked mobilizer are
192 prescribed. If you lock just the accelerations, then velocity and position
193 remain free. If you lock velocity, then the generalized speeds u are prescribed
194 to specified values, accelerations udot are prescribed to zero, and positions
195 will remain free. If you lock position (the default locking level) then the
196 generalized coordinates q are prescribed to specified values and the speeds u
197 and accelerations udot are prescribed to zero. Prescribed values may be obtained
198 from the current state, or set explicitly. The lock() and lockAt() methods when
199 called at position level will modify q in the given state if necessary to
200 satisfy the locked position, and will set u to zero. When called at velocity
201 level they will modify u if necessary to satisfied the locked velocity, but will
202 leave q unchanged.
203
204 You can also specify that a mobilizer is locked by default (at acceleration,
205 velocity, or position level). In that case the prescribed value is recorded
206 when the state is realized to Stage::Model, using values taken from the
207 state at that time.
208
209 If this mobilizer is driven by a Motion object, locking overrides that while
210 the lock is active; when unlocked the Motion object resumes control. **/
211 /**@{**/
212 /** Lock this mobilizer's position or velocity at its current value, or lock the
213 acceleration to zero, depending on the \p level parameter. The prescribed
214 position or velocity is taken from \p state and recorded. If the lock is at
215 the position (q) level then the generalized speeds u for this mobilizer are set
216 to zero in the \p state. If the mobilizer was already locked, the new lock
217 specification overrides it. The \p state must already have been realized to at
218 least Stage::Model and will be lowered to Stage::Model on return since a lock is
219 an Instance-stage change. **/
220 void lock(State& state, Motion::Level level=Motion::Position) const;
221
222 /** Lock this mobilizer's q, u, or udot to the given scalar \p value, depending
223 on \p level. When locking at the position level (the default), this mobilizer's
224 q in \p state is set to \p value, and u in \p state is set to zero. When locking
225 at velocity level, this mobilizer's u in \p state is set to \p value but the q
226 is left unchanged. This signature of lockAt(), taking \p value as a scalar, is
227 only permitted for mobilizers that have just a single q and u, such as a Pin or
228 Slider mobilizer. See the other signatures of lockAt() for multi-coordinate
229 mobilizers. **/
230 void lockAt(State& state, Real value,
231 Motion::Level level=Motion::Position) const;
232
233 /** Lock this mobilizer's q, u, or udot to the given Vector \p value, depending
234 on \p level. When locking at the position level (the default), this mobilizer's
235 q's in \p state is set to \p value, and its u's in \p state are set to zero.
236 When locking at velocity level, this mobilizer's u's in \p state are set to
237 \p value but its q's are left unchanged. The Vector must be the expected length,
238 either nq=getNumQ() for the default Motion::Position level, or nu=getNumU() for
239 Motion::Velocity or Motion::Acceleration levels.
240 @see getNumQ(), getNumU() **/
241 void lockAt(State& state, const Vector& value,
242 Motion::Level level=Motion::Position) const;
243
244 /** Lock this mobilizer's q, u, or udot to the given Vec\<N\> \p value,
245 depending on the \p level. When locking at the position level (the default),
246 this mobilizer's q's in \p state is set to \p value, and its u's in \p state are
247 set to zero. When locking at velocity level, this mobilizer's u's in \p state
248 are set to \p value but its q's are left unchanged. The size N of the given Vec
249 must match the expected length, either nq=getNumQ() for the default
250 Motion::Position level, or nu=getNumU() for Motion::Velocity or
251 Motion::Acceleration levels.
252 @see getNumQ(), getNumU() **/
253 template <int N> SimTK_SIMBODY_EXPORT // instantiated in library
254 void lockAt(State& state, const Vec<N>& value,
255 Motion::Level level=Motion::Position) const;
256
257 /** Unlock this mobilizer, returning it to its normal behavior which may be
258 free motion or may be controlled by a Motion object. If the mobilizer is
259 already unlocked nothing happens. **/
260 void unlock(State& state) const;
261
262 /** Check whether this mobilizer is currently locked in the given \p state. **/
isLocked(const State & state)263 bool isLocked(const State& state) const
264 { return getLockLevel(state)!=Motion::NoLevel; }
265
266 /** Returns the lock level if the mobilizer is locked in the given \p state,
267 otherwise Motion::NoLevel. **/
268 Motion::Level getLockLevel(const State& state) const;
269
270 /** Return the q, u, or udot value at which this mobilizer is locked, depending
271 on the lock level, as a Vector of the appropriate length. If the mobilizer is
272 not currently locked, a zero-length Vector is returned. **/
273 Vector getLockValueAsVector(const State& state) const;
274
275 /** Change whether this mobilizer is initially locked. On construction the
276 mobilizer is unlocked by default; you can change that here. To respecify that
277 the motion is unlocked, use Motion::NoLevel as the \p level. This is a
278 topological change; you'll have to call realizeTopology() again and get a new
279 State if you call this method. If the default lock is at the position or
280 velocity level, the required q or u value is recorded at the time a \p state is
281 realized to Stage::Model. A default Motion::Acceleration lock always prescribes
282 the accelerations udot to zero. **/
283 MobilizedBody& lockByDefault(Motion::Level level=Motion::Position);
284
285 /** Check whether this mobilizer is to be locked in the default state. **/
isLockedByDefault()286 bool isLockedByDefault() const
287 { return getLockByDefaultLevel()!=Motion::NoLevel; }
288
289 /** Returns the level at which the mobilizer is locked by default, if it is
290 locked by default, otherwise Motion::NoLevel. **/
291 Motion::Level getLockByDefaultLevel() const;
292 /**@}**/
293
294 //////////////////////////
295 // STATE ACCESS METHODS //
296 //////////////////////////
297
298 //------------------------------------------------------------------------------
299 /** @name State Access - Bodies
300 These methods extract already-computed information from the State or State
301 cache, or set values in the State. **/
302 /**@{**/
303
304 /** Extract from the state cache the already-calculated spatial configuration
305 X_GB of body B's body frame, measured with respect to the Ground frame and
306 expressed in the Ground frame. That is, we return the location of the body
307 frame's origin, and the orientation of its x, y, and z axes, as the Transform
308 X_GB. This notation is intended to convey unambiguously the sense of this
309 transform, which is as follows: if you have a station (body fixed point) S on
310 body B, represented by position vector p_BS (a.k.a. p_BoS) from the origin Bo
311 of B to the point S and expressed in the B frame, then p_GS=X_GB*p_BS where
312 p_GS (== p_GoS) is the position vector from the Ground origin Go to the point in
313 space currently coincident with S and expressed in the Ground frame. The inverse
314 transformation is obtained using the "~" operator where ~X_GB=X_BG, so that
315 p_BS = ~X_GB*p_GS. This response is available at Position stage. **/
316 const Transform& getBodyTransform(const State& state) const; // X_GB
317
318 /** Extract from the state cache the already-calculated spatial orientation R_GB
319 of body B's body frame x, y, and z axes expressed in the Ground frame, as the
320 Rotation matrix R_GB. The sense of this rotation matrix is such that if you have
321 a vector v fixed on body B, represented by the vector v_B expressed in the B
322 frame, then v_G=R_GB*v_B where v_G is the same vector but re-expressed in the
323 Ground frame. The inverse transformation is obtained using the "~" operator
324 where ~R_GB=R_BG, so that v_B = ~R_GB*v_G. This response is available at
325 Position stage. **/
getBodyRotation(const State & state)326 const Rotation& getBodyRotation(const State& state) const {
327 return getBodyTransform(state).R();
328 }
329 /** Extract from the state cache the already-calculated spatial location of body
330 B's body frame origin Bo, measured from the Ground origin Go and expressed in
331 the Ground frame, as the position vector p_GB (== p_GoBo). This response is
332 available at Position stage. **/
getBodyOriginLocation(const State & state)333 const Vec3& getBodyOriginLocation(const State& state) const {
334 return getBodyTransform(state).p();
335 }
336
337 /** At stage Position or higher, return the cross-mobilizer transform X_FM, the
338 body's inboard mobilizer frame M measured and expressed in the parent body's
339 corresponding outboard frame F. **/
340 const Transform& getMobilizerTransform(const State& state) const; // X_FM
341
342 /** Extract from the state cache the already-calculated spatial velocity V_GB of
343 this body's reference frame B, measured with respect to the Ground frame and
344 expressed in the Ground frame. That is, we return the linear velocity v_GB of
345 the body frame's origin in G, and the body's angular velocity w_GB as the
346 spatial velocity vector V_GB = {w_GB, v_GB}. This response is available at
347 Velocity stage. **/
348 const SpatialVec& getBodyVelocity(const State& state) const; // V_GB
349
350 /** Extract from the state cache the already-calculated inertial angular
351 velocity vector w_GB of this body B, measured with respect to the Ground frame
352 and expressed in the Ground frame. This response is available at Velocity
353 stage. **/
getBodyAngularVelocity(const State & state)354 const Vec3& getBodyAngularVelocity(const State& state) const { // w_GB
355 return getBodyVelocity(state)[0];
356 }
357 /** Extract from the state cache the already-calculated inertial linear velocity
358 vector v_GB (more explicitly, v_GBo) of this body B's origin point Bo, measured
359 with respect to the Ground frame and expressed in the Ground frame. This
360 response is available at Velocity stage. **/
getBodyOriginVelocity(const State & state)361 const Vec3& getBodyOriginVelocity(const State& state) const { // v_GB
362 return getBodyVelocity(state)[1];
363 }
364
365 /** At stage Velocity or higher, return the cross-mobilizer velocity V_FM, the
366 relative velocity of this body's "moving" mobilizer frame M in the parent body's
367 corresponding "fixed" frame F, measured and expressed in F. Note that this isn't
368 the usual spatial velocity since it isn't expressed in G. **/
369 const SpatialVec& getMobilizerVelocity(const State& state) const; // V_FM
370
371 /** Extract from the state cache the already-calculated spatial acceleration
372 A_GB of this body's reference frame B, measured with respect to the Ground frame
373 and expressed in the Ground frame. That is, we return the linear acceleration
374 a_GB of the body frame's origin in G, and the body's angular acceleration b_GB
375 as the spatial acceleration vector A_GB = {b_GB, a_GB}. This response is
376 available at Acceleration stage. **/
377 const SpatialVec& getBodyAcceleration(const State& state) const; // A_GB
378
379 /** Extract from the state cache the already-calculated inertial angular
380 acceleration vector b_GB of this body B, measured with respect to the Ground
381 frame and expressed in the Ground frame. This response is available at
382 Acceleration stage. **/
getBodyAngularAcceleration(const State & state)383 const Vec3& getBodyAngularAcceleration(const State& state) const { // b_GB
384 return getBodyAcceleration(state)[0];
385 }
386
387 /** Extract from the state cache the already-calculated inertial linear
388 acceleration vector a_GB (more explicitly, a_GBo) of this body B's origin point
389 Bo, measured with respect to the Ground frame and expressed in the Ground frame.
390 This response is available at Acceleration stage. **/
getBodyOriginAcceleration(const State & state)391 const Vec3& getBodyOriginAcceleration(const State& state) const { // a_GB
392 return getBodyAcceleration(state)[1];
393 }
394
395 /** TODO: Not implemented yet -- any volunteers? At stage Acceleration, return
396 the cross-mobilizer acceleration A_FM, the relative acceleration of body B's
397 "moving" mobilizer frame M in the parent body's corresponding "fixed" frame F,
398 measured and expressed in F. Note that this isn't the usual spatial acceleration
399 since it isn't expressed in G. **/
getMobilizerAcceleration(const State & state)400 const SpatialVec& getMobilizerAcceleration(const State& state) const { // A_FM
401 SimTK_ASSERT_ALWAYS(!"unimplemented method",
402 "MobilizedBody::getMobilizerAcceleration() not yet implemented -- volunteers?");
403 return *(new SpatialVec());
404 }
405
406 /** Return a reference to this body's mass properties in the State cache. The
407 State must have been realized to Stage::Instance or higher. **/
408 const MassProperties& getBodyMassProperties(const State& state) const;
409
410 /** Return a reference to the already-calculated SpatialInertia of this body,
411 taken about the body's origin (\e not its mass center), and expressed in the
412 Ground frame. The State must have been realized to Stage::Position or higher. **/
413 const SpatialInertia& getBodySpatialInertiaInGround(const State& state) const;
414
415 /** Return the mass of this body. The State must have been realized to
416 Stage::Instance. **/
getBodyMass(const State & state)417 Real getBodyMass(const State& state) const {
418 return getBodyMassProperties(state).getMass();
419 }
420
421 /** Return this body's center of mass station (i.e., the vector fixed in the
422 body, going from body origin to body mass center, expressed in the body frame.)
423 The State must have been realized to Stage::Instance or higher. **/
getBodyMassCenterStation(const State & state)424 const Vec3& getBodyMassCenterStation(const State& state) const {
425 return getBodyMassProperties(state).getMassCenter();
426 }
427
428 /** Return a reference to this body's unit inertia matrix in the State cache,
429 taken about the body origin and expressed in the body frame. The State must have
430 been realized to Stage::Instance or higher. **/
getBodyUnitInertiaAboutBodyOrigin(const State & state)431 const UnitInertia& getBodyUnitInertiaAboutBodyOrigin(const State& state) const {
432 return getBodyMassProperties(state).getUnitInertia();
433 }
434
435 /** Return a reference to this mobilizer's frame F fixed on the parent body P,
436 as the fixed Transform from P's body frame to the frame F fixed to P. If this
437 frame is changeable, the result comes from the State cache, otherwise it is from
438 the MobilizedBody object itself. The State must have been realized to
439 Stage::Instance or higher. **/
440 const Transform& getInboardFrame (const State& state) const; // X_PF
441 /** Return a reference to this MobilizedBody's mobilizer frame M, as the fixed
442 Transform from this body B's frame to the frame M fixed on B. If this frame is
443 changeable, the result comes from the State cache, otherwise it is from the
444 MobilizedBody object itself. The State must have been realized to
445 Stage::Instance or higher. **/
446 const Transform& getOutboardFrame(const State& state) const; // X_BM
447
448 /** TODO: not implemented yet. Set the location and orientation of the inboard
449 (parent) mobilizer frame F, fixed to this mobilizer's parent body P.
450 @see setDefaultInboardFrame() **/
451 void setInboardFrame (State& state, const Transform& X_PF) const;
452 /** TODO: not implemented yet. Set the location and orientation of the outboard
453 mobilizer frame M, fixed to this body B.
454 @see setDefaultOutboardFrame() **/
455 void setOutboardFrame(State& state, const Transform& X_BM) const;
456
457 // End of State Access - Bodies
458 /**@}**/
459
460
461 //------------------------------------------------------------------------------
462 /** @name State Access - Mobilizer generalized coordinates q and speeds u
463 These methods extract q- or u-related information from the State or State cache,
464 or set q or u values in the State. **/
465 /**@{**/
466 /** Return the number of generalized coordinates q currently in use by this
467 mobilizer. State must have been realized to Stage::Model. **/
468 int getNumQ(const State& state) const;
469 /** Return the number of generalized speeds u currently in use by this
470 mobilizer. State must have been realized to Stage::Model. **/
471 int getNumU(const State& state) const;
472
473 /** Return the global QIndex of the first q for this mobilizer; all the q's
474 range from getFirstQIndex() to QIndex(getFirstQIndex()+getNumQ()-1). **/
475 QIndex getFirstQIndex(const State& state) const;
476
477 /** Return the global UIndex of the first u for this mobilizer; all the u's
478 range from getFirstUIndex() to UIndex(getFirstUIndex()+getNumU()-1). **/
479 UIndex getFirstUIndex(const State& state) const;
480
481 /** Determine how generalized coordinate q values are being determined.
482 @param[in] state Must be realized to Instance stage. **/
483 Motion::Method getQMotionMethod(const State& state) const;
484 /** Determine how generalized speed u values are being determined.
485 @param[in] state Must be realized to Instance stage. **/
486 Motion::Method getUMotionMethod(const State& state) const;
487 /** Determine how generalized acceleration udot values are being determined.
488 @param[in] state Must be realized to Instance stage. **/
489 Motion::Method getUDotMotionMethod(const State& state) const;
490
491 /** Return one of the generalized coordinates q from this mobilizer's partition
492 of the matter subsystem's full q vector in the State. The particular coordinate
493 is selected using the \p which parameter, numbering from zero to
494 getNumQ()-1. **/
495 Real getOneQ(const State& state, int which) const;
496
497 /** Return one of the generalized speeds u from this mobilizer's partition of
498 the matter subsystem's full u vector in the State. The particular coordinate is
499 selected using the \p which parameter, numbering from zero to getNumU()-1. **/
500 Real getOneU(const State& state, int which) const;
501
502 /** Return as a Vector of length getNumQ() all the generalized coordinates q
503 currently in use by this mobilizer, from this mobilizer's partion in the matter
504 subsystem's full q vector in the State. **/
505 Vector getQAsVector(const State& state) const;
506 /** Return as a Vector of length getNumU() all the generalized speeds u
507 currently in use by this mobilizer, from this mobilizer's partion in the matter
508 subsystem's full u vector in the State. **/
509 Vector getUAsVector(const State& state) const;
510
511 /** Return one of the generalized coordinate derivatives qdot from this
512 mobilizer's partition of the matter subsystem's full qdot vector in the State
513 cache. The particular coordinate is selected using the \p which parameter,
514 numbering from zero to getNumQ()-1. **/
515 Real getOneQDot (const State& state, int which) const;
516 /** Return as a Vector of length getNumQ() all the generalized coordinate
517 derivatives qdot currently in use by this mobilizer, from this mobilizer's
518 partition in the matter subsystem's full qdot vector in the State cache. **/
519 Vector getQDotAsVector(const State& state) const;
520
521 /** Return one of the generalized accelerations udot from this mobilizer's
522 partition of the matter subsystem's full udot vector in the State cache. The
523 particular coordinate is selected using the \p which parameter, numbering from
524 zero to getNumU()-1. **/
525 Real getOneUDot(const State& state, int which) const;
526 /** Return one of the generalized coordinate second derivatives qdotdot from
527 this mobilizer's partition of the matter subsystem's full qdotdot vector in the
528 State cache. The particular coordinate is selected using the \p which parameter,
529 numbering from zero to getNumQ()-1. **/
530 Real getOneQDotDot(const State& state, int which) const;
531 /** Return as a Vector of length getNumU() all the generalized accelerations
532 udot currently in use by this mobilizer, from this mobilizer's partion in the
533 matter subsystem's full udot vector in the State cache. **/
534 Vector getUDotAsVector(const State& state) const;
535 /** Return as a Vector of length getNumQ() all the generalized coordinate
536 second derivatives qdotdot currently in use by this mobilizer, from this
537 mobilizer's partion in the matter subsystem's full qdotdot vector in the
538 State cache. **/
539 Vector getQDotDotAsVector(const State& state) const;
540
541 /** Return the generalized forces tau resulting from prescribed (known)
542 acceleration, corresponding to each of this mobilizer's mobilities, as a Vector
543 of length nu=getNumU().
544
545 If this mobilizer has prescribed accelerations udot due to an active lock or
546 Motion object, the set of generalized forces tau that must be added in order to
547 produce those accelerations is calculated at Acceleration stage. There is one
548 scalar tau per mobility and they can be returned individually or as a Vector.
549 The return value is zero if this mobilizer is currently free. **/
550 Vector getTauAsVector(const State& state) const;
551
552 /** Return one of the tau forces resulting from prescribed (known) acceleration,
553 corresponding to one of this mobilizer's mobilities as selected here using the
554 \p which parameter, numbered from zero to getNumU()-1.
555 @see getTauAsVector() for more information **/
556 Real getOneTau(const State& state, MobilizerUIndex which) const;
557
558 /** Set one of the generalized coordinates q to value \p v, in this mobilizer's
559 partition of the matter subsystem's full q vector in the State. The particular
560 coordinate is selected using the \p which parameter, numbering from zero to
561 getNumQ()-1. **/
562 void setOneQ(State& state, int which, Real v) const;
563 /** Set one of the generalized speeds u to value \p v, in this mobilizer's
564 partition of the matter subsystem's full u vector in the State. The particular
565 coordinate is selected using the \p which parameter, numbering from zero to
566 getNumU()-1. **/
567 void setOneU(State& state, int which, Real v) const;
568
569 /** Set all of the generalized coordinates q to value \p v (a Vector of length
570 getNumQ()), in this mobilizer's partition of the matter subsystem's full q
571 vector in the State. **/
572 void setQFromVector(State& state, const Vector& v) const;
573 /** Set all of the generalized speeds u to value \p v (a Vector of length
574 getNumU()), in this mobilizer's partition of the matter subsystem's full u
575 vector in the State. **/
576 void setUFromVector(State& state, const Vector& v) const;
577
578 /** Adjust this mobilizer's q's to best approximate the supplied Transform
579 which requests a particular relative orientation and translation between the
580 F "fixed" frame and M "moving" frame connected by this mobilizer.
581
582 This set of methods sets the generalized coordinates, or speeds (state
583 variables) for just the mobilizer associated with this MobilizedBody (ignoring
584 all other mobilizers and constraints), without requiring knowledge of the
585 meanings of the individual state variables. The idea here is to provide a
586 physically-meaningful quantity relating the mobilizer's inboard and outboard
587 frames, and then ask the mobilizer to set its state variables to reproduce that
588 quantity to the extent it can.
589
590 These methods can be called in Stage::Model, however the routines may consult
591 the current values of the state variables in some cases, so you must make sure
592 they have been set to reasonable, or at least innocuous values (zero will work).
593 In no circumstance will any of these methods look at any state variables that
594 belong to another mobilizer; they are limited to working locally with just the
595 current mobilizer.
596
597 Routines which specify only translation (linear velocity) may use rotational
598 coordinates to help satisfy the translation requirement. An alternate "Only"
599 method is available to forbid modification of purely rotational coordinates in
600 that case. When a mobilizer uses state variables which have combined rotational
601 and translational character (e.g. a screw joint) consult the documentation for
602 the derived MobilizedBody class to find out how that mobilizer responds to these
603 routines.
604
605 There is no guarantee that the desired physical quantity will be achieved by
606 these routines; you can check on return if you're worried. Individual mobilizers
607 make specific promises about what they will do; consult the documentation. These
608 routines do not throw exceptions even for absurd requests like specifying a
609 rotation for a sliding mobilizer. Nothing happens if there are no mobilities
610 here, i.e. Ground or a Weld mobilizer. **/
611 void setQToFitTransform(State& state, const Transform& X_FM) const;
612
613 /** Adjust this mobilizer's q's to best approximate the supplied Rotation matrix
614 which requests a particular relative orientation between the "fixed" frame F and
615 "moving" frame M connected by this mobilizer.
616 @see setQToFitTransform() **/
617 void setQToFitRotation(State& state, const Rotation& R_FM) const;
618
619 /** Adjust this mobilizer's q's to best approximate the supplied position vector
620 which requests a particular offset between the origins of the F "fixed" frame
621 and M "moving" frame connected by this mobilizer, with \e any q's (rotational
622 or translational) being modified if doing so helps satisfy the request.
623 @see setQToFitTransform() **/
624 void setQToFitTranslation(State& state, const Vec3& p_FM) const;
625
626 /** Adjust this mobilizer's u's (generalized speeds) to best approximate the
627 supplied spatial velocity \p V_FM which requests the relative angular and linear
628 velocity between the "fixed" and "moving" frames connected by this mobilizer.
629 Routines which affect generalized speeds u depend on the generalized coordinates
630 q already having been set; they never change these coordinates.
631 @see setQToFitTransform() **/
632 void setUToFitVelocity(State& state, const SpatialVec& V_FM) const;
633
634 /** Adjust this mobilizer's u's (generalized speeds) to best approximate the
635 supplied angular velocity \p w_FM which requests a particular relative angular
636 between the "fixed" and "moving" frames connected by this mobilizer.
637 @see setQToFitTransform(), setUToFitVelocity() **/
638 void setUToFitAngularVelocity(State& state, const Vec3& w_FM) const;
639
640 /** Adjust <em>any</em> of this mobilizer's u's (generalized speeds) to best
641 approximate the supplied linear velocity \p v_FM which requests a particular
642 velocity for the "moving" frame M origin in the "fixed" frame F on the parent
643 where these are the frames connected by this mobilizer.
644 @see setQToFitTransform(), setUToFitVelocity() **/
645 void setUToFitLinearVelocity(State& state, const Vec3& v_FM) const;
646
647 /** Expert use only: obtain a column of the hinge matrix H corresponding to one
648 of this mobilizer's mobilities (actually a column of H_PB_G; what Jain calls H*
649 and Schwieters calls H^T). This is the matrix that maps generalized speeds u to
650 the cross-body relative spatial velocity V_PB_G via V_PB_G=H*u. Note that
651 although H relates child body B to parent body B, it is expressed in the ground
652 frame G so the resulting cross-body velocity of B in P is also expressed in G.
653 The supplied state must have been realized through Position stage because H
654 varies with this mobilizer's generalized coordinates q.
655 @see getH_FMCol() **/
656 SpatialVec getHCol(const State& state, MobilizerUIndex ux) const;
657
658 /** Expert use only: obtain a column of the mobilizer-local hinge matrix H_FM
659 which maps generalized speeds u to cross-mobilizer spatial velocity V_FM via
660 V_FM=H_FM*u. Note that H and V here are expressed in the parent body's (inboard)
661 frame F. The supplied state must have been realized through Position stage
662 because H varies with this mobilizer's generalized coordinates q.
663 @see getHCol() **/
664 SpatialVec getH_FMCol(const State& state, MobilizerUIndex ux) const;
665
666 // End of State Access Methods.
667 /**@}**/
668
669 /////////////////////
670 // BASIC OPERATORS //
671 /////////////////////
672
673 //------------------------------------------------------------------------------
674 /** @name Basic Operators
675
676 These methods use state variables and Response methods to compute basic
677 quantities which cannot be precomputed, but which can be implemented with an
678 inline combination of basic floating point operations which can be reliably
679 determined at compile time. The method names and descriptions use the following
680 terms:
681 - %Body or ThisBody: the %Body B associated with the current %MobilizedBody.
682 ThisBody is implied when no other Body is mentioned.
683 - %Ground: the "MobilizedBody" G representing the %Ground reference frame which
684 never moves.
685 - AnotherBody: the %Body A being referenced, which in general is neither
686 ThisBody nor %Ground.
687 - Station: a point S fixed on ThisBody B, located by a position vector p_BS (or
688 more explicitly, p_BoS) from the B-frame origin Bo to the point S, expressed
689 in the B-frame coordinate system.
690 - %Vector: a vector v fixed on ThisBody B, given by a vector v_B expressed in
691 the B-frame coordinate system.
692 - Direction: a unit vector u fixed on ThisBody B, given by a unit vector u_B
693 expressed in the B-frame coordinate system.
694 - Frame: an origin and coordinate axes F fixed on ThisBody B, given by a
695 transform X_BF that locates F's origin (a Station) in B and expresses each of
696 F's axes (Directions) in B.
697 - Origin: the Station located at (0,0,0) in ThisBody frame B, that is, body B's
698 origin point.
699 - MassCenter: the Station on ThisBody B which is the center of mass for B.
700 - GroundPoint, GroundVector: a Point P or Vector v on the %Ground "Body" G.
701 These are measured and expressed in the %Ground frame, as p_GP or v_G.
702 - AnotherBodyStation, AnotherBodyVector, etc.: a Station S or %Vector v on
703 AnotherBody A. These are measured and expressed in the A frame, as p_AS
704 or v_A.
705 - Mobilizer frame M: the mobilizer's outboard "moving" frame, fixed to
706 ThisBody B.
707 - Mobilizer frame F: the mobilizer's inboard "fixed" frame, fixed to the parent
708 body P. **/
709
710 /**@{**/
711
712 /** Return X_AB, the spatial transform giving this body B's frame in body A's
713 frame. Cost is 63 flops. If you know that one of the bodies is Ground, use the
714 0-cost response getBodyTransform() instead of this operators. This operator is
715 available in Position stage.
716 @see getBodyTransform() **/
findBodyTransformInAnotherBody(const State & state,const MobilizedBody & inBodyA)717 Transform findBodyTransformInAnotherBody(const State& state,
718 const MobilizedBody& inBodyA) const
719 {
720 const Transform& X_GA = inBodyA.getBodyTransform(state);
721 const Transform& X_GB = this->getBodyTransform(state);
722
723 return ~X_GA*X_GB; // X_AB=X_AG*X_GB
724 }
725
726 /** Return R_AB, the rotation matrix giving this body B's axes in body A's
727 frame. Cost is 45 flops. If you know that one of the bodies is Ground, use the
728 0-cost response getBodyRotation() instead of this operators. This operator is
729 available in Position stage.
730 @see getBodyRotation() **/
findBodyRotationInAnotherBody(const State & state,const MobilizedBody & inBodyA)731 Rotation findBodyRotationInAnotherBody(const State& state,
732 const MobilizedBody& inBodyA) const
733 {
734 const Rotation& R_GA = inBodyA.getBodyRotation(state);
735 const Rotation& R_GB = this->getBodyRotation(state);
736
737 return ~R_GA*R_GB; // R_AB=R_AG*R_GB
738 }
739
740 /** Return the station on another body A (that is, a point measured and
741 expressed in A) that is currently coincident in space with the origin Bo of
742 this body B. Cost is 18 flops. This operator is available at Position stage.
743 Note: "findBodyOriginLocationInGround" doesn't exist because it would be the
744 same as the no-cost response method getBodyOriginLocation().
745 @see getBodyOriginLocation() **/
findBodyOriginLocationInAnotherBody(const State & state,const MobilizedBody & toBodyA)746 Vec3 findBodyOriginLocationInAnotherBody
747 (const State& state, const MobilizedBody& toBodyA) const {
748 return toBodyA.findStationAtGroundPoint(state,
749 getBodyOriginLocation(state));
750 }
751
752 /** Return the angular and linear velocity of body B's frame in body A's frame,
753 expressed in body A, and arranged as a SpatialVec. Cost is 51 flops. If you know
754 inBodyA is Ground, don't use this routine; use the response method
755 getBodyVelocity() which is free. This operator is available in Velocity stage.
756 @see getBodyVelocity() **/
findBodyVelocityInAnotherBody(const State & state,const MobilizedBody & inBodyA)757 SpatialVec findBodyVelocityInAnotherBody
758 (const State& state, const MobilizedBody& inBodyA) const
759 {
760 const SpatialVec& V_GB = this->getBodyVelocity(state);
761 const SpatialVec& V_GA = inBodyA.getBodyVelocity(state);
762 // angular velocity of B in A, exp in G
763 const Vec3 w_AB_G = V_GB[0]-V_GA[0]; // 3 flops
764
765 // Angular vel. was easy, but for linear vel. we have to add in an wXr term.
766
767 const Transform& X_GB = getBodyTransform(state);
768 const Transform& X_GA = inBodyA.getBodyTransform(state);
769 // vector from Ao to Bo, exp in G
770 const Vec3 p_AB_G = X_GB.p() - X_GA.p(); // 3 flops
771 // d/dt p taken in G
772 const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1]; // 3 flops
773
774 // d/dt p taken in A, exp in G
775 const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G; // 12 flops
776
777 // We're done, but answer is expressed in Ground. Reexpress in A and return.
778 return ~X_GA.R()*SpatialVec(w_AB_G, v_AB_G); // 30 flops
779 }
780
781 /** Return the angular velocity w_AB of body B's frame in body A's frame,
782 expressed in body A. Cost is 18 flops. If you know inBodyA is Ground, don't use
783 this routine; use the response method getBodyAngularVelocity() which is free.
784 This operator is available in Velocity stage.
785 @see getBodyAngularVelocity() **/
findBodyAngularVelocityInAnotherBody(const State & state,const MobilizedBody & inBodyA)786 Vec3 findBodyAngularVelocityInAnotherBody(const State& state,
787 const MobilizedBody& inBodyA) const
788 {
789 const Vec3& w_GB = getBodyAngularVelocity(state);
790 const Vec3& w_GA = inBodyA.getBodyAngularVelocity(state);
791 // angular velocity of B in A, exp in G
792 const Vec3 w_AB_G = w_GB-w_GA; // 3 flops
793
794 // Now reexpress in A.
795 return inBodyA.expressGroundVectorInBodyFrame(state, w_AB_G); // 15 flops
796 }
797
798 /** Return the velocity of body B's origin point in body A's frame, expressed in
799 body A. Cost is 51 flops. If you know inBodyA is Ground, don't use this routine;
800 use the response method getBodyOriginVelocity() which is free. This operator is
801 available in Velocity stage.
802 @see getBodyOriginVelocity() **/
findBodyOriginVelocityInAnotherBody(const State & state,const MobilizedBody & inBodyA)803 Vec3 findBodyOriginVelocityInAnotherBody(const State& state,
804 const MobilizedBody& inBodyA) const
805 {
806 // Doesn't save much to special case this one.
807 return findBodyVelocityInAnotherBody(state,inBodyA)[1];
808 }
809
810 /** Return the angular and linear acceleration of body B's frame in body A's
811 frame, expressed in body A, and arranged as a SpatialVec. Cost is 105 flops. If
812 you know that inBodyA is Ground, don't use this operator; instead, use the
813 response method getBodyAcceleration() which is free. This operator is available
814 in Acceleration stage.
815 @see getBodyAcceleration() **/
findBodyAccelerationInAnotherBody(const State & state,const MobilizedBody & inBodyA)816 SpatialVec findBodyAccelerationInAnotherBody(const State& state,
817 const MobilizedBody& inBodyA) const
818 {
819 const Transform& X_GA = inBodyA.getBodyTransform(state);
820 const SpatialVec& V_GA = inBodyA.getBodyVelocity(state);
821 const SpatialVec& A_GA = inBodyA.getBodyAcceleration(state);
822 const Transform& X_GB = this->getBodyTransform(state);
823 const SpatialVec& V_GB = this->getBodyVelocity(state);
824 const SpatialVec& A_GB = this->getBodyAcceleration(state);
825
826 return findRelativeAcceleration(X_GA, V_GA, A_GA,
827 X_GB, V_GB, A_GB);
828 }
829
830 /** Return the angular acceleration of body B's frame in body A's frame,
831 expressed in body A. Cost is 33 flops. If you know \p inBodyA is Ground, don't
832 use this operator; instead use the response method getBodyAngularAccleration()
833 which is free. This operator is available at AccelerationStage.
834 @see getBodyAngularAcceleration() **/
findBodyAngularAccelerationInAnotherBody(const State & state,const MobilizedBody & inBodyA)835 Vec3 findBodyAngularAccelerationInAnotherBody
836 (const State& state, const MobilizedBody& inBodyA) const
837 {
838 const Rotation& R_GA = inBodyA.getBodyRotation(state);
839 const Vec3& w_GA = inBodyA.getBodyAngularVelocity(state);
840 const Vec3& w_GB = this->getBodyAngularVelocity(state);
841 const Vec3& b_GA = inBodyA.getBodyAngularAcceleration(state);
842 const Vec3& b_GB = this->getBodyAngularAcceleration(state);
843
844 // relative ang. vel. of B in A, exp. in G
845 const Vec3 w_AB_G = w_GB - w_GA; // 3 flops
846 const Vec3 w_AB_G_dot = b_GB - b_GA; // d/dt of w_AB_G taken in G; 3 flops
847
848 // We have the derivative in G; change it to derivative in A by adding
849 // in contribution caused by motion of G in A, that is w_AG X w_AB_G.
850 // (Note that w_AG=-w_GA.)
851 const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G; // ang. accel. of B in A
852 // 12 flops
853
854 return ~R_GA * b_AB_G; // taken in A, expressed in A; 15 flops
855 }
856
857 /** Return the acceleration of body B's origin point in body A's frame,
858 expressed in body A. Cost is 105 flops. If you know that inBodyA is Ground,
859 don't use this operator; instead, use the response method
860 getBodyOriginAcceleration() which is free. This operator is available at
861 Acceleration stage.
862 @see getBodyOriginAcceleration() **/
findBodyOriginAccelerationInAnotherBody(const State & state,const MobilizedBody & inBodyA)863 Vec3 findBodyOriginAccelerationInAnotherBody(const State& state,
864 const MobilizedBody& inBodyA) const
865 {
866 // Not much to be saved by trying to optimize this since the linear part
867 // is the most expensive to calculate.
868 return findBodyAccelerationInAnotherBody(state,inBodyA)[1];
869 }
870
871 /** Return the spatial reaction force (moment and force) applied by the
872 mobilizer to body B at the location of the mobilizer frame M (fixed to body B,
873 but not necessarily at the body frame origin), expressed in Ground. This
874 operator is available at Acceleration stage. Cost is about 120 flops.
875 @see findMobilizerReactionOnParentAtFInGround()
876 @see findMobilizerReactionOnBodyAtOriginInGround()
877 @see SimTK::SimbodyMatterSubsystem::calcMobilizerReactionForces() **/
878 SpatialVec findMobilizerReactionOnBodyAtMInGround(const State& state) const;
879
880 /** Return the spatial reaction force (moment and force) applied by the
881 mobilizer to body B but shifted to the B frame origin, and expressed in Ground.
882 This operator is available at Acceleration stage. Cost is about 90 flops.
883 @see findMobilizerReactionOnParentAtOriginInGround()
884 @see findMobilizerReactionOnBodyAtMInGround()
885 @see SimTK::SimbodyMatterSubsystem::calcMobilizerReactionForces() **/
886 SpatialVec findMobilizerReactionOnBodyAtOriginInGround
887 (const State& state) const;
888
889 /** Return the spatial reaction force (moment and force) applied by the
890 mobilizer to the parent (inboard) body P at the location of the inboard "fixed"
891 mobilizer frame F (fixed to body P, but not necessarily at the P frame origin),
892 expressed in Ground. This operator is available at Acceleration stage. Cost is
893 about 140 flops.
894 @see findMobilizerReactionOnBodyAtMInGround()
895 @see findMobilizerReactionOnParentAtOriginInGround()
896 @see SimTK::SimbodyMatterSubsystem::calcMobilizerReactionForces() **/
897 SpatialVec findMobilizerReactionOnParentAtFInGround(const State& state) const;
898
899 /** Return the spatial reaction force (moment and force) applied by the
900 mobilizer to the parent (inboard) body P at the location of the P frame origin,
901 and expressed in Ground. This operator is available at Acceleration stage. Cost
902 is about 110 flops.
903 @see findMobilizerReactionOnBodyAtOriginInGround()
904 @see findMobilizerReactionOnParentAtFInGround()
905 @see SimTK::SimbodyMatterSubsystem::calcMobilizerReactionForces() **/
906 SpatialVec findMobilizerReactionOnParentAtOriginInGround
907 (const State& state) const;
908
909 /** Return the Cartesian (ground) location that is currently coincident with a
910 station (point) S fixed on body B. That is, we return
911 locationInG=X_GB*stationOnB which means the result is measured from the Ground
912 origin and expressed in Ground. In more precise notation, we're calculating
913 p_GS = X_GB * p_BS for position vectors p_GS and p_BS. Cost is 18 flops. This
914 operator is available at Position stage. **/
findStationLocationInGround(const State & state,const Vec3 & stationOnB)915 Vec3 findStationLocationInGround
916 (const State& state, const Vec3& stationOnB) const {
917 return getBodyTransform(state) * stationOnB;
918 }
919
920
921 /** Given a station S on this body B, return the location on another body A
922 which is at the same location in space. That is, we return
923 locationOnA=X_AB*locationOnB, which means the result is measured from the body A
924 origin and expressed in body A. In more precise notation, we're calculating
925 p_AS = X_AB * p_BS, which we actually calculate as p_AS = X_AG*(X_GB*p_BS). Cost
926 is 36 flops. Note: if you know that one of the bodies is Ground, use one of the
927 routines above which is specialized for Ground to avoid half the work. This
928 operator is available at Position stage or higher. **/
findStationLocationInAnotherBody(const State & state,const Vec3 & stationOnB,const MobilizedBody & toBodyA)929 Vec3 findStationLocationInAnotherBody
930 (const State& state, const Vec3& stationOnB, const MobilizedBody& toBodyA)
931 const
932 {
933 return toBodyA.findStationAtGroundPoint(state,
934 findStationLocationInGround(state,stationOnB));
935 }
936
937 /** Given a station fixed on body B, return its inertial (Cartesian) velocity,
938 that is, its velocity relative to the Ground frame, expressed in the Ground
939 frame. Cost is 27 flops. If you know the station is the body origin (0,0,0)
940 don't use this routine; use the response getBodyOriginVelocity() which is free.
941 This operator is available at Velocity stage.
942 @see getBodyOriginVelocity() **/
findStationVelocityInGround(const State & state,const Vec3 & stationOnB)943 Vec3 findStationVelocityInGround
944 (const State& state, const Vec3& stationOnB) const {
945 const Vec3& w = getBodyAngularVelocity(state); // in G
946 const Vec3& v = getBodyOriginVelocity(state); // in G
947 const Vec3 r = expressVectorInGroundFrame(state,stationOnB); // 15 flops
948 return v + w % r; // 12 flops
949 }
950
951
952 /** Return the velocity of a station S fixed on body B, in body A's frame,
953 expressed in body A. Cost is 93 flops. If you know \p inBodyA is Ground, don't
954 use this operator; instead use the operator findStationVelocityInGround() which
955 is much cheaper. This operator is available in Velocity stage.
956 @see findStationVelocityInGround() **/
findStationVelocityInAnotherBody(const State & state,const Vec3 & stationOnBodyB,const MobilizedBody & inBodyA)957 Vec3 findStationVelocityInAnotherBody(const State& state,
958 const Vec3& stationOnBodyB,//p_BS
959 const MobilizedBody& inBodyA) const
960 {
961 const SpatialVec V_AB =
962 findBodyVelocityInAnotherBody(state, inBodyA); //51 flops
963 // Bo->S rexpressed in A but not shifted to Ao
964 const Vec3 p_BS_A =
965 expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA); //30
966 return V_AB[1] + (V_AB[0] % p_BS_A); //12 flops
967 }
968
969
970 /** Given a station fixed on body B, return its inertial (Cartesian)
971 acceleration, that is, its acceleration relative to the ground frame, expressed
972 in the ground frame. Cost is 48 flops. If you know the station is the body
973 origin (0,0,0) don't use this routine; use the response
974 getBodyOriginAcceleration() which is free. This operator is available at
975 Acceleration stage.
976 @see getBodyOriginAcceleration() **/
findStationAccelerationInGround(const State & state,const Vec3 & stationOnB)977 Vec3 findStationAccelerationInGround
978 (const State& state, const Vec3& stationOnB) const {
979 const Vec3& w = getBodyAngularVelocity(state); // in G
980 const Vec3& b = getBodyAngularAcceleration(state); // in G
981 const Vec3& a = getBodyOriginAcceleration(state); // in G
982
983 const Vec3 r = expressVectorInGroundFrame(state,stationOnB); // 15 flops
984 return a + b % r + w % (w % r); // 33 flops
985 }
986
987 /** Return the acceleration of a station S fixed on body B, in another body A's
988 frame, expressed in body A. Cost is 186 flops. If you know that \p inBodyA is
989 Ground, don't use this operator; instead, use the operator
990 findStationAccelerationInGround() which is much cheaper. This operator is
991 available in Acceleration stage. **/
findStationAccelerationInAnotherBody(const State & state,const Vec3 & stationOnBodyB,const MobilizedBody & inBodyA)992 Vec3 findStationAccelerationInAnotherBody(const State& state,
993 const Vec3& stationOnBodyB,
994 const MobilizedBody& inBodyA) const
995 {
996 const Vec3 w_AB =
997 findBodyAngularVelocityInAnotherBody(state,inBodyA); // 18 flops
998 const SpatialVec A_AB =
999 findBodyAccelerationInAnotherBody(state,inBodyA); // 105 flops
1000 // Bo->S rexpressed in A but not shifted to Ao
1001 const Vec3 p_BS_A =
1002 expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA); // 30
1003
1004 return A_AB[1] + (A_AB[0] % p_BS_A) + w_AB % (w_AB % p_BS_A); // 33 flops
1005 }
1006
1007 /** It is cheaper to calculate a station's ground location and velocity together
1008 than to do them separately. Here we can return them both in 30 flops, vs. 45 to
1009 do them in two calls. This operator is available at Velocity stage. **/
findStationLocationAndVelocityInGround(const State & state,const Vec3 & locationOnB,Vec3 & locationOnGround,Vec3 & velocityInGround)1010 void findStationLocationAndVelocityInGround
1011 (const State& state, const Vec3& locationOnB,
1012 Vec3& locationOnGround, Vec3& velocityInGround) const
1013 {
1014 const Vec3& p_GB = getBodyOriginLocation(state);
1015 const Vec3 p_BS_G = expressVectorInGroundFrame(state,locationOnB);//15flops
1016 locationOnGround = p_GB + p_BS_G; // 3flops
1017
1018 const Vec3& w_GB = getBodyAngularVelocity(state);
1019 const Vec3& v_GB = getBodyOriginVelocity(state);
1020 velocityInGround = v_GB + w_GB % p_BS_G; // 12 flops
1021 }
1022
1023
1024 /** It is cheaper to calculate a station's ground location, velocity, and
1025 acceleration together than to do them separately. Here we can return them all in
1026 54 flops, vs. 93 to do them in three calls. This operator is available at
1027 Acceleration stage. **/
findStationLocationVelocityAndAccelerationInGround(const State & state,const Vec3 & locationOnB,Vec3 & locationOnGround,Vec3 & velocityInGround,Vec3 & accelerationInGround)1028 void findStationLocationVelocityAndAccelerationInGround
1029 (const State& state, const Vec3& locationOnB,
1030 Vec3& locationOnGround, Vec3& velocityInGround, Vec3& accelerationInGround)
1031 const
1032 {
1033 const Rotation& R_GB = getBodyRotation(state);
1034 const Vec3& p_GB = getBodyOriginLocation(state);
1035
1036 // re-express station vector p_BS in G
1037 const Vec3 r = R_GB*locationOnB; // 15 flops
1038 locationOnGround = p_GB + r; // 3 flops
1039
1040 const Vec3& w = getBodyAngularVelocity(state); // in G
1041 const Vec3& v = getBodyOriginVelocity(state); // in G
1042 const Vec3& b = getBodyAngularAcceleration(state); // in G
1043 const Vec3& a = getBodyOriginAcceleration(state); // in G
1044
1045 const Vec3 wXr = w % r; // "whipping" velocity w X r due to ang vel; 9 flops
1046 velocityInGround = v + wXr; // v + w X r (3 flops)
1047 accelerationInGround = a + b % r + w % wXr; // 24 flops
1048 }
1049
1050 /** Return the Cartesian (ground) location of this body B's mass center. Cost is
1051 18 flops. This operator is available at Position stage. **/
findMassCenterLocationInGround(const State & state)1052 Vec3 findMassCenterLocationInGround(const State& state) const {
1053 return findStationLocationInGround(state,getBodyMassCenterStation(state));
1054 }
1055
1056 /** Return the point of another body A that is currently coincident in space
1057 with the mass center CB of this body B. Cost is 36 flops. This operator is
1058 available at Position stage. **/
findMassCenterLocationInAnotherBody(const State & state,const MobilizedBody & toBodyA)1059 Vec3 findMassCenterLocationInAnotherBody
1060 (const State& state, const MobilizedBody& toBodyA) const {
1061 return findStationLocationInAnotherBody(state,
1062 getBodyMassCenterStation(state),toBodyA);
1063 }
1064
1065 /** Return the station (point) S of this body B that is coincident with the
1066 given Ground location. That is we return locationOnB = X_BG*locationInG, which
1067 means the result is measured from the body origin Bo and expressed in the body
1068 frame. In more precise notation, we're calculating p_BS = X_BG*p_GS. Cost is 18
1069 flops. This operator is available at
1070 Position stage. **/
findStationAtGroundPoint(const State & state,const Vec3 & locationInG)1071 Vec3 findStationAtGroundPoint
1072 (const State& state, const Vec3& locationInG) const {
1073 return ~getBodyTransform(state) * locationInG;
1074 }
1075
1076 /** Return the station (point) on this body B that is coincident with the given
1077 station on another body A. That is we return stationOnB = X_BA * stationOnA,
1078 which means the result is measured from the body origin Bo and expressed in the
1079 body frame. Cost is 36 flops. This operator is available at Position stage.
1080 @see findStationLocationInAnotherBody() **/
findStationAtAnotherBodyStation(const State & state,const MobilizedBody & fromBodyA,const Vec3 & stationOnA)1081 Vec3 findStationAtAnotherBodyStation
1082 (const State& state, const MobilizedBody& fromBodyA,
1083 const Vec3& stationOnA) const {
1084 return fromBodyA.findStationLocationInAnotherBody(state, stationOnA, *this);
1085 }
1086
1087 /** Return the station S of this body that is currently coincident in space with
1088 the origin Ao of another body A. Cost is 18 flops. This operator is available at
1089 Position stage. **/
findStationAtAnotherBodyOrigin(const State & state,const MobilizedBody & fromBodyA)1090 Vec3 findStationAtAnotherBodyOrigin
1091 (const State& state, const MobilizedBody& fromBodyA) const {
1092 return findStationAtGroundPoint(state,
1093 fromBodyA.getBodyOriginLocation(state));
1094 }
1095
1096 /** Return the station S of this body that is currently coincident in space with
1097 the mass center Ac of another body A. Cost is 36 flops. This operator is
1098 available at Position stage. **/
findStationAtAnotherBodyMassCenter(const State & state,const MobilizedBody & fromBodyA)1099 Vec3 findStationAtAnotherBodyMassCenter
1100 (const State& state, const MobilizedBody& fromBodyA) const {
1101 return fromBodyA.findStationLocationInAnotherBody(state,
1102 getBodyMassCenterStation(state),*this);
1103 }
1104
1105 /** Return the current Ground-frame pose (position and orientation) of a frame
1106 F that is fixed to body B. That is, we return X_GF=X_GB*X_BF. Cost is 63 flops.
1107 This operator is available at Position stage. **/
findFrameTransformInGround(const State & state,const Transform & frameOnB)1108 Transform findFrameTransformInGround
1109 (const State& state, const Transform& frameOnB) const {
1110 return getBodyTransform(state) * frameOnB;
1111 }
1112
1113 /** Return the current Ground-frame spatial velocity V_GF (that is, angular and
1114 linear velocity) of a frame F that is fixed to body B. The angular velocity of F
1115 is the same as that of B, but the linear velocity is the velocity of F's origin
1116 Fo rather than B's origin Bo. This operator is available at Velocity stage. Cost
1117 is 27 flops. **/
findFrameVelocityInGround(const State & state,const Transform & frameOnB)1118 SpatialVec findFrameVelocityInGround
1119 (const State& state, const Transform& frameOnB) const {
1120 return SpatialVec(getBodyAngularVelocity(state),
1121 findStationVelocityInGround(state,frameOnB.p()));
1122 }
1123
1124 /** Return the current Ground-frame spatial acceleration A_GF (that is, angular
1125 and linear acceleration) of a frame F that is fixed to body B. The angular
1126 acceleration of F is the same as that of B, but the linear acceleration is the
1127 acceleration of F's origin Fo rather than B's origin Bo. This operator is
1128 available at Acceleration stage. Cost is 48 flops. **/
findFrameAccelerationInGround(const State & state,const Transform & frameOnB)1129 SpatialVec findFrameAccelerationInGround
1130 (const State& state, const Transform& frameOnB) const {
1131 return SpatialVec(getBodyAngularAcceleration(state),
1132 findStationAccelerationInGround(state,frameOnB.p()));
1133 }
1134
1135 /** Re-express a vector expressed in this body B's frame into the same vector in
1136 G, by applying only a rotation. That is, we return vectorInG = R_GB * vectorInB.
1137 Cost is 15 flops. This operator is available at Position stage. **/
expressVectorInGroundFrame(const State & state,const Vec3 & vectorInB)1138 Vec3 expressVectorInGroundFrame
1139 (const State& state, const Vec3& vectorInB) const {
1140 return getBodyRotation(state)*vectorInB;
1141 }
1142
1143 /** Re-express a vector expressed in Ground into the same vector expressed in
1144 this body B, by applying only rotation. That is, we return
1145 vectorInB = R_BG * vectorInG. Cost is 15 flops. This operator is available at
1146 Position stage. **/
expressGroundVectorInBodyFrame(const State & state,const Vec3 & vectorInG)1147 Vec3 expressGroundVectorInBodyFrame
1148 (const State& state, const Vec3& vectorInG) const {
1149 return ~getBodyRotation(state)*vectorInG;
1150 }
1151
1152 /** Re-express a vector expressed in this body B into the same vector expressed
1153 in body A, by applying only a rotation. That is, we return
1154 vectorInA = R_AB * vectorInB. Cost is 30 flops. This operator is available at
1155 Position stage. Note: if you know one of the bodies is Ground, call one of the
1156 specialized methods above to save 15 flops. **/
expressVectorInAnotherBodyFrame(const State & state,const Vec3 & vectorInB,const MobilizedBody & inBodyA)1157 Vec3 expressVectorInAnotherBodyFrame
1158 (const State& state, const Vec3& vectorInB,
1159 const MobilizedBody& inBodyA) const
1160 {
1161 return inBodyA.expressGroundVectorInBodyFrame(state,
1162 expressVectorInGroundFrame(state,vectorInB));
1163 }
1164
1165 /** Re-express this body B's mass properties in Ground by applying only a
1166 rotation, not a shift of reference point. The mass properties remain measured in
1167 the body B frame, taken about the body B origin Bo, but are reexpressed in
1168 Ground. **/
expressMassPropertiesInGroundFrame(const State & state)1169 MassProperties expressMassPropertiesInGroundFrame(const State& state) const {
1170 const MassProperties& M_Bo_B = getBodyMassProperties(state);
1171 const Rotation& R_GB = getBodyRotation(state);
1172 return M_Bo_B.reexpress(~R_GB);
1173 }
1174
1175 /** Re-express this body B's mass properties in another body A's frame by
1176 applying only a rotation, not a shift of reference point. The mass properties
1177 remain measured in the body B frame, taken about the body B origin Bo, but are
1178 reexpressed in A. **/
expressMassPropertiesInAnotherBodyFrame(const State & state,const MobilizedBody & inBodyA)1179 MassProperties expressMassPropertiesInAnotherBodyFrame
1180 (const State& state, const MobilizedBody& inBodyA) const {
1181 const MassProperties& M_Bo_B = getBodyMassProperties(state);
1182 const Rotation R_AB = findBodyRotationInAnotherBody(state,inBodyA);
1183 return M_Bo_B.reexpress(~R_AB);
1184 }
1185
1186 // End of Basic Operators.
1187 /**@}**/
1188
1189 //------------------------------------------------------------------------------
1190 /** @name High-Level Operators
1191 High level operators combine State Access and Basic Operators with run-time
1192 tests to calculate more complex %MobilizedBody-specific quantities, with more
1193 complicated implementations that can exploit special cases at run time. **/
1194
1195 /**@{**/
1196 /** Return the mass properties of body B, measured from and about the B
1197 origin Bo, but expressed in Ground and then returned as a Spatial Inertia
1198 Matrix. The mass properties are arranged in the SpatialMat like this:
1199 <pre>
1200 M=[ I_Bo crossMat(m*Bc) ]
1201 [ ~crossMat(m*Bc) diag(m) ]
1202 </pre>
1203 where I_Bo is the inertia taken about the B frame origin Bo, and Bc is the
1204 vector p_BoBc from B's origin to its mass center.
1205
1206 The Spatial Inertia Matrix for Ground has infinite mass and inertia, with
1207 the cross terms set to zero. That is, it looks like a 6x6 diagonal matrix
1208 with Infinity on the diagonals.
1209
1210 @par Required stage
1211 \c Stage::Position, unless \p objectBodyB == \c GroundIndex **/
calcBodySpatialInertiaMatrixInGround(const State & state)1212 SpatialMat calcBodySpatialInertiaMatrixInGround(const State& state) const
1213 {
1214 if (isGround())
1215 return SpatialMat(Mat33(Infinity)); // sets diagonals to Inf
1216
1217 const MassProperties& mp = getBodyMassProperties(state);
1218 const Rotation& R_GB = getBodyRotation(state);
1219 // re-express in Ground without shifting, convert to spatial mat.
1220 return mp.reexpress(~R_GB).toSpatialMat();
1221 }
1222
1223 /** Return the central inertia for body B, that is, the inertia taken about
1224 body B's mass center Bc, and expressed in B.
1225 ///
1226 @par Required stage
1227 \c Stage::Instance **/
calcBodyCentralInertia(const State & state,MobilizedBodyIndex objectBodyB)1228 Inertia calcBodyCentralInertia(const State& state,
1229 MobilizedBodyIndex objectBodyB) const
1230 {
1231 return getBodyMassProperties(state).calcCentralInertia();
1232 }
1233
1234 /** Return the inertia of this body B, taken about an arbitrary point PA of
1235 body A, and expressed in body A.
1236 TODO: this needs testing! **/
calcBodyInertiaAboutAnotherBodyStation(const State & state,const MobilizedBody & inBodyA,const Vec3 & aboutLocationOnBodyA)1237 Inertia calcBodyInertiaAboutAnotherBodyStation
1238 (const State& state, const MobilizedBody& inBodyA,
1239 const Vec3& aboutLocationOnBodyA) const
1240 {
1241 // get B's mass props MB, measured about Bo, exp. in B
1242 const MassProperties& MB_Bo_B = getBodyMassProperties(state);
1243
1244 // Calculate the vector from the body B origin (current "about" point) to
1245 // the new "about" point PA, expressed in B.
1246 const Vec3 p_Bo_PA =
1247 findStationAtAnotherBodyStation(state, inBodyA, aboutLocationOnBodyA);
1248
1249 // Now shift the "about" point for body B's inertia IB to PA, but still
1250 // expressed in B.
1251 const Inertia IB_PA_B = MB_Bo_B.calcShiftedInertia(p_Bo_PA);
1252
1253 // Finally reexpress the inertia in the A frame.
1254 const Rotation R_BA =
1255 inBodyA.findBodyRotationInAnotherBody(state, *this);
1256 const Inertia IB_PA_A = IB_PA_B.reexpress(R_BA);
1257 return IB_PA_A;
1258 }
1259
1260
1261 /** Calculate body B's momentum (angular, linear) measured and expressed in
1262 Ground, but taken about the body origin Bo. **/
calcBodyMomentumAboutBodyOriginInGround(const State & state)1263 SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State& state) {
1264 const MassProperties M_Bo_G = expressMassPropertiesInGroundFrame(state);
1265 const SpatialVec& V_GB = getBodyVelocity(state);
1266 return M_Bo_G.toSpatialMat() * V_GB;
1267 }
1268
1269 /** Calculate body B's momentum (angular, linear) measured and expressed in
1270 Ground, but taken about the body mass center Bc. **/
calcBodyMomentumAboutBodyMassCenterInGround(const State & state)1271 SpatialVec calcBodyMomentumAboutBodyMassCenterInGround
1272 (const State& state) const {
1273 const MassProperties& M_Bo_B = getBodyMassProperties(state);
1274 const Rotation& R_GB = getBodyRotation(state);
1275
1276 // Given a central inertia matrix I, angular velocity w, and mass center
1277 // velocity v, the central angular momentum is Iw and linear momentum is mv.
1278 const Inertia I_Bc_B = M_Bo_B.calcCentralInertia();
1279 const Inertia I_Bc_G = I_Bc_B.reexpress(~R_GB);
1280 const Real mb = M_Bo_B.getMass();
1281 const Vec3& w_GB = getBodyAngularVelocity(state);
1282 Vec3 v_GBc =
1283 findStationVelocityInGround(state, M_Bo_B.getMassCenter());
1284
1285 return SpatialVec( I_Bc_G*w_GB, mb*v_GBc );
1286 }
1287
1288 /** Calculate the distance from a station PB on body B to a station PA on
1289 body A. We are given the location vectors (stations) p_Bo_PB and p_Ao_PA,
1290 expressed in their respective frames. We return |p_PB_PA|. **/
calcStationToStationDistance(const State & state,const Vec3 & locationOnBodyB,const MobilizedBody & bodyA,const Vec3 & locationOnBodyA)1291 Real calcStationToStationDistance(const State& state,
1292 const Vec3& locationOnBodyB,
1293 const MobilizedBody& bodyA,
1294 const Vec3& locationOnBodyA) const
1295 {
1296 if (isSameMobilizedBody(bodyA))
1297 return (locationOnBodyA-locationOnBodyB).norm();
1298
1299 const Vec3 r_Go_PB =
1300 this->findStationLocationInGround(state,locationOnBodyB);
1301 const Vec3 r_Go_PA =
1302 bodyA.findStationLocationInGround(state,locationOnBodyA);
1303 return (r_Go_PA - r_Go_PB).norm();
1304 }
1305
1306 /** Calculate the time rate of change of distance from a fixed point PB on
1307 body B to a fixed point PA on body A. We are given the location vectors
1308 p_Bo_PB and p_Ao_PA, expressed in their respective frames. We return
1309 d/dt |p_BoAo|, under the assumption that the time derivatives of the two given
1310 vectors in their own frames is zero. **/
calcStationToStationDistanceTimeDerivative(const State & state,const Vec3 & locationOnBodyB,const MobilizedBody & bodyA,const Vec3 & locationOnBodyA)1311 Real calcStationToStationDistanceTimeDerivative
1312 (const State& state,
1313 const Vec3& locationOnBodyB,
1314 const MobilizedBody& bodyA,
1315 const Vec3& locationOnBodyA) const
1316 {
1317 if (isSameMobilizedBody(bodyA))
1318 return 0;
1319
1320 Vec3 rB, rA, vB, vA;
1321 this->findStationLocationAndVelocityInGround(state,locationOnBodyB,rB,vB);
1322 bodyA.findStationLocationAndVelocityInGround(state,locationOnBodyA,rA,vA);
1323 const Vec3 r = rA-rB, v = vA-vB;
1324 const Real d = r.norm();
1325
1326 // When the points are coincident, the rate of change of distance is just
1327 // their relative speed. Otherwise, it is the speed along the direction of
1328 // separation.
1329 if (d==0) return v.norm();
1330 else return dot(v, r/d);
1331 }
1332
1333
1334 /** Calculate the second time derivative of distance from a fixed point PB on
1335 body B to a fixed point PA on body A. We are given the position vectors
1336 (stations) p_Bo_PB and p_Ao_PA, expressed in their respective frames. We return
1337 d^2/dt^2 |p_PB_PA|, under the assumption that the time derivatives of the two
1338 given vectors in their own frames is zero. **/
calcStationToStationDistance2ndTimeDerivative(const State & state,const Vec3 & locationOnBodyB,const MobilizedBody & bodyA,const Vec3 & locationOnBodyA)1339 Real calcStationToStationDistance2ndTimeDerivative
1340 (const State& state,
1341 const Vec3& locationOnBodyB,
1342 const MobilizedBody& bodyA,
1343 const Vec3& locationOnBodyA) const
1344 {
1345 if (isSameMobilizedBody(bodyA))
1346 return 0;
1347
1348 Vec3 rB, rA, vB, vA, aB, aA;
1349 this->findStationLocationVelocityAndAccelerationInGround
1350 (state,locationOnBodyB,rB,vB,aB);
1351 bodyA.findStationLocationVelocityAndAccelerationInGround
1352 (state,locationOnBodyA,rA,vA,aA);
1353
1354 const Vec3 r = rA-rB, v = vA-vB, a = aA-aB;
1355 const Real d = r.norm();
1356
1357 // This method is the time derivative of
1358 // calcFixedPointToPointDistanceTimeDerivative(), so it must follow the same
1359 // two cases. That is, when the points are coincident the change in
1360 // separation rate is the time derivative of the speed |v|, otherwise it is
1361 // the time derivative of the speed along the separation vector.
1362
1363 if (d==0) {
1364 // Return d/dt |v|. This has two cases: if |v| is zero, the rate of
1365 // change of speed is just the points' relative acceleration magnitude.
1366 // Otherwise, it is the acceleration in the direction of the current
1367 // relative velocity vector.
1368 const Real s = v.norm(); // speed
1369 if (s==0) return a.norm();
1370 else return dot(a, v/s);
1371 }
1372
1373 // Points are separated.
1374 const Vec3 u = r/d; // u is separation direction (a unit vector from B to A)
1375 const Vec3 vp = v - dot(v,u)*u; // velocity perp. to separation direction
1376 return dot(a,u) + dot(vp,v)/d;
1377 }
1378
1379
1380 /** TODO: not implemented yet -- any volunteers? Return the velocity of a
1381 point P moving on body B, in body A's frame, expressed in body A. **/
calcBodyMovingPointVelocityInBody(const State & state,const Vec3 & locationOnBodyB,const Vec3 & velocityOnBodyB,const MobilizedBody & inBodyA)1382 Vec3 calcBodyMovingPointVelocityInBody
1383 (const State& state,
1384 const Vec3& locationOnBodyB,
1385 const Vec3& velocityOnBodyB,
1386 const MobilizedBody& inBodyA) const
1387 {
1388 SimTK_ASSERT_ALWAYS(!"unimplemented method",
1389 "MobilizedBody::calcBodyMovingPointVelocityInBody()"
1390 " is not yet implemented -- any volunteers?");
1391 return Vec3::getNaN();
1392 }
1393
1394
1395 /** TODO: not implemented yet -- any volunteers? Return the velocity of a point
1396 P moving (and possibly accelerating) on body B, in body A's frame, expressed in
1397 body A. **/
calcBodyMovingPointAccelerationInBody(const State & state,const Vec3 & locationOnBodyB,const Vec3 & velocityOnBodyB,const Vec3 & accelerationOnBodyB,const MobilizedBody & inBodyA)1398 Vec3 calcBodyMovingPointAccelerationInBody
1399 (const State& state,
1400 const Vec3& locationOnBodyB,
1401 const Vec3& velocityOnBodyB,
1402 const Vec3& accelerationOnBodyB,
1403 const MobilizedBody& inBodyA) const
1404 {
1405 SimTK_ASSERT_ALWAYS(!"unimplemented method",
1406 "MobilizedBody::calcBodyMovingPointAccelerationInBody()"
1407 " is not yet implemented -- any volunteers?");
1408 return Vec3::getNaN();
1409 }
1410
1411 /** TODO: not implemented yet -- any volunteers? Calculate the time rate of
1412 change of distance from a moving point PB on body B to a moving point PA on body
1413 A. We are given the location vectors p_Bo_PB and p_Ao_PA, and the velocities of
1414 PB in B and PA in A, all expressed in their respective frames. We return
1415 d/dt |p_BoAo|, taking into account the (given) time derivatives of the locations
1416 in their local frames, as well as the relative velocities of the bodies. **/
calcMovingPointToPointDistanceTimeDerivative(const State & state,const Vec3 & locationOnBodyB,const Vec3 & velocityOnBodyB,const MobilizedBody & bodyA,const Vec3 & locationOnBodyA,const Vec3 & velocityOnBodyA)1417 Real calcMovingPointToPointDistanceTimeDerivative
1418 (const State& state,
1419 const Vec3& locationOnBodyB,
1420 const Vec3& velocityOnBodyB,
1421 const MobilizedBody& bodyA,
1422 const Vec3& locationOnBodyA,
1423 const Vec3& velocityOnBodyA) const
1424 {
1425 SimTK_ASSERT_ALWAYS(!"unimplemented method",
1426 "MobilizedBody::calcMovingPointToPointDistanceTimeDerivative()"
1427 " is not yet implemented -- any volunteers?");
1428 return NaN;
1429 }
1430
1431 /** TODO: not implemented yet -- any volunteers? Calculate the second time
1432 derivative of distance from a moving point PB on body B to a moving point PA on
1433 body A. We are given the location vectors p_Bo_PB and p_Ao_PA, and the
1434 velocities and accelerations of PB in B and PA in A, all expressed in their
1435 respective frames. We return d^2/dt^2 |p_AoBo|, taking into account the time
1436 derivatives of the locations in their local frames, as well as the relative
1437 velocities and accelerations of the bodies. **/
calcMovingPointToPointDistance2ndTimeDerivative(const State & state,const Vec3 & locationOnBodyB,const Vec3 & velocityOnBodyB,const Vec3 & accelerationOnBodyB,const MobilizedBody & bodyA,const Vec3 & locationOnBodyA,const Vec3 & velocityOnBodyA,const Vec3 & accelerationOnBodyA)1438 Real calcMovingPointToPointDistance2ndTimeDerivative
1439 (const State& state,
1440 const Vec3& locationOnBodyB,
1441 const Vec3& velocityOnBodyB,
1442 const Vec3& accelerationOnBodyB,
1443 const MobilizedBody& bodyA,
1444 const Vec3& locationOnBodyA,
1445 const Vec3& velocityOnBodyA,
1446 const Vec3& accelerationOnBodyA) const
1447 {
1448 SimTK_ASSERT_ALWAYS(!"unimplemented method",
1449 "MobilizedBody::calcMovingPointToPointDistance2ndTimeDerivative()"
1450 " is not yet implemented -- any volunteers?");
1451 return NaN;
1452 }
1453
1454
1455 // End of High Level Operators.
1456 /**@}**/
1457
1458
1459 //////////////////////////
1460 // CONSTRUCTION METHODS //
1461 //////////////////////////
1462
1463 /** The default constructor provides an empty %MobilizedBody handle that
1464 can be assigned to reference any type of %MobilizedBody. **/
MobilizedBody()1465 MobilizedBody() {}
1466
1467 /** Internal use only. **/
1468 explicit MobilizedBody(MobilizedBodyImpl* r);
1469
1470 //------------------------------------------------------------------------------
1471 /** @name Construction and Misc Methods
1472 These methods are the base class services which are used while building a
1473 concrete %MobilizedBody, or to query a %MobilizedBody to find out how it was
1474 built. These are unlikely to be used by end users of MobilizedBodies. **/
1475 /**@{**/
1476
1477 /** Return a const reference to the Body contained within this %MobilizedBody.
1478 This refers to an internal copy of the Body that is owned by the
1479 %MobilizedBody. **/
1480 const Body& getBody() const;
1481
1482 /** Return a writable reference to the Body contained within this
1483 %MobilizedBody. This refers to an internal copy of the Body that is owned by the
1484 %MobilizedBody. Calling this method invalidates the %MobilizedBody's topology,
1485 so the containing System's realizeTopology() method must be called again. **/
1486 Body& updBody();
1487
1488 /** Replace the Body contained within this %MobilizedBody with a new one.
1489 Calling this method invalidates the %MobilizedBody's topology, so the containing
1490 System's realizeTopology() method must be called again. A reference to this
1491 %MobilizedBody is returned so that this can be chained like an assignment
1492 operator. **/
1493 MobilizedBody& setBody(const Body&);
1494
1495 /** Convenience method to add DecorativeGeometry specified relative to the
1496 new (outboard) body's reference frame B. This is equivalent to
1497 `this->updBody().addDecoration(X_BD,geometry)`. Note that the Body may already
1498 have had some DecorativeGeometry on it when it was first put into this
1499 %MobilizedBody; this just adds more and the returned ordinal is larger. The
1500 given \p geometry object is *copied* here; we do not keep a reference to the
1501 supplied object. Use the underlying Body object's accessors to obtain a
1502 reference to the DecorativeGeometry copy stored here.
1503 @see getBody(), Body::addDecoration() **/
addBodyDecoration(const Transform & X_BD,const DecorativeGeometry & geometry)1504 int addBodyDecoration(const Transform& X_BD,
1505 const DecorativeGeometry& geometry) {
1506 return updBody().addDecoration(X_BD, geometry);
1507 }
1508 /** Convenience method for use when the \p geometry is supplied in the body
1509 frame. This is the same as `addBodyDecoration(Transform(),geometry)`. **/
addBodyDecoration(const DecorativeGeometry & geometry)1510 int addBodyDecoration(const DecorativeGeometry& geometry) {
1511 return updBody().addDecoration(geometry);
1512 }
1513
1514 /** Add decorative geometry specified relative to the outboard mobilizer frame M
1515 attached to body B, and associated with the mobilizer rather than the body. This
1516 DecorativeGeometry is kept in a separate list from the body decorations and
1517 inboard decorations. Returns an ordinal that can be used to identify this
1518 outboard decoration later (numbered starting from zero for outboard decorations
1519 only). **/
1520 int addOutboardDecoration(const Transform& X_MD,
1521 const DecorativeGeometry& geometry);
1522 /** Return the count of decorations added with addOutboardDecoration(). **/
1523 int getNumOutboardDecorations() const;
1524 /** Return a const reference to the i'th outboard decoration. **/
1525 const DecorativeGeometry& getOutboardDecoration(int i) const;
1526 /** Return a writable reference to the i'th outboard decoration. **/
1527 DecorativeGeometry& updOutboardDecoration(int i);
1528
1529 /** Add decorative geometry specified relative to the inboard mobilizer frame F
1530 attached to the parent body P, and associated with the mobilizer rather than
1531 the body. This DecorativeGeometry is kept in a separate list from the body
1532 decorations and outboard decorations. Returns an ordinal that can be used to
1533 identify this inboard decoration later (numbered starting from zero for inboard
1534 decorations only). **/
1535 int addInboardDecoration(const Transform& X_FD,
1536 const DecorativeGeometry& geometry);
1537 /** Return the count of decorations added with addInboardDecoration(). **/
1538 int getNumInboardDecorations() const;
1539 /** Return a const reference to the i'th inboard decoration. **/
1540 const DecorativeGeometry& getInboardDecoration(int i) const;
1541 /** Return a writable reference to the i'th inboard decoration. **/
1542 DecorativeGeometry& updInboardDecoration(int i);
1543
1544 /** If the contained Body can have its mass properties set to the supplied value
1545 \p m its mass properties are changed, otherwise the method fails. Calling this
1546 method invalidates the MobilizedBody's topology, so the containing matter
1547 subsystem's realizeTopology() method must be called again. A reference to this
1548 %MobilizedBody is returned so that this can be chained like an assignment
1549 operator. **/
setDefaultMassProperties(const MassProperties & m)1550 MobilizedBody& setDefaultMassProperties(const MassProperties& m) {
1551 updBody().setDefaultRigidBodyMassProperties(m); // might not be allowed
1552 return *this;
1553 }
1554
1555 /** Return the mass properties of the Body stored within this MobilizedBody. **/
getDefaultMassProperties()1556 const MassProperties& getDefaultMassProperties() const {
1557 // every body type can do this
1558 return getBody().getDefaultRigidBodyMassProperties();
1559 }
1560
1561 /** Provide a unique Motion object for this %MobilizedBody. The %MobilizedBody
1562 takes over ownership of the Motion object and is responsible for cleaning up
1563 its heap space when the time comes. This is a Topology-changing operation and
1564 consequently requires write access to the %MobilizedBody which will propagate
1565 to invalidate the containing Subsystem and System's topology. There can only
1566 be one Motion object per mobilizer; this method will throw an exception if
1567 there is already one here. **/
1568 void adoptMotion(Motion& ownerHandle);
1569
1570 /** If there is a Motion object associated with this MobilizedBody it is
1571 removed; otherwise, nothing happens. If a Motion is deleted, the containing
1572 System's topology is invalidated. **/
1573 void clearMotion();
1574
1575 /** Check whether this %MobilizedBody has an associated Motion object. This does
1576 not tell you whether the Motion object is currently enabled or in use; just
1577 whether it is available. **/
1578 bool hasMotion() const;
1579
1580 /** If there is a Motion object associated with this MobilizedBody, this returns
1581 a const reference to it. Otherwise it will throw an exception. You can check
1582 first using hasMotion(). Note that there is no provision to obtain a writable
1583 reference to the contained Motion object; if you want to change it clear the
1584 existing object instead and replace it with a new one.
1585 @see hasMotion() **/
1586 const Motion& getMotion() const;
1587
1588 /** Change this mobilizer's frame F on the parent body P. Calling this method
1589 invalidates the %MobilizedBody's topology, so the containing matter subsystem's
1590 realizeTopology() method must be called again. A reference to this
1591 %MobilizedBody is returned so that this can be chained like an assignment
1592 operator. **/
1593 MobilizedBody& setDefaultInboardFrame (const Transform& X_PF);
1594 /** Change this mobilizer's frame M fixed on this (the outboard) body B. Calling
1595 this method invalidates the %MobilizedBody's topology, so the containing
1596 matter subsystem's realizeTopology() method must be called again. A reference
1597 to this %MobilizedBody is returned so that this can be chained like an
1598 assignment operator. **/
1599 MobilizedBody& setDefaultOutboardFrame(const Transform& X_BM);
1600
1601 /** Return a reference to this mobilizer's default for the frame F fixed on the
1602 parent (inboard) body P, as the fixed Transform from P's body frame to the frame
1603 F fixed to P. This default Transform is stored with the %MobilizedBody object,
1604 not the State. **/
1605 const Transform& getDefaultInboardFrame() const; // X_PF
1606 /** Return a reference to this %MobilizedBody's default for mobilizer frame M,
1607 as the fixed Transform from this body B's frame to the frame M fixed on B. This
1608 default Transform is stored with the %MobilizedBody object, not the State. **/
1609 const Transform& getDefaultOutboardFrame() const; // X_BM
1610
1611 /** This is an implicit conversion from %MobilizedBody to MobilizedBodyIndex
1612 when needed. This will fail unless this %MobilizedBody is owned by some
1613 SimbodyMatterSubsystem. We guarantee that the MobilizedBodyIndex of a mobilized
1614 body is numerically larger than the MobilizedBodyIndex of its parent. **/
MobilizedBodyIndex()1615 operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}
1616
1617 /** Return the MobilizedBodyIndex of this MobilizedBody within the owning
1618 SimbodyMatterSubsystem. This will fail unless this MobilizedBody is owned by
1619 some SimbodyMatterSubsystem. We guarantee that the MobilizedBodyIndex of a
1620 mobilized body is numerically larger than the MobilizedBodyIndex of its
1621 parent. **/
1622 MobilizedBodyIndex getMobilizedBodyIndex() const;
1623
1624 /** Return a reference to the MobilizedBody serving as the parent body of the
1625 current MobilizedBody. This call will fail if the current MobilizedBody is
1626 Ground, since Ground has no parent. **/
1627 const MobilizedBody& getParentMobilizedBody() const;
1628
1629 /** Return a reference to this %MobilizedBody's oldest ancestor other than
1630 Ground, or return Ground if this %MobilizedBody is Ground. That is, we return
1631 the "base" %MobilizedBody for this %MobilizedBody, meaning the one which
1632 connects this branch of the multibody tree directly to Ground. **/
1633 const MobilizedBody& getBaseMobilizedBody() const;
1634
1635 /** Obtain a reference to the SimbodyMatterSubsystem which contains this
1636 %MobilizedBody. This will fail unless this MobilizedBody is owned by some
1637 SimbodyMatterSubsystem. **/
1638 const SimbodyMatterSubsystem& getMatterSubsystem() const;
1639 /** Obtain a writable reference to the SimbodyMatterSubsystem which contains
1640 this %MobilizedBody. This will fail unless this %MobilizedBody is owned by some
1641 SimbodyMatterSubsystem. **/
1642 SimbodyMatterSubsystem& updMatterSubsystem();
1643
1644 /** Determine whether the current MobilizedBody object is owned by a matter
1645 subsystem. **/
1646 bool isInSubsystem() const;
1647
1648 /** Determine whether a given MobilizedBody \p mobod is in the same matter
1649 subsystem as the current body. If the bodies are not in a subsystem, this
1650 routine will return \c false. **/
1651 bool isInSameSubsystem(const MobilizedBody& mobod) const;
1652
1653 /** Determine whether a given %MobilizedBody \p mobod is the same %MobilizedBody
1654 as this one. For this to be true the handles must not be empty, and the
1655 implementation objects must be <em>the same object</em> not separate objects
1656 with identical contents. **/
1657 bool isSameMobilizedBody(const MobilizedBody& mobod) const;
1658
1659 /** Determine whether this %MobilizedBody is Ground, meaning that it is actually
1660 body 0 of some matter subsytem, not just that its body type is Ground. **/
1661 bool isGround() const;
1662
1663 /** Return this mobilized body's level in the tree of bodies, starting with
1664 Ground at 0, mobilized bodies directly connected to Ground at 1, mobilized
1665 bodies directly connected to those at 2, etc. This is callable after
1666 realizeTopology(). This is the graph distance of the body from Ground. **/
1667 int getLevelInMultibodyTree() const;
1668
1669 /** Create a new %MobilizedBody which is identical to this one, except that it
1670 has a different parent (and consequently might belong to a different
1671 MultibodySystem). **/
1672 MobilizedBody& cloneForNewParent(MobilizedBody& parent) const;
1673
1674
1675 // Utility operators //
1676
1677 /** This utility selects one of the q's (generalized coordinates) associated
1678 with this mobilizer from a supplied "q-like" Vector, meaning a Vector which is
1679 the same length as the Vector of q's for the containing matter subsystem. **/
1680 Real getOneFromQPartition
1681 (const State& state, int which, const Vector& qlike) const;
1682
1683 /** This utility returns a writable reference to one of the q's (generalized
1684 coordinates) associated with this mobilizer from a supplied "q-like" Vector,
1685 meaning a Vector which is the same length as the Vector of q's for the
1686 containing matter subsystem. **/
1687 Real& updOneFromQPartition
1688 (const State& state, int which, Vector& qlike) const;
1689
1690 /** This utility selects one of the u's (generalized speeds) associated with
1691 this mobilizer from a supplied "u-like" Vector, meaning a Vector which is
1692 the same length as the Vector of u's for the containing matter subsystem. **/
1693 Real getOneFromUPartition
1694 (const State& state, int which, const Vector& ulike) const;
1695
1696 /** This utility returns a writable reference to one of the u's (generalized
1697 speeds) associated with this mobilizer from a supplied "u-like" Vector, meaning
1698 a Vector which is the same length as the Vector of u's for the containing matter
1699 subsystem. **/
1700 Real& updOneFromUPartition(const State& state, int which, Vector& ulike) const;
1701
1702 /** This utility adds in the supplied generalized force \p f (a scalar) to
1703 the appropriate slot of the supplied \p mobilityForces Vector, which is a
1704 "u-like" Vector. Note that we are <em>adding</em> this not <em>setting</em> it
1705 so it important that \p mobilityForces be initialized to zero before making a
1706 set of calls to applyOneMobilityForce(). **/
applyOneMobilityForce(const State & state,int which,Real f,Vector & mobilityForces)1707 void applyOneMobilityForce(const State& state, int which, Real f,
1708 Vector& mobilityForces) const
1709 {
1710 updOneFromUPartition(state,which,mobilityForces) += f;
1711 }
1712
1713 /** Given a generalized force in the q-space of this mobilizer, convert it to
1714 the equivalent generalized mobility force (u-space force). This uses the
1715 kinematic coupling matrix N that appears in equation (1) qdot=N*u. Here we
1716 compute (2) fu = ~N*fq (that's N transpose, not inverse).
1717
1718 Simbody deals with generalized forces in mobility (u) space, but sometimes these
1719 are more convenient to generate in generalized coordinate (q) space. In that
1720 case this utility method is useful to perform the conversion from q space to u
1721 space that is necessary for communicating the force to Simbody.
1722
1723 @param[in] state
1724 A State already realized through Position stage, from which this mobilizer's
1725 kinematic coupling matrix N(q) is obtained.
1726 @param[in] fq
1727 This is a generalized force in the space of the generalized coordinates q
1728 rather than the generalized speeds u. The length of \p fq must be nq, the
1729 number of q's currently being used by this mobilizer in the given \p state.
1730 (This can depend on a Model-stage state variable.)
1731 @param[out] fu
1732 This is the generalized force in mobility space (the space of the
1733 generalized speeds u) that is equivalent to \p fq. \p fu will be resized if
1734 necessary to length nu, the number of u's being used by this mobilizer.
1735
1736 <h3>Theory</h3>
1737 The physical quantity power (force times velocity) must not change as a result
1738 of a change of coordinates. Hence we must have ~fq*qdot==~fu*u which follows
1739 from equations (1) and (2): multiply (1) by ~fq to get
1740 <pre>
1741 ~fq*qdot= ~fq*N*u
1742 = ~(~N*fq)*u
1743 = ~fu*u from equation (2).
1744 </pre>
1745 For any mobilizer where qdot==u this simply copies the input to the output.
1746 Otherwise a multiplication by ~N is done, but that is very fast since N has
1747 already been computed. Cost depends on type of mobilizer but is unlikely to
1748 exceed 25 flops. **/
1749 void convertQForceToUForce(const State& state,
1750 const Array_<Real,MobilizerQIndex>& fq,
1751 Array_<Real,MobilizerUIndex>& fu) const;
1752
1753 /** This utility adds in the supplied spatial force \p spatialForceInG
1754 (consisting of a torque vector, and a force vector to be applied at the current
1755 body's origin) to the appropriate slot of the supplied \p bodyForcesInG Vector.
1756 Note that we are <em>adding</em> this not <em>setting</em> it so it is important
1757 that \p bodyForcesInG be initialized to zero before making a set of calls to
1758 applyBodyForce(). **/
1759 void applyBodyForce(const State& state, const SpatialVec& spatialForceInG,
1760 Vector_<SpatialVec>& bodyForcesInG) const;
1761
1762 /** This utility adds in the supplied pure torque \p torqueInG to the
1763 appropriate slot of the supplied \p bodyForcesInG Vector. Note that we are
1764 <em>adding</em> this not <em>setting</em> it so it is important that
1765 \p bodyForcesInG be initialized to zero before making a set of calls to
1766 applyBodyTorque(). **/
1767 void applyBodyTorque(const State& state, const Vec3& torqueInG,
1768 Vector_<SpatialVec>& bodyForcesInG) const;
1769
1770 /** This utility adds in the supplied force \p forceInG applied at a point
1771 \p pointInB to the appropriate slot of the supplied \p bodyForcesInG Vector.
1772 Notes:
1773 - we are <em>adding</em> this not <em>setting</em> it so it is important that
1774 \p bodyForcesInG be initialized to zero before making a set of calls to
1775 applyForceToBodyPoint().
1776 - \p pointInB represents a fixed station of B and is provided by giving the
1777 vector from body B's origin to the point, expressed in the B frame, while
1778 the applied force (and resulting body forces and torques) are expressed
1779 in the ground frame. **/
1780 void applyForceToBodyPoint
1781 (const State& state, const Vec3& pointInB, const Vec3& forceInG,
1782 Vector_<SpatialVec>& bodyForcesInG) const;
1783
1784 // End of Construction and Misc Methods.
1785 /**@}**/
1786
1787 /////////////////////////////////////
1788 // BUILT IN MOBILIZER DECLARATIONS //
1789 /////////////////////////////////////
1790
1791 //------------------------------------------------------------------------------
1792 // These are the built-in MobilizedBody types. Each of these has a known
1793 // number of coordinates and speeds (at least a default number) so
1794 // can define routines which return and accept specific-size arguments, e.g.
1795 // Real (for 1-dof mobilizer) and Vec5 (for 5-dof mobilizer). Here is the
1796 // conventional interface that each built-in should provide. The base type
1797 // provides similar routines but using variable-sized or "one at a time"
1798 // arguments. (Vec<1> here will actually be a Real; assume the built-in
1799 // MobilizedBody class is "BuiltIn")
1800 //
1801 // BuiltIn& setDefaultQ(const Vec<nq>&);
1802 // const Vec<nq>& getDefaultQ() const;
1803 //
1804 // const Vec<nq>& getQ[Dot[Dot]](const State&) const;
1805 // const Vec<nu>& getU[Dot](const State&) const;
1806 //
1807 // void setQ(State&, const Vec<nq>&) const;
1808 // void setU(State&, const Vec<nu>&) const;
1809 //
1810 // const Vec<nq>& getMyPartQ(const State&, const Vector& qlike) const;
1811 // const Vec<nu>& getMyPartU(const State&, const Vector& ulike) const;
1812 //
1813 // Vec<nq>& updMyPartQ(const State&, Vector& qlike) const;
1814 // Vec<nu>& updMyPartU(const State&, Vector& ulike) const;
1815 //
1816 // Each built in mobilized body type is declared in its own header
1817 // file using naming convention MobilizedBody_Pin.h, for example. All the
1818 // built-in headers are collected in MobilizedBody_BuiltIns.h; you should
1819 // include new ones there also.
1820
1821
1822 class Pin;
1823 typedef Pin Torsion; ///< Synonym for Pin mobilizer.
1824 typedef Pin Revolute; ///< Synonym for Pin mobilizer.
1825
1826 class Universal;
1827 class Cylinder;
1828 class Weld;
1829
1830 class Slider;
1831 typedef Slider Prismatic; ///< Synonym for Slider mobilizer.
1832
1833 class Translation;
1834 typedef Translation Cartesian; ///< Synonym for Translation mobilizer.
1835 typedef Translation CartesianCoords; ///< Synonym for Translation mobilizer.
1836
1837 class BendStretch;
1838 typedef BendStretch PolarCoords; ///< Synonym for BendStretch mobilizer.
1839
1840 class SphericalCoords;
1841 class LineOrientation;
1842
1843 class Planar;
1844 class Gimbal;
1845 class Bushing;
1846
1847 class Ball;
1848 typedef Ball Orientation; ///< Synonym for Ball mobilizer.
1849 typedef Ball Spherical; ///< Synonym for Ball mobilizer.
1850
1851 class Free;
1852 class FreeLine;
1853 class Screw;
1854 class Ellipsoid;
1855 class Custom;
1856 class Ground;
1857 class FunctionBased;
1858
1859 // Internal use only.
1860 class PinImpl;
1861 class SliderImpl;
1862 class UniversalImpl;
1863 class CylinderImpl;
1864 class BendStretchImpl;
1865 class PlanarImpl;
1866 class GimbalImpl;
1867 class BushingImpl;
1868 class BallImpl;
1869 class TranslationImpl;
1870 class SphericalCoordsImpl;
1871 class FreeImpl;
1872 class LineOrientationImpl;
1873 class FreeLineImpl;
1874 class WeldImpl;
1875 class ScrewImpl;
1876 class EllipsoidImpl;
1877 class CustomImpl;
1878 class GroundImpl;
1879 class FunctionBasedImpl;
1880 };
1881
1882 } // namespace SimTK
1883
1884 #endif // SimTK_SIMBODY_MOBILIZED_BODY_H_
1885
1886
1887
1888