Simbody  3.7
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;
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;
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_
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&#39;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&#39;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&#39;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