Simbody  3.8
AssemblyCondition_Markers.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_
2 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm) *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2010-14 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
27 #include "SimTKcommon.h"
31 
32 #include <map>
33 
34 namespace SimTK {
35 
36 //------------------------------------------------------------------------------
37 // MARKERS
38 //------------------------------------------------------------------------------
82 
83 // This is a private class used in the implementation below but not
84 // accessible through the API.
85 struct Marker {
86  Marker(const String& name, MobilizedBodyIndex bodyB,
87  const Vec3& markerInB, Real weight = 1)
88  : name(name), bodyB(bodyB), markerInB(markerInB), weight(weight)
89  { assert(weight >= 0); }
90 
91  Marker(MobilizedBodyIndex bodyB, const Vec3& markerInB, Real weight=1)
92  : name(""), bodyB(bodyB), markerInB(markerInB), weight(weight)
93  { assert(weight >= 0); }
94 
95  String name;
96  MobilizedBodyIndex bodyB;
97  Vec3 markerInB;
98  Real weight;
99 };
100 
101 public:
102 
107 
108 
109 
110 //------------------------------------------------------------------------------
116 
120 Markers() : AssemblyCondition("Markers") {}
121 
141 MarkerIx addMarker(const String& name, MobilizedBodyIndex bodyB,
142  const Vec3& markerInB, Real weight=1)
143 { SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0,
144  "Markers::addMarker()", "Illegal marker weight %g.", weight);
145  uninitializeAssembler();
146  // Forget any previously-established observation/marker correspondence.
147  observation2marker.clear(); marker2observation.clear();
148  observations.clear();
149  const MarkerIx ix(markers.size());
150  String nm = String::trimWhiteSpace(name);
151  if (nm.empty())
152  nm = String("_UNNAMED_") + String(ix);
153 
154  std::pair< std::map<String,MarkerIx>::iterator, bool >
155  found = markersByName.insert(std::make_pair(nm,ix));
156  SimTK_ERRCHK2_ALWAYS(found.second, // true if insertion was done
157  "Markers::addMarker()",
158  "Marker name '%s' was already use for Marker %d.",
159  nm.c_str(), (int)found.first->second);
160 
161  markers.push_back(Marker(nm,bodyB,markerInB,weight));
162  return ix;
163 }
164 
169 MarkerIx addMarker(MobilizedBodyIndex bodyB, const Vec3& markerInB,
170  Real weight=1)
171 { return addMarker("", bodyB, markerInB, weight); }
172 
173 
194 void defineObservationOrder(const Array_<MarkerIx>& observationOrder) {
195  uninitializeAssembler();
196  if (observationOrder.empty()) {
197  observation2marker.resize(markers.size());
198  for (MarkerIx mx(0); mx < markers.size(); ++mx)
199  observation2marker[ObservationIx(mx)] = mx;
200  } else
201  observation2marker = observationOrder;
202  marker2observation.clear();
203  // We might need to grow this more, but this is an OK starting guess.
204  marker2observation.resize(observation2marker.size()); // all invalid
205  for (ObservationIx ox(0); ox < observation2marker.size(); ++ox) {
206  const MarkerIx mx = observation2marker[ox];
207  if (!mx.isValid()) continue;
208 
209  if (marker2observation.size() <= mx)
210  marker2observation.resize(mx+1);
211  SimTK_ERRCHK4_ALWAYS(!marker2observation[mx].isValid(),
212  "Markers::defineObservationOrder()",
213  "An attempt was made to associate Marker %d (%s) with"
214  " Observations %d and %d; only one Observation per Marker"
215  " is permitted.",
216  (int)mx, getMarkerName(mx).c_str(),
217  (int)marker2observation[mx], (int)ox);
218 
219  marker2observation[mx] = ox;
220  }
221  // Make room for marker observations.
222  observations.clear();
223  observations.resize(observation2marker.size(),Vec3(NaN));
224 }
225 
231 void defineObservationOrder(const Array_<String>& observationOrder)
232 { Array_<MarkerIx> markerIxs(observationOrder.size());
233  for (ObservationIx ox(0); ox < observationOrder.size(); ++ox)
234  markerIxs[ox] = getMarkerIx(observationOrder[ox]);
235  defineObservationOrder(markerIxs); }
236 
238 // no copy required
239 void defineObservationOrder(const std::vector<String>& observationOrder)
240 { defineObservationOrder(ArrayViewConst_<String>(observationOrder)); }
241 
242 
244 // must copy
245 void defineObservationOrder(const Array_<std::string>& observationOrder)
246 { const Array_<String> observations(observationOrder); // copy
247  defineObservationOrder(observations); }
248 
250 // must copy
251 void defineObservationOrder(const std::vector<std::string>& observationOrder)
252 { const Array_<String> observations(observationOrder); // copy
253  defineObservationOrder(observations); }
254 
256 void defineObservationOrder(int n, const char* const observationOrder[])
257 { Array_<MarkerIx> markerIxs(n);
258  for (ObservationIx ox(0); ox < n; ++ox)
259  markerIxs[ox] = getMarkerIx(String(observationOrder[ox]));
260  defineObservationOrder(markerIxs); }
265 //------------------------------------------------------------------------------
272 
275 int getNumMarkers() const {return markers.size();}
276 
280 const String& getMarkerName(MarkerIx ix)
281 { return markers[ix].name; }
285 const MarkerIx getMarkerIx(const String& name)
286 { std::map<String,MarkerIx>::const_iterator p = markersByName.find(name);
287  return p == markersByName.end() ? MarkerIx() : p->second; }
288 
291 Real getMarkerWeight(MarkerIx mx)
292 { return markers[mx].weight; }
293 
296 { return markers[mx].bodyB; }
297 
299 const Vec3& getMarkerStation(MarkerIx mx) const
300 { return markers[mx].markerInB; }
301 
307 int getNumObservations() const {return observation2marker.size();}
308 
314 ObservationIx getObservationIxForMarker(MarkerIx mx) const
315 { return marker2observation[mx]; }
316 
319 bool hasObservation(MarkerIx mx) const
320 { return getObservationIxForMarker(mx).isValid(); }
321 
327 MarkerIx getMarkerIxForObservation(ObservationIx ox) const
328 { return observation2marker[ox]; }
329 
332 bool hasMarker(ObservationIx ox) const
333 { return getMarkerIxForObservation(ox).isValid();}
334 
340  static const Array_<MarkerIx> empty;
341  SimTK_ERRCHK_ALWAYS(isInAssembler(), "Markers::getMarkersOnBody()",
342  "This method can't be called until the Markers object has been"
343  " adopted by an Assembler.");
344  initializeAssembler();
345  PerBodyMarkers::const_iterator bodyp = bodiesWithMarkers.find(mbx);
346  return bodyp == bodiesWithMarkers.end() ? empty : bodyp->second;
347 }
352 //------------------------------------------------------------------------------
358 
362 void moveOneObservation(ObservationIx ox, const Vec3& observation)
363 { SimTK_ERRCHK_ALWAYS(!observations.empty(), "Assembler::moveOneObservation()",
364  "There are currently no observations defined. Either the Assembler"
365  " needs to be initialized to get the default observation order, or you"
366  " should call defineObservationOrder() explicitly.");
367  SimTK_ERRCHK2_ALWAYS(ox.isValid() && ox < observations.size(),
368  "Assembler::moveOneObservation()", "ObservationIx %d is invalid or"
369  " out of range; there are %d observations currently defined. Use"
370  " defineObservationOrder() to specify the set of observations and how"
371  " they correspond to markers.",
372  (int)ox, (int)observations.size());
373  observations[ox] = observation;
374 }
375 
385 void moveAllObservations(const Array_<Vec3>& observations)
386 { SimTK_ERRCHK2_ALWAYS((int)observations.size() == (int)observation2marker.size(),
387  "Markers::moveAllObservations()",
388  "Number of observations provided (%d) differs from the number of"
389  " observations (%d) last defined with defineObservationOrder().",
390  observations.size(), observation2marker.size());
391  this->observations = observations; }
392 
402 void changeMarkerWeight(MarkerIx mx, Real weight) {
403  SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0,
404  "Markers::changeMarkerWeight()", "Illegal marker weight %g.", weight);
405 
406  Marker& marker = markers[mx];
407  if (marker.weight == weight)
408  return;
409 
410  if (marker.weight == 0 || weight == 0)
411  uninitializeAssembler(); // qualitative change
412 
413  marker.weight = weight;
414 }
415 
420 const Vec3& getObservation(ObservationIx ox) const {return observations[ox];}
428 { return observations; }
429 
433 Vec3 findCurrentMarkerLocation(MarkerIx mx) const;
434 
444 Real findCurrentMarkerError(MarkerIx mx) const
445 { return std::sqrt(findCurrentMarkerErrorSquared(mx)); }
446 
455  const ObservationIx ox = getObservationIxForMarker(mx);
456  if (!ox.isValid()) return 0; // no observation for this marker
457  const Vec3& loc = getObservation(ox);
458  if (!loc.isFinite()) return 0; // NaN in observation; error is ignored
459  return (findCurrentMarkerLocation(mx) - loc).normSqr();
460 }
465 //------------------------------------------------------------------------------
469 int initializeCondition() const override;
470 void uninitializeCondition() const override;
471 int calcErrors(const State& state, Vector& err) const override;
472 int calcErrorJacobian(const State& state, Matrix& jacobian) const override;
473 int getNumErrors(const State& state) const override;
474 int calcGoal(const State& state, Real& goal) const override;
475 int calcGoalGradient(const State& state, Vector& grad) const override;
478 //------------------------------------------------------------------------------
479  private:
480 //------------------------------------------------------------------------------
481 const Marker& getMarker(MarkerIx i) const {return markers[i];}
482 Marker& updMarker(MarkerIx i) {uninitializeAssembler(); return markers[i];}
483 
484  // data members
485 
486 // Marker definition. Any change here except a quantitative change to the
487 // marker's weight uninitializes the Assembler.
488 Array_<Marker,MarkerIx> markers;
489 std::map<String,MarkerIx> markersByName;
490 
491 // Observation-marker corresondence specification. Any change here
492 // uninitializes the Assembler.
493 Array_<MarkerIx,ObservationIx> observation2marker;
494 
495 // For convience in mapping from a marker to its corresponding observation.
496 // ObservationIx will be invalid if a particular marker has no associated
497 // observation.
498 Array_<ObservationIx,MarkerIx> marker2observation;
499 
500 // This is the current set of marker location observations, one per entry in
501 // the observation2marker array. Changing the values here does not uninitialize
502 // the Assembler.
503 Array_<Vec3,ObservationIx> observations;
504 
505 // After initialize, this groups the markers by body and weeds out
506 // any zero-weighted markers. TODO: skip low-weighted markers, at
507 // least at the start of the assembly.
508 typedef std::map<MobilizedBodyIndex,Array_<MarkerIx> > PerBodyMarkers;
509 mutable PerBodyMarkers bodiesWithMarkers;
510 };
511 
512 } // namespace SimTK
513 
514 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_
#define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:289
#define SimTK_ERRCHK_ALWAYS(cond, whereChecked, msg)
Definition: ExceptionMacros.h:281
#define SimTK_ERRCHK4_ALWAYS(cond, whereChecked, fmt, a1, a2, a3, a4)
Definition: ExceptionMacros.h:297
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
Includes internal headers providing declarations for the basic SimTK Core classes,...
Every Simbody header and source file should include this header before any other Simbody header.
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:324
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2075
bool empty() const
Return true if there are no elements currently stored in this array.
Definition: Array.h:2080
void clear()
Erase all the elements currently in this array without changing the capacity; equivalent to erase(beg...
Definition: Array.h:2598
Define an assembly condition consisting of a scalar goal and/or a related set of assembly error equat...
Definition: AssemblyCondition.h:44
This AssemblyCondition specifies a correspondence between stations on mobilized bodies ("markers") an...
Definition: AssemblyCondition_Markers.h:81
const Vec3 & getObservation(ObservationIx ox) const
Return the current value of the location for this observation.
Definition: AssemblyCondition_Markers.h:420
int getNumObservations() const
Return the number of observations that were defined via the last call to defineObservationOrder().
Definition: AssemblyCondition_Markers.h:307
const String & getMarkerName(MarkerIx ix)
Return the unique marker name assigned to the marker whose index is provided.
Definition: AssemblyCondition_Markers.h:280
void defineObservationOrder(const Array_< std::string > &observationOrder)
Define observation order using an Array_ of std::string.
Definition: AssemblyCondition_Markers.h:245
const Array_< MarkerIx > & getMarkersOnBody(MobilizedBodyIndex mbx)
The Markers assembly condition organizes the markers by body after initialization; call this to get t...
Definition: AssemblyCondition_Markers.h:339
Real findCurrentMarkerError(MarkerIx mx) const
Using the current value of the internal state, calculate the distance between the given marker's curr...
Definition: AssemblyCondition_Markers.h:444
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers, MarkerIx)
Define the MarkerIx type which is just a uniquely-typed int.
void changeMarkerWeight(MarkerIx mx, Real weight)
Change the weight associated with a particular marker.
Definition: AssemblyCondition_Markers.h:402
void defineObservationOrder(const std::vector< std::string > &observationOrder)
Define observation order using an std::vector of std::string.
Definition: AssemblyCondition_Markers.h:251
Real findCurrentMarkerErrorSquared(MarkerIx mx) const
Using the current value of the internal state, calculate the (unweighted) square of the distance betw...
Definition: AssemblyCondition_Markers.h:454
MarkerIx getMarkerIxForObservation(ObservationIx ox) const
Return the MarkerIx of the marker that is associated with the given observation, or an invalid index ...
Definition: AssemblyCondition_Markers.h:327
void defineObservationOrder(int n, const char *const observationOrder[])
Define observation order using a C array of const char* names.
Definition: AssemblyCondition_Markers.h:256
int getNumErrors(const State &state) const override
Override to supply an efficient method for determining how many errors will be returned by calcErrors...
ObservationIx getObservationIxForMarker(MarkerIx mx) const
Return the ObservationIx of the observation that is currently associated with the given marker,...
Definition: AssemblyCondition_Markers.h:314
int calcGoalGradient(const State &state, Vector &grad) const override
Override to supply an analytic gradient for this assembly condition's goal.
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers, ObservationIx)
Define the ObservationIx type which is just a uniquely-typed int.
int initializeCondition() const override
This is called whenever the Assembler is initialized in case this assembly condition wants to do some...
const Array_< Vec3, ObservationIx > & getAllObservations() const
Return the current values of all the observed locations.
Definition: AssemblyCondition_Markers.h:427
const Vec3 & getMarkerStation(MarkerIx mx) const
Get the station (fixed location in its body frame) of the given marker.
Definition: AssemblyCondition_Markers.h:299
void uninitializeCondition() const override
This is called whenever the containing Assembler is uninitialized in case this assembly condition has...
int calcErrorJacobian(const State &state, Matrix &jacobian) const override
Override to supply an analytic Jacobian for the assembly errors returned by calcErrors().
MobilizedBodyIndex getMarkerBody(MarkerIx mx) const
Get the MobilizedBodyIndex of the body associated with this marker.
Definition: AssemblyCondition_Markers.h:295
Markers()
The default constructor creates an empty Markers AssemblyCondition object that should be filled in wi...
Definition: AssemblyCondition_Markers.h:120
void moveAllObservations(const Array_< Vec3 > &observations)
Set the observed marker locations for a new observation frame.
Definition: AssemblyCondition_Markers.h:385
bool hasObservation(MarkerIx mx) const
Return true if the supplied marker is currently associated with an observation.
Definition: AssemblyCondition_Markers.h:319
int calcGoal(const State &state, Real &goal) const override
Calculate the current contribution (>= 0) of this assembly condition to the goal value that is being ...
MarkerIx addMarker(const String &name, MobilizedBodyIndex bodyB, const Vec3 &markerInB, Real weight=1)
Define a new marker attached to a particular MobilizedBody.
Definition: AssemblyCondition_Markers.h:141
bool hasMarker(ObservationIx ox) const
Return true if the supplied observation is currently associated with a marker.
Definition: AssemblyCondition_Markers.h:332
MarkerIx addMarker(MobilizedBodyIndex bodyB, const Vec3 &markerInB, Real weight=1)
Define an unnamed marker.
Definition: AssemblyCondition_Markers.h:169
void defineObservationOrder(const Array_< MarkerIx > &observationOrder)
Define the meaning of the observation data by giving the MarkerIx associated with each observation.
Definition: AssemblyCondition_Markers.h:194
void defineObservationOrder(const std::vector< String > &observationOrder)
Define observation order using an std::vector of SimTK::String.
Definition: AssemblyCondition_Markers.h:239
int calcErrors(const State &state, Vector &err) const override
Calculate the amount by which this assembly condition is violated by the q values in the given state,...
void defineObservationOrder(const Array_< String > &observationOrder)
Define the meaning of the observations by giving the marker name corresponding to each observation,...
Definition: AssemblyCondition_Markers.h:231
void moveOneObservation(ObservationIx ox, const Vec3 &observation)
Move a single marker's observed location without moving any of the others.
Definition: AssemblyCondition_Markers.h:362
const MarkerIx getMarkerIx(const String &name)
Return the marker index associated with the given marker name.
Definition: AssemblyCondition_Markers.h:285
Real getMarkerWeight(MarkerIx mx)
Get the weight currently in use for the specified marker; this can be changed dynamically via changeM...
Definition: AssemblyCondition_Markers.h:291
int getNumMarkers() const
Return a count n of the number of currently-defined markers.
Definition: AssemblyCondition_Markers.h:275
Vec3 findCurrentMarkerLocation(MarkerIx mx) const
Using the current value of the internal state, calculate the ground frame location of a particular ma...
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
SimTK::String is a plug-compatible std::string replacement (plus some additional functionality) inten...
Definition: String.h:65
String & trimWhiteSpace()
Trim this String in place, removing all the initial leading and trailing white space,...
bool isFinite() const
Return true if no element of this Vec contains an Infinity or a NaN anywhere.
Definition: Vec.h:942
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
bool isFinite(const negator< float > &x)
Definition: negator.h:285
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:607