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
34namespace 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.
93struct 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
110public:
111
116
117
118
119//------------------------------------------------------------------------------
125
129OrientationSensors() : AssemblyCondition("OrientationSensors") {}
130
150OSensorIx 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());
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
179OSensorIx addOSensor(MobilizedBodyIndex bodyB, const Rotation& orientationInB,
180 Real weight=1)
181{ return addOSensor("", bodyB, orientationInB, weight); }
182
183
204void 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(),
234 Rotation().setRotationToNaN());
235}
236
242void 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
250void defineObservationOrder(const std::vector<String>& observationOrder)
251{ defineObservationOrder(ArrayViewConst_<String>(observationOrder)); }
252
253
255// must copy
256void defineObservationOrder(const Array_<std::string>& observationOrder)
257{ const Array_<String> observations(observationOrder); // copy
258 defineObservationOrder(observations); }
259
261// must copy
262void defineObservationOrder(const std::vector<std::string>& observationOrder)
263{ const Array_<String> observations(observationOrder); // copy
264 defineObservationOrder(observations); }
265
267void 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
286int getNumOSensors() const {return osensors.size();}
287
291const String& getOSensorName(OSensorIx ix)
292{ return osensors[ix].name; }
293
297const 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
304{ return osensors[mx].weight; }
305
308{ return osensors[mx].bodyB; }
309
312const Rotation& getOSensorStation(OSensorIx mx) const
313{ return osensors[mx].orientationInB; }
314
320int getNumObservations() const {return observation2osensor.size();}
321
327ObservationIx getObservationIxForOSensor(OSensorIx mx) const
328{ return osensor2observation[mx]; }
329
332bool hasObservation(OSensorIx mx) const
333{ return getObservationIxForOSensor(mx).isValid(); }
334
340OSensorIx getOSensorIxForObservation(ObservationIx ox) const
341{ return observation2osensor[ox]; }
342
345bool 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
375void 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
398void 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
417void 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
436const Rotation& getObservation(ObservationIx ox) const
437{ return observations[ox]; }
438
446{ return observations; }
447
453
460Real 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//------------------------------------------------------------------------------
478int initializeCondition() const override;
479void uninitializeCondition() const override;
480int calcErrors(const State& state, Vector& err) const override;
481int calcErrorJacobian(const State& state, Matrix& jacobian) const override;
482int getNumErrors(const State& state) const override;
483int calcGoal(const State& state, Real& goal) const override;
484int calcGoalGradient(const State& state, Vector& grad) const override;
487//------------------------------------------------------------------------------
488 private:
489//------------------------------------------------------------------------------
490const OSensor& getOSensor(OSensorIx i) const {return osensors[i];}
491OSensor& 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.
497Array_<OSensor,OSensorIx> osensors;
498std::map<String,OSensorIx> osensorsByName;
499
500// Observation-osensor corresondence specification. Any change here
501// uninitializes the Assembler.
502Array_<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.
507Array_<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.
512Array_<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.
517typedef std::map<MobilizedBodyIndex,Array_<OSensorIx> > PerBodyOSensors;
518mutable PerBodyOSensors bodiesWithOSensors;
519};
520
521} // namespace SimTK
522
523#endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_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
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
String & trimWhiteSpace()
Trim this String in place, removing all the initial leading and trailing white space,...
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
RowVectorBase< typename CNT< ELEM >::TAbs > abs(const RowVectorBase< ELEM > &v)
Definition: VectorMath.h:120
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