Simbody
3.5
|
00001 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_ 00002 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_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-14 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/Assembler.h" 00030 #include "simbody/internal/AssemblyCondition.h" 00031 00032 #include <map> 00033 00034 namespace SimTK { 00035 00036 //------------------------------------------------------------------------------ 00037 // MARKERS 00038 //------------------------------------------------------------------------------ 00081 class SimTK_SIMBODY_EXPORT Markers : public AssemblyCondition { 00082 00083 // This is a private class used in the implementation below but not 00084 // accessible through the API. 00085 struct Marker { 00086 Marker(const String& name, MobilizedBodyIndex bodyB, 00087 const Vec3& markerInB, Real weight = 1) 00088 : name(name), bodyB(bodyB), markerInB(markerInB), weight(weight) 00089 { assert(weight >= 0); } 00090 00091 Marker(MobilizedBodyIndex bodyB, const Vec3& markerInB, Real weight=1) 00092 : name(""), bodyB(bodyB), markerInB(markerInB), weight(weight) 00093 { assert(weight >= 0); } 00094 00095 String name; 00096 MobilizedBodyIndex bodyB; 00097 Vec3 markerInB; 00098 Real weight; 00099 }; 00100 00101 public: 00102 00104 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers,MarkerIx); 00106 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers,ObservationIx); 00107 00108 00109 00110 //------------------------------------------------------------------------------ 00116 00120 Markers() : AssemblyCondition("Markers") {} 00121 00141 MarkerIx addMarker(const String& name, MobilizedBodyIndex bodyB, 00142 const Vec3& markerInB, Real weight=1) 00143 { SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 00144 "Markers::addMarker()", "Illegal marker weight %g.", weight); 00145 uninitializeAssembler(); 00146 // Forget any previously-established observation/marker correspondence. 00147 observation2marker.clear(); marker2observation.clear(); 00148 observations.clear(); 00149 const MarkerIx ix(markers.size()); 00150 String nm = String::trimWhiteSpace(name); 00151 if (nm.empty()) 00152 nm = String("_UNNAMED_") + String(ix); 00153 00154 std::pair< std::map<String,MarkerIx>::iterator, bool > 00155 found = markersByName.insert(std::make_pair(nm,ix)); 00156 SimTK_ERRCHK2_ALWAYS(found.second, // true if insertion was done 00157 "Markers::addMarker()", 00158 "Marker name '%s' was already use for Marker %d.", 00159 nm.c_str(), (int)found.first->second); 00160 00161 markers.push_back(Marker(nm,bodyB,markerInB,weight)); 00162 return ix; 00163 } 00164 00169 MarkerIx addMarker(MobilizedBodyIndex bodyB, const Vec3& markerInB, 00170 Real weight=1) 00171 { return addMarker("", bodyB, markerInB, weight); } 00172 00173 00194 void defineObservationOrder(const Array_<MarkerIx>& observationOrder) { 00195 uninitializeAssembler(); 00196 if (observationOrder.empty()) { 00197 observation2marker.resize(markers.size()); 00198 for (MarkerIx mx(0); mx < markers.size(); ++mx) 00199 observation2marker[ObservationIx(mx)] = mx; 00200 } else 00201 observation2marker = observationOrder; 00202 marker2observation.clear(); 00203 // We might need to grow this more, but this is an OK starting guess. 00204 marker2observation.resize(observation2marker.size()); // all invalid 00205 for (ObservationIx ox(0); ox < observation2marker.size(); ++ox) { 00206 const MarkerIx mx = observation2marker[ox]; 00207 if (!mx.isValid()) continue; 00208 00209 if (marker2observation.size() <= mx) 00210 marker2observation.resize(mx+1); 00211 SimTK_ERRCHK4_ALWAYS(!marker2observation[mx].isValid(), 00212 "Markers::defineObservationOrder()", 00213 "An attempt was made to associate Marker %d (%s) with" 00214 " Observations %d and %d; only one Observation per Marker" 00215 " is permitted.", 00216 (int)mx, getMarkerName(mx).c_str(), 00217 (int)marker2observation[mx], (int)ox); 00218 00219 marker2observation[mx] = ox; 00220 } 00221 // Make room for marker observations. 00222 observations.clear(); 00223 observations.resize(observation2marker.size(),Vec3(NaN)); 00224 } 00225 00231 void defineObservationOrder(const Array_<String>& observationOrder) 00232 { Array_<MarkerIx> markerIxs(observationOrder.size()); 00233 for (ObservationIx ox(0); ox < observationOrder.size(); ++ox) 00234 markerIxs[ox] = getMarkerIx(observationOrder[ox]); 00235 defineObservationOrder(markerIxs); } 00236 00238 // no copy required 00239 void defineObservationOrder(const std::vector<String>& observationOrder) 00240 { defineObservationOrder(ArrayViewConst_<String>(observationOrder)); } 00241 00242 00244 // must copy 00245 void defineObservationOrder(const Array_<std::string>& observationOrder) 00246 { const Array_<String> observations(observationOrder); // copy 00247 defineObservationOrder(observations); } 00248 00250 // must copy 00251 void defineObservationOrder(const std::vector<std::string>& observationOrder) 00252 { const Array_<String> observations(observationOrder); // copy 00253 defineObservationOrder(observations); } 00254 00256 void defineObservationOrder(int n, const char* const observationOrder[]) 00257 { Array_<MarkerIx> markerIxs(n); 00258 for (ObservationIx ox(0); ox < n; ++ox) 00259 markerIxs[ox] = getMarkerIx(String(observationOrder[ox])); 00260 defineObservationOrder(markerIxs); } 00265 //------------------------------------------------------------------------------ 00272 00275 int getNumMarkers() const {return markers.size();} 00276 00280 const String& getMarkerName(MarkerIx ix) 00281 { return markers[ix].name; } 00285 const MarkerIx getMarkerIx(const String& name) 00286 { std::map<String,MarkerIx>::const_iterator p = markersByName.find(name); 00287 return p == markersByName.end() ? MarkerIx() : p->second; } 00288 00291 Real getMarkerWeight(MarkerIx mx) 00292 { return markers[mx].weight; } 00293 00295 MobilizedBodyIndex getMarkerBody(MarkerIx mx) const 00296 { return markers[mx].bodyB; } 00297 00299 const Vec3& getMarkerStation(MarkerIx mx) const 00300 { return markers[mx].markerInB; } 00301 00307 int getNumObservations() const {return observation2marker.size();} 00308 00314 ObservationIx getObservationIxForMarker(MarkerIx mx) const 00315 { return marker2observation[mx]; } 00316 00319 bool hasObservation(MarkerIx mx) const 00320 { return getObservationIxForMarker(mx).isValid(); } 00321 00327 MarkerIx getMarkerIxForObservation(ObservationIx ox) const 00328 { return observation2marker[ox]; } 00329 00332 bool hasMarker(ObservationIx ox) const 00333 { return getMarkerIxForObservation(ox).isValid();} 00334 00339 const Array_<MarkerIx>& getMarkersOnBody(MobilizedBodyIndex mbx) { 00340 static const Array_<MarkerIx> empty; 00341 SimTK_ERRCHK_ALWAYS(isInAssembler(), "Markers::getMarkersOnBody()", 00342 "This method can't be called until the Markers object has been" 00343 " adopted by an Assembler."); 00344 initializeAssembler(); 00345 PerBodyMarkers::const_iterator bodyp = bodiesWithMarkers.find(mbx); 00346 return bodyp == bodiesWithMarkers.end() ? empty : bodyp->second; 00347 } 00352 //------------------------------------------------------------------------------ 00358 00362 void moveOneObservation(ObservationIx ox, const Vec3& observation) 00363 { SimTK_ERRCHK_ALWAYS(!observations.empty(), "Assembler::moveOneObservation()", 00364 "There are currently no observations defined. Either the Assembler" 00365 " needs to be initialized to get the default observation order, or you" 00366 " should call defineObservationOrder() explicitly."); 00367 SimTK_ERRCHK2_ALWAYS(ox.isValid() && ox < observations.size(), 00368 "Assembler::moveOneObservation()", "ObservationIx %d is invalid or" 00369 " out of range; there are %d observations currently defined. Use" 00370 " defineObservationOrder() to specify the set of observations and how" 00371 " they correspond to markers.", 00372 (int)ox, (int)observations.size()); 00373 observations[ox] = observation; 00374 } 00375 00385 void moveAllObservations(const Array_<Vec3>& observations) 00386 { SimTK_ERRCHK2_ALWAYS((int)observations.size() == (int)observation2marker.size(), 00387 "Markers::moveAllObservations()", 00388 "Number of observations provided (%d) differs from the number of" 00389 " observations (%d) last defined with defineObservationOrder().", 00390 observations.size(), observation2marker.size()); 00391 this->observations = observations; } 00392 00402 void changeMarkerWeight(MarkerIx mx, Real weight) { 00403 SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 00404 "Markers::changeMarkerWeight()", "Illegal marker weight %g.", weight); 00405 00406 Marker& marker = markers[mx]; 00407 if (marker.weight == weight) 00408 return; 00409 00410 if (marker.weight == 0 || weight == 0) 00411 uninitializeAssembler(); // qualitative change 00412 00413 marker.weight = weight; 00414 } 00415 00420 const Vec3& getObservation(ObservationIx ox) const {return observations[ox];} 00427 const Array_<Vec3,ObservationIx>& getAllObservations() const 00428 { return observations; } 00429 00433 Vec3 findCurrentMarkerLocation(MarkerIx mx) const; 00434 00444 Real findCurrentMarkerError(MarkerIx mx) const 00445 { return std::sqrt(findCurrentMarkerErrorSquared(mx)); } 00446 00454 Real findCurrentMarkerErrorSquared(MarkerIx mx) const { 00455 const ObservationIx ox = getObservationIxForMarker(mx); 00456 if (!ox.isValid()) return 0; // no observation for this marker 00457 const Vec3& loc = getObservation(ox); 00458 if (!loc.isFinite()) return 0; // NaN in observation; error is ignored 00459 return (findCurrentMarkerLocation(mx) - loc).normSqr(); 00460 } 00465 //------------------------------------------------------------------------------ 00469 int initializeCondition() const OVERRIDE_11; 00470 void uninitializeCondition() const OVERRIDE_11; 00471 int calcErrors(const State& state, Vector& err) const OVERRIDE_11; 00472 int calcErrorJacobian(const State& state, Matrix& jacobian) const OVERRIDE_11; 00473 int getNumErrors(const State& state) const OVERRIDE_11; 00474 int calcGoal(const State& state, Real& goal) const OVERRIDE_11; 00475 int calcGoalGradient(const State& state, Vector& grad) const OVERRIDE_11; 00478 //------------------------------------------------------------------------------ 00479 private: 00480 //------------------------------------------------------------------------------ 00481 const Marker& getMarker(MarkerIx i) const {return markers[i];} 00482 Marker& updMarker(MarkerIx i) {uninitializeAssembler(); return markers[i];} 00483 00484 // data members 00485 00486 // Marker definition. Any change here except a quantitative change to the 00487 // marker's weight uninitializes the Assembler. 00488 Array_<Marker,MarkerIx> markers; 00489 std::map<String,MarkerIx> markersByName; 00490 00491 // Observation-marker corresondence specification. Any change here 00492 // uninitializes the Assembler. 00493 Array_<MarkerIx,ObservationIx> observation2marker; 00494 00495 // For convience in mapping from a marker to its corresponding observation. 00496 // ObservationIx will be invalid if a particular marker has no associated 00497 // observation. 00498 Array_<ObservationIx,MarkerIx> marker2observation; 00499 00500 // This is the current set of marker location observations, one per entry in 00501 // the observation2marker array. Changing the values here does not uninitialize 00502 // the Assembler. 00503 Array_<Vec3,ObservationIx> observations; 00504 00505 // After initialize, this groups the markers by body and weeds out 00506 // any zero-weighted markers. TODO: skip low-weighted markers, at 00507 // least at the start of the assembly. 00508 typedef std::map<MobilizedBodyIndex,Array_<MarkerIx> > PerBodyMarkers; 00509 mutable PerBodyMarkers bodiesWithMarkers; 00510 }; 00511 00512 } // namespace SimTK 00513 00514 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_