1 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_
2 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_
87 const Vec3& markerInB,
Real weight = 1)
88 : name(name), bodyB(bodyB), markerInB(markerInB), weight(weight)
89 { assert(weight >= 0); }
92 : name(
""), bodyB(bodyB), markerInB(markerInB), weight(weight)
93 { assert(weight >= 0); }
142 const Vec3& markerInB,
Real weight=1)
144 "Markers::addMarker()",
"Illegal marker weight %g.", weight);
145 uninitializeAssembler();
147 observation2marker.clear(); marker2observation.clear();
148 observations.clear();
149 const MarkerIx ix(markers.size());
154 std::pair< std::map<String,MarkerIx>::iterator,
bool >
155 found = markersByName.insert(std::make_pair(nm,ix));
157 "Markers::addMarker()",
158 "Marker name '%s' was already use for Marker %d.",
159 nm.c_str(), (
int)found.first->second);
161 markers.push_back(Marker(nm,bodyB,markerInB,weight));
171 {
return addMarker(
"", bodyB, markerInB, weight); }
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;
201 observation2marker = observationOrder;
202 marker2observation.
clear();
204 marker2observation.resize(observation2marker.size());
205 for (ObservationIx ox(0); ox < observation2marker.size(); ++ox) {
206 const MarkerIx mx = observation2marker[ox];
207 if (!mx.isValid())
continue;
209 if (marker2observation.size() <= mx)
210 marker2observation.resize(mx+1);
212 "Markers::defineObservationOrder()",
213 "An attempt was made to associate Marker %d (%s) with"
214 " Observations %d and %d; only one Observation per Marker"
216 (
int)mx, getMarkerName(mx).c_str(),
217 (
int)marker2observation[mx], (
int)ox);
219 marker2observation[mx] = ox;
222 observations.clear();
223 observations.resize(observation2marker.size(),
Vec3(
NaN));
233 for (ObservationIx ox(0); ox < observationOrder.
size(); ++ox)
234 markerIxs[ox] = getMarkerIx(observationOrder[ox]);
235 defineObservationOrder(markerIxs); }
247 defineObservationOrder(observations); }
253 defineObservationOrder(observations); }
258 for (ObservationIx ox(0); ox < n; ++ox)
259 markerIxs[ox] = getMarkerIx(
String(observationOrder[ox]));
260 defineObservationOrder(markerIxs); }
281 {
return markers[ix].name; }
286 { std::map<String,MarkerIx>::const_iterator p = markersByName.find(name);
287 return p == markersByName.end() ? MarkerIx() : p->second; }
292 {
return markers[mx].weight; }
296 {
return markers[mx].bodyB; }
300 {
return markers[mx].markerInB; }
315 {
return marker2observation[mx]; }
320 {
return getObservationIxForMarker(mx).isValid(); }
328 {
return observation2marker[ox]; }
333 {
return getMarkerIxForObservation(ox).isValid();}
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;
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.");
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;
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; }
404 "Markers::changeMarkerWeight()",
"Illegal marker weight %g.", weight);
406 Marker& marker = markers[mx];
407 if (marker.weight == weight)
410 if (marker.weight == 0 || weight == 0)
411 uninitializeAssembler();
413 marker.weight = weight;
428 {
return observations; }
445 {
return std::sqrt(findCurrentMarkerErrorSquared(mx)); }
455 const ObservationIx ox = getObservationIxForMarker(mx);
456 if (!ox.isValid())
return 0;
457 const Vec3& loc = getObservation(ox);
459 return (findCurrentMarkerLocation(mx) - loc).normSqr();
481 const Marker& getMarker(MarkerIx i)
const {
return markers[i];}
482 Marker& updMarker(MarkerIx i) {uninitializeAssembler();
return markers[i];}
488 Array_<Marker,MarkerIx> markers;
489 std::map<String,MarkerIx> markersByName;
493 Array_<MarkerIx,ObservationIx> observation2marker;
498 Array_<ObservationIx,MarkerIx> marker2observation;
503 Array_<Vec3,ObservationIx> observations;
508 typedef std::map<MobilizedBodyIndex,Array_<MarkerIx> > PerBodyMarkers;
509 mutable PerBodyMarkers bodiesWithMarkers;
#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