1 #ifndef SimTK_SIMBODY_ASSEMBLER_H_
2 #define SimTK_SIMBODY_ASSEMBLER_H_
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;
163 class AssembleFailed;
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;
210 return tolerance > 0 ? tolerance
211 : (accuracy > 0 ? accuracy/10 :
Real(0.1)/OODefaultAccuracy);
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;
234 {
return accuracy > 0 ? accuracy :
Real(1)/OODefaultAccuracy; }
244 { assert(systemConstraints.isValid());
245 setAssemblyConditionWeight(systemConstraints,weight);
252 { assert(systemConstraints.isValid());
253 return getAssemblyConditionWeight(systemConstraints); }
264 "Assembler::setAssemblyConditionWeight()");
266 "Illegal weight %g; weight must be nonnegative.", weight);
268 weights[condition] = weight;
280 "Assembler::getAssemblyConditionWeight()");
281 return weights[condition];
288 AssemblyConditionIndex
298 AssemblyConditionIndex
309 getMatterSubsystem().convertToEulerAngles(state, internalState);
310 system.realizeModel(internalState);
323 { setInternalState(state); initialize(); }
356 setInternalState(state);
357 Real achievedCost = assemble();
358 updateFromInternalState(state);
383 system.realizeModel(state);
384 if (!getMatterSubsystem().getUseEulerAngles(state)) {
386 getMatterSubsystem().convertToQuaternions(getInternalState(),
390 state.
updQ() = getInternalState().getQ();
406 { uninitialize(); userLockedMobilizers.insert(mbx); }
413 { uninitialize(); userLockedMobilizers.erase(mbx); }
427 { uninitialize(); userLockedQs[mbx].insert(qx); }
434 { LockedQs::iterator p = userLockedQs.find(mbx);
435 if (p == userLockedQs.end())
return;
436 QSet& qs = p->second;
440 userLockedQs.erase(p);
452 "The given range [%g,%g] is illegal because the lower bound is"
453 " greater than the upper bound.", lowerBound, upperBound);
455 { unrestrictQ(mbx,qx);
return; }
457 userRestrictedQs[mbx][qx] =
Vec2(lowerBound,upperBound);
466 { RestrictedQs::iterator p = userRestrictedQs.find(mbx);
467 if (p == userRestrictedQs.end())
return;
468 QRanges& qranges = p->second;
469 if (qranges.erase(qx)) {
472 userRestrictedQs.erase(p);
515 { forceNumericalGradient = yesno; }
519 { forceNumericalJacobian = yesno; }
527 { useRMSErrorNorm = yesno; }
554 reporters.push_back(&reporter);
561 {
return freeQ2Q.size(); }
567 {
return freeQ2Q[freeQIndex]; }
574 {
return q2FreeQ[qx]; }
580 else return Vec2(lower[freeQIndex], upper[freeQIndex]);
591 {
return system.getMatterSubsystem(); }
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];
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)];
620 void reinitializeWithExtraQsLocked
621 (
const Array_<QIndex>& toBeLocked)
const;
628 const MultibodySystem& system;
629 Array_<const EventReporter*> reporters;
632 static const int OODefaultAccuracy = 1000;
635 bool forceNumericalGradient;
636 bool forceNumericalJacobian;
637 bool useRMSErrorNorm;
644 LockedMobilizers userLockedMobilizers;
647 LockedQs userLockedQs;
649 RestrictedQs userRestrictedQs;
654 Array_<AssemblyCondition*,AssemblyConditionIndex>
656 Array_<Real,AssemblyConditionIndex> weights;
661 AssemblyConditionIndex systemConstraints;
665 mutable bool alreadyInitialized;
668 mutable Array_<QIndex> extraQsLocked;
671 mutable std::set<QIndex> lockedQs;
672 mutable Array_<FreeQIndex,QIndex> q2FreeQ;
673 mutable Array_<QIndex,FreeQIndex> freeQ2Q;
675 mutable Vector lower, upper;
678 mutable Array_<AssemblyConditionIndex> errors;
679 mutable Array_<int> nTermsPerError;
680 mutable Array_<AssemblyConditionIndex> goals;
682 class AssemblerSystem;
683 mutable AssemblerSystem* asmSys;
684 mutable Optimizer* optimizer;
686 mutable int nAssemblySteps;
687 mutable int nInitializations;
689 friend class AssemblerSystem;
#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
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
void initialize(const State &state)
Set the internal State and initialize.
Definition: Assembler.h:322
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
const MultibodySystem & getMultibodySystem() const
Return a reference to the MultibodySystem associated with this Assembler (that is,...
Definition: Assembler.h:586
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(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
Assembler & setAssemblyConditionWeight(AssemblyConditionIndex condition, Real weight)
Set the weight to be used for this AssemblyCondition.
Definition: Assembler.h:261
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 & setSystemConstraintsWeight(Real weight)
Change how the System's enabled built-in Constraints are weighted as compared to other assembly condi...
Definition: Assembler.h:243
int getNumGoalEvals() const
Return the number of goal evaluations.
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
Assembler & setAccuracy(Real accuracy=0)
Set the accuracy to which a solution should be pursued.
Definition: Assembler.h:223
void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
Unlock one of this mobilizer's q's if it was locked.
Definition: Assembler.h:433
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
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...
const SimbodyMatterSubsystem & getMatterSubsystem() const
Return a reference to the SimbodyMatterSubsystem that is contained in the MultibodySystem that is ass...
Definition: Assembler.h:590
Real getErrorToleranceInUse() const
Obtain the tolerance setting that will be used during the next assemble() or track() call.
Definition: Assembler.h:209
void uninitialize() const
Uninitialize the Assembler.
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.
Assembler & setErrorTolerance(Real tolerance=0)
Set the assembly error tolerance.
Definition: Assembler.h:197
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...
const State & getInternalState() const
This provides read-only access to the Assembler's internal State; you probably should use updateFromI...
Definition: Assembler.h:548
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
@ Position
Spatial configuration available.
Definition: Stage.h:74
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
Vector & updQ(SubsystemIndex)
const Vector & getQ(SubsystemIndex) const
Per-subsystem access to the global shared variables.
int size() const
Definition: VectorBase.h:396
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
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_<Real>.
Definition: BigMatrix.h:1477
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:607
SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex)