1#ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
2#define SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
96 : name(name), bodyB(bodyB), orientationInB(orientationInB), weight(weight)
97 { assert(weight >= 0); }
101 : name(
""), bodyB(bodyB), orientationInB(orientationInB), weight(weight)
102 { assert(weight >= 0); }
153 "OrientationSensors::addOSensor()",
154 "Illegal orientation sensor weight %g.", weight);
155 uninitializeAssembler();
157 observation2osensor.clear(); osensor2observation.clear();
158 observations.clear();
159 const OSensorIx ix(osensors.size());
160 String nm = String::trimWhiteSpace(name);
164 std::pair< std::map<String,OSensorIx>::iterator,
bool >
165 found = osensorsByName.insert(std::make_pair(nm,ix));
167 "OSensors::addOSensor()",
168 "OSensor name '%s' was already use for OSensor %d.",
169 nm.c_str(), (
int)found.first->second);
171 osensors.push_back(OSensor(nm,bodyB,orientationInB,weight));
181{
return addOSensor(
"", bodyB, orientationInB, weight); }
205 uninitializeAssembler();
206 if (observationOrder.
empty()) {
207 observation2osensor.resize(osensors.size());
208 for (OSensorIx mx(0); mx < osensors.size(); ++mx)
209 observation2osensor[ObservationIx(mx)] = mx;
211 observation2osensor = observationOrder;
212 osensor2observation.
clear();
214 osensor2observation.resize(observation2osensor.size());
215 for (ObservationIx ox(0); ox < observation2osensor.size(); ++ox) {
216 const OSensorIx mx = observation2osensor[ox];
217 if (!mx.isValid())
continue;
219 if (osensor2observation.size() <= mx)
220 osensor2observation.resize(mx+1);
222 "OSensors::defineObservationOrder()",
223 "An attempt was made to associate OSensor %d (%s) with"
224 " Observations %d and %d; only one Observation per OSensor"
226 (
int)mx, getOSensorName(mx).c_str(),
227 (
int)osensor2observation[mx], (
int)ox);
229 osensor2observation[mx] = ox;
232 observations.clear();
233 observations.resize(observation2osensor.size(),
244 for (ObservationIx ox(0); ox < observationOrder.
size(); ++ox)
245 osensorIxs[ox] = getOSensorIx(observationOrder[ox]);
246 defineObservationOrder(osensorIxs); }
258 defineObservationOrder(observations); }
264 defineObservationOrder(observations); }
269 for (ObservationIx ox(0); ox < n; ++ox)
270 osensorIxs[ox] = getOSensorIx(
String(observationOrder[ox]));
271 defineObservationOrder(osensorIxs); }
292{
return osensors[ix].name; }
298{ std::map<String,OSensorIx>::const_iterator p = osensorsByName.find(name);
299 return p == osensorsByName.end() ? OSensorIx() : p->second; }
304{
return osensors[mx].weight; }
308{
return osensors[mx].bodyB; }
313{
return osensors[mx].orientationInB; }
328{
return osensor2observation[mx]; }
333{
return getObservationIxForOSensor(mx).isValid(); }
341{
return observation2osensor[ox]; }
346{
return getOSensorIxForObservation(ox).isValid();}
355 "This method can't be called until the OSensors object has been"
356 " adopted by an Assembler.");
357 initializeAssembler();
358 PerBodyOSensors::const_iterator bodyp = bodiesWithOSensors.find(mbx);
359 return bodyp == bodiesWithOSensors.end() ? empty : bodyp->second;
377 "There are currently no observations defined. Either the Assembler"
378 " needs to be initialized to get the default observation order, or you"
379 " should call defineObservationOrder() explicitly.");
381 "Assembler::moveOneObservation()",
"ObservationIx %d is invalid or"
382 " out of range; there are %d observations currently defined. Use"
383 " defineObservationOrder() to specify the set of observations and how"
384 " they correspond to osensors.",
385 (
int)ox, (
int)observations.size());
386 observations[ox] = observation;
400 == (
int)observation2osensor.size(),
401 "OSensors::moveAllObservations()",
402 "Number of observations provided (%d) differs from the number of"
403 " observations (%d) last defined with defineObservationOrder().",
404 observations.
size(), observation2osensor.size());
405 this->observations = observations;
419 "OSensors::changeOSensorWeight()",
420 "Illegal osensor weight %g.", weight);
422 OSensor& osensor = osensors[mx];
423 if (osensor.weight == weight)
426 if (osensor.weight == 0 || weight == 0)
427 uninitializeAssembler();
429 osensor.weight = weight;
437{
return observations[ox]; }
446{
return observations; }
461 const ObservationIx ox = getObservationIxForOSensor(mx);
462 if (!ox.isValid())
return 0;
463 const Rotation& R_GO = getObservation(ox);
465 const Rotation R_GS = findCurrentOSensorOrientation(mx);
468 return std::abs(aa_SO[0]);
490const OSensor& getOSensor(OSensorIx i)
const {
return osensors[i];}
491OSensor& updOSensor(OSensorIx i) {uninitializeAssembler();
return osensors[i];}
497Array_<OSensor,OSensorIx> osensors;
498std::map<String,OSensorIx> osensorsByName;
502Array_<OSensorIx,ObservationIx> observation2osensor;
507Array_<ObservationIx,OSensorIx> osensor2observation;
512Array_<Rotation,ObservationIx> observations;
517typedef std::map<MobilizedBodyIndex,Array_<OSensorIx> > PerBodyOSensors;
518mutable PerBodyOSensors bodiesWithOSensors;
#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
bool isFinite() const
Return true if no element contains an Infinity or a NaN.
Definition Mat.h:1114
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
This AssemblyCondition specifies a correspondence between orientation sensors fixed on mobilized bodi...
Definition AssemblyCondition_OrientationSensors.h:89
bool hasObservation(OSensorIx mx) const
Return true if the supplied osensor is currently associated with an observation.
Definition AssemblyCondition_OrientationSensors.h:332
int initializeCondition() const override
This is called whenever the Assembler is initialized in case this assembly condition wants to do some...
const OSensorIx getOSensorIx(const String &name)
Return the osensor index associated with the given osensor name.
Definition AssemblyCondition_OrientationSensors.h:297
void moveAllObservations(const Array_< Rotation > &observations)
Set the observed osensor orientations for a new observation frame.
Definition AssemblyCondition_OrientationSensors.h:398
void defineObservationOrder(const std::vector< std::string > &observationOrder)
Define observation order using an std::vector of std::string.
Definition AssemblyCondition_OrientationSensors.h:262
void uninitializeCondition() const override
This is called whenever the containing Assembler is uninitialized in case this assembly condition has...
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(OrientationSensors, OSensorIx)
Define the OSensorIx type which is just a uniquely-typed int.
void defineObservationOrder(const Array_< std::string > &observationOrder)
Define observation order using an Array_ of std::string.
Definition AssemblyCondition_OrientationSensors.h:256
void defineObservationOrder(const Array_< String > &observationOrder)
Define the meaning of the observations by giving the osensor name corresponding to each observation,...
Definition AssemblyCondition_OrientationSensors.h:242
void moveOneObservation(ObservationIx ox, const Rotation &observation)
Move a single osensor's observed orientation without moving any of the others.
Definition AssemblyCondition_OrientationSensors.h:375
OSensorIx getOSensorIxForObservation(ObservationIx ox) const
Return the OSensorIx of the osensor that is associated with the given observation,...
Definition AssemblyCondition_OrientationSensors.h:340
void changeOSensorWeight(OSensorIx mx, Real weight)
Change the weight associated with a particular osensor.
Definition AssemblyCondition_OrientationSensors.h:417
const Rotation & getOSensorStation(OSensorIx mx) const
Get the orientation (coordinate axes fixed in its body frame) of the given osensor.
Definition AssemblyCondition_OrientationSensors.h:312
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(OrientationSensors, ObservationIx)
Define the ObservationIx type which is just a uniquely-typed int.
int calcGoalGradient(const State &state, Vector &grad) const override
Override to supply an analytic gradient for this assembly condition's goal.
void defineObservationOrder(int n, const char *const observationOrder[])
Define observation order using a C array of const char* names.
Definition AssemblyCondition_OrientationSensors.h:267
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 ...
Real getOSensorWeight(OSensorIx mx)
Get the weight currently in use for the specified osensor; this can be changed dynamically via change...
Definition AssemblyCondition_OrientationSensors.h:303
void defineObservationOrder(const Array_< OSensorIx > &observationOrder)
Define the meaning of the observation data by giving the OSensorIx associated with each observation.
Definition AssemblyCondition_OrientationSensors.h:204
int calcErrorJacobian(const State &state, Matrix &jacobian) const override
Override to supply an analytic Jacobian for the assembly errors returned by calcErrors().
Rotation findCurrentOSensorOrientation(OSensorIx mx) const
Using the current value of the internal state, calculate the ground frame orientation of a particular...
OSensorIx addOSensor(MobilizedBodyIndex bodyB, const Rotation &orientationInB, Real weight=1)
Define an unnamed osensor.
Definition AssemblyCondition_OrientationSensors.h:179
int getNumOSensors() const
Return a count n of the number of currently-defined osensors.
Definition AssemblyCondition_OrientationSensors.h:286
ObservationIx getObservationIxForOSensor(OSensorIx mx) const
Return the ObservationIx of the observation that is currently associated with the given osensor,...
Definition AssemblyCondition_OrientationSensors.h:327
MobilizedBodyIndex getOSensorBody(OSensorIx mx) const
Get the MobilizedBodyIndex of the body associated with this osensor.
Definition AssemblyCondition_OrientationSensors.h:307
bool hasOSensor(ObservationIx ox) const
Return true if the supplied observation is currently associated with a osensor.
Definition AssemblyCondition_OrientationSensors.h:345
const Rotation & getObservation(ObservationIx ox) const
Return the current value of the orientation for this observation.
Definition AssemblyCondition_OrientationSensors.h:436
Real findCurrentOSensorError(OSensorIx mx) const
Using the current value of the internal state, calculate the error between the given osensor's curren...
Definition AssemblyCondition_OrientationSensors.h:460
const String & getOSensorName(OSensorIx ix)
Return the unique osensor name assigned to the osensor whose index is provided.
Definition AssemblyCondition_OrientationSensors.h:291
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,...
OrientationSensors()
The default constructor creates an empty OrientationSensors AssemblyCondition object that should be f...
Definition AssemblyCondition_OrientationSensors.h:129
void defineObservationOrder(const std::vector< String > &observationOrder)
Define observation order using an std::vector of SimTK::String.
Definition AssemblyCondition_OrientationSensors.h:250
const Array_< OSensorIx > & getOSensorsOnBody(MobilizedBodyIndex mbx)
The OSensors assembly condition organizes the osensors by body after initialization; call this to get...
Definition AssemblyCondition_OrientationSensors.h:352
OSensorIx addOSensor(const String &name, MobilizedBodyIndex bodyB, const Rotation &orientationInB, Real weight=1)
Define a new orientation sensor (osensor) attached to a particular MobilizedBody.
Definition AssemblyCondition_OrientationSensors.h:150
int getNumErrors(const State &state) const override
Override to supply an efficient method for determining how many errors will be returned by calcErrors...
const Array_< Rotation, ObservationIx > & getAllObservations() const
Return the current values of all the observed orientations.
Definition AssemblyCondition_OrientationSensors.h:445
int getNumObservations() const
Return the number of observations that were defined via the last call to defineObservationOrder().
Definition AssemblyCondition_OrientationSensors.h:320
Vec4P convertRotationToAngleAxis() const
Converts rotation matrix to an equivalent angle-axis representation in canonicalized form.
Definition Rotation.h:836
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
This is a fixed-length column vector designed for no-overhead inline computation.
Definition Vec.h:184
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