Simbody 3.7
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
33namespace SimTK {
34
35class SimbodyMatterSubsystem;
36class MobilizedBody;
37class SimbodyMatterSubtree;
38class SimbodyMatterSubtreeResults;
39
110public:
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;
228private:
229 SubtreeRep* rep;
230 const SubtreeRep& getRep() const {assert(rep);return *rep;}
231 SubtreeRep& updRep() {assert(rep);return *rep;}
232};
233
235operator<<(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 */
243public:
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
256
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;
278private:
280 SubtreeResultsRep* rep;
281 const SubtreeResultsRep& getRep() const {assert(rep);return *rep;}
282 SubtreeResultsRep& updRep() {assert(rep);return *rep;}
283};
284
286operator<<(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
const SpatialVec & getSubtreeBodyAcceleration(SubtreeBodyIndex) const
const Vector & getSubtreeUDot() const
const Vector & getSubtreeU() const
const Array_< QIndex > & getQSubset() const
void addMobilities(SubtreeBodyIndex, QIndex qStart, int nq, UIndex uStart, int nu)
const Vector & getSubtreeQ() const
void realizeModel(const Vector &stateQ, const Vector &stateU)
const Array_< UIndex > & getUSubset() const
SimbodyMatterSubtreeResults & operator=(const SimbodyMatterSubtreeResults &)
void findSubtreeBodyU(SubtreeBodyIndex, SubtreeUIndex &uStart, int &nu) const
const SpatialVec & getSubtreeBodyVelocity(SubtreeBodyIndex) const
SimbodyMatterSubtreeResults(const SimbodyMatterSubtreeResults &)
const Transform & getSubtreeBodyTransform(SubtreeBodyIndex) const
void findSubtreeBodyQ(SubtreeBodyIndex, SubtreeQIndex &qStart, int &nq) 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
const Array_< MobilizedBodyIndex > & getTerminalBodies() const
SimbodyMatterSubtree(const SimbodyMatterSubsystem &, const Array_< MobilizedBodyIndex > &terminalBodies)
const SimbodyMatterSubsystem & getSimbodyMatterSubsystem() const
void calcAccelerationsFromSubtreeUDot(const State &, const Vector &subUDot, SimbodyMatterSubtreeResults &) const
SimbodyMatterSubtree & addTerminalBody(MobilizedBodyIndex)
void perturbAccelerations(const State &, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults &) const
MobilizedBodyIndex getAncestorMobilizedBodyIndex() 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
void calcVelocitiesFromSubtreeU(const State &, const Vector &subU, SimbodyMatterSubtreeResults &) const
SubtreeBodyIndex getParentSubtreeBodyIndex(SubtreeBodyIndex) const
const Array_< MobilizedBodyIndex > & getAllBodies() 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
const Array_< SubtreeBodyIndex > & getChildSubtreeBodyIndices(SubtreeBodyIndex) 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:606