Simbody  3.5
AssemblyCondition_Markers.h
Go to the documentation of this file.
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_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines