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);
319 void initialize()
const;
323 { setInternalState(state); initialize(); }
356 setInternalState(state);
357 Real achievedCost = assemble();
358 updateFromInternalState(state);
366 Real calcCurrentGoal()
const;
375 Real calcCurrentErrorNorm()
const;
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);
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;
515 { forceNumericalGradient = yesno; }
519 { forceNumericalJacobian = yesno; }
527 { useRMSErrorNorm = yesno; }
537 void uninitialize()
const;
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]);
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;
694 #endif // SimTK_SIMBODY_ASSEMBLER_H_ void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
Unlock one of this mobilizer's q'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's q'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'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'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's System, update its q'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'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'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'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'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
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's will be used as free q'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'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'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'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