Simbody  3.5
Assembler.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_ASSEMBLER_H_
00002 #define SimTK_SIMBODY_ASSEMBLER_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                               Simbody(tm)                                  *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2010-12 Stanford University and the Authors.        *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00027 #include "SimTKcommon.h"
00028 #include "simbody/internal/common.h"
00029 #include "simbody/internal/MultibodySystem.h"
00030 #include "simbody/internal/SimbodyMatterSubsystem.h"
00031 
00032 #include <set>
00033 #include <map>
00034 #include <cassert>
00035 #include <cmath>
00036 
00037 namespace SimTK {
00038 
00039 SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex);
00040 
00041 class AssemblyCondition;
00042 
00148 class SimTK_SIMBODY_EXPORT Assembler : public Study {
00149     typedef std::set<MobilizedBodyIndex>            LockedMobilizers;
00150     typedef std::set<MobilizerQIndex>               QSet;
00151     typedef std::map<MobilizedBodyIndex, QSet>      LockedQs;
00152     typedef std::map<MobilizerQIndex, Vec2>         QRanges;
00153     typedef std::map<MobilizedBodyIndex, QRanges>   RestrictedQs;
00154 public:
00155 
00158 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Assembler,FreeQIndex);
00159 
00163 class AssembleFailed;
00167 class TrackFailed;
00168 
00186 explicit Assembler(const MultibodySystem& system);
00187 
00197 Assembler& setErrorTolerance(Real tolerance=0) {
00198     SimTK_ERRCHK1_ALWAYS(0 <= tolerance,
00199         "Assembler::setTolerance()", "The requested error tolerance %g"
00200         " is illegal; we require 0 <= tolerance, with 0 indicating that"
00201         " the default tolerance (accuracy/10) is to be used.", tolerance);
00202     this->tolerance = tolerance;
00203     return *this;
00204 }
00209 Real getErrorToleranceInUse() const {   
00210     return tolerance > 0 ? tolerance 
00211            : (accuracy > 0 ? accuracy/10 : Real(0.1)/OODefaultAccuracy); 
00212 }
00213 
00223 Assembler& setAccuracy(Real accuracy=0) {
00224     SimTK_ERRCHK2_ALWAYS(0 <= accuracy && accuracy < 1,
00225         "Assembler::setAccuracy()", "The requested accuracy %g is illegal;"
00226         " we require 0 <= accuracy < 1, with 0 indicating that the default"
00227         " accuracy (%g) is to be used.", Real(1)/OODefaultAccuracy, accuracy);
00228     this->accuracy = accuracy;
00229     return *this;
00230 }
00233 Real getAccuracyInUse() const 
00234 {   return accuracy > 0 ? accuracy : Real(1)/OODefaultAccuracy; }
00235 
00236 
00243 Assembler& setSystemConstraintsWeight(Real weight)
00244 {   assert(systemConstraints.isValid());
00245     setAssemblyConditionWeight(systemConstraints,weight);
00246     return *this; }
00247 
00251 Real getSystemConstraintsWeight() const
00252 {   assert(systemConstraints.isValid());
00253     return getAssemblyConditionWeight(systemConstraints); }
00254 
00261 Assembler& setAssemblyConditionWeight(AssemblyConditionIndex condition, 
00262                                       Real                   weight) {
00263     SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
00264         "Assembler::setAssemblyConditionWeight()");
00265     SimTK_ERRCHK1_ALWAYS(weight >= 0, "Assembler::setAssemblyConditionWeight()",
00266         "Illegal weight %g; weight must be nonnegative.", weight);
00267     uninitialize();
00268     weights[condition] = weight;
00269     return *this;
00270 }
00271 
00278 Real getAssemblyConditionWeight(AssemblyConditionIndex condition) const {
00279     SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
00280         "Assembler::getAssemblyConditionWeight()");
00281     return weights[condition];
00282 }
00283 
00288 AssemblyConditionIndex 
00289     adoptAssemblyError(AssemblyCondition* p);
00298 AssemblyConditionIndex 
00299     adoptAssemblyGoal(AssemblyCondition* p, Real weight=1);
00300 
00301 
00307 Assembler& setInternalState(const State& state) {
00308     uninitialize();
00309     getMatterSubsystem().convertToEulerAngles(state, internalState);
00310     system.realizeModel(internalState);
00311     return *this;
00312 }
00319 void initialize() const;
00322 void initialize(const State& state)
00323 {   setInternalState(state); initialize(); }
00330 
00337 Real assemble();
00338 
00347 Real track(Real frameTime = -1);
00348 
00355 Real assemble(State& state) {
00356     setInternalState(state);
00357     Real achievedCost = assemble(); // throws if it fails
00358     updateFromInternalState(state);
00359     return achievedCost;
00360 }
00361 
00362 
00366 Real calcCurrentGoal() const;
00375 Real calcCurrentErrorNorm() const;
00376 
00377 
00382 void updateFromInternalState(State& state) const {
00383     system.realizeModel(state); // allocates q's if they haven't been yet
00384     if (!getMatterSubsystem().getUseEulerAngles(state)) {
00385         State tempState;
00386         getMatterSubsystem().convertToQuaternions(getInternalState(),
00387                                                   tempState);
00388         state.updQ() = tempState.getQ();
00389     } else 
00390         state.updQ() = getInternalState().getQ();
00391 }
00401 
00405 void lockMobilizer(MobilizedBodyIndex mbx)
00406 {   uninitialize(); userLockedMobilizers.insert(mbx); }
00412 void unlockMobilizer(MobilizedBodyIndex mbx) 
00413 {   uninitialize(); userLockedMobilizers.erase(mbx); }
00414 
00426 void lockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
00427 {   uninitialize(); userLockedQs[mbx].insert(qx); }
00428 
00433 void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
00434 {   LockedQs::iterator p = userLockedQs.find(mbx);
00435     if (p == userLockedQs.end()) return;
00436     QSet& qs = p->second;
00437     if (qs.erase(qx)) { // returns 0 if nothing erased
00438         uninitialize();
00439         if (qs.empty())
00440             userLockedQs.erase(p); // remove the whole mobilized body
00441     }
00442 }
00443 
00449 void restrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx,
00450                Real lowerBound, Real upperBound)
00451 {   SimTK_ERRCHK2_ALWAYS(lowerBound <= upperBound, "Assembler::restrictQ()", 
00452         "The given range [%g,%g] is illegal because the lower bound is"
00453         " greater than the upper bound.", lowerBound, upperBound);
00454     if (lowerBound == -Infinity && upperBound == Infinity)
00455     {   unrestrictQ(mbx,qx); return; }
00456     uninitialize(); 
00457     userRestrictedQs[mbx][qx] = Vec2(lowerBound,upperBound); 
00458 }
00459 
00460 
00465 void unrestrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
00466 {   RestrictedQs::iterator p = userRestrictedQs.find(mbx);
00467     if (p == userRestrictedQs.end()) return;
00468     QRanges& qranges = p->second;
00469     if (qranges.erase(qx)) { // returns 0 if nothing erased
00470         uninitialize();
00471         if (qranges.empty())
00472             userRestrictedQs.erase(p); // remove the whole mobilized body
00473     }
00474 }
00487 int getNumGoalEvals()  const;
00489 int getNumErrorEvals() const;
00491 int getNumGoalGradientEvals()   const;
00493 int getNumErrorJacobianEvals()   const;
00496 int getNumAssemblySteps() const;
00499 int getNumInitializations() const;
00503 void resetStats() const;
00511 
00514 void setForceNumericalGradient(bool yesno)
00515 {   forceNumericalGradient = yesno; }
00518 void setForceNumericalJacobian(bool yesno)
00519 {   forceNumericalJacobian = yesno; }
00520 
00526 void setUseRMSErrorNorm(bool yesno)
00527 {   useRMSErrorNorm = yesno; }
00531 bool isUsingRMSErrorNorm() const {return useRMSErrorNorm;}
00532 
00537 void uninitialize() const;
00540 bool isInitialized() const {return alreadyInitialized;}
00541 
00548 const State& getInternalState() const {return internalState;}
00549 
00553 void addReporter(const EventReporter& reporter) {
00554     reporters.push_back(&reporter);
00555 }
00556 
00560 int getNumFreeQs() const 
00561 {   return freeQ2Q.size(); }
00562 
00566 QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const
00567 {   return freeQ2Q[freeQIndex]; }
00568 
00573 FreeQIndex getFreeQIndexOfQ(QIndex qx) const 
00574 {   return q2FreeQ[qx]; }
00575 
00578 Vec2 getFreeQBounds(FreeQIndex freeQIndex) const {
00579     if (!lower.size()) return Vec2(-Infinity, Infinity);
00580     else return Vec2(lower[freeQIndex], upper[freeQIndex]);
00581 }
00582 
00586 const MultibodySystem& getMultibodySystem() const 
00587 {   return system; }
00590 const SimbodyMatterSubsystem& getMatterSubsystem() const
00591 {   return system.getMatterSubsystem(); }
00596 ~Assembler();
00597 
00598 
00599 
00600 //------------------------------------------------------------------------------
00601                            private: // methods
00602 //------------------------------------------------------------------------------
00603 // Note that the internalState is realized to Stage::Position on return.
00604 void setInternalStateFromFreeQs(const Vector& freeQs) {
00605     assert(freeQs.size() == getNumFreeQs());
00606     Vector& q = internalState.updQ();
00607     for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx)
00608         q[getQIndexOfFreeQ(fx)] = freeQs[fx];
00609     system.realize(internalState, Stage::Position);
00610 }
00611 
00612 Vector getFreeQsFromInternalState() const {
00613     Vector freeQs(getNumFreeQs());
00614     const Vector& q = internalState.getQ();
00615     for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx)
00616         freeQs[fx] = q[getQIndexOfFreeQ(fx)];
00617     return freeQs;
00618 }
00619 
00620 void reinitializeWithExtraQsLocked
00621     (const Array_<QIndex>& toBeLocked) const;
00622 
00623 
00624 
00625 //------------------------------------------------------------------------------
00626                            private: // data members 
00627 //------------------------------------------------------------------------------
00628 const MultibodySystem&          system;
00629 Array_<const EventReporter*>    reporters; // just references; don't delete
00630 
00631 // These members affect the behavior of the assembly algorithm.
00632 static const int OODefaultAccuracy = 1000; // 1/accuracy if acc==0
00633 Real    accuracy;               // 0 means use 1/OODefaultAccuracy
00634 Real    tolerance;              // 0 means use accuracy/10
00635 bool    forceNumericalGradient; // ignore analytic gradient methods
00636 bool    forceNumericalJacobian; // ignore analytic Jacobian methods
00637 bool    useRMSErrorNorm;        // what norm defines success?
00638 
00639 // Changes to any of these data members set isInitialized()=false.
00640 State                           internalState;
00641 
00642 // These are the mobilizers that were set in lockMobilizer(). They are
00643 // separate from those involved in individually-locked q's.
00644 LockedMobilizers                userLockedMobilizers;
00645 // These are locks placed on individual q's; they are independent of the
00646 // locked mobilizer settings.
00647 LockedQs                        userLockedQs;
00648 // These are range restrictions placed on individual q's.
00649 RestrictedQs                    userRestrictedQs;
00650 
00651 // These are (condition,weight) pairs with weight==Infinity meaning
00652 // constraint; weight==0 meaning currently ignored; and any other
00653 // positive weight meaning a goal.
00654 Array_<AssemblyCondition*,AssemblyConditionIndex> 
00655                                         conditions;
00656 Array_<Real,AssemblyConditionIndex>     weights;
00657 
00658 // We always have an assembly condition for the Constraints which are
00659 // enabled in the System; this is the index which can be used to 
00660 // retrieve that condition. The default weight is Infinity.
00661 AssemblyConditionIndex                  systemConstraints;
00662 
00663 
00664 // These are filled in when the Assembler is initialized.
00665 mutable bool                            alreadyInitialized;
00666 
00667 // These are extra q's we removed for numerical reasons.
00668 mutable Array_<QIndex>                  extraQsLocked;
00669 
00670 // These represent restrictions on the independent variables (q's).
00671 mutable std::set<QIndex>                lockedQs;
00672 mutable Array_<FreeQIndex,QIndex>       q2FreeQ;    // nq of these
00673 mutable Array_<QIndex,FreeQIndex>       freeQ2Q;    // nfreeQ of these
00674 // 0 length if no bounds; otherwise, index by FreeQIndex.
00675 mutable Vector                          lower, upper;
00676 
00677 // These represent the active assembly conditions.
00678 mutable Array_<AssemblyConditionIndex>  errors;
00679 mutable Array_<int>                     nTermsPerError;
00680 mutable Array_<AssemblyConditionIndex>  goals;
00681 
00682 class AssemblerSystem; // local class
00683 mutable AssemblerSystem* asmSys;
00684 mutable Optimizer*       optimizer;
00685 
00686 mutable int nAssemblySteps;   // count assemble() and track() calls
00687 mutable int nInitializations; // # times we had to reinitialize
00688 
00689 friend class AssemblerSystem;
00690 };
00691 
00692 } // namespace SimTK
00693 
00694 #endif // SimTK_SIMBODY_ASSEMBLER_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines