Simbody  3.5
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
AssemblyCondition_OrientationSensors.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
2 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_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) 2014 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 // ORIENTATION SENSORS
38 //------------------------------------------------------------------------------
90 
91 // This is a private class used in the implementation below but not
92 // accessible through the API.
93 struct OSensor {
94  OSensor(const String& name, MobilizedBodyIndex bodyB,
95  const Rotation& orientationInB, Real weight = 1)
96  : name(name), bodyB(bodyB), orientationInB(orientationInB), weight(weight)
97  { assert(weight >= 0); }
98 
99  OSensor(MobilizedBodyIndex bodyB, const Rotation& orientationInB,
100  Real weight=1)
101  : name(""), bodyB(bodyB), orientationInB(orientationInB), weight(weight)
102  { assert(weight >= 0); }
103 
104  String name;
105  MobilizedBodyIndex bodyB;
106  Rotation orientationInB;
107  Real weight;
108 };
109 
110 public:
111 
116 
117 
118 
119 //------------------------------------------------------------------------------
125 
129 OrientationSensors() : AssemblyCondition("OrientationSensors") {}
130 
150 OSensorIx addOSensor(const String& name, MobilizedBodyIndex bodyB,
151  const Rotation& orientationInB, Real weight=1)
152 { SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0,
153  "OrientationSensors::addOSensor()",
154  "Illegal orientation sensor weight %g.", weight);
155  uninitializeAssembler();
156  // Forget any previously-established observation/osensor correspondence.
157  observation2osensor.clear(); osensor2observation.clear();
158  observations.clear();
159  const OSensorIx ix(osensors.size());
160  String nm = String::trimWhiteSpace(name);
161  if (nm.empty())
162  nm = String("_UNNAMED_") + String(ix);
163 
164  std::pair< std::map<String,OSensorIx>::iterator, bool >
165  found = osensorsByName.insert(std::make_pair(nm,ix));
166  SimTK_ERRCHK2_ALWAYS(found.second, // true if insertion was done
167  "OSensors::addOSensor()",
168  "OSensor name '%s' was already use for OSensor %d.",
169  nm.c_str(), (int)found.first->second);
170 
171  osensors.push_back(OSensor(nm,bodyB,orientationInB,weight));
172  return ix;
173 }
174 
179 OSensorIx addOSensor(MobilizedBodyIndex bodyB, const Rotation& orientationInB,
180  Real weight=1)
181 { return addOSensor("", bodyB, orientationInB, weight); }
182 
183 
204 void defineObservationOrder(const Array_<OSensorIx>& observationOrder) {
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;
210  } else
211  observation2osensor = observationOrder;
212  osensor2observation.clear();
213  // We might need to grow this more, but this is an OK starting guess.
214  osensor2observation.resize(observation2osensor.size()); // all invalid
215  for (ObservationIx ox(0); ox < observation2osensor.size(); ++ox) {
216  const OSensorIx mx = observation2osensor[ox];
217  if (!mx.isValid()) continue;
218 
219  if (osensor2observation.size() <= mx)
220  osensor2observation.resize(mx+1);
221  SimTK_ERRCHK4_ALWAYS(!osensor2observation[mx].isValid(),
222  "OSensors::defineObservationOrder()",
223  "An attempt was made to associate OSensor %d (%s) with"
224  " Observations %d and %d; only one Observation per OSensor"
225  " is permitted.",
226  (int)mx, getOSensorName(mx).c_str(),
227  (int)osensor2observation[mx], (int)ox);
228 
229  osensor2observation[mx] = ox;
230  }
231  // Make room for osensor observations.
232  observations.clear();
233  observations.resize(observation2osensor.size(),
235 }
236 
242 void defineObservationOrder(const Array_<String>& observationOrder)
243 { Array_<OSensorIx> osensorIxs(observationOrder.size());
244  for (ObservationIx ox(0); ox < observationOrder.size(); ++ox)
245  osensorIxs[ox] = getOSensorIx(observationOrder[ox]);
246  defineObservationOrder(osensorIxs); }
247 
249 // no copy required
250 void defineObservationOrder(const std::vector<String>& observationOrder)
251 { defineObservationOrder(ArrayViewConst_<String>(observationOrder)); }
252 
253 
255 // must copy
256 void defineObservationOrder(const Array_<std::string>& observationOrder)
257 { const Array_<String> observations(observationOrder); // copy
258  defineObservationOrder(observations); }
259 
261 // must copy
262 void defineObservationOrder(const std::vector<std::string>& observationOrder)
263 { const Array_<String> observations(observationOrder); // copy
264  defineObservationOrder(observations); }
265 
267 void defineObservationOrder(int n, const char* const observationOrder[])
268 { Array_<OSensorIx> osensorIxs(n);
269  for (ObservationIx ox(0); ox < n; ++ox)
270  osensorIxs[ox] = getOSensorIx(String(observationOrder[ox]));
271  defineObservationOrder(osensorIxs); }
276 //------------------------------------------------------------------------------
283 
286 int getNumOSensors() const {return osensors.size();}
287 
291 const String& getOSensorName(OSensorIx ix)
292 { return osensors[ix].name; }
293 
297 const OSensorIx getOSensorIx(const String& name)
298 { std::map<String,OSensorIx>::const_iterator p = osensorsByName.find(name);
299  return p == osensorsByName.end() ? OSensorIx() : p->second; }
300 
303 Real getOSensorWeight(OSensorIx mx)
304 { return osensors[mx].weight; }
305 
308 { return osensors[mx].bodyB; }
309 
312 const Rotation& getOSensorStation(OSensorIx mx) const
313 { return osensors[mx].orientationInB; }
314 
320 int getNumObservations() const {return observation2osensor.size();}
321 
327 ObservationIx getObservationIxForOSensor(OSensorIx mx) const
328 { return osensor2observation[mx]; }
329 
332 bool hasObservation(OSensorIx mx) const
333 { return getObservationIxForOSensor(mx).isValid(); }
334 
340 OSensorIx getOSensorIxForObservation(ObservationIx ox) const
341 { return observation2osensor[ox]; }
342 
345 bool hasOSensor(ObservationIx ox) const
346 { return getOSensorIxForObservation(ox).isValid();}
347 
353  static const Array_<OSensorIx> empty;
354  SimTK_ERRCHK_ALWAYS(isInAssembler(), "OSensors::getOSensorsOnBody()",
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;
360 }
365 //------------------------------------------------------------------------------
371 
375 void moveOneObservation(ObservationIx ox, const Rotation& observation) {
376  SimTK_ERRCHK_ALWAYS(!observations.empty(), "Assembler::moveOneObservation()",
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.");
380  SimTK_ERRCHK2_ALWAYS(ox.isValid() && ox < observations.size(),
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;
387 }
388 
398 void moveAllObservations(const Array_<Rotation>& observations) {
399  SimTK_ERRCHK2_ALWAYS( (int)observations.size()
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;
406 }
407 
417 void changeOSensorWeight(OSensorIx mx, Real weight) {
418  SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0,
419  "OSensors::changeOSensorWeight()",
420  "Illegal osensor weight %g.", weight);
421 
422  OSensor& osensor = osensors[mx];
423  if (osensor.weight == weight)
424  return;
425 
426  if (osensor.weight == 0 || weight == 0)
427  uninitializeAssembler(); // qualitative change
428 
429  osensor.weight = weight;
430 }
431 
436 const Rotation& getObservation(ObservationIx ox) const
437 { return observations[ox]; }
438 
446 { return observations; }
447 
452 Rotation findCurrentOSensorOrientation(OSensorIx mx) const;
453 
460 Real findCurrentOSensorError(OSensorIx mx) const {
461  const ObservationIx ox = getObservationIxForOSensor(mx);
462  if (!ox.isValid()) return 0; // no observation for this osensor
463  const Rotation& R_GO = getObservation(ox);
464  if (!R_GO.isFinite()) return 0; // NaN in observation; error is ignored
465  const Rotation R_GS = findCurrentOSensorOrientation(mx);
466  const Rotation R_SO = ~R_GS*R_GO; // orientation error, in S
467  const Vec4 aa_SO = R_SO.convertRotationToAngleAxis();
468  return std::abs(aa_SO[0]);
469 }
474 //------------------------------------------------------------------------------
478 int initializeCondition() const OVERRIDE_11;
479 void uninitializeCondition() const OVERRIDE_11;
480 int calcErrors(const State& state, Vector& err) const OVERRIDE_11;
481 int calcErrorJacobian(const State& state, Matrix& jacobian) const OVERRIDE_11;
482 int getNumErrors(const State& state) const OVERRIDE_11;
483 int calcGoal(const State& state, Real& goal) const OVERRIDE_11;
484 int calcGoalGradient(const State& state, Vector& grad) const OVERRIDE_11;
487 //------------------------------------------------------------------------------
488  private:
489 //------------------------------------------------------------------------------
490 const OSensor& getOSensor(OSensorIx i) const {return osensors[i];}
491 OSensor& updOSensor(OSensorIx i) {uninitializeAssembler(); return osensors[i];}
492 
493  // data members
494 
495 // OSensor definition. Any change here except a quantitative change to the
496 // osensor's weight uninitializes the Assembler.
497 Array_<OSensor,OSensorIx> osensors;
498 std::map<String,OSensorIx> osensorsByName;
499 
500 // Observation-osensor corresondence specification. Any change here
501 // uninitializes the Assembler.
502 Array_<OSensorIx,ObservationIx> observation2osensor;
503 
504 // For convience in mapping from an osensor to its corresponding observation.
505 // ObservationIx will be invalid if a particular osensor has no associated
506 // observation.
507 Array_<ObservationIx,OSensorIx> osensor2observation;
508 
509 // This is the current set of osensor orientation observations, one per entry in
510 // the observation2osensor array. Changing the values here does not uninitialize
511 // the Assembler.
512 Array_<Rotation,ObservationIx> observations;
513 
514 // After initialize, this groups the osensors by body and weeds out
515 // any zero-weighted osensors. TODO: skip low-weighted osensors, at
516 // least at the start of the assembly.
517 typedef std::map<MobilizedBodyIndex,Array_<OSensorIx> > PerBodyOSensors;
518 mutable PerBodyOSensors bodiesWithOSensors;
519 };
520 
521 } // namespace SimTK
522 
523 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:48
#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
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:2037
const Rotation & getObservation(ObservationIx ox) const
Return the current value of the orientation for this observation.
Definition: AssemblyCondition_OrientationSensors.h:436
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
Rotation_ & setRotationToNaN()
Construct Rotation_ filled with NaNs.
Definition: Rotation.h:152
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
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
bool hasOSensor(ObservationIx ox) const
Return true if the supplied observation is currently associated with a osensor.
Definition: AssemblyCondition_OrientationSensors.h:345
#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...
bool hasObservation(OSensorIx mx) const
Return true if the supplied osensor is currently associated with an observation.
Definition: AssemblyCondition_OrientationSensors.h:332
The SimTK::Array_ container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
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:577
ObservationIx getObservationIxForOSensor(OSensorIx mx) const
Return the ObservationIx of the observation that is currently associated with the given osensor...
Definition: AssemblyCondition_OrientationSensors.h:327
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
const Array_< Rotation, ObservationIx > & getAllObservations() const
Return the current values of all the observed orientations.
Definition: AssemblyCondition_OrientationSensors.h:445
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
Rotation_< Real > Rotation
Definition: Rotation.h:47
bool isFinite(const negator< float > &x)
Definition: negator.h:287
Vec4P convertRotationToAngleAxis() const
Converts rotation matrix to an equivalent angle-axis representation in canonicalized form...
Definition: Rotation.h:836
const String & getOSensorName(OSensorIx ix)
Return the unique osensor name assigned to the osensor whose index is provided.
Definition: AssemblyCondition_OrientationSensors.h:291
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
MobilizedBodyIndex getOSensorBody(OSensorIx mx) const
Get the MobilizedBodyIndex of the body associated with this osensor.
Definition: AssemblyCondition_OrientationSensors.h:307
#define OVERRIDE_11
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:251
#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:430
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
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
int getNumOSensors() const
Return a count n of the number of currently-defined osensors.
Definition: AssemblyCondition_OrientationSensors.h:286
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
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
bool empty() const
Return true if there are no elements currently stored in this array.
Definition: Array.h:2042
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).
This is a fixed-length column vector designed for no-overhead inline computation. ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:589
OSensorIx addOSensor(MobilizedBodyIndex bodyB, const Rotation &orientationInB, Real weight=1)
Define an unnamed osensor.
Definition: AssemblyCondition_OrientationSensors.h:179
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
int getNumObservations() const
Return the number of observations that were defined via the last call to defineObservationOrder().
Definition: AssemblyCondition_OrientationSensors.h:320
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:1112
void defineObservationOrder(const std::vector< std::string > &observationOrder)
Define observation order using an std::vector of std::string.
Definition: AssemblyCondition_OrientationSensors.h:262
void clear()
Erase all the elements currently in this array without changing the capacity; equivalent to erase(beg...
Definition: Array.h:2522
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
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