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());
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; }
452 Rotation findCurrentOSensorOrientation(OSensorIx mx)
const;
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);
478 int initializeCondition()
const override;
479 void uninitializeCondition()
const override;
480 int calcErrors(
const State& state,
Vector& err)
const override;
481 int calcErrorJacobian(
const State& state,
Matrix& jacobian)
const override;
482 int getNumErrors(
const State& state)
const override;
483 int calcGoal(
const State& state,
Real& goal)
const override;
484 int calcGoalGradient(
const State& state,
Vector& grad)
const override;
490 const OSensor& getOSensor(OSensorIx i)
const {
return osensors[i];}
491 OSensor& updOSensor(OSensorIx i) {uninitializeAssembler();
return osensors[i];}
498 std::map<String,OSensorIx> osensorsByName;
517 typedef std::map<MobilizedBodyIndex,Array_<OSensorIx> > PerBodyOSensors;
518 mutable PerBodyOSensors bodiesWithOSensors;
523 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_ OSensorIx getOSensorIxForObservation(ObservationIx ox) const
Return the OSensorIx of the osensor that is associated with the given observation, or an invalid index if the observation doesn't correspond to any osensor (in which case it is being ignored).
Definition: AssemblyCondition_OrientationSensors.h:340
bool isFinite() const
Return true if no element contains an Infinity or a NaN.
Definition: Mat.h:1114
bool hasObservation(OSensorIx mx) const
Return true if the supplied osensor is currently associated with an observation.
Definition: AssemblyCondition_OrientationSensors.h:332
#define SimTK_ERRCHK2_ALWAYS(cond, whereChecked, fmt, a1, a2)
Definition: ExceptionMacros.h:289
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
bool empty() const
Return true if there are no elements currently stored in this array.
Definition: Array.h:2080
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
size_type size() const
Return the current number of elements stored in this array.
Definition: Array.h:2075
const OSensorIx getOSensorIx(const String &name)
Return the osensor index associated with the given osensor name.
Definition: AssemblyCondition_OrientationSensors.h:297
#define SimTK_ERRCHK1_ALWAYS(cond, whereChecked, fmt, a1)
Definition: ExceptionMacros.h:285
int getNumOSensors() const
Return a count n of the number of currently-defined osensors.
Definition: AssemblyCondition_OrientationSensors.h:286
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
void moveAllObservations(const Array_< Rotation > &observations)
Set the observed osensor orientations for a new observation frame.
Definition: AssemblyCondition_OrientationSensors.h:398
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 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
OrientationSensors()
The default constructor creates an empty OrientationSensors AssemblyCondition object that should be f...
Definition: AssemblyCondition_OrientationSensors.h:129
void changeOSensorWeight(OSensorIx mx, Real weight)
Change the weight associated with a particular osensor.
Definition: AssemblyCondition_OrientationSensors.h:417
#define SimTK_ERRCHK4_ALWAYS(cond, whereChecked, fmt, a1, a2, a3, a4)
Definition: ExceptionMacros.h:297
Every Simbody header and source file should include this header before any other Simbody header...
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
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
Rotation_< Real > Rotation
Definition: Rotation.h:47
bool isFinite(const negator< float > &x)
Definition: negator.h:285
const String & getOSensorName(OSensorIx ix)
Return the unique osensor name assigned to the osensor whose index is provided.
Definition: AssemblyCondition_OrientationSensors.h:291
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:618
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:53
Rotation_ & setRotationToNaN()
Construct Rotation_ filled with NaNs.
Definition: Rotation.h:152
#define SimTK_ERRCHK_ALWAYS(cond, whereChecked, msg)
Definition: ExceptionMacros.h:281
This AssemblyCondition specifies a correspondence between orientation sensors fixed on mobilized bodi...
Definition: AssemblyCondition_OrientationSensors.h:89
void defineObservationOrder(int n, const char *const observationOrder[])
Define observation order using a C array of const char* names.
Definition: AssemblyCondition_OrientationSensors.h:267
void defineObservationOrder(const std::vector< String > &observationOrder)
Define observation order using an std::vector of SimTK::String.
Definition: AssemblyCondition_OrientationSensors.h:250
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
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
#define SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(PARENT, NAME)
Define a local Index class within a Parent class.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:437
int getNumObservations() const
Return the number of observations that were defined via the last call to defineObservationOrder().
Definition: AssemblyCondition_OrientationSensors.h:320
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:51
SimTK::String is a plug-compatible std::string replacement (plus some additional functionality) inten...
Definition: String.h:62
void defineObservationOrder(const Array_< std::string > &observationOrder)
Define observation order using an Array_ of std::string.
Definition: AssemblyCondition_OrientationSensors.h:256
const Rotation & getObservation(ObservationIx ox) const
Return the current value of the orientation for this observation.
Definition: AssemblyCondition_OrientationSensors.h:436
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
bool hasOSensor(ObservationIx ox) const
Return true if the supplied observation is currently associated with a osensor.
Definition: AssemblyCondition_OrientationSensors.h:345
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
void clear()
Erase all the elements currently in this array without changing the capacity; equivalent to erase(beg...
Definition: Array.h:2598
String & trimWhiteSpace()
Trim this String in place, removing all the initial leading and trailing white space, as defined by std::isspace() which typically includes space, tab (\t), newline (\n), return (\r), and form feed (\f).
OSensorIx addOSensor(MobilizedBodyIndex bodyB, const Rotation &orientationInB, Real weight=1)
Define an unnamed osensor.
Definition: AssemblyCondition_OrientationSensors.h:179
const Array_< Rotation, ObservationIx > & getAllObservations() const
Return the current values of all the observed orientations.
Definition: AssemblyCondition_OrientationSensors.h:445
Vec4P convertRotationToAngleAxis() const
Converts rotation matrix to an equivalent angle-axis representation in canonicalized form...
Definition: Rotation.h:836
ObservationIx getObservationIxForOSensor(OSensorIx mx) const
Return the ObservationIx of the observation that is currently associated with the given osensor...
Definition: AssemblyCondition_OrientationSensors.h:327
Define an assembly condition consisting of a scalar goal and/or a related set of assembly error equat...
Definition: AssemblyCondition.h:44
void defineObservationOrder(const std::vector< std::string > &observationOrder)
Define observation order using an std::vector of std::string.
Definition: AssemblyCondition_OrientationSensors.h:262
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
MobilizedBodyIndex getOSensorBody(OSensorIx mx) const
Get the MobilizedBodyIndex of the body associated with this osensor.
Definition: AssemblyCondition_OrientationSensors.h:307
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