Simbody
|
00001 #ifndef SimTK_SIMBODY_ASSEMBLER_H_ 00002 #define SimTK_SIMBODY_ASSEMBLER_H_ 00003 00004 /* -------------------------------------------------------------------------- * 00005 * SimTK Core: SimTK Simbody(tm) * 00006 * -------------------------------------------------------------------------- * 00007 * This is part of the SimTK Core 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. * 00011 * * 00012 * Portions copyright (c) 2010 Stanford University and the Authors. * 00013 * Authors: Michael Sherman * 00014 * Contributors: * 00015 * * 00016 * Permission is hereby granted, free of charge, to any person obtaining a * 00017 * copy of this software and associated documentation files (the "Software"), * 00018 * to deal in the Software without restriction, including without limitation * 00019 * the rights to use, copy, modify, merge, publish, distribute, sublicense, * 00020 * and/or sell copies of the Software, and to permit persons to whom the * 00021 * Software is furnished to do so, subject to the following conditions: * 00022 * * 00023 * The above copyright notice and this permission notice shall be included in * 00024 * all copies or substantial portions of the Software. * 00025 * * 00026 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR * 00027 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, * 00028 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL * 00029 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, * 00030 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR * 00031 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE * 00032 * USE OR OTHER DEALINGS IN THE SOFTWARE. * 00033 * -------------------------------------------------------------------------- */ 00034 00035 #include "SimTKcommon.h" 00036 #include "simbody/internal/common.h" 00037 #include "simbody/internal/MultibodySystem.h" 00038 #include "simbody/internal/SimbodyMatterSubsystem.h" 00039 00040 #include <set> 00041 #include <map> 00042 #include <cassert> 00043 #include <cmath> 00044 00045 namespace SimTK { 00046 00047 SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex); 00048 00049 class AssemblyCondition; 00050 00154 class SimTK_SIMBODY_EXPORT Assembler : public Study { 00155 typedef std::set<MobilizedBodyIndex> LockedMobilizers; 00156 typedef std::set<MobilizerQIndex> QSet; 00157 typedef std::map<MobilizedBodyIndex, QSet> LockedQs; 00158 typedef std::map<MobilizerQIndex, Vec2> QRanges; 00159 typedef std::map<MobilizedBodyIndex, QRanges> RestrictedQs; 00160 public: 00161 00164 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Assembler,FreeQIndex); 00165 00169 class AssembleFailed; 00173 class TrackFailed; 00174 00191 explicit Assembler(const MultibodySystem& system); 00192 00202 void setErrorTolerance(Real tolerance=0) { 00203 SimTK_ERRCHK1_ALWAYS(0 <= tolerance, 00204 "Assembler::setTolerance()", "The requested error tolerance %g" 00205 " is illegal; we require 0 <= tolerance, with 0 indicating that" 00206 " the default tolerance (accuracy/10) is to be used.", tolerance); 00207 this->tolerance = tolerance; 00208 } 00213 Real getErrorToleranceInUse() const { 00214 return tolerance > 0 ? tolerance 00215 : (accuracy > 0 ? accuracy/10 : Real(0.1)/OODefaultAccuracy); 00216 } 00217 00227 void setAccuracy(Real accuracy=0) { 00228 SimTK_ERRCHK2_ALWAYS(0 <= accuracy && accuracy < 1, 00229 "Assembler::setAccuracy()", "The requested accuracy %g is illegal;" 00230 " we require 0 <= accuracy < 1, with 0 indicating that the default" 00231 " accuracy (%g) is to be used.", Real(1)/OODefaultAccuracy, accuracy); 00232 this->accuracy = accuracy; 00233 } 00236 Real getAccuracyInUse() const 00237 { return accuracy > 0 ? accuracy : Real(1)/OODefaultAccuracy; } 00238 00239 00246 void setSystemConstraintsWeight(Real weight) 00247 { assert(systemConstraints.isValid()); 00248 setAssemblyConditionWeight(systemConstraints,weight); } 00249 00253 Real getSystemConstraintsWeight() const 00254 { assert(systemConstraints.isValid()); 00255 return getAssemblyConditionWeight(systemConstraints); } 00256 00263 void setAssemblyConditionWeight(AssemblyConditionIndex condition, Real weight) { 00264 SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(), 00265 "Assembler::setAssemblyConditionWeight()"); 00266 SimTK_ERRCHK1_ALWAYS(weight >= 0, "Assembler::setAssemblyConditionWeight()", 00267 "Illegal weight %g; weight must be nonnegative.", weight); 00268 uninitialize(); 00269 weights[condition] = weight; 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 void setInternalState(const State& state) { 00308 uninitialize(); 00309 getMatterSubsystem().convertToEulerAngles(state, internalState); 00310 system.realizeModel(internalState); 00311 } 00318 void initialize() const; 00321 void initialize(const State& state) 00322 { setInternalState(state); initialize(); } 00329 00336 Real assemble(); 00337 00346 Real track(Real frameTime = -1); 00347 00354 Real assemble(State& state) { 00355 setInternalState(state); 00356 Real achievedCost = assemble(); // throws if it fails 00357 updateFromInternalState(state); 00358 return achievedCost; 00359 } 00360 00361 00365 Real calcCurrentGoal() const; 00374 Real calcCurrentErrorNorm() const; 00375 00376 00381 void updateFromInternalState(State& state) const { 00382 system.realizeModel(state); // allocates q's if they haven't been yet 00383 if (!getMatterSubsystem().getUseEulerAngles(state)) { 00384 State tempState; 00385 getMatterSubsystem().convertToQuaternions(getInternalState(), 00386 tempState); 00387 state.updQ() = tempState.getQ(); 00388 } else 00389 state.updQ() = getInternalState().getQ(); 00390 } 00398 00402 void lockMobilizer(MobilizedBodyIndex mbx) 00403 { uninitialize(); userLockedMobilizers.insert(mbx); } 00407 void unlockMobilizer(MobilizedBodyIndex mbx) 00408 { uninitialize(); userLockedMobilizers.erase(mbx); } 00409 00421 void lockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx) 00422 { uninitialize(); userLockedQs[mbx].insert(qx); } 00423 00427 void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx) 00428 { LockedQs::iterator p = userLockedQs.find(mbx); 00429 if (p == userLockedQs.end()) return; 00430 QSet& qs = p->second; 00431 if (qs.erase(qx)) { // returns 0 if nothing erased 00432 uninitialize(); 00433 if (qs.empty()) 00434 userLockedQs.erase(p); // remove the whole mobilized body 00435 } 00436 } 00437 00443 void restrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx, 00444 Real lowerBound, Real upperBound) 00445 { SimTK_ERRCHK2_ALWAYS(lowerBound <= upperBound, "Assembler::restrictQ()", 00446 "The given range [%g,%g] is illegal because the lower bound is" 00447 " greater than the upper bound.", lowerBound, upperBound); 00448 if (lowerBound == -Infinity && upperBound == Infinity) 00449 { unrestrictQ(mbx,qx); return; } 00450 uninitialize(); 00451 userRestrictedQs[mbx][qx] = Vec2(lowerBound,upperBound); 00452 } 00453 00454 00459 void unrestrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx) 00460 { RestrictedQs::iterator p = userRestrictedQs.find(mbx); 00461 if (p == userRestrictedQs.end()) return; 00462 QRanges& qranges = p->second; 00463 if (qranges.erase(qx)) { // returns 0 if nothing erased 00464 uninitialize(); 00465 if (qranges.empty()) 00466 userRestrictedQs.erase(p); // remove the whole mobilized body 00467 } 00468 } 00481 int getNumGoalEvals() const; 00483 int getNumErrorEvals() const; 00485 int getNumGoalGradientEvals() const; 00487 int getNumErrorJacobianEvals() const; 00490 int getNumAssemblySteps() const; 00493 int getNumInitializations() const; 00497 void resetStats() const; 00505 00508 void setForceNumericalGradient(bool yesno) 00509 { forceNumericalGradient = yesno; } 00512 void setForceNumericalJacobian(bool yesno) 00513 { forceNumericalJacobian = yesno; } 00514 00520 void setUseRMSErrorNorm(bool yesno) 00521 { useRMSErrorNorm = yesno; } 00525 bool isUsingRMSErrorNorm() const {return useRMSErrorNorm;} 00526 00531 void uninitialize() const; 00534 bool isInitialized() const {return alreadyInitialized;} 00535 00542 const State& getInternalState() const {return internalState;} 00543 00547 void addReporter(const EventReporter& reporter) { 00548 reporters.push_back(&reporter); 00549 } 00550 00554 int getNumFreeQs() const 00555 { return freeQ2Q.size(); } 00556 00560 QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const 00561 { return freeQ2Q[freeQIndex]; } 00562 00567 FreeQIndex getFreeQIndexOfQ(QIndex qx) const 00568 { return q2FreeQ[qx]; } 00569 00572 Vec2 getFreeQBounds(FreeQIndex freeQIndex) const { 00573 if (!lower.size()) return Vec2(-Infinity, Infinity); 00574 else return Vec2(lower[freeQIndex], upper[freeQIndex]); 00575 } 00576 00580 const MultibodySystem& getMultibodySystem() const 00581 { return system; } 00584 const SimbodyMatterSubsystem& getMatterSubsystem() const 00585 { return system.getMatterSubsystem(); } 00590 ~Assembler(); 00591 00592 00593 00594 //------------------------------------------------------------------------------ 00595 private: // methods 00596 //------------------------------------------------------------------------------ 00597 void setInternalStateFromFreeQs(const Vector& freeQs) { 00598 assert(freeQs.size() == getNumFreeQs()); 00599 Vector& q = internalState.updQ(); 00600 for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx) 00601 q[getQIndexOfFreeQ(fx)] = freeQs[fx]; 00602 system.realize(internalState, Stage::Position); 00603 } 00604 00605 Vector getFreeQsFromInternalState() const { 00606 Vector freeQs(getNumFreeQs()); 00607 const Vector& q = internalState.getQ(); 00608 for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx) 00609 freeQs[fx] = q[getQIndexOfFreeQ(fx)]; 00610 return freeQs; 00611 } 00612 00613 void reinitializeWithExtraQsLocked 00614 (const Array_<QIndex>& toBeLocked) const; 00615 00616 00617 00618 //------------------------------------------------------------------------------ 00619 private: // data members 00620 //------------------------------------------------------------------------------ 00621 const MultibodySystem& system; 00622 Array_<const EventReporter*> reporters; // just references; don't delete 00623 00624 // These members affect the behavior of the assembly algorithm. 00625 static const int OODefaultAccuracy = 1000; // 1/accuracy if acc==0 00626 Real accuracy; // 0 means use 1/OODefaultAccuracy 00627 Real tolerance; // 0 means use accuracy/10 00628 bool forceNumericalGradient; // ignore analytic gradient methods 00629 bool forceNumericalJacobian; // ignore analytic Jacobian methods 00630 bool useRMSErrorNorm; // what norm defines success? 00631 00632 // Changes to any of these data members set isInitialized()=false. 00633 State internalState; 00634 00635 // These are the mobilizers that were set in lockMobilizer(). They are 00636 // separate from those involved in individually-locked q's. 00637 LockedMobilizers userLockedMobilizers; 00638 // These are locks placed on individual q's; they are independent of the 00639 // locked mobilizer settings. 00640 LockedQs userLockedQs; 00641 // These are range restrictions placed on individual q's. 00642 RestrictedQs userRestrictedQs; 00643 00644 // These are (condition,weight) pairs with weight==Infinity meaning 00645 // constraint; weight==0 meaning currently ignored; and any other 00646 // positive weight meaning a goal. 00647 Array_<AssemblyCondition*,AssemblyConditionIndex> 00648 conditions; 00649 Array_<Real,AssemblyConditionIndex> weights; 00650 00651 // We always have an assembly condition for the Constraints which are 00652 // enabled in the System; this is the index which can be used to 00653 // retrieve that condition. The default weight is Infinity. 00654 AssemblyConditionIndex systemConstraints; 00655 00656 00657 // These are filled in when the Assembler is initialized. 00658 mutable bool alreadyInitialized; 00659 00660 // These are extra q's we removed for numerical reasons. 00661 mutable Array_<QIndex> extraQsLocked; 00662 00663 // These represent restrictions on the independent variables (q's). 00664 mutable std::set<QIndex> lockedQs; 00665 mutable Array_<FreeQIndex,QIndex> q2FreeQ; // nq of these 00666 mutable Array_<QIndex,FreeQIndex> freeQ2Q; // nfreeQ of these 00667 // 0 length if no bounds; otherwise, index by FreeQIndex. 00668 mutable Vector lower, upper; 00669 00670 // These represent the active assembly conditions. 00671 mutable Array_<AssemblyConditionIndex> errors; 00672 mutable Array_<int> nTermsPerError; 00673 mutable Array_<AssemblyConditionIndex> goals; 00674 00675 class AssemblerSystem; // local class 00676 mutable AssemblerSystem* asmSys; 00677 mutable Optimizer* optimizer; 00678 00679 mutable int nAssemblySteps; // count assemble() and track() calls 00680 mutable int nInitializations; // # times we had to reinitialize 00681 00682 friend class AssemblerSystem; 00683 }; 00684 00685 00686 00687 //------------------------------------------------------------------------------ 00688 // ASSEMBLY CONDITION 00689 //------------------------------------------------------------------------------ 00696 class SimTK_SIMBODY_EXPORT AssemblyCondition { 00697 public: 00698 00701 explicit AssemblyCondition(const String& name) 00702 : name(name), assembler(0) {} 00703 00705 virtual ~AssemblyCondition() {} 00706 00713 virtual int initializeCondition() const {return 0;} 00714 00717 virtual void uninitializeCondition() const {} 00718 00727 virtual int calcErrors(const State& state, Vector& err) const 00728 { return -1; } 00729 00739 virtual int calcErrorJacobian(const State& state, Matrix& jacobian) const 00740 { return -1; } 00741 00749 virtual int getNumErrors(const State& state) const 00750 { Vector err; 00751 const int status = calcErrors(state, err); 00752 if (status == 0) 00753 return err.size(); 00754 SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::getNumErrors()", 00755 "The default implementation of getNumErrors() depends on" 00756 " calcErrors() but that method was not implemented for assembly" 00757 " condition '%s'.", name.c_str()); 00758 SimTK_ERRCHK2_ALWAYS(status == 0, "AssemblyCondition::getNumErrors()", 00759 "The default implementation of getNumErrors() uses calcErrors()" 00760 " which returned status %d (assembly condition '%s').", 00761 status, name.c_str()); 00762 return -1; // NOTREACHED 00763 } 00764 00769 virtual int calcGoal(const State& state, Real& goal) const 00770 { static Vector err; 00771 const int status = calcErrors(state, err); 00772 if (status == 0) 00773 { goal = err.normSqr() / std::max(1,err.size()); 00774 return 0; } 00775 SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::calcGoal()", 00776 "The default implementation of calcGoal() depends on calcErrors()" 00777 " but that method was not implemented for assembly condition '%s'.", 00778 name.c_str()); 00779 SimTK_ERRCHK2_ALWAYS(status == 0, "AssemblyCondition::calcGoal()", 00780 "The default implementation of calcGoal() uses calcErrors() which" 00781 " returned status %d (assembly condition '%s').", 00782 status, name.c_str()); 00783 return -1; // NOTREACHED 00784 } 00785 00792 virtual int calcGoalGradient(const State& state, Vector& gradient) const 00793 { return -1; } 00794 00796 const char* getName() const {return name.c_str();} 00797 00800 bool isInAssembler() const {return assembler != 0;} 00804 const Assembler& getAssembler() const 00805 { assert(assembler); return *assembler;} 00809 AssemblyConditionIndex getAssemblyConditionIndex() const 00810 { return myAssemblyConditionIndex; } 00811 00812 //------------------------------------------------------------------------------ 00813 protected: 00814 //------------------------------------------------------------------------------ 00815 // These are useful when writing concrete AssemblyConditions. 00816 00819 int getNumFreeQs() const {return getAssembler().getNumFreeQs();} 00823 QIndex getQIndexOfFreeQ(Assembler::FreeQIndex fx) const 00824 { return getAssembler().getQIndexOfFreeQ(fx); } 00828 Assembler::FreeQIndex getFreeQIndexOfQ(QIndex qx) const 00829 { return getAssembler().getFreeQIndexOfQ(qx); } 00831 const MultibodySystem& getMultibodySystem() const 00832 { return getAssembler().getMultibodySystem(); } 00835 const SimbodyMatterSubsystem& getMatterSubsystem() const 00836 { return getMultibodySystem().getMatterSubsystem(); } 00837 00840 void initializeAssembler() const { 00841 // The Assembler will in turn invoke initializeCondition(). 00842 if (isInAssembler()) getAssembler().initialize(); 00843 else initializeCondition(); 00844 } 00845 00849 void uninitializeAssembler() const { 00850 // The Assembler will in turn invoke uninitializeCondition(). 00851 if (isInAssembler()) getAssembler().uninitialize(); 00852 else uninitializeCondition(); 00853 } 00854 00855 //------------------------------------------------------------------------------ 00856 private: 00857 //------------------------------------------------------------------------------ 00858 // This method is used by the Assembler when the AssemblyCondition object 00859 // is adopted. 00860 friend class Assembler; 00861 void setAssembler(const Assembler& assembler, AssemblyConditionIndex acx) { 00862 assert(!this->assembler); 00863 this->assembler = &assembler; 00864 this->myAssemblyConditionIndex = acx; 00865 } 00866 00867 String name; // assembly condition name 00868 const Assembler* assembler; 00869 AssemblyConditionIndex myAssemblyConditionIndex; 00870 }; 00871 00872 00873 00874 //------------------------------------------------------------------------------ 00875 // Q VALUE 00876 //------------------------------------------------------------------------------ 00880 class QValue : public AssemblyCondition { 00881 public: 00885 QValue(MobilizedBodyIndex mbx, MobilizerQIndex qx, 00886 Real value) 00887 : AssemblyCondition("QValue"), 00888 mobodIndex(mbx), qIndex(qx), value(value) {} 00889 00892 Real getValue() const {return value;} 00895 void setValue(Real newValue) {value=newValue;} 00896 00897 // For constraint: 00898 int getNumEquations(const State&) const {return 1;} 00899 int calcErrors(const State& state, Vector& error) const { 00900 const SimbodyMatterSubsystem& matter = getMatterSubsystem(); 00901 const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex); 00902 error.resize(1); 00903 error[0] = mobod.getOneQ(state, qIndex) - value; 00904 return 0; 00905 } 00906 // Error jacobian is a zero-row except for a 1 in this q's entry (if 00907 // this q is free). 00908 int calcErrorJacobian(const State& state, Matrix& J) const { 00909 const SimbodyMatterSubsystem& matter = getMatterSubsystem(); 00910 const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex); 00911 J.resize(1, getNumFreeQs()); 00912 J = 0; // will have at most one non-zero 00913 00914 // Find the FreeQIndex corresponding to this q. 00915 const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex); 00916 const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx); 00917 00918 // If this q isn't free then there is no way to affect the error 00919 // so the Jacobian stays all-zero. 00920 if (thisFreeIx.isValid()) 00921 J(0,thisFreeIx) = 1; 00922 00923 return 0; 00924 } 00925 00926 // For goal: goal = (q-value)^2 / 2 (the /2 is for gradient beauty) 00927 int calcGoal(const State& state, Real& goal) const { 00928 const SimbodyMatterSubsystem& matter = getMatterSubsystem(); 00929 const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex); 00930 goal = square(mobod.getOneQ(state, qIndex) - value) / 2; 00931 return 0; 00932 } 00933 // Return a gradient with only this q's entry non-zero (if 00934 // this q is free). 00935 int calcGoalGradient(const State& state, Vector& grad) const { 00936 const SimbodyMatterSubsystem& matter = getMatterSubsystem(); 00937 const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex); 00938 grad.resize(getNumFreeQs()); 00939 grad = 0; // will have at most one non-zero 00940 00941 // Find the FreeQIndex corresponding to this q. 00942 const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex); 00943 const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx); 00944 00945 // If this q isn't free then there is no way to affect the goal 00946 // so the gradient stays all-zero. 00947 if (thisFreeIx.isValid()) 00948 grad[thisFreeIx] = mobod.getOneQ(state, qIndex) - value; 00949 00950 return 0; 00951 } 00952 00953 private: 00954 MobilizedBodyIndex mobodIndex; 00955 MobilizerQIndex qIndex; 00956 Real value; 00957 }; 00958 00959 00960 00961 //------------------------------------------------------------------------------ 00962 // MARKERS 00963 //------------------------------------------------------------------------------ 01006 class SimTK_SIMBODY_EXPORT Markers : public AssemblyCondition { 01007 01008 // This is a private class used in the implementation below but not 01009 // accessible through the API. 01010 struct Marker { 01011 Marker(const String& name, MobilizedBodyIndex bodyB, 01012 const Vec3& markerInB, Real weight = 1) 01013 : name(name), bodyB(bodyB), markerInB(markerInB), weight(weight) 01014 { assert(weight >= 0); } 01015 01016 Marker(MobilizedBodyIndex bodyB, const Vec3& markerInB, Real weight=1) 01017 : name(""), bodyB(bodyB), markerInB(markerInB), weight(weight) 01018 { assert(weight >= 0); } 01019 01020 String name; 01021 MobilizedBodyIndex bodyB; 01022 Vec3 markerInB; 01023 Real weight; 01024 }; 01025 01026 public: 01027 01029 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers,MarkerIx); 01031 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers,ObservationIx); 01032 01033 01034 01035 //------------------------------------------------------------------------------ 01041 01045 Markers() : AssemblyCondition("Markers") {} 01046 01066 MarkerIx addMarker(const String& name, MobilizedBodyIndex bodyB, 01067 const Vec3& markerInB, Real weight=1) 01068 { SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 01069 "Markers::addMarker()", "Illegal marker weight %g.", weight); 01070 uninitializeAssembler(); 01071 // Forget any previously-established observation/marker correspondence. 01072 observation2marker.clear(); marker2observation.clear(); 01073 observations.clear(); 01074 const MarkerIx ix(markers.size()); 01075 String nm = String::trimWhiteSpace(name); 01076 if (nm.empty()) 01077 nm = String("_UNNAMED_") + String(ix); 01078 01079 std::pair< std::map<String,MarkerIx>::iterator, bool > 01080 found = markersByName.insert(std::make_pair(nm,ix)); 01081 SimTK_ERRCHK2_ALWAYS(found.second, // true if insertion was done 01082 "Markers::addMarker()", 01083 "Marker name '%s' was already use for Marker %d.", 01084 nm.c_str(), (int)found.first->second); 01085 01086 markers.push_back(Marker(nm,bodyB,markerInB,weight)); 01087 return ix; 01088 } 01089 01094 MarkerIx addMarker(MobilizedBodyIndex bodyB, const Vec3& markerInB, 01095 Real weight=1) 01096 { return addMarker("", bodyB, markerInB, weight); } 01097 01098 01118 void defineObservationOrder(const Array_<MarkerIx>& observationOrder) { 01119 uninitializeAssembler(); 01120 if (observationOrder.empty()) { 01121 observation2marker.resize(markers.size()); 01122 for (MarkerIx mx(0); mx < markers.size(); ++mx) 01123 observation2marker[ObservationIx(mx)] = mx; 01124 } else 01125 observation2marker = observationOrder; 01126 marker2observation.clear(); 01127 // We might need to grow this more, but this is an OK starting guess. 01128 marker2observation.resize(observation2marker.size()); // all invalid 01129 for (ObservationIx ox(0); ox < observation2marker.size(); ++ox) { 01130 const MarkerIx mx = observation2marker[ox]; 01131 if (!mx.isValid()) continue; 01132 01133 if (marker2observation.size() <= mx) 01134 marker2observation.resize(mx+1); 01135 SimTK_ERRCHK4_ALWAYS(!marker2observation[mx].isValid(), 01136 "Markers::defineObservationOrder()", 01137 "An attempt was made to associate Marker %d (%s) with" 01138 " Observations %d and %d; only one Observation per Marker" 01139 " is permitted.", 01140 (int)mx, getMarkerName(mx).c_str(), 01141 (int)marker2observation[mx], (int)ox); 01142 01143 marker2observation[mx] = ox; 01144 } 01145 // Make room for marker observations. 01146 observations.clear(); 01147 observations.resize(observation2marker.size(),Vec3(NaN)); 01148 } 01149 01155 void defineObservationOrder(const Array_<String>& observationOrder) 01156 { Array_<MarkerIx> markerIxs(observationOrder.size()); 01157 for (ObservationIx ox(0); ox < observationOrder.size(); ++ox) 01158 markerIxs[ox] = getMarkerIx(observationOrder[ox]); 01159 defineObservationOrder(markerIxs); } 01160 01162 // no copy required 01163 void defineObservationOrder(const std::vector<String>& observationOrder) 01164 { defineObservationOrder(ArrayViewConst_<String>(observationOrder)); } 01165 01166 01168 // must copy 01169 void defineObservationOrder(const Array_<std::string>& observationOrder) 01170 { const Array_<String> observations(observationOrder); // copy 01171 defineObservationOrder(observations); } 01172 01174 // must copy 01175 void defineObservationOrder(const std::vector<std::string>& observationOrder) 01176 { const Array_<String> observations(observationOrder); // copy 01177 defineObservationOrder(observations); } 01178 01180 void defineObservationOrder(int n, const char* const observationOrder[]) 01181 { Array_<MarkerIx> markerIxs(n); 01182 for (ObservationIx ox(0); ox < n; ++ox) 01183 markerIxs[ox] = getMarkerIx(String(observationOrder[ox])); 01184 defineObservationOrder(markerIxs); } 01189 //------------------------------------------------------------------------------ 01196 01199 int getNumMarkers() const {return markers.size();} 01200 01204 const String& getMarkerName(MarkerIx ix) 01205 { return markers[ix].name; } 01209 const MarkerIx getMarkerIx(const String& name) 01210 { std::map<String,MarkerIx>::const_iterator p = markersByName.find(name); 01211 return p == markersByName.end() ? MarkerIx() : p->second; } 01212 01215 Real getMarkerWeight(MarkerIx mx) 01216 { return markers[mx].weight; } 01217 01219 MobilizedBodyIndex getMarkerBody(MarkerIx mx) const 01220 { return markers[mx].bodyB; } 01221 01223 const Vec3& getMarkerStation(MarkerIx mx) const 01224 { return markers[mx].markerInB; } 01225 01231 int getNumObservations() const {return observation2marker.size();} 01232 01238 ObservationIx getObservationIxForMarker(MarkerIx mx) const 01239 { return marker2observation[mx]; } 01240 01243 bool hasObservation(MarkerIx mx) const 01244 { return getObservationIxForMarker(mx).isValid(); } 01245 01251 MarkerIx getMarkerIxForObservation(ObservationIx ox) const 01252 { return observation2marker[ox]; } 01253 01256 bool hasMarker(ObservationIx ox) const 01257 { return getMarkerIxForObservation(ox).isValid();} 01258 01263 const Array_<MarkerIx>& getMarkersOnBody(MobilizedBodyIndex mbx) { 01264 static const Array_<MarkerIx> empty; 01265 SimTK_ERRCHK_ALWAYS(isInAssembler(), "Markers::getMarkersOnBody()", 01266 "This method can't be called until the Markers object has been" 01267 " adopted by an Assembler."); 01268 initializeAssembler(); 01269 PerBodyMarkers::const_iterator bodyp = bodiesWithMarkers.find(mbx); 01270 return bodyp == bodiesWithMarkers.end() ? empty : bodyp->second; 01271 } 01276 //------------------------------------------------------------------------------ 01282 01286 void moveOneObservation(ObservationIx ox, const Vec3& observation) 01287 { SimTK_ERRCHK_ALWAYS(!observations.empty(), "Assembler::moveOneObservation()", 01288 "There are currently no observations defined. Either the Assembler" 01289 " needs to be initialized to get the default observation order, or you" 01290 " should call defineObservationOrder() explicitly."); 01291 SimTK_ERRCHK2_ALWAYS(ox.isValid() && ox < observations.size(), 01292 "Assembler::moveOneObservation()", "ObservationIx %d is invalid or" 01293 " out of range; there are %d observations currently defined. Use" 01294 " defineObservationOrder() to specify the set of observations and how" 01295 " they correspond to markers.", 01296 (int)ox, (int)observations.size()); 01297 observations[ox] = observation; 01298 } 01299 01309 void moveAllObservations(const Array_<Vec3>& observations) 01310 { SimTK_ERRCHK2_ALWAYS(observations.size() == observation2marker.size(), 01311 "Markers::moveAllObservations()", 01312 "Number of observations provided (%d) differs from the number of" 01313 " observations (%d) last defined with defineObservationOrder().", 01314 observations.size(), observation2marker.size()); 01315 this->observations = observations; } 01316 01326 void changeMarkerWeight(MarkerIx mx, Real weight) { 01327 SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 01328 "Markers::changeMarkerWeight()", "Illegal marker weight %g.", weight); 01329 01330 Marker& marker = markers[mx]; 01331 if (marker.weight == weight) 01332 return; 01333 01334 if (marker.weight == 0 || weight == 0) 01335 uninitializeAssembler(); // qualitative change 01336 01337 marker.weight = weight; 01338 } 01339 01344 const Vec3& getObservation(ObservationIx ox) const {return observations[ox];} 01351 const Array_<Vec3,ObservationIx>& getAllObservations() const 01352 { return observations; } 01353 01357 Vec3 findCurrentMarkerLocation(MarkerIx mx) const; 01358 01368 Real findCurrentMarkerError(MarkerIx mx) const 01369 { return std::sqrt(findCurrentMarkerErrorSquared(mx)); } 01370 01378 Real findCurrentMarkerErrorSquared(MarkerIx mx) const { 01379 const ObservationIx ox = getObservationIxForMarker(mx); 01380 if (!ox.isValid()) return 0; // no observation for this marker 01381 const Vec3& loc = getObservation(ox); 01382 if (!loc.isFinite()) return 0; // NaN in observation; error is ignored 01383 return (findCurrentMarkerLocation(mx) - loc).normSqr(); 01384 } 01389 //------------------------------------------------------------------------------ 01393 int calcErrors(const State& state, Vector& err) const; 01394 int calcErrorJacobian(const State& state, Matrix& jacobian) const; 01395 int getNumErrors(const State& state) const; 01396 int calcGoal(const State& state, Real& goal) const; 01397 int calcGoalGradient(const State& state, Vector& grad) const; 01398 int initializeCondition() const; 01399 void uninitializeCondition() const; 01402 //------------------------------------------------------------------------------ 01403 private: 01404 //------------------------------------------------------------------------------ 01405 const Marker& getMarker(MarkerIx i) const {return markers[i];} 01406 Marker& updMarker(MarkerIx i) {uninitializeAssembler(); return markers[i];} 01407 01408 // data members 01409 01410 // Marker definition. Any change here except a quantitative change to the 01411 // marker's weight uninitializes the Assembler. 01412 Array_<Marker,MarkerIx> markers; 01413 std::map<String,MarkerIx> markersByName; 01414 01415 // Observation-marker corresondence specification. Any change here 01416 // uninitializes the Assembler. 01417 Array_<MarkerIx,ObservationIx> observation2marker; 01418 01419 // For convience in mapping from a marker to its corresponding observation. 01420 // ObservationIx will be invalid if a particular marker has no associated 01421 // observation. 01422 Array_<ObservationIx,MarkerIx> marker2observation; 01423 01424 // This is the current set of marker location observations, one per entry in 01425 // the observation2marker array. Changing the values here does not uninitialize 01426 // the Assembler. 01427 Array_<Vec3,ObservationIx> observations; 01428 01429 // After initialize, this groups the markers by body and weeds out 01430 // any zero-weighted markers. TODO: skip low-weighted markers, at 01431 // least at the start of the assembly. 01432 typedef std::map<MobilizedBodyIndex,Array_<MarkerIx> > PerBodyMarkers; 01433 mutable PerBodyMarkers bodiesWithMarkers; 01434 }; 01435 01436 } // namespace SimTK 01437 01438 #endif // SimTK_SIMBODY_ASSEMBLER_H_