Simbody 3.7
Loading...
Searching...
No Matches
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
37namespace SimTK {
38
39SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex);
40
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;
154public:
155
159
163class AssembleFailed;
167class TrackFailed;
168
186explicit 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
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
261Assembler& 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
278Real getAssemblyConditionWeight(AssemblyConditionIndex condition) const {
279 SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
280 "Assembler::getAssemblyConditionWeight()");
281 return weights[condition];
282}
283
288AssemblyConditionIndex
298AssemblyConditionIndex
300
301
308 uninitialize();
309 getMatterSubsystem().convertToEulerAngles(state, internalState);
310 system.realizeModel(internalState);
311 return *this;
312}
319void initialize() const;
322void initialize(const State& state)
323{ setInternalState(state); initialize(); }
330
338
347Real track(Real frameTime = -1);
348
356 setInternalState(state);
357 Real achievedCost = assemble(); // throws if it fails
358 updateFromInternalState(state);
359 return achievedCost;
360}
361
362
376
377
382void 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}
487int getNumGoalEvals() const;
503void resetStats() const;
511
515{ forceNumericalGradient = yesno; }
519{ forceNumericalJacobian = yesno; }
520
526void setUseRMSErrorNorm(bool yesno)
527{ useRMSErrorNorm = yesno; }
531bool isUsingRMSErrorNorm() const {return useRMSErrorNorm;}
532
537void uninitialize() const;
540bool isInitialized() const {return alreadyInitialized;}
541
548const State& getInternalState() const {return internalState;}
549
553void addReporter(const EventReporter& reporter) {
554 reporters.push_back(&reporter);
555}
556
560int getNumFreeQs() const
561{ return freeQ2Q.size(); }
562
566QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const
567{ return freeQ2Q[freeQIndex]; }
568
573FreeQIndex getFreeQIndexOfQ(QIndex qx) const
574{ return q2FreeQ[qx]; }
575
578Vec2 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(); }
597
598
599
600//------------------------------------------------------------------------------
601 private: // methods
602//------------------------------------------------------------------------------
603// Note that the internalState is realized to Stage::Position on return.
604void 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
612Vector 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
620void reinitializeWithExtraQsLocked
621 (const Array_<QIndex>& toBeLocked) const;
622
623
624
625//------------------------------------------------------------------------------
626 private: // data members
627//------------------------------------------------------------------------------
628const MultibodySystem& system;
629Array_<const EventReporter*> reporters; // just references; don't delete
630
631// These members affect the behavior of the assembly algorithm.
632static const int OODefaultAccuracy = 1000; // 1/accuracy if acc==0
633Real accuracy; // 0 means use 1/OODefaultAccuracy
634Real tolerance; // 0 means use accuracy/10
635bool forceNumericalGradient; // ignore analytic gradient methods
636bool forceNumericalJacobian; // ignore analytic Jacobian methods
637bool useRMSErrorNorm; // what norm defines success?
638
639// Changes to any of these data members set isInitialized()=false.
640State internalState;
641
642// These are the mobilizers that were set in lockMobilizer(). They are
643// separate from those involved in individually-locked q's.
644LockedMobilizers userLockedMobilizers;
645// These are locks placed on individual q's; they are independent of the
646// locked mobilizer settings.
647LockedQs userLockedQs;
648// These are range restrictions placed on individual q's.
649RestrictedQs 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.
654Array_<AssemblyCondition*,AssemblyConditionIndex>
655 conditions;
656Array_<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.
661AssemblyConditionIndex systemConstraints;
662
663
664// These are filled in when the Assembler is initialized.
665mutable bool alreadyInitialized;
666
667// These are extra q's we removed for numerical reasons.
668mutable Array_<QIndex> extraQsLocked;
669
670// These represent restrictions on the independent variables (q's).
671mutable std::set<QIndex> lockedQs;
672mutable Array_<FreeQIndex,QIndex> q2FreeQ; // nq of these
673mutable Array_<QIndex,FreeQIndex> freeQ2Q; // nfreeQ of these
674// 0 length if no bounds; otherwise, index by FreeQIndex.
675mutable Vector lower, upper;
676
677// These represent the active assembly conditions.
678mutable Array_<AssemblyConditionIndex> errors;
679mutable Array_<int> nTermsPerError;
680mutable Array_<AssemblyConditionIndex> goals;
681
682class AssemblerSystem; // local class
683mutable AssemblerSystem* asmSys;
684mutable Optimizer* optimizer;
685
686mutable int nAssemblySteps; // count assemble() and track() calls
687mutable int nInitializations; // # times we had to reinitialize
688
689friend class AssemblerSystem;
690};
691
692} // namespace SimTK
693
694#endif // SimTK_SIMBODY_ASSEMBLER_H_
#define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition ExceptionMacros.h:289
#define SimTK_INDEXCHECK_ALWAYS(ix, ub, where)
Definition ExceptionMacros.h:106
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition ExceptionMacros.h:285
#define SimTK_DEFINE_UNIQUE_INDEX_TYPE(NAME)
Use this macro to define a unique "Index" type which is just a type-safe non-negative int,...
Definition SimTKcommon/include/SimTKcommon/internal/common.h:426
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
This Study attempts to find a configuration (set of joint coordinates q) of a Simbody MultibodySystem...
Definition Assembler.h:148
Assembler & setSystemConstraintsWeight(Real weight)
Change how the System's enabled built-in Constraints are weighted as compared to other assembly condi...
Definition Assembler.h:243
void initialize(const State &state)
Set the internal State and initialize.
Definition Assembler.h:322
Assembler & setAssemblyConditionWeight(AssemblyConditionIndex condition, Real weight)
Set the weight to be used for this AssemblyCondition.
Definition Assembler.h:261
AssemblyConditionIndex adoptAssemblyGoal(AssemblyCondition *p, Real weight=1)
Add an assembly goal to this Assembler study, taking over ownership of the heap-allocated AssemblyCon...
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
bool isUsingRMSErrorNorm() const
Determine whether we are currently using the RMS norm for constraint errors; if not we're using the d...
Definition Assembler.h:531
int getNumErrorEvals() const
Return the number of assembly error condition evaluations.
Real getAssemblyConditionWeight(AssemblyConditionIndex condition) const
Return the weight currently in use for this AssemblyCondition.
Definition Assembler.h:278
Assembler & setAccuracy(Real accuracy=0)
Set the accuracy to which a solution should be pursued.
Definition Assembler.h:223
Assembler(const MultibodySystem &system)
Create an Assembler study for the given MultibodySystem.
void restrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx, Real lowerBound, Real upperBound)
Restrict a q to remain within a given range.
Definition Assembler.h:449
~Assembler()
Destruct the Assembler objects and any Assembly Condition objects it contains.
void lockMobilizer(MobilizedBodyIndex mbx)
Lock this mobilizer at its starting position.
Definition Assembler.h:405
Real calcCurrentErrorNorm() const
This is the weighted norm of the assembly constraint errors directly comparable with the assembly err...
Real getAccuracyInUse() const
Obtain the accuracy setting that will be used during the next assemble() or track() call.
Definition Assembler.h:233
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
const SimbodyMatterSubsystem & getMatterSubsystem() const
Return a reference to the SimbodyMatterSubsystem that is contained in the MultibodySystem that is ass...
Definition Assembler.h:590
void resetStats() const
Reset all counters to zero; except for the number of initializations counter this also happens whenev...
Real calcCurrentGoal() const
Return the goal value attained by the internal State's current settings for the free q's; this is a w...
Real assemble(State &state)
Given an initial value for the State, modify the q's in it to satisfy all the assembly conditions to ...
Definition Assembler.h:355
int getNumAssemblySteps() const
Return the number of assembly steps; that is, the number of calls to assemble() or track() since last...
AssemblyConditionIndex adoptAssemblyError(AssemblyCondition *p)
Add an assembly error condition to this Assembler study, taking over ownership of the heap-allocated ...
Vec2 getFreeQBounds(FreeQIndex freeQIndex) const
Return the allowable range for a particular free q.
Definition Assembler.h:578
int getNumErrorJacobianEvals() const
Return the number of assembly error condition Jacobian evaluations.
QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const
Return the absolute q index associated with a free q.
Definition Assembler.h:566
int getNumFreeQs() const
Return the number of q's which are free to be changed by this already-initialized assembly analysis.
Definition Assembler.h:560
Assembler & setInternalState(const State &state)
Set the Assembler's internal state from an existing state which must be suitable for use with the Ass...
Definition Assembler.h:307
int getNumGoalEvals() const
Return the number of goal evaluations.
Assembler & setErrorTolerance(Real tolerance=0)
Set the assembly error tolerance.
Definition Assembler.h:197
void unlockMobilizer(MobilizedBodyIndex mbx)
Unlock this mobilizer as a whole; some of its q's may remain locked if they were locked individually.
Definition Assembler.h:412
void lockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
Lock one of this mobilizer's q's at its initial value.
Definition Assembler.h:426
FreeQIndex getFreeQIndexOfQ(QIndex qx) const
A subset of the q's will be used as free q's for solving the assembly problem.
Definition Assembler.h:573
void addReporter(const EventReporter &reporter)
Given a reference to an EventReporter, use this Reporter to provide progress reporting.
Definition Assembler.h:553
int getNumInitializations() const
Return the number of system initializations performed since this Assembler was created or the most re...
bool isInitialized() const
Check whether the Assembler has been initialized since the last change was made to its contents.
Definition Assembler.h:540
void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
Unlock one of this mobilizer's q's if it was locked.
Definition Assembler.h:433
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
Real assemble()
Starting with the current value of the internally-maintained State, modify the q's in it to satisfy a...
Real getErrorToleranceInUse() const
Obtain the tolerance setting that will be used during the next assemble() or track() call.
Definition Assembler.h:209
const State & getInternalState() const
This provides read-only access to the Assembler's internal State; you probably should use updateFromI...
Definition Assembler.h:548
void uninitialize() const
Uninitialize the Assembler.
const MultibodySystem & getMultibodySystem() const
Return a reference to the MultibodySystem associated with this Assembler (that is,...
Definition Assembler.h:586
void updateFromInternalState(State &state) const
Given an existing State that is suitable for the Assembler's System, update its q's from those found ...
Definition Assembler.h:382
void initialize() const
Initialize the Assembler to prepare for performing assembly analysis.
int getNumGoalGradientEvals() const
Return the number of goal gradient evaluations.
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Assembler, FreeQIndex)
Assembler::FreeQIndex is a unique integer type used for accessing the subset of q's that the Assemble...
Real getSystemConstraintsWeight() const
Return the current weight being given to the System's built-in Constraints; the default is Infinity.
Definition Assembler.h:251
Real track(Real frameTime=-1)
Continue a series of assembly steps that is already in progress, without restarting or reanalyzing th...
void unrestrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
Unrestrict a particular generalized coordinate q if it was previously restricted.
Definition Assembler.h:465
Define an assembly condition consisting of a scalar goal and/or a related set of assembly error equat...
Definition AssemblyCondition.h:44
An EventReporter is an object that defines an event that can occur within a system.
Definition EventReporter.h:53
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition MultibodySystem.h:48
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
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition State.h:280
const Vector & getQ(SubsystemIndex) const
Per-subsystem access to the global shared variables.
Vector & updQ(SubsystemIndex)
Definition Study.h:56
int size() const
Definition VectorBase.h:396
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition Assembler.h:37
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