Simbody
3.5
|
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_