Simbody  3.5
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:
113  SimbodyMatterSubtree& operator=(const SimbodyMatterSubtree&);
115 
118  const Array_<MobilizedBodyIndex>& terminalBodies);
119 
120  void setSimbodyMatterSubsystem(const SimbodyMatterSubsystem& matter);
121  const SimbodyMatterSubsystem& getSimbodyMatterSubsystem() const;
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 
127  SimbodyMatterSubtree& addTerminalBody(MobilizedBodyIndex);
128 
129  void realizeTopology();
130 
131  int getNumSubtreeBodies() const; // includes ancestor
132  MobilizedBodyIndex getAncestorMobilizedBodyIndex() const;
133 
134  // These are in the same order they were added; body[i] is the terminus
135  // of branch i.
136  const Array_<MobilizedBodyIndex>& getTerminalBodies() const;
137 
138  // These are indexed by SubtreeBodyIndex starting with 0 for the ancestor
139  // body and monotonically increasing outwards along a branch.
140  const Array_<MobilizedBodyIndex>& getAllBodies() const;
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.
152  void initializeSubtreeResults(const State&, SimbodyMatterSubtreeResults&) const;
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.
158  bool isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults&) const;
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.
167  void copyPositionsFromState(const State&, SimbodyMatterSubtreeResults&) const;
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.
172  void calcPositionsFromSubtreeQ(const State&, const Vector& subQ, SimbodyMatterSubtreeResults&) const;
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.
186  void copyVelocitiesFromState(const State&, SimbodyMatterSubtreeResults&) const;
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.
191  void calcVelocitiesFromSubtreeU(const State&, const Vector& subU, SimbodyMatterSubtreeResults&) const;
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.
196  void calcVelocitiesFromZeroU(const State&, SimbodyMatterSubtreeResults&) const;
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.
210  void copyAccelerationsFromState(const State&, SimbodyMatterSubtreeResults&) const;
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.
215  void calcAccelerationsFromSubtreeUDot(const State&, const Vector& subUDot, SimbodyMatterSubtreeResults&) const;
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).
221  void calcAccelerationsFromZeroUDot(const State&, SimbodyMatterSubtreeResults&) const;
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_
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
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
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:50
A SimbodyMatterSubtree is a view of a connected subgraph of the tree of mobilized bodies in a Simbody...
Definition: SimbodyMatterSubtree.h:109
Every Simbody header and source file should include this header before any other Simbody header...
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:593
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
Definition: SimbodyMatterSubtree.h:242
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
Unique integer type for Subsystem-local q indexing.
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition: CompliantContactSubsystem.h:387
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133