Simbody  3.8
SimbodyMatterSubtree.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_MATTER_SUBTREE_H_
2 #define SimTK_SIMBODY_MATTER_SUBTREE_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-12 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKcommon.h"
29 
30 #include <cassert>
31 #include <iosfwd>
32 
33 namespace SimTK {
34 
35 class SimbodyMatterSubsystem;
36 class MobilizedBody;
37 class SimbodyMatterSubtree;
38 class SimbodyMatterSubtreeResults;
39 
110 public:
115 
118  const Array_<MobilizedBodyIndex>& terminalBodies);
119 
122 
123  // This doesn't change the associated SimbodyMatterSubsystem if there
124  // is one, but does remove all the bodies from the SimbodyMatterSubtree.
125  void clear();
126 
128 
130 
131  int getNumSubtreeBodies() const; // includes ancestor
133 
134  // These are in the same order they were added; body[i] is the terminus
135  // of branch i.
137 
138  // These are indexed by SubtreeBodyIndex starting with 0 for the ancestor
139  // body and monotonically increasing outwards along a branch.
141 
142  // 0 returns an invalid Index
143  SubtreeBodyIndex getParentSubtreeBodyIndex(SubtreeBodyIndex) const;
145  getChildSubtreeBodyIndices(SubtreeBodyIndex) const;
146 
147  // MODEL STAGE
148 
149  // State must be realized to at least Stage::Model for this call to work.
150  // The supplied SimbodyMatterSubtreeResults object is allocated and properly initialized to
151  // be able to hold computation results from this SimbodyMatterSubtree.
153 
154  // This can be used as a sanity check that initializeSubtreeResults() was
155  // already called in this SimbodyMatterSubtree to produce these
156  // SimbodyMatterSubtreeResults. It is by no means exhaustive but will catch
157  // egregious errors.
159 
160  // POSITION STAGE
161 
162  // State must be realized to at least Stage::Position for this to work. SimbodyMatterSubtreeResults
163  // must have already been initialized to work with this SimbodyMatterSubtree. SimbodyMatterSubtreeResults stage
164  // will be Stage::Position after this call. All body transforms will be the same as
165  // the corresponding ones in the state, except they will be measured from the ancestor
166  // frame instead of ground. SimbodyMatterSubtree q's will be identical to corresponding State q's.
168 
169  // State must be realized to Stage::Instance. subQ must be the right length for this
170  // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must have been properly initialized. SimbodyMatterSubtreeResults
171  // stage will be Stage::Position after this call.
173 
174  // Calculates a perturbed position result starting with the subQ's and position results
175  // which must already be in SimbodyMatterSubtreeResults.
176  void perturbPositions(const State&, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;
177 
178 
179  // VELOCITY STAGE
180 
181  // State must be realized to at least Stage::Velocity for this to work. SimbodyMatterSubtreeResults
182  // must already be at Stage::Position. SimbodyMatterSubtreeResults stage
183  // will be Stage::Velocity after this call. All subtree body spatial velocities will be
184  // the same as in the State, except measured relative to A and expressed in A. SimbodyMatterSubtree u's
185  // will be identical to corresponding State u's.
187 
188  // State must be realized to Stage::Instance. subU must be the right length for this
189  // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must already be at Stage::Position. SimbodyMatterSubtreeResults
190  // stage will be Stage::Velocity after this call.
192 
193  // State must be realized to Stage::Instance and SimbodyMatterSubtreeResults must already be at
194  // Stage::Position. SimbodyMatterSubtreeResults stage will be Stage::Velocity after this call, but
195  // all SimbodyMatterSubtree u's and body velocities will be zero.
197 
198  // Calculates a perturbed velocity result starting with the subU's and velocity results
199  // which must already be in SimbodyMatterSubtreeResults.
200  void perturbVelocities(const State&, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;
201 
202 
203  // ACCELERATION STAGE
204 
205  // State must be realized to at least Stage::Acceleration for this to work. SimbodyMatterSubtreeResults
206  // must already be at Stage::Velocity. SimbodyMatterSubtreeResults stage
207  // will be Stage::Acceleration after this call. All subtree body spatial accelerations will be
208  // the same as in the State, except measured relative to A and expressed in A. SimbodyMatterSubtree udots
209  // will be identical to corresponding State udots.
211 
212  // State must be realized to Stage::Instance. subUDot must be the right length for this
213  // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must already be at Stage::Velocity. SimbodyMatterSubtreeResults
214  // stage will be Stage::Acceleration after this call.
216 
217  // State must be realized to Stage::Instance and SimbodyMatterSubtreeResults must already be at
218  // Stage::Velocity. SimbodyMatterSubtreeResults stage will be Stage::Acceleration after this call.
219  // All SimbodyMatterSubtree udots's will be zero, body accelerations will have only their bias values
220  // (coriolis accelerations from nonzero u's).
222 
223  // Calculates a perturbed velocity result starting with the subUDot's and acceleration results
224  // which must already be in SimbodyMatterSubtreeResults.
225  void perturbAccelerations(const State&, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;
226 
227  class SubtreeRep;
228 private:
229  SubtreeRep* rep;
230  const SubtreeRep& getRep() const {assert(rep);return *rep;}
231  SubtreeRep& updRep() {assert(rep);return *rep;}
232 };
233 
234 SimTK_SIMBODY_EXPORT std::ostream&
235 operator<<(std::ostream&, const SimbodyMatterSubtree&);
236 
237 /*
238  * This is the writable "cache" for a SimbodyMatterSubtree. Once the full State has
239  * been realized to the Model stage, a SimbodyMatterSubtree can initialize one of these
240  * objects and then use it to hold operator results.
241  */
243 public:
248 
249  void clear();
250 
251  void reallocateBodies(int nBodies);
252  void addMobilities(SubtreeBodyIndex, QIndex qStart, int nq, UIndex uStart, int nu);
253  void realizeModel(const Vector& stateQ, const Vector& stateU);
254 
255  Stage getStage() const;
256 
257  int getNumSubtreeBodies() const;
258  int getNumSubtreeQs() const;
259  int getNumSubtreeUs() const;
260 
261  const Vector& getSubtreeQ() const;
262  const Transform& getSubtreeBodyTransform(SubtreeBodyIndex) const; // from ancestor frame
263 
264  const Vector& getSubtreeU() const;
265  const SpatialVec& getSubtreeBodyVelocity(SubtreeBodyIndex) const; // measured & expressed in ancestor frame
266 
267  const Vector& getSubtreeUDot() const;
268  const SpatialVec& getSubtreeBodyAcceleration(SubtreeBodyIndex) const; // measured & expressed in ancestor frame
269 
270  // These are indexed by SubtreeQIndex and SubtreeUIndex.
271  const Array_<QIndex>& getQSubset() const; // subset of Subsystem Qs used by this SimbodyMatterSubtree
272  const Array_<UIndex>& getUSubset() const; // subset of Subsystem Us used by this SimbodyMatterSubtree
273 
274  void findSubtreeBodyQ(SubtreeBodyIndex, SubtreeQIndex& qStart, int& nq) const; // indices into QSubset
275  void findSubtreeBodyU(SubtreeBodyIndex, SubtreeUIndex& uStart, int& nu) const; // indices into USubset
276 
277  class SubtreeResultsRep;
278 private:
279  friend class SimbodyMatterSubtree;
280  SubtreeResultsRep* rep;
281  const SubtreeResultsRep& getRep() const {assert(rep);return *rep;}
282  SubtreeResultsRep& updRep() {assert(rep);return *rep;}
283 };
284 
285 SimTK_SIMBODY_EXPORT std::ostream&
286 operator<<(std::ostream&, const SimbodyMatterSubtreeResults&);
287 
288 } // namespace SimTK
289 
290 #endif // SimTK_SIMBODY_MATTER_SUBTREE_H_
Includes internal headers providing declarations for the basic SimTK Core classes,...
Every Simbody header and source file should include this header before any other Simbody header.
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
Unique integer type for Subsystem-local q indexing.
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
Definition: SimbodyMatterSubtree.h:242
SimbodyMatterSubtreeResults & operator=(const SimbodyMatterSubtreeResults &)
const Array_< QIndex > & getQSubset() const
void addMobilities(SubtreeBodyIndex, QIndex qStart, int nq, UIndex uStart, int nu)
const SpatialVec & getSubtreeBodyAcceleration(SubtreeBodyIndex) const
const Array_< UIndex > & getUSubset() const
void realizeModel(const Vector &stateQ, const Vector &stateU)
const Vector & getSubtreeUDot() const
const Transform & getSubtreeBodyTransform(SubtreeBodyIndex) const
const Vector & getSubtreeQ() const
void findSubtreeBodyU(SubtreeBodyIndex, SubtreeUIndex &uStart, int &nu) const
const Vector & getSubtreeU() const
SimbodyMatterSubtreeResults(const SimbodyMatterSubtreeResults &)
void findSubtreeBodyQ(SubtreeBodyIndex, SubtreeQIndex &qStart, int &nq) const
const SpatialVec & getSubtreeBodyVelocity(SubtreeBodyIndex) const
A SimbodyMatterSubtree is a view of a connected subgraph of the tree of mobilized bodies in a Simbody...
Definition: SimbodyMatterSubtree.h:109
void copyPositionsFromState(const State &, SimbodyMatterSubtreeResults &) const
SimbodyMatterSubtree(const SimbodyMatterSubsystem &, const Array_< MobilizedBodyIndex > &terminalBodies)
void calcAccelerationsFromSubtreeUDot(const State &, const Vector &subUDot, SimbodyMatterSubtreeResults &) const
void perturbAccelerations(const State &, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults &) const
MobilizedBodyIndex getAncestorMobilizedBodyIndex() const
const Array_< MobilizedBodyIndex > & getTerminalBodies() const
void perturbPositions(const State &, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults &) const
void calcPositionsFromSubtreeQ(const State &, const Vector &subQ, SimbodyMatterSubtreeResults &) const
void setSimbodyMatterSubsystem(const SimbodyMatterSubsystem &matter)
void perturbVelocities(const State &, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults &) const
SimbodyMatterSubtree & addTerminalBody(MobilizedBodyIndex)
void calcVelocitiesFromSubtreeU(const State &, const Vector &subU, SimbodyMatterSubtreeResults &) const
const Array_< MobilizedBodyIndex > & getAllBodies() const
const SimbodyMatterSubsystem & getSimbodyMatterSubsystem() const
SubtreeBodyIndex getParentSubtreeBodyIndex(SubtreeBodyIndex) const
const Array_< SubtreeBodyIndex > & getChildSubtreeBodyIndices(SubtreeBodyIndex) const
SimbodyMatterSubtree & operator=(const SimbodyMatterSubtree &)
void copyVelocitiesFromState(const State &, SimbodyMatterSubtreeResults &) const
void initializeSubtreeResults(const State &, SimbodyMatterSubtreeResults &) const
bool isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults &) const
void calcVelocitiesFromZeroU(const State &, SimbodyMatterSubtreeResults &) const
SimbodyMatterSubtree(const SimbodyMatterSubsystem &)
SimbodyMatterSubtree(const SimbodyMatterSubtree &)
void calcAccelerationsFromZeroUDot(const State &, SimbodyMatterSubtreeResults &) const
void copyAccelerationsFromState(const State &, SimbodyMatterSubtreeResults &) const
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:66
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
Unique integer type for Subsystem-local u indexing.
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition: CompliantContactSubsystem.h:387
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607