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"
28 #include "simbody/internal/common.h"
29 #include "simbody/internal/MultibodySystem.h"
30 #include "simbody/internal/SimbodyMatterSubsystem.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 
43 /** This Study attempts to find a configuration (set of joint coordinates q)
44 of a Simbody MultibodySystem that satisfies the System's position Constraints
45 plus optional additional assembly conditions. If successful, the final set
46 of q's will satisfy the constraints to within a specified tolerance. The
47 Assembler also supports high-performance repeated assembly (also known as
48 "inverse kinematics" or "tracking") where only small changes are expected
49 between a series of observation frames.
50 
51 The complete specification for an Assembly study consists of four elements:
52   - The subset of q's which may be modified by the study.
53   - Limits on the allowable range of values for each q.
54   - A set of assembly error conditions that \e must be satisfied.
55   - A set of weighted assembly goals that are to be achieved as best we can.
56 
57 By default, all q's may be modified with no range restrictions. The q's whose
58 value is a prescribed function of time will be set to that value, while free
59 q's are available for satisfying the assembly error conditions and goals. The
60 assembly error conditions are just the errors in the position (holonomic)
61 constraints that are present in the MultibodySystem and currently enabled.
62 (Quaternion normalization constraints will also be satisfied, but do not
63 generate assembly errors.) There are no default assembly goals. This is very
64 similar in behavior to the System's project() method except that project()
65 considers it an error if the constraints aren't already close to being
66 satisfied initially, while Assembler will attempt to satisfy them regardless,
67 and may take a series of increasingly desperate measures to do so.
68 
69 <h2>Basic assembly:</h2>
70 This is the most common use of the Assembler: modify a System's given State
71 so that its configuration (set of generalized coordinates q) satisfies the
72 position-affecting Motion and Constraint objects in the System that are
73 currently enabled in that State. This is done to a default tolerance if you
74 don't provide one, and that tolerance is suitable for use with subsequent
75 dynamic studies that are run at their default tolerances. Although the assembly
76 begins from the configuration provided in the initial state, no assumption is
77 made about how close this initial configuration is to one that satisfies the
78 assembly conditions.
79 @code
80   MultibodySystem system;
81   // ... build system; get initial state
82 
83   Assembler assembler(system); // construct the Assembler study object
84   try // modify state to satisfy Constraints
85   {   assembler.assemble(state); }
86   catch (const std::exception& exc)
87   {   std::cout << "Assembly failed: " << exc.what() << std::endl; }
88 @endcode
89 
90 <h2>Inverse kinematics (repeated assembly):</h2>
91 After the initial assembly done as above, an inverse kinematic study consists
92 of a series of assembly solutions for a sequence of small changes to the
93 assembly conditions. A common example is the tracking of a time series of
94 marker observations. Thus each "tracking" assembly computation may assume:
95   - There has been no change to the problem structure.
96   - The internal State is already close to the desired solution and has not
97     been changed since the last tracking frame solution.
98 
99 Allowable (gradual) changes between tracking frames are:
100   - Time, which may affect prescribed motion or other time-dependent
101     Constraints as well as time-dependent assembly conditions.
102   - Weights on assembly goals. Continuous changes to goal weights are
103     permitted, but changes between 0 and nonzero or between finite and
104     Infinity should not be made because these are structural changes
105     to the form of the problem and require reinitialization.
106   - Any changes allowed by the assembly conditions currently included in this
107     Assembler, for example marker location observations for the Markers
108     assembly condition.
109 
110 The track() method performs assembly analysis under these assumptions and can
111 be much faster than assemble(). Here is an outline of code that performs
112 repeated tracking of data from a series of observation frames, each associated
113 with a frame time:
114 @code
115   MultibodySystem system;
116   // ... build system; get initial state
117   Assembler assembler(system); // construct the Assembler Study object
118   // ... set up assembly conditions; perform initial assemble() as above;
119   //     assume assembled result is in State myState.
120 
121   try // track a series of small changes to the assembly conditions
122   {   for (int i=0; i < numFrames; ++i) {
123           // ... update assembly conditions for frame[i]
124           assembler.track(frameTime[i]);
125           assembler.updateFromInternalState(myState); // update time and qs
126           // ... do something with the results in myState
127       }
128   }
129   catch (const std::exception& exc)
130   {   std::cout << "Tracking failed: " << exc.what() << std::endl; }
131 @endcode
132 
133 <h2>Optional settings:</h2>
134 Optional settings include:
135   - Locking particular mobilizers so that their q's can't be changed.
136   - Setting bounds on the acceptable range of values for some of the q's.
137   - Defining additional assembly conditions.
138   - Assigning assembly conditions to be errors ("needs") or weighted goals
139     ("wants").
140 
141 Assembly errors are specified by giving an assembly condition a weight of
142 Infinity. Anything with a lower weight is a goal and will be combined with all
143 the other goals into a single scalar objective. The built-in Constraints are
144 normally treated with infinite weight, but you can change them to goals instead
145 if you like; sometimes that can be useful as a step in getting a difficult-to-
146 assemble system assembled.
147 **/
148 class SimTK_SIMBODY_EXPORT Assembler : public Study {
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 
156 /** Assembler::FreeQIndex is a unique integer type used for accessing
157 the subset of q's that the Assembler is permitted to change. **/
158 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Assembler,FreeQIndex);
159 
160 /** This class is the exception object thrown when a call to assemble()
161 is unable to achieve the required error tolerance. It is derived from
162 std::exception so details can be obtained via the what() method. **/
163 class AssembleFailed;
164 /** This class is the exception object thrown when a call to track()
165 is unable to achieve the required error tolerance. It is derived from
166 std::exception so details can be obtained via the what() method. **/
167 class TrackFailed;
168 
169 /** @name             Construction and setup
170 By default, the only assembly condition is that any Simbody Constraints
171 in the System that are enabled must be satisifed to within the assembly
172 tolerance. (Prescribed motions specified with Motion objects are satisfied
173 exactly.) You can selectively enable and disable Constraints in the
174 state using the ordinary Constraint::disable() and enable() methods. You
175 can also apply an overall weighting to these Constraints here if you want;
176 if the weight is zero they will be ignored; if Infinity they are treated
177 as must-satisfy assembly error conditions; any other number is used to
178 weight the RMS Constraint error into the scalar objective along
179 with the other goals. Additional assembly conditions may be specified with
180 these methods; some predefined conditions are available, most notably
181 Markers for tracking observed marker locations. **/
182 /*@{*/
183 /** Create an Assembler study for the given MultibodySystem. The
184 Assembler's current state is set to the System's default state but with
185 Euler angles used instead of quaternions. **/
186 explicit Assembler(const MultibodySystem& system);
187 
188 /** Set the assembly error tolerance. This value is tested against a norm
189 of all the assembly error conditions to determine whether an assemble() or
190 track() operation was successful. Note that assembly errors may have
191 arbitrary units (for example, built in Constraint errors may be distances
192 or angles); in general they must be scaled so that the same tolerance
193 value can be used for all of them. By default, tolerance is set to
194 accuracy/10 if accuracy has been set, otherwise 1e-4; calling
195 setErrorTolerance() with no argument or with zero restores it to its default
196 behavior. **/
197 Assembler& setErrorTolerance(Real tolerance=0) {
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 }
205 /** Obtain the tolerance setting that will be used during the next
206 assemble() or track() call. Note that this may be an explicitly-set
207 tolerance or a default value calculated as accuracy/10 if accuracy has been
208 set, otherwise 1e-4. **/
getErrorToleranceInUse()209 Real getErrorToleranceInUse() const {
210     return tolerance > 0 ? tolerance
211            : (accuracy > 0 ? accuracy/10 : Real(0.1)/OODefaultAccuracy);
212 }
213 
214 /** Set the accuracy to which a solution should be pursued. This is a
215 unitless value that is roughly interpreted as a request for a certain
216 number of "correct" digits in the "answer", so that an accuracy of 0.001
217 means "1/10 of 1 percent" or roughly three digits, meaning that we would
218 like the assembly to stop when the goal is within 0.1% of its minimum.
219 However, if you don't say otherwise, this number is also used to set the
220 absolute error tolerance used to determine whether the assembly succeeded
221 or failed, by the following formula: error tolerance = accuracy/10. By
222 default, we set accuracy=1e-3 and tolerance=1e-4. **/
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 }
231 /** Obtain the accuracy setting that will be used during the next
232 assemble() or track() call. The default is to use 1e-3, i.e., 1/10 of 1%. **/
getAccuracyInUse()233 Real getAccuracyInUse() const
234 {   return accuracy > 0 ? accuracy : Real(1)/OODefaultAccuracy; }
235 
236 
237 /** Change how the System's enabled built-in Constraints are weighted as
238 compared to other assembly conditions. If this is Infinity (the default) then
239 the built-ins are treated as must-satisfy constraints; otherwise they are
240 included in the assembly cost function with the given weight. If the weight is
241 given as zero the built-in Constraints will be ignored altogether.
242 @see setAssemblyConditionWeight() **/
setSystemConstraintsWeight(Real weight)243 Assembler& setSystemConstraintsWeight(Real weight)
244 {   assert(systemConstraints.isValid());
245     setAssemblyConditionWeight(systemConstraints,weight);
246     return *this; }
247 
248 /** Return the current weight being given to the System's built-in
249 Constraints; the default is Infinity.
250 @see getAssemblyConditionWeight() **/
getSystemConstraintsWeight()251 Real getSystemConstraintsWeight() const
252 {   assert(systemConstraints.isValid());
253     return getAssemblyConditionWeight(systemConstraints); }
254 
255 /** Set the weight to be used for this AssemblyCondition. If the weight is set
256 to 0, this condition will be disabled and will be ignored. If the weight is
257 set to Infinity, the condition will be treated as an assembly error condition
258 that must be satisfied to tolerance. Otherwise (finite weight) the condition
259 will be treated as an assembly goal and the weight will be used to combine its
260 cost function with that of the other assembly goals. **/
setAssemblyConditionWeight(AssemblyConditionIndex condition,Real weight)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 
272 /** Return the weight currently in use for this AssemblyCondition. If the
273 returned value is 0, this condition is being ignored. If the weight is
274 Infinity, then the condition is being treated as an assembly error condition
275 that must be satisfied to tolerance. Otherwise (finite weight) this is an
276 assembly goal and the weight is used to combine its cost function with that
277 of the other assembly goals. **/
getAssemblyConditionWeight(AssemblyConditionIndex condition)278 Real getAssemblyConditionWeight(AssemblyConditionIndex condition) const {
279     SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
280         "Assembler::getAssemblyConditionWeight()");
281     return weights[condition];
282 }
283 
284 /** Add an assembly error condition to this Assembler study, taking over
285 ownership of the heap-allocated AssemblyCondition object. We will use the
286 calcErrors() method of this object to determine errors whose norm \e must be
287 driven below tolerance for an assembly to be considered successful. **/
288 AssemblyConditionIndex
289     adoptAssemblyError(AssemblyCondition* p);
290 /** Add an assembly goal to this Assembler study, taking over ownership
291 of the heap-allocated AssemblyCondition object. We will use normally use the
292 calcGoal() method of this object to calculate its contribution to the
293 assembly goal cost function. An optional weight can be provided that is used
294 when combining this cost with those of other goals to form the overall cost
295 function; the default weight is 1. If the weight is 0 the goal is ignored and
296 not evaluated at all; if the weight is Infinity this is actually an assembly
297 constraint and we'll use its calcErrors() method instead. **/
298 AssemblyConditionIndex
299     adoptAssemblyGoal(AssemblyCondition* p, Real weight=1);
300 
301 
302 /** Set the Assembler's internal state from an existing state which must
303 be suitable for use with the Assembler's System as supplied at the time
304 the Assembler was constructed. All variables are copied, not just q's, so
305 the Assembler must be reinitialized after this call in case modeling
306 options, instance variables, or time have changed. **/
setInternalState(const State & state)307 Assembler& setInternalState(const State& state) {
308     uninitialize();
309     getMatterSubsystem().convertToEulerAngles(state, internalState);
310     system.realizeModel(internalState);
311     return *this;
312 }
313 /** Initialize the Assembler to prepare for performing assembly analysis.
314 This is normally called automatically when assemble() is called, but you
315 can call it explicitly and then access methods that report on the
316 properties of the system on which the analysis will be performed. The
317 internal state should already have been set; if you want to provide the
318 state now use initialize(State). **/
319 void initialize() const;
320 /** Set the internal State and initialize. See setInternalState() and
321 initialize() methods for more information. **/
initialize(const State & state)322 void initialize(const State& state)
323 {   setInternalState(state); initialize(); }
324 /*@}*/
325 
326 /** @name                    Execution
327 These methods perform assembly or tracking analysis, determine how
328 successful they were, and obtain results. **/
329 /*@{*/
330 
331 /** Starting with the current value of the internally-maintained State,
332 modify the q's in it to satisfy all the assembly conditions to within a
333 tolerance. The actual tolerance achieved is returned as the function value.
334 @return The goal value actually achieved (not the error norm; that is
335 guaranteed to be no greater than the error tolerance if this returns at
336 all.  **/
337 Real assemble();
338 
339 /** Continue a series of assembly steps that is already in progress,
340 without restarting or reanalyzing the system, and optionally providing
341 a new frame time. This is designed for use with a series of assembly
342 frames that are close together so that no heroic measures are needed to
343 go from one to the next. For the first frame, and any time there might be
344 a change to the problem structure or a major change to the state, use
345 assemble() instead of track(). See the Assembler class documentation for
346 more information and usage examples. **/
347 Real track(Real frameTime = -1);
348 
349 /** Given an initial value for the State, modify the q's in it to satisfy
350 all the assembly conditions to within a tolerance. The actual tolerance
351 achieved is returned as the function value.
352 @param[in,out]      state
353     The initial and final State value. Only q's are modified.
354 @return The tolerance actually achieved.  **/
assemble(State & state)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 
363 /** Return the goal value attained by the internal State's current settings
364 for the free q's; this is a weighted sum of the individual goal values for
365 each assembly goal. Goal values are nonnegative scalars. **/
366 Real calcCurrentGoal() const;
367 /** This is the weighted norm of the assembly constraint errors directly
368 comparable with the assembly error tolerance setting. That is, if this
369 number is less than or equal to tolerance (as returned by
370 getErrorToleranceInUse()), then the current state is a feasible
371 assembly solution (although it may not be optimal). Note that by default
372 we use the infinity norm (maximum absolute value of any error term) but
373 that you can specify use of an RMS norm instead via setUseRMSErrorNorm().
374 @see getErrorToleranceInUse(), setUseRMSErrorNorm() **/
375 Real calcCurrentErrorNorm() const;
376 
377 
378 /** Given an existing State that is suitable for the Assembler's System,
379 update its q's from those found in the Assembler's internal State, leaving
380 everything else unchanged. We will convert from Euler angles to quaternions
381 if the destination State is set to use quaternions. **/
updateFromInternalState(State & state)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 }
392 /*@}*/
393 
394 /** @name                Parameter restrictions
395 These methods restrict which q's are allowed to be modified while trying
396 to assemble the system, or restrict the range within which the final q's
397 must lie. A prescribed mobilizer is always treated as locked; its q's
398 are set to their prescribed values and are not changed to satisfy assembly
399 conditions. **/
400 /*@{*/
401 
402 /** Lock this mobilizer at its starting position. This overrides any
403 individual q specifications, so even if a q was specifically unlocked it
404 will not move until the mobilizer as a whole is unlocked. **/
lockMobilizer(MobilizedBodyIndex mbx)405 void lockMobilizer(MobilizedBodyIndex mbx)
406 {   uninitialize(); userLockedMobilizers.insert(mbx); }
407 /** Unlock this mobilizer as a whole; some of its q's may remain locked
408 if they were locked individually. It is OK if this mobilizer was already
409 unlocked; in that case this does nothing. Attempts to unlock a prescribed
410 mobilizer have no effect; you must disable the mobilizer's Motion object
411 instead. **/
unlockMobilizer(MobilizedBodyIndex mbx)412 void unlockMobilizer(MobilizedBodyIndex mbx)
413 {   uninitialize(); userLockedMobilizers.erase(mbx); }
414 
415 /** Lock one of this mobilizer's q's at its initial value. Be careful with
416 this method because it requires that you understand the order of the
417 generalized coordinates used by this particular mobilizer during assembly.
418 In particular, the mobilizer will be modeled with Euler angles rather than
419 quaternions and you must know the Euler sequence it uses in that case (that
420 is, body- or space-fixed, 2 or 3 axes, and the rotation order). It is
421 preferable to use lockMobilizer() instead since that will lock all the q's
422 however they are defined. Note that locking individual q's with this method
423 is independent of whole-mobilizer locking. If you unlock the mobilizer with
424 unlockMobilizer(), any q's which have been explicitly locked with lockQ()
425 will remain locked. **/
lockQ(MobilizedBodyIndex mbx,MobilizerQIndex qx)426 void lockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
427 {   uninitialize(); userLockedQs[mbx].insert(qx); }
428 
429 /** Unlock one of this mobilizer's q's if it was locked. Note that this will
430 not take effect immediately if the mobilizer as a whole has been locked with
431 lockMobilizer(); you have to unlockMobilizer() first. This has no effect on
432 a prescribed q. **/
unlockQ(MobilizedBodyIndex mbx,MobilizerQIndex qx)433 void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
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 
444 /** Restrict a q to remain within a given range. Caution: this requires that
445 you understand the order of the generalized coordinates used by this
446 particular mobilizer during assembly; see lockQ() for a discussion. You can
447 use -Infinity or Infinity to indicate that the q is not bounded in one
448 direction. This has no effect on a prescribed q. **/
restrictQ(MobilizedBodyIndex mbx,MobilizerQIndex qx,Real lowerBound,Real upperBound)449 void restrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx,
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 
461 /** Unrestrict a particular generalized coordinate q if it was previously
462 restricted. Note that this is independent of whether the q has been locked
463 with lockMobilizer() or lockQ(); that is, the q may still be locked even
464 though it is now unrestricted. This has no effect on a prescribed q. **/
unrestrictQ(MobilizedBodyIndex mbx,MobilizerQIndex qx)465 void unrestrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
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 }
475 /*@}*/
476 
477 
478 
479 /** @name                    Statistics
480 The Assembler keeps counters of various internal operations it performs
481 during execution; these methods access those counters. These can be helpful
482 in evaluating the effects of various ways of structuring the assembly or
483 tracking problem. Counters can also be reset to zero manually by calling
484 resetStats(). **/
485 /*@{*/
486 /** Return the number of goal evaluations. **/
487 int getNumGoalEvals()  const;
488 /** Return the number of assembly error condition evaluations. **/
489 int getNumErrorEvals() const;
490 /** Return the number of goal gradient evaluations. **/
491 int getNumGoalGradientEvals()   const;
492 /** Return the number of assembly error condition Jacobian evaluations. **/
493 int getNumErrorJacobianEvals()   const;
494 /** Return the number of assembly steps; that is, the number of calls to
495 assemble() or track() since last initialization. **/
496 int getNumAssemblySteps() const;
497 /** Return the number of system initializations performed since this
498 Assembler was created or the most recent resetStats() call. **/
499 int getNumInitializations() const;
500 /** Reset all counters to zero; except for the number of initializations
501 counter this also happens whenever the assembler system is reinitialized
502 either explicitly or due to system changes. **/
503 void resetStats() const;
504 /*@}*/
505 
506 
507 /** @name                Advanced options
508 These are primarily useful for debugging while developing new
509 AssemblyCondition classes. **/
510 /*@{*/
511 
512 /** This is useful for debugging but should not be used otherwise
513 since the analytic gradient is to be preferred. **/
setForceNumericalGradient(bool yesno)514 void setForceNumericalGradient(bool yesno)
515 {   forceNumericalGradient = yesno; }
516 /** This is useful for debugging but should not be used otherwise
517 since the analytic Jacobian is to be preferred. **/
setForceNumericalJacobian(bool yesno)518 void setForceNumericalJacobian(bool yesno)
519 {   forceNumericalJacobian = yesno; }
520 
521 /** Use an RMS norm for the assembly errors rather than the default
522 infinity norm (max absolute value). RMS is less stringent and defines
523 success based on on a good "average" case rather than a good worst case.
524 If there are n error terms ei, the default norm is e=max_i(abs(ei)) and
525 the RMS norm is e=sqrt(sum_i(ei^2)/n). **/
setUseRMSErrorNorm(bool yesno)526 void setUseRMSErrorNorm(bool yesno)
527 {   useRMSErrorNorm = yesno; }
528 /** Determine whether we are currently using the RMS norm for constraint
529 errors; if not we're using the default infinity norm (max absolute value).
530 **/
isUsingRMSErrorNorm()531 bool isUsingRMSErrorNorm() const {return useRMSErrorNorm;}
532 
533 /** Uninitialize the Assembler. After this call the Assembler must be
534 initialized again before an assembly study can be performed. Normally this
535 is called automatically when changes are made; you can call it explicitly
536 if you want. **/
537 void uninitialize() const;
538 /** Check whether the Assembler has been initialized since the last change
539 was made to its contents. **/
isInitialized()540 bool isInitialized() const {return alreadyInitialized;}
541 
542 /** This provides read-only access to the Assembler's internal State; you
543 probably should use updateFromInternalState() to transfer just q's from
544 the internal state to your own State. Be aware that the internal state is
545 always maintained using Euler angles for rotations rather than quaternions,
546 while updateFromInternalState() will make sure you get the rotations in the
547 form you want. **/
getInternalState()548 const State& getInternalState() const {return internalState;}
549 
550 /** Given a reference to an EventReporter, use this Reporter to provide
551 progress reporting. The EventReporter object must be owned by someone
552 else and persist throughout the lifetime of this Assembler object. **/
addReporter(const EventReporter & reporter)553 void addReporter(const EventReporter& reporter) {
554     reporters.push_back(&reporter);
555 }
556 
557 /** Return the number of q's which are free to be changed by this
558 already-initialized assembly analysis. The rest of the q's are locked
559 at their initial values or set to their prescribed values. **/
getNumFreeQs()560 int getNumFreeQs() const
561 {   return freeQ2Q.size(); }
562 
563 /** Return the absolute q index associated with a free q. Every free q
564 is associated with a q so this will always return a valid index if the
565 free q index is in range. **/
getQIndexOfFreeQ(FreeQIndex freeQIndex)566 QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const
567 {   return freeQ2Q[freeQIndex]; }
568 
569 /** A subset of the q's will be used as free q's for solving the assembly
570 problem. Given an absolute q index, this will return the corresponding
571 free q index if there is one; otherwise, the returned index will be
572 invalid meaning that this q is currently locked. **/
getFreeQIndexOfQ(QIndex qx)573 FreeQIndex getFreeQIndexOfQ(QIndex qx) const
574 {   return q2FreeQ[qx]; }
575 
576 /** Return the allowable range for a particular free q. If this free q is
577 unrestricted the returned range will be [-Infinity,Infinity]. **/
getFreeQBounds(FreeQIndex freeQIndex)578 Vec2 getFreeQBounds(FreeQIndex freeQIndex) const {
579     if (!lower.size()) return Vec2(-Infinity, Infinity);
580     else return Vec2(lower[freeQIndex], upper[freeQIndex]);
581 }
582 
583 /** Return a reference to the MultibodySystem associated with this
584 Assembler (that is, the System that was supplied in the Assembler's
585 constructor. **/
getMultibodySystem()586 const MultibodySystem& getMultibodySystem() const
587 {   return system; }
588 /** Return a reference to the SimbodyMatterSubsystem that is contained
589 in the MultibodySystem that is associated with this Assembler. **/
getMatterSubsystem()590 const SimbodyMatterSubsystem& getMatterSubsystem() const
591 {   return system.getMatterSubsystem(); }
592 /*@}*/
593 
594 /** Destruct the Assembler objects and any Assembly Condition objects it
595 contains. **/
596 ~Assembler();
597 
598 
599 
600 //------------------------------------------------------------------------------
601                            private: // methods
602 //------------------------------------------------------------------------------
603 // Note that the internalState is realized to Stage::Position on return.
setInternalStateFromFreeQs(const Vector & freeQs)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 
getFreeQsFromInternalState()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_
695