Simbody 3.7
Loading...
Searching...
No Matches
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
34namespace 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.
85struct 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;
97 Vec3 markerInB;
98 Real weight;
99};
100
101public:
102
107
108
109
110//------------------------------------------------------------------------------
116
120Markers() : AssemblyCondition("Markers") {}
121
141MarkerIx 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
169MarkerIx addMarker(MobilizedBodyIndex bodyB, const Vec3& markerInB,
170 Real weight=1)
171{ return addMarker("", bodyB, markerInB, weight); }
172
173
194void 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
231void 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
239void defineObservationOrder(const std::vector<String>& observationOrder)
240{ defineObservationOrder(ArrayViewConst_<String>(observationOrder)); }
241
242
244// must copy
245void defineObservationOrder(const Array_<std::string>& observationOrder)
246{ const Array_<String> observations(observationOrder); // copy
247 defineObservationOrder(observations); }
248
250// must copy
251void defineObservationOrder(const std::vector<std::string>& observationOrder)
252{ const Array_<String> observations(observationOrder); // copy
253 defineObservationOrder(observations); }
254
256void 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
275int getNumMarkers() const {return markers.size();}
276
280const String& getMarkerName(MarkerIx ix)
281{ return markers[ix].name; }
285const 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
292{ return markers[mx].weight; }
293
296{ return markers[mx].bodyB; }
297
299const Vec3& getMarkerStation(MarkerIx mx) const
300{ return markers[mx].markerInB; }
301
307int getNumObservations() const {return observation2marker.size();}
308
314ObservationIx getObservationIxForMarker(MarkerIx mx) const
315{ return marker2observation[mx]; }
316
319bool hasObservation(MarkerIx mx) const
320{ return getObservationIxForMarker(mx).isValid(); }
321
327MarkerIx getMarkerIxForObservation(ObservationIx ox) const
328{ return observation2marker[ox]; }
329
332bool 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
362void 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
385void 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
402void 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
420const Vec3& getObservation(ObservationIx ox) const {return observations[ox];}
428{ return observations; }
429
434
444Real 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//------------------------------------------------------------------------------
469int initializeCondition() const override;
470void uninitializeCondition() const override;
471int calcErrors(const State& state, Vector& err) const override;
472int calcErrorJacobian(const State& state, Matrix& jacobian) const override;
473int getNumErrors(const State& state) const override;
474int calcGoal(const State& state, Real& goal) const override;
475int calcGoalGradient(const State& state, Vector& grad) const override;
478//------------------------------------------------------------------------------
479 private:
480//------------------------------------------------------------------------------
481const Marker& getMarker(MarkerIx i) const {return markers[i];}
482Marker& 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.
488Array_<Marker,MarkerIx> markers;
489std::map<String,MarkerIx> markersByName;
490
491// Observation-marker corresondence specification. Any change here
492// uninitializes the Assembler.
493Array_<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.
498Array_<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.
503Array_<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.
508typedef std::map<MobilizedBodyIndex,Array_<MarkerIx> > PerBodyMarkers;
509mutable 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
int getNumObservations() const
Return the number of observations that were defined via the last call to defineObservationOrder().
Definition AssemblyCondition_Markers.h:307
void defineObservationOrder(const Array_< std::string > &observationOrder)
Define observation order using an Array_ of std::string.
Definition AssemblyCondition_Markers.h:245
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
const Vec3 & getMarkerStation(MarkerIx mx) const
Get the station (fixed location in its body frame) of the given marker.
Definition AssemblyCondition_Markers.h:299
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_< 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
void uninitializeCondition() const override
This is called whenever the containing Assembler is uninitialized in case this assembly condition has...
const String & getMarkerName(MarkerIx ix)
Return the unique marker name assigned to the marker whose index is provided.
Definition AssemblyCondition_Markers.h:280
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
const Vec3 & getObservation(ObservationIx ox) const
Return the current value of the location for this observation.
Definition AssemblyCondition_Markers.h:420
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
const Array_< Vec3, ObservationIx > & getAllObservations() const
Return the current values of all the observed locations.
Definition AssemblyCondition_Markers.h:427
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:62
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:606