Simbody  3.7
Assembler.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_ASSEMBLER_H_
2 #define SimTK_SIMBODY_ASSEMBLER_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) 2010-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"
31 
32 #include <set>
33 #include <map>
34 #include <cassert>
35 #include <cmath>
36 
37 namespace SimTK {
38 
39 SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex);
40 
41 class AssemblyCondition;
42 
149  typedef std::set<MobilizedBodyIndex> LockedMobilizers;
150  typedef std::set<MobilizerQIndex> QSet;
151  typedef std::map<MobilizedBodyIndex, QSet> LockedQs;
152  typedef std::map<MobilizerQIndex, Vec2> QRanges;
153  typedef std::map<MobilizedBodyIndex, QRanges> RestrictedQs;
154 public:
155 
159 
163 class AssembleFailed;
167 class TrackFailed;
168 
186 explicit Assembler(const MultibodySystem& system);
187 
198  SimTK_ERRCHK1_ALWAYS(0 <= tolerance,
199  "Assembler::setTolerance()", "The requested error tolerance %g"
200  " is illegal; we require 0 <= tolerance, with 0 indicating that"
201  " the default tolerance (accuracy/10) is to be used.", tolerance);
202  this->tolerance = tolerance;
203  return *this;
204 }
210  return tolerance > 0 ? tolerance
211  : (accuracy > 0 ? accuracy/10 : Real(0.1)/OODefaultAccuracy);
212 }
213 
223 Assembler& setAccuracy(Real accuracy=0) {
224  SimTK_ERRCHK2_ALWAYS(0 <= accuracy && accuracy < 1,
225  "Assembler::setAccuracy()", "The requested accuracy %g is illegal;"
226  " we require 0 <= accuracy < 1, with 0 indicating that the default"
227  " accuracy (%g) is to be used.", Real(1)/OODefaultAccuracy, accuracy);
228  this->accuracy = accuracy;
229  return *this;
230 }
234 { return accuracy > 0 ? accuracy : Real(1)/OODefaultAccuracy; }
235 
236 
244 { assert(systemConstraints.isValid());
245  setAssemblyConditionWeight(systemConstraints,weight);
246  return *this; }
247 
252 { assert(systemConstraints.isValid());
253  return getAssemblyConditionWeight(systemConstraints); }
254 
261 Assembler& setAssemblyConditionWeight(AssemblyConditionIndex condition,
262  Real weight) {
263  SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
264  "Assembler::setAssemblyConditionWeight()");
265  SimTK_ERRCHK1_ALWAYS(weight >= 0, "Assembler::setAssemblyConditionWeight()",
266  "Illegal weight %g; weight must be nonnegative.", weight);
267  uninitialize();
268  weights[condition] = weight;
269  return *this;
270 }
271 
278 Real getAssemblyConditionWeight(AssemblyConditionIndex condition) const {
279  SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
280  "Assembler::getAssemblyConditionWeight()");
281  return weights[condition];
282 }
283 
288 AssemblyConditionIndex
289  adoptAssemblyError(AssemblyCondition* p);
298 AssemblyConditionIndex
299  adoptAssemblyGoal(AssemblyCondition* p, Real weight=1);
300 
301 
308  uninitialize();
309  getMatterSubsystem().convertToEulerAngles(state, internalState);
310  system.realizeModel(internalState);
311  return *this;
312 }
319 void initialize() const;
322 void initialize(const State& state)
323 { setInternalState(state); initialize(); }
330 
337 Real assemble();
338 
347 Real track(Real frameTime = -1);
348 
355 Real assemble(State& state) {
356  setInternalState(state);
357  Real achievedCost = assemble(); // throws if it fails
358  updateFromInternalState(state);
359  return achievedCost;
360 }
361 
362 
366 Real calcCurrentGoal() const;
375 Real calcCurrentErrorNorm() const;
376 
377 
382 void updateFromInternalState(State& state) const {
383  system.realizeModel(state); // allocates q's if they haven't been yet
384  if (!getMatterSubsystem().getUseEulerAngles(state)) {
385  State tempState;
386  getMatterSubsystem().convertToQuaternions(getInternalState(),
387  tempState);
388  state.updQ() = tempState.getQ();
389  } else
390  state.updQ() = getInternalState().getQ();
391 }
401 
406 { uninitialize(); userLockedMobilizers.insert(mbx); }
413 { uninitialize(); userLockedMobilizers.erase(mbx); }
414 
427 { uninitialize(); userLockedQs[mbx].insert(qx); }
428 
434 { LockedQs::iterator p = userLockedQs.find(mbx);
435  if (p == userLockedQs.end()) return;
436  QSet& qs = p->second;
437  if (qs.erase(qx)) { // returns 0 if nothing erased
438  uninitialize();
439  if (qs.empty())
440  userLockedQs.erase(p); // remove the whole mobilized body
441  }
442 }
443 
450  Real lowerBound, Real upperBound)
451 { SimTK_ERRCHK2_ALWAYS(lowerBound <= upperBound, "Assembler::restrictQ()",
452  "The given range [%g,%g] is illegal because the lower bound is"
453  " greater than the upper bound.", lowerBound, upperBound);
454  if (lowerBound == -Infinity && upperBound == Infinity)
455  { unrestrictQ(mbx,qx); return; }
456  uninitialize();
457  userRestrictedQs[mbx][qx] = Vec2(lowerBound,upperBound);
458 }
459 
460 
466 { RestrictedQs::iterator p = userRestrictedQs.find(mbx);
467  if (p == userRestrictedQs.end()) return;
468  QRanges& qranges = p->second;
469  if (qranges.erase(qx)) { // returns 0 if nothing erased
470  uninitialize();
471  if (qranges.empty())
472  userRestrictedQs.erase(p); // remove the whole mobilized body
473  }
474 }
487 int getNumGoalEvals() const;
489 int getNumErrorEvals() const;
491 int getNumGoalGradientEvals() const;
493 int getNumErrorJacobianEvals() const;
496 int getNumAssemblySteps() const;
499 int getNumInitializations() const;
503 void resetStats() const;
511 
515 { forceNumericalGradient = yesno; }
519 { forceNumericalJacobian = yesno; }
520 
526 void setUseRMSErrorNorm(bool yesno)
527 { useRMSErrorNorm = yesno; }
531 bool isUsingRMSErrorNorm() const {return useRMSErrorNorm;}
532 
537 void uninitialize() const;
540 bool isInitialized() const {return alreadyInitialized;}
541 
548 const State& getInternalState() const {return internalState;}
549 
553 void addReporter(const EventReporter& reporter) {
554  reporters.push_back(&reporter);
555 }
556 
560 int getNumFreeQs() const
561 { return freeQ2Q.size(); }
562 
566 QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const
567 { return freeQ2Q[freeQIndex]; }
568 
573 FreeQIndex getFreeQIndexOfQ(QIndex qx) const
574 { return q2FreeQ[qx]; }
575 
578 Vec2 getFreeQBounds(FreeQIndex freeQIndex) const {
579  if (!lower.size()) return Vec2(-Infinity, Infinity);
580  else return Vec2(lower[freeQIndex], upper[freeQIndex]);
581 }
582 
587 { return system; }
591 { return system.getMatterSubsystem(); }
596 ~Assembler();
597 
598 
599 
600 //------------------------------------------------------------------------------
601  private: // methods
602 //------------------------------------------------------------------------------
603 // Note that the internalState is realized to Stage::Position on return.
604 void setInternalStateFromFreeQs(const Vector& freeQs) {
605  assert(freeQs.size() == getNumFreeQs());
606  Vector& q = internalState.updQ();
607  for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx)
608  q[getQIndexOfFreeQ(fx)] = freeQs[fx];
609  system.realize(internalState, Stage::Position);
610 }
611 
612 Vector getFreeQsFromInternalState() const {
613  Vector freeQs(getNumFreeQs());
614  const Vector& q = internalState.getQ();
615  for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx)
616  freeQs[fx] = q[getQIndexOfFreeQ(fx)];
617  return freeQs;
618 }
619 
620 void reinitializeWithExtraQsLocked
621  (const Array_<QIndex>& toBeLocked) const;
622 
623 
624 
625 //------------------------------------------------------------------------------
626  private: // data members
627 //------------------------------------------------------------------------------
628 const MultibodySystem& system;
629 Array_<const EventReporter*> reporters; // just references; don't delete
630 
631 // These members affect the behavior of the assembly algorithm.
632 static const int OODefaultAccuracy = 1000; // 1/accuracy if acc==0
633 Real accuracy; // 0 means use 1/OODefaultAccuracy
634 Real tolerance; // 0 means use accuracy/10
635 bool forceNumericalGradient; // ignore analytic gradient methods
636 bool forceNumericalJacobian; // ignore analytic Jacobian methods
637 bool useRMSErrorNorm; // what norm defines success?
638 
639 // Changes to any of these data members set isInitialized()=false.
640 State internalState;
641 
642 // These are the mobilizers that were set in lockMobilizer(). They are
643 // separate from those involved in individually-locked q's.
644 LockedMobilizers userLockedMobilizers;
645 // These are locks placed on individual q's; they are independent of the
646 // locked mobilizer settings.
647 LockedQs userLockedQs;
648 // These are range restrictions placed on individual q's.
649 RestrictedQs userRestrictedQs;
650 
651 // These are (condition,weight) pairs with weight==Infinity meaning
652 // constraint; weight==0 meaning currently ignored; and any other
653 // positive weight meaning a goal.
654 Array_<AssemblyCondition*,AssemblyConditionIndex>
655  conditions;
656 Array_<Real,AssemblyConditionIndex> weights;
657 
658 // We always have an assembly condition for the Constraints which are
659 // enabled in the System; this is the index which can be used to
660 // retrieve that condition. The default weight is Infinity.
661 AssemblyConditionIndex systemConstraints;
662 
663 
664 // These are filled in when the Assembler is initialized.
665 mutable bool alreadyInitialized;
666 
667 // These are extra q's we removed for numerical reasons.
668 mutable Array_<QIndex> extraQsLocked;
669 
670 // These represent restrictions on the independent variables (q's).
671 mutable std::set<QIndex> lockedQs;
672 mutable Array_<FreeQIndex,QIndex> q2FreeQ; // nq of these
673 mutable Array_<QIndex,FreeQIndex> freeQ2Q; // nfreeQ of these
674 // 0 length if no bounds; otherwise, index by FreeQIndex.
675 mutable Vector lower, upper;
676 
677 // These represent the active assembly conditions.
678 mutable Array_<AssemblyConditionIndex> errors;
679 mutable Array_<int> nTermsPerError;
680 mutable Array_<AssemblyConditionIndex> goals;
681 
682 class AssemblerSystem; // local class
683 mutable AssemblerSystem* asmSys;
684 mutable Optimizer* optimizer;
685 
686 mutable int nAssemblySteps; // count assemble() and track() calls
687 mutable int nInitializations; // # times we had to reinitialize
688 
689 friend class AssemblerSystem;
690 };
691 
692 } // namespace SimTK
693 
694 #endif // SimTK_SIMBODY_ASSEMBLER_H_
void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
Unlock one of this mobilizer&#39;s q&#39;s if it was locked.
Definition: Assembler.h:433
const Vector & getQ(SubsystemIndex) const
Per-subsystem access to the global shared variables.
Vector & updQ(SubsystemIndex)
void lockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
Lock one of this mobilizer&#39;s q&#39;s at its initial value.
Definition: Assembler.h:426
#define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:289
void unlockMobilizer(MobilizedBodyIndex mbx)
Unlock this mobilizer as a whole; some of its q&#39;s may remain locked if they were locked individually...
Definition: Assembler.h:412
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex)
void initialize(const State &state)
Set the internal State and initialize.
Definition: Assembler.h:322
Assembler & setErrorTolerance(Real tolerance=0)
Set the assembly error tolerance.
Definition: Assembler.h:197
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
void addReporter(const EventReporter &reporter)
Given a reference to an EventReporter, use this Reporter to provide progress reporting.
Definition: Assembler.h:553
Vec2 getFreeQBounds(FreeQIndex freeQIndex) const
Return the allowable range for a particular free q.
Definition: Assembler.h:578
Assembler & setAssemblyConditionWeight(AssemblyConditionIndex condition, Real weight)
Set the weight to be used for this AssemblyCondition.
Definition: Assembler.h:261
Assembler & setInternalState(const State &state)
Set the Assembler&#39;s internal state from an existing state which must be suitable for use with the Ass...
Definition: Assembler.h:307
void updateFromInternalState(State &state) const
Given an existing State that is suitable for the Assembler&#39;s System, update its q&#39;s from those found ...
Definition: Assembler.h:382
const SimbodyMatterSubsystem & getMatterSubsystem() const
Every Simbody header and source file should include this header before any other Simbody header...
This Study attempts to find a configuration (set of joint coordinates q) of a Simbody MultibodySystem...
Definition: Assembler.h:148
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
void realizeModel(State &state) const
Realize the model to be used for subsequent computations with the given state.
Real assemble(State &state)
Given an initial value for the State, modify the q&#39;s in it to satisfy all the assembly conditions to ...
Definition: Assembler.h:355
void setForceNumericalGradient(bool yesno)
This is useful for debugging but should not be used otherwise since the analytic gradient is to be pr...
Definition: Assembler.h:514
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
void unrestrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
Unrestrict a particular generalized coordinate q if it was previously restricted. ...
Definition: Assembler.h:465
void realize(const State &state, Stage stage=Stage::HighestRuntime) const
Realize the given state to the indicated stage.
bool isInitialized() const
Check whether the Assembler has been initialized since the last change was made to its contents...
Definition: Assembler.h:540
Real getSystemConstraintsWeight() const
Return the current weight being given to the System&#39;s built-in Constraints; the default is Infinity...
Definition: Assembler.h:251
Spatial configuration available.
Definition: Stage.h:74
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const
Return the absolute q index associated with a free q.
Definition: Assembler.h:566
Unique integer type for Subsystem-local q indexing.
bool isUsingRMSErrorNorm() const
Determine whether we are currently using the RMS norm for constraint errors; if not we&#39;re using the d...
Definition: Assembler.h:531
#define SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(PARENT, NAME)
Define a local Index class within a Parent class.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:437
Real getAssemblyConditionWeight(AssemblyConditionIndex condition) const
Return the weight currently in use for this AssemblyCondition.
Definition: Assembler.h:278
int getNumFreeQs() const
Return the number of q&#39;s which are free to be changed by this already-initialized assembly analysis...
Definition: Assembler.h:560
#define SimTK_INDEXCHECK_ALWAYS(ix, ub, where)
Definition: ExceptionMacros.h:106
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
void lockMobilizer(MobilizedBodyIndex mbx)
Lock this mobilizer at its starting position.
Definition: Assembler.h:405
void setUseRMSErrorNorm(bool yesno)
Use an RMS norm for the assembly errors rather than the default infinity norm (max absolute value)...
Definition: Assembler.h:526
An EventReporter is an object that defines an event that can occur within a system.
Definition: EventReporter.h:53
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
Definition: Study.h:56
void restrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx, Real lowerBound, Real upperBound)
Restrict a q to remain within a given range.
Definition: Assembler.h:449
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
FreeQIndex getFreeQIndexOfQ(QIndex qx) const
A subset of the q&#39;s will be used as free q&#39;s for solving the assembly problem.
Definition: Assembler.h:573
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_<Real>.
Definition: BigMatrix.h:1473
void setForceNumericalJacobian(bool yesno)
This is useful for debugging but should not be used otherwise since the analytic Jacobian is to be pr...
Definition: Assembler.h:518
Real getErrorToleranceInUse() const
Obtain the tolerance setting that will be used during the next assemble() or track() call...
Definition: Assembler.h:209
int size() const
Definition: VectorBase.h:396
const MultibodySystem & getMultibodySystem() const
Return a reference to the MultibodySystem associated with this Assembler (that is, the System that was supplied in the Assembler&#39;s constructor.
Definition: Assembler.h:586
Define an assembly condition consisting of a scalar goal and/or a related set of assembly error equat...
Definition: AssemblyCondition.h:44
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
const SimbodyMatterSubsystem & getMatterSubsystem() const
Return a reference to the SimbodyMatterSubsystem that is contained in the MultibodySystem that is ass...
Definition: Assembler.h:590
Assembler & setSystemConstraintsWeight(Real weight)
Change how the System&#39;s enabled built-in Constraints are weighted as compared to other assembly condi...
Definition: Assembler.h:243
Assembler & setAccuracy(Real accuracy=0)
Set the accuracy to which a solution should be pursued.
Definition: Assembler.h:223
Real getAccuracyInUse() const
Obtain the accuracy setting that will be used during the next assemble() or track() call...
Definition: Assembler.h:233
const State & getInternalState() const
This provides read-only access to the Assembler&#39;s internal State; you probably should use updateFromI...
Definition: Assembler.h:548
Vec< 2 > Vec2
This is the most common 2D vector type: a column of 2 Real values stored consecutively in memory (pac...
Definition: SmallMatrix.h:126