Simbody
|
This Study attempts to find a configuration (set of joint coordinates q) of a Simbody MultibodySystem that satisfies the System's position Constraints plus optional additional assembly conditions. More...
#include <Assembler.h>
Public Member Functions | |
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 Assembler is permitted to change. | |
~Assembler () | |
Destruct the Assembler objects and any Assembly Condition objects it contains. | |
Construction and setup | |
By default, the only assembly condition is that any Simbody Constraints in the System that are enabled must be satisifed to within the assembly tolerance. You can selectively enable and disable Constraints in the state using the ordinary Constraint::disable() and enable() methods. You can also apply an overall weighting to these Constraints here if you want; if the weight is zero they will be ignored; if Infinity they are treated as must-satisfy assembly error conditions; any other number is used to weight the RMS Constraint error into the scalar objective along with the other goals. Additional assembly conditions may be specified with these methods; some predefined conditions are available, most notably Markers for tracking observed marker locations. | |
Assembler (const MultibodySystem &system) | |
Create an Assembler study for the given MultibodySystem. | |
void | setErrorTolerance (Real tolerance=0) |
Set the assembly error tolerance. | |
Real | getErrorToleranceInUse () const |
Obtain the tolerance setting that will be used during the next assemble() or track() call. | |
void | setAccuracy (Real accuracy=0) |
Set the accuracy to which a solution should be pursued. | |
Real | getAccuracyInUse () const |
Obtain the accuracy setting that will be used during the next assemble() or track() call. | |
void | setSystemConstraintsWeight (Real weight) |
Change how the System's enabled built-in Constraints are weighted as compared to other assembly conditions. | |
Real | getSystemConstraintsWeight () const |
Return the current weight being given to the System's built-in Constraints; the default is Infinity. | |
void | setAssemblyConditionWeight (AssemblyConditionIndex condition, Real weight) |
Set the weight to be used for this AssemblyCondition. | |
Real | getAssemblyConditionWeight (AssemblyConditionIndex condition) const |
Return the weight currently in use for this AssemblyCondition. | |
AssemblyConditionIndex | adoptAssemblyError (AssemblyCondition *p) |
Add an assembly error condition to this Assembler study, taking over ownership of the heap-allocated AssemblyCondition object. | |
AssemblyConditionIndex | adoptAssemblyGoal (AssemblyCondition *p, Real weight=1) |
Add an assembly goal to this Assembler study, taking over ownership of the heap-allocated AssemblyCondition object. | |
void | setInternalState (const State &state) |
Set the Assembler's internal state from an existing state which must be suitable for use with the Assembler's System as supplied at the time the Assembler was constructed. | |
void | initialize () const |
Initialize the Assembler to prepare for performing assembly analysis. | |
void | initialize (const State &state) |
Set the internal State and initialize. | |
Execution | |
These methods perform assembly or tracking analysis, determine how successful they were, and obtain results. | |
Real | assemble () |
Starting with the current value of the internally-maintained State, modify the q's in it to satisfy all the assembly conditions to within a tolerance. | |
Real | track (Real frameTime=-1) |
Continue a series of assembly steps that is already in progress, without restarting or reanalyzing the system, and optionally providing a new frame time. | |
Real | assemble (State &state) |
Given an initial value for the State, modify the q's in it to satisfy all the assembly conditions to within a tolerance. | |
Real | calcCurrentGoal () const |
Return the goal value attained by the internal State's current settings for the free q's; this is a weighted sum of the individual goal values for each assembly goal. | |
Real | calcCurrentErrorNorm () const |
This is the weighted norm of the assembly constraint errors directly comparable with the assembly error tolerance setting. | |
void | updateFromInternalState (State &state) const |
Given an existing State that is suitable for the Assembler's System, update its q's from those found in the Assembler's internal State, leaving everything else unchanged. | |
Parameter restrictions | |
These methods restrict which q's are allowed to be modified while trying to assemble the system, or restrict the range within which the final q's must lie. | |
void | lockMobilizer (MobilizedBodyIndex mbx) |
Lock this mobilizer at its starting position. | |
void | unlockMobilizer (MobilizedBodyIndex mbx) |
Unlock this mobilizer as a whole; some of its q's may remain locked if they were locked individually. | |
void | lockQ (MobilizedBodyIndex mbx, MobilizerQIndex qx) |
Lock one of this mobilizer's q's at its initial value. | |
void | unlockQ (MobilizedBodyIndex mbx, MobilizerQIndex qx) |
Unlock one of this mobilizer's q's if it was locked. | |
void | restrictQ (MobilizedBodyIndex mbx, MobilizerQIndex qx, Real lowerBound, Real upperBound) |
Restrict a q to remain within a given range. | |
void | unrestrictQ (MobilizedBodyIndex mbx, MobilizerQIndex qx) |
Unrestrict a particular generalized coordinate q if it was previously restricted. | |
Statistics | |
The Assembler keeps counters of various internal operations it performs during execution; these methods access those counters. These can be helpful in evaluating the effects of various ways of structuring the assembly or tracking problem. Counters can also be reset to zero manually by calling resetStats(). | |
int | getNumGoalEvals () const |
Return the number of goal evaluations. | |
int | getNumErrorEvals () const |
Return the number of assembly error condition evaluations. | |
int | getNumGoalGradientEvals () const |
Return the number of goal gradient evaluations. | |
int | getNumErrorJacobianEvals () const |
Return the number of assembly error condition Jacobian evaluations. | |
int | getNumAssemblySteps () const |
Return the number of assembly steps; that is, the number of calls to assemble() or track() since last initialization. | |
int | getNumInitializations () const |
Return the number of system initializations performed since this Assembler was created or the most recent resetStats() call. | |
void | resetStats () const |
Reset all counters to zero; except for the number of initializations counter this also happens whenever the assembler system is reinitialized either explicitly or due to system changes. | |
Advanced options | |
These are primarily useful for debugging while developing new AssemblyCondition classes. | |
void | setForceNumericalGradient (bool yesno) |
This is useful for debugging but should not be used otherwise since the analytic gradient is to be preferred. | |
void | setForceNumericalJacobian (bool yesno) |
This is useful for debugging but should not be used otherwise since the analytic Jacobian is to be preferred. | |
void | setUseRMSErrorNorm (bool yesno) |
Use an RMS norm for the assembly errors rather than the default infinity norm (max absolute value). | |
bool | isUsingRMSErrorNorm () const |
Determine whether we are currently using the RMS norm for constraint errors; if not we're using the default infinity norm (max absolute value). | |
void | uninitialize () const |
Uninitialize the Assembler. | |
bool | isInitialized () const |
Check whether the Assembler has been initialized since the last change was made to its contents. | |
const State & | getInternalState () const |
This provides read-only access to the Assembler's internal State; you probably should use updateFromInternalState() to transfer just q's from the internal state to your own State. | |
void | addReporter (const EventReporter &reporter) |
Given a reference to an EventReporter, use this Reporter to provide progress reporting. | |
int | getNumFreeQs () const |
Return the number of q's which are free to be changed by this already-initialized assembly analysis. | |
QIndex | getQIndexOfFreeQ (FreeQIndex freeQIndex) const |
Return the absolute q index associated with a free q. | |
FreeQIndex | getFreeQIndexOfQ (QIndex qx) const |
A subset of the q's will be used as free q's for solving the assembly problem. | |
Vec2 | getFreeQBounds (FreeQIndex freeQIndex) const |
Return the allowable range for a particular free q. | |
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. | |
const SimbodyMatterSubsystem & | getMatterSubsystem () const |
Return a reference to the SimbodyMatterSubsystem that is contained in the MultibodySystem that is associated with this Assembler. | |
Friends | |
class | AssemblerSystem |
This Study attempts to find a configuration (set of joint coordinates q) of a Simbody MultibodySystem that satisfies the System's position Constraints plus optional additional assembly conditions.
If successful, the final set of q's will satisfy the constraints to within a specified tolerance. The Assembler also supports high-performance repeated assembly (also known as "inverse kinematics" or "tracking") where only small changes are expected between a series of observation frames.
The complete specification for an Assembly study consists of four elements:
By default, all q's may be modified with no range restrictions. The assembly error conditions are just the errors in the position (holonomic) constraints that are present in the MultibodySystem and currently enabled. (Quaternion normalization constraints will also be satisfied, but do not generate assembly errors.) There are no default assembly goals. This is very similiar in behavior to the System's project() method except that project() considers it an error if the constraints aren't already close to being satisfied initially, while Assembler will attempt to satisfy them regardless, and may take a series of increasingly desperate measures to do so.
This is the most common use of the Assembler: modify a System's given State so that its configuration (set of generalized coordinates q) satisfies the System's built-in Constraints that are currently enabled in that State. This is done to a default tolerance if you don't provide one, and that tolerance is suitable for use with subsequent dynamic studies that are run at their default tolerances. Although the assembly begins from the configuration provided in the initial state, no assumption is made about how close this initial configuration is to one that satisfies the assembly conditions.
MultibodySystem system; // ... build system; get initial state Assembler assembler(system); // construct the Assembler study object try // modify state to satisfy Constraints { assembler.assemble(state); } catch (std::exception exc) { std::cout << "Assembly failed: " << exc.what() << std::endl; }
After the initial assembly done as above, an inverse kinematic study consists of a series of assembly solutions for a sequence of small changes to the assembly conditions. A common example is the tracking of a time series of marker observations. Thus each "tracking" assembly computation may assume:
Allowable (gradual) changes between tracking frames are:
The track() method performs assembly analysis under these assumptions and can be much faster than assemble(). Here is an outline of code that performs repeated tracking of data from a series of observation frames, each associated with a frame time:
MultibodySystem system; // ... build system; get initial state Assembler assembler(system); // construct the Assembler Study object // ... set up assembly conditions; perform initial assemble() as above; // assume assembled result is in State myState. try // track a series of small changes to the assembly conditions { for (int i=0; i < numFrames; ++i) { // ... update assembly conditions for frame[i] assembler.track(frameTime[i]); assembler.updateFromInternalState(myState); // update time and qs // ... do something with the results in myState } } catch (std::exception exc) { std::cout << "Tracking failed: " << exc.what() << std::endl; }
Optional settings include:
Assembly errors are specified by giving an assembly condition a weight of Infinity. Anything with a lower weight is a goal and will be combined with all the other goals into a single scalar objective. The built-in Constraints are normally treated with infinite weight, but you can change them to goals instead if you like; sometimes that can be useful as a step in getting a difficult-to- assemble system assembled.
SimTK::Assembler::Assembler | ( | const MultibodySystem & | system | ) | [explicit] |
Create an Assembler study for the given MultibodySystem.
The Assembler's current state is set to the System's default state but with Euler angles used instead of quaternions.
SimTK::Assembler::~Assembler | ( | ) |
Destruct the Assembler objects and any Assembly Condition objects it contains.
SimTK::Assembler::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 Assembler is permitted to change.
void SimTK::Assembler::setErrorTolerance | ( | Real | tolerance = 0 | ) | [inline] |
Set the assembly error tolerance.
This value is tested against a norm of all the assembly error conditions to determine whether an assemble() or track() operation was successful. Note that assembly errors may have arbitrary units (for example, built in Constraint errors may be distances or angles); in general they must be scaled so that the same tolerance value can be used for all of them. By default, tolerance is set to accuracy/10 if accuracy has been set, otherwise 1e-4; calling setErrorTolerance() with no argument or with zero restores it to its default behavior.
Real SimTK::Assembler::getErrorToleranceInUse | ( | ) | const [inline] |
Obtain the tolerance setting that will be used during the next assemble() or track() call.
Note that this may be an explicitly-set tolerance or a default value calculated as accuracy/10 if accuracy has been set, otherwise 1e-4.
void SimTK::Assembler::setAccuracy | ( | Real | accuracy = 0 | ) | [inline] |
Set the accuracy to which a solution should be pursued.
This is a unitless value that is roughly interpreted as a request for a certain number of "correct" digits in the "answer", so that an accuracy of 0.001 means "1/10 of 1 percent" or roughly three digits, meaning that we would like the assembly to stop when the goal is within 0.1% of its minimum. However, if you don't say otherwise, this number is also used to set the absolute error tolerance used to determine whether the assembly succeeded or failed, by the following formula: error tolerance = accuracy/10. By default, we set accuracy=1e-3 and tolerance=1e-4.
Real SimTK::Assembler::getAccuracyInUse | ( | ) | const [inline] |
Obtain the accuracy setting that will be used during the next assemble() or track() call.
The default is to use 1e-3, i.e., 1/10 of 1%.
void SimTK::Assembler::setSystemConstraintsWeight | ( | Real | weight | ) | [inline] |
Change how the System's enabled built-in Constraints are weighted as compared to other assembly conditions.
If this is Infinity (the default) then the built-ins are treated as must-satisfy constraints; otherwise they are included in the assembly cost function with the given weight. If the weight is given as zero the built-in Constraints will be ignored altogether.
Real SimTK::Assembler::getSystemConstraintsWeight | ( | ) | const [inline] |
Return the current weight being given to the System's built-in Constraints; the default is Infinity.
void SimTK::Assembler::setAssemblyConditionWeight | ( | AssemblyConditionIndex | condition, |
Real | weight | ||
) | [inline] |
Set the weight to be used for this AssemblyCondition.
If the weight is set to 0, this condition will be disabled and will be ignored. If the weight is set to Infinity, the condition will be treated as an assembly error condition that must be satisfied to tolerance. Otherwise (finite weight) the condition will be treated as an assembly goal and the weight will be used to combine its cost function with that of the other assembly goals.
Real SimTK::Assembler::getAssemblyConditionWeight | ( | AssemblyConditionIndex | condition | ) | const [inline] |
Return the weight currently in use for this AssemblyCondition.
If the returned value is 0, this condition is being ignored. If the weight is Infinity, then the condition is being treated as an assembly error condition that must be satisfied to tolerance. Otherwise (finite weight) this is an assembly goal and the weight is used to combine its cost function with that of the other assembly goals.
AssemblyConditionIndex SimTK::Assembler::adoptAssemblyError | ( | AssemblyCondition * | p | ) |
Add an assembly error condition to this Assembler study, taking over ownership of the heap-allocated AssemblyCondition object.
We will use the calcErrors() method of this object to determine errors whose norm must be driven below tolerance for an assembly to be considered successful.
AssemblyConditionIndex SimTK::Assembler::adoptAssemblyGoal | ( | AssemblyCondition * | p, |
Real | weight = 1 |
||
) |
Add an assembly goal to this Assembler study, taking over ownership of the heap-allocated AssemblyCondition object.
We will use normally use the calcGoal() method of this object to calculate its contribution to the assembly goal cost function. An optional weight can be provided that is used when combining this cost with those of other goals to form the overall cost function; the default weight is 1. If the weight is 0 the goal is ignored and not evaluated at all; if the weight is Infinity this is actually an assembly constraint and we'll use its calcErrors() method instead.
void SimTK::Assembler::setInternalState | ( | const State & | state | ) | [inline] |
Set the Assembler's internal state from an existing state which must be suitable for use with the Assembler's System as supplied at the time the Assembler was constructed.
All variables are copied, not just q's, so the Assembler must be reinitialized after this call in case modeling options, instance variables, or time have changed.
void SimTK::Assembler::initialize | ( | ) | const |
Initialize the Assembler to prepare for performing assembly analysis.
This is normally called automatically when assemble() is called, but you can call it explicitly and then access methods that report on the properties of the system on which the analysis will be performed. The internal state should already have been set; if you want to provide the state now use initialize(State).
void SimTK::Assembler::initialize | ( | const State & | state | ) | [inline] |
Set the internal State and initialize.
See setInternalState() and initialize() methods for more information.
Real SimTK::Assembler::assemble | ( | ) |
Starting with the current value of the internally-maintained State, modify the q's in it to satisfy all the assembly conditions to within a tolerance.
The actual tolerance achieved is returned as the function value.
Real SimTK::Assembler::track | ( | Real | frameTime = -1 | ) |
Continue a series of assembly steps that is already in progress, without restarting or reanalyzing the system, and optionally providing a new frame time.
This is designed for use with a series of assembly frames that are close together so that no heroic measures are needed to go from one to the next. For the first frame, and any time there might be a change to the problem structure or a major change to the state, use assemble() instead of track(). See the Assembler class documentation for more information and usage examples.
Real SimTK::Assembler::assemble | ( | State & | state | ) | [inline] |
Given an initial value for the State, modify the q's in it to satisfy all the assembly conditions to within a tolerance.
The actual tolerance achieved is returned as the function value.
[in,out] | state | The initial and final State value. Only q's are modified. |
Real SimTK::Assembler::calcCurrentGoal | ( | ) | const |
Return the goal value attained by the internal State's current settings for the free q's; this is a weighted sum of the individual goal values for each assembly goal.
Goal values are nonnegative scalars.
Real SimTK::Assembler::calcCurrentErrorNorm | ( | ) | const |
This is the weighted norm of the assembly constraint errors directly comparable with the assembly error tolerance setting.
That is, if this number is less than or equal to tolerance (as returned by getErrorToleranceInUse()), then the current state is a feasible assembly solution (although it may not be optimal). Note that by default we use the infinity norm (maximum absolute value of any error term) but that you can specify use of an RMS norm instead via setUseRMSErrorNorm().
void SimTK::Assembler::updateFromInternalState | ( | State & | state | ) | const [inline] |
void SimTK::Assembler::lockMobilizer | ( | MobilizedBodyIndex | mbx | ) | [inline] |
Lock this mobilizer at its starting position.
This overrides any individual q specifications, so even if a q was specifically unlocked it will not move until the mobilizer as a whole is unlocked.
void SimTK::Assembler::unlockMobilizer | ( | MobilizedBodyIndex | mbx | ) | [inline] |
Unlock this mobilizer as a whole; some of its q's may remain locked if they were locked individually.
It is OK if this mobilizer was already unlocked; in that case this does nothing.
void SimTK::Assembler::lockQ | ( | MobilizedBodyIndex | mbx, |
MobilizerQIndex | qx | ||
) | [inline] |
Lock one of this mobilizer's q's at its initial value.
Be careful with this method because it requires that you understand the order of the generalized coordinates used by this particular mobilizer during assembly. In particular, the mobilizer will be modeled with Euler angles rather than quaternions and you must know the Euler sequence it uses in that case (that is, body- or space-fixed, 2 or 3 axes, and the rotation order). It is preferable to use lockMobilizer() instead since that will lock all the q's however they are defined. Note that locking individual q's with this method is independent of whole-mobilizer locking. If you unlock the mobilizer with unlockMobilizer(), any q's which have been explicitly locked with lockQ() will remain locked.
void SimTK::Assembler::unlockQ | ( | MobilizedBodyIndex | mbx, |
MobilizerQIndex | qx | ||
) | [inline] |
Unlock one of this mobilizer's q's if it was locked.
Note that this will not take effect immediately if the mobilizer as a whole has been locked with lockMobilizer(); you have to unlockMobilizer() first.
void SimTK::Assembler::restrictQ | ( | MobilizedBodyIndex | mbx, |
MobilizerQIndex | qx, | ||
Real | lowerBound, | ||
Real | upperBound | ||
) | [inline] |
Restrict a q to remain within a given range.
Caution: this requires that you understand the order of the generalized coordinates used by this particular mobilizer during assembly; see lockQ() for a discussion. You can use -Infinity or Infinity to indicate that the q is not bounded in one direction.
void SimTK::Assembler::unrestrictQ | ( | MobilizedBodyIndex | mbx, |
MobilizerQIndex | qx | ||
) | [inline] |
Unrestrict a particular generalized coordinate q if it was previously restricted.
Note that this is independent of whether the q has been locked with lockMobilizer() or lockQ(); that is, the q may still be locked even though it is now unrestricted.
int SimTK::Assembler::getNumGoalEvals | ( | ) | const |
Return the number of goal evaluations.
int SimTK::Assembler::getNumErrorEvals | ( | ) | const |
Return the number of assembly error condition evaluations.
int SimTK::Assembler::getNumGoalGradientEvals | ( | ) | const |
Return the number of goal gradient evaluations.
int SimTK::Assembler::getNumErrorJacobianEvals | ( | ) | const |
Return the number of assembly error condition Jacobian evaluations.
int SimTK::Assembler::getNumAssemblySteps | ( | ) | const |
Return the number of assembly steps; that is, the number of calls to assemble() or track() since last initialization.
int SimTK::Assembler::getNumInitializations | ( | ) | const |
Return the number of system initializations performed since this Assembler was created or the most recent resetStats() call.
void SimTK::Assembler::resetStats | ( | ) | const |
Reset all counters to zero; except for the number of initializations counter this also happens whenever the assembler system is reinitialized either explicitly or due to system changes.
void SimTK::Assembler::setForceNumericalGradient | ( | bool | yesno | ) | [inline] |
This is useful for debugging but should not be used otherwise since the analytic gradient is to be preferred.
void SimTK::Assembler::setForceNumericalJacobian | ( | bool | yesno | ) | [inline] |
This is useful for debugging but should not be used otherwise since the analytic Jacobian is to be preferred.
void SimTK::Assembler::setUseRMSErrorNorm | ( | bool | yesno | ) | [inline] |
Use an RMS norm for the assembly errors rather than the default infinity norm (max absolute value).
RMS is less stringent and defines success based on on a good "average" case rather than a good worst case. If there are n error terms ei, the default norm is e=max_i(abs(ei)) and the RMS norm is e=sqrt(sum_i(ei^2)/n).
bool SimTK::Assembler::isUsingRMSErrorNorm | ( | ) | const [inline] |
Determine whether we are currently using the RMS norm for constraint errors; if not we're using the default infinity norm (max absolute value).
void SimTK::Assembler::uninitialize | ( | ) | const |
bool SimTK::Assembler::isInitialized | ( | ) | const [inline] |
Check whether the Assembler has been initialized since the last change was made to its contents.
const State& SimTK::Assembler::getInternalState | ( | ) | const [inline] |
This provides read-only access to the Assembler's internal State; you probably should use updateFromInternalState() to transfer just q's from the internal state to your own State.
Be aware that the internal state is always maintained using Euler angles for rotations rather than quaternions, while updateFromInternalState() will make sure you get the rotations in the form you want.
void SimTK::Assembler::addReporter | ( | const EventReporter & | reporter | ) | [inline] |
Given a reference to an EventReporter, use this Reporter to provide progress reporting.
The EventReporter object must be owned by someone else and persist throughout the lifetime of this Assembler object.
int SimTK::Assembler::getNumFreeQs | ( | ) | const [inline] |
Return the number of q's which are free to be changed by this already-initialized assembly analysis.
The rest of the q's are locked at their initial values.
QIndex SimTK::Assembler::getQIndexOfFreeQ | ( | FreeQIndex | freeQIndex | ) | const [inline] |
Return the absolute q index associated with a free q.
Every free q is associated with a q so this will always return a valid index if the free q index is in range.
FreeQIndex SimTK::Assembler::getFreeQIndexOfQ | ( | QIndex | qx | ) | const [inline] |
A subset of the q's will be used as free q's for solving the assembly problem.
Given an absolute q index, this will return the corresponding free q index if there is one; otherwise, the returned index will be invalid meaning that this q is currently locked.
Vec2 SimTK::Assembler::getFreeQBounds | ( | FreeQIndex | freeQIndex | ) | const [inline] |
Return the allowable range for a particular free q.
If this free q is unrestricted the returned range will be [-Infinity,Infinity].
const MultibodySystem& SimTK::Assembler::getMultibodySystem | ( | ) | const [inline] |
Return a reference to the MultibodySystem associated with this Assembler (that is, the System that was supplied in the Assembler's constructor.
const SimbodyMatterSubsystem& SimTK::Assembler::getMatterSubsystem | ( | ) | const [inline] |
Return a reference to the SimbodyMatterSubsystem that is contained in the MultibodySystem that is associated with this Assembler.
friend class AssemblerSystem [friend] |