Simbody  3.5
AssemblyCondition_OrientationSensors.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
00002 #define SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                               Simbody(tm)                                  *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2014 Stanford University and the Authors.           *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors:                                                              *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00027 #include "SimTKcommon.h"
00028 #include "simbody/internal/common.h"
00029 #include "simbody/internal/Assembler.h"
00030 #include "simbody/internal/AssemblyCondition.h"
00031 
00032 #include <map>
00033 
00034 namespace SimTK {
00035 
00036 //------------------------------------------------------------------------------
00037 //                           ORIENTATION SENSORS
00038 //------------------------------------------------------------------------------
00089 class SimTK_SIMBODY_EXPORT OrientationSensors : public AssemblyCondition {
00090 
00091 // This is a private class used in the implementation below but not
00092 // accessible through the API.
00093 struct OSensor {
00094     OSensor(const String& name, MobilizedBodyIndex bodyB, 
00095             const Rotation& orientationInB, Real weight = 1)
00096     :   name(name), bodyB(bodyB), orientationInB(orientationInB), weight(weight) 
00097     { assert(weight >= 0); }
00098 
00099     OSensor(MobilizedBodyIndex bodyB, const Rotation& orientationInB, 
00100             Real weight=1)
00101     :   name(""), bodyB(bodyB), orientationInB(orientationInB), weight(weight) 
00102     { assert(weight >= 0); }
00103 
00104     String              name;
00105     MobilizedBodyIndex  bodyB;
00106     Rotation            orientationInB;
00107     Real                weight; 
00108 };
00109 
00110 public:
00111 
00113 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(OrientationSensors,OSensorIx);
00115 SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(OrientationSensors,ObservationIx);
00116 
00117 
00118 
00119 //------------------------------------------------------------------------------
00125 
00129 OrientationSensors() : AssemblyCondition("OrientationSensors") {}
00130 
00150 OSensorIx addOSensor(const String& name, MobilizedBodyIndex bodyB, 
00151                      const Rotation& orientationInB, Real weight=1)
00152 {   SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 
00153         "OrientationSensors::addOSensor()", 
00154         "Illegal orientation sensor weight %g.", weight);
00155     uninitializeAssembler();
00156     // Forget any previously-established observation/osensor correspondence.
00157     observation2osensor.clear(); osensor2observation.clear(); 
00158     observations.clear();
00159     const OSensorIx ix(osensors.size());
00160     String nm = String::trimWhiteSpace(name);
00161     if (nm.empty())
00162         nm = String("_UNNAMED_") + String(ix);
00163 
00164     std::pair< std::map<String,OSensorIx>::iterator, bool >
00165         found = osensorsByName.insert(std::make_pair(nm,ix));
00166     SimTK_ERRCHK2_ALWAYS(found.second, // true if insertion was done
00167         "OSensors::addOSensor()",
00168         "OSensor name '%s' was already use for OSensor %d.",
00169         nm.c_str(), (int)found.first->second); 
00170 
00171     osensors.push_back(OSensor(nm,bodyB,orientationInB,weight));
00172     return ix; 
00173 }
00174 
00179 OSensorIx addOSensor(MobilizedBodyIndex bodyB, const Rotation& orientationInB,
00180                      Real weight=1)
00181 {   return addOSensor("", bodyB, orientationInB, weight); }
00182 
00183 
00204 void defineObservationOrder(const Array_<OSensorIx>& observationOrder) {
00205     uninitializeAssembler();
00206     if (observationOrder.empty()) {
00207         observation2osensor.resize(osensors.size());
00208         for (OSensorIx mx(0); mx < osensors.size(); ++mx)
00209             observation2osensor[ObservationIx(mx)] = mx;
00210     } else 
00211         observation2osensor = observationOrder;
00212     osensor2observation.clear(); 
00213     // We might need to grow this more, but this is an OK starting guess.
00214     osensor2observation.resize(observation2osensor.size()); // all invalid
00215     for (ObservationIx ox(0); ox < observation2osensor.size(); ++ox) {
00216         const OSensorIx mx = observation2osensor[ox];
00217         if (!mx.isValid()) continue;
00218 
00219         if (osensor2observation.size() <= mx)
00220             osensor2observation.resize(mx+1);
00221         SimTK_ERRCHK4_ALWAYS(!osensor2observation[mx].isValid(),
00222             "OSensors::defineObservationOrder()", 
00223             "An attempt was made to associate OSensor %d (%s) with" 
00224             " Observations %d and %d; only one Observation per OSensor"
00225             " is permitted.",
00226             (int)mx, getOSensorName(mx).c_str(), 
00227             (int)osensor2observation[mx], (int)ox);
00228 
00229         osensor2observation[mx] = ox;
00230     }
00231     // Make room for osensor observations.
00232     observations.clear();
00233     observations.resize(observation2osensor.size(),
00234                         Rotation().setRotationToNaN());
00235 }
00236 
00242 void defineObservationOrder(const Array_<String>& observationOrder) 
00243 {   Array_<OSensorIx> osensorIxs(observationOrder.size());
00244     for (ObservationIx ox(0); ox < observationOrder.size(); ++ox)
00245         osensorIxs[ox] = getOSensorIx(observationOrder[ox]);
00246     defineObservationOrder(osensorIxs); }
00247 
00249 // no copy required
00250 void defineObservationOrder(const std::vector<String>& observationOrder)
00251 {   defineObservationOrder(ArrayViewConst_<String>(observationOrder)); }
00252 
00253 
00255 // must copy
00256 void defineObservationOrder(const Array_<std::string>& observationOrder) 
00257 {   const Array_<String> observations(observationOrder); // copy
00258     defineObservationOrder(observations); }
00259 
00261 // must copy
00262 void defineObservationOrder(const std::vector<std::string>& observationOrder) 
00263 {   const Array_<String> observations(observationOrder); // copy
00264     defineObservationOrder(observations); }
00265 
00267 void defineObservationOrder(int n, const char* const observationOrder[]) 
00268 {   Array_<OSensorIx> osensorIxs(n);
00269     for (ObservationIx ox(0); ox < n; ++ox)
00270         osensorIxs[ox] = getOSensorIx(String(observationOrder[ox]));
00271     defineObservationOrder(osensorIxs); }
00276 //------------------------------------------------------------------------------
00283 
00286 int getNumOSensors() const {return osensors.size();}
00287 
00291 const String& getOSensorName(OSensorIx ix) 
00292 {   return osensors[ix].name; }
00293 
00297 const OSensorIx getOSensorIx(const String& name) 
00298 {   std::map<String,OSensorIx>::const_iterator p = osensorsByName.find(name);
00299     return p == osensorsByName.end() ? OSensorIx() : p->second; }
00300 
00303 Real getOSensorWeight(OSensorIx mx)
00304 {   return osensors[mx].weight; }
00305 
00307 MobilizedBodyIndex getOSensorBody(OSensorIx mx) const
00308 {   return osensors[mx].bodyB; }
00309 
00312 const Rotation& getOSensorStation(OSensorIx mx) const
00313 {   return osensors[mx].orientationInB; }
00314 
00320 int getNumObservations() const {return observation2osensor.size();}
00321 
00327 ObservationIx getObservationIxForOSensor(OSensorIx mx) const 
00328 {   return osensor2observation[mx]; }
00329 
00332 bool hasObservation(OSensorIx mx) const 
00333 {   return getObservationIxForOSensor(mx).isValid(); }
00334 
00340 OSensorIx getOSensorIxForObservation(ObservationIx ox) const 
00341 {   return observation2osensor[ox]; }
00342 
00345 bool hasOSensor(ObservationIx ox) const 
00346 {   return getOSensorIxForObservation(ox).isValid();}
00347 
00352 const Array_<OSensorIx>& getOSensorsOnBody(MobilizedBodyIndex mbx) {
00353     static const Array_<OSensorIx> empty;
00354     SimTK_ERRCHK_ALWAYS(isInAssembler(), "OSensors::getOSensorsOnBody()",
00355         "This method can't be called until the OSensors object has been"
00356         " adopted by an Assembler.");
00357     initializeAssembler();
00358     PerBodyOSensors::const_iterator bodyp = bodiesWithOSensors.find(mbx);
00359     return bodyp == bodiesWithOSensors.end() ? empty : bodyp->second;
00360 }
00365 //------------------------------------------------------------------------------
00371 
00375 void moveOneObservation(ObservationIx ox, const Rotation& observation) {
00376     SimTK_ERRCHK_ALWAYS(!observations.empty(), "Assembler::moveOneObservation()",
00377         "There are currently no observations defined. Either the Assembler"
00378         " needs to be initialized to get the default observation order, or you"
00379         " should call defineObservationOrder() explicitly.");
00380     SimTK_ERRCHK2_ALWAYS(ox.isValid() && ox < observations.size(),
00381         "Assembler::moveOneObservation()", "ObservationIx %d is invalid or"
00382         " out of range; there are %d observations currently defined. Use"
00383         " defineObservationOrder() to specify the set of observations and how"
00384         " they correspond to osensors.", 
00385         (int)ox, (int)observations.size()); 
00386     observations[ox] = observation; 
00387 }
00388 
00398 void moveAllObservations(const Array_<Rotation>& observations) {
00399     SimTK_ERRCHK2_ALWAYS(   (int)observations.size() 
00400                          == (int)observation2osensor.size(),
00401         "OSensors::moveAllObservations()",
00402         "Number of observations provided (%d) differs from the number of"
00403         " observations (%d) last defined with defineObservationOrder().",
00404         observations.size(), observation2osensor.size());
00405     this->observations = observations;
00406 }
00407 
00417 void changeOSensorWeight(OSensorIx mx, Real weight) {
00418    SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 
00419         "OSensors::changeOSensorWeight()", 
00420         "Illegal osensor weight %g.", weight);
00421 
00422     OSensor& osensor = osensors[mx];
00423     if (osensor.weight == weight)
00424         return;
00425 
00426     if (osensor.weight == 0 || weight == 0)
00427         uninitializeAssembler(); // qualitative change
00428 
00429     osensor.weight = weight;
00430 }
00431 
00436 const Rotation& getObservation(ObservationIx ox) const 
00437 {   return observations[ox]; }
00438 
00445 const Array_<Rotation,ObservationIx>& getAllObservations() const
00446 {   return observations; }
00447 
00452 Rotation findCurrentOSensorOrientation(OSensorIx mx) const;
00453 
00460 Real findCurrentOSensorError(OSensorIx mx) const {
00461     const ObservationIx ox = getObservationIxForOSensor(mx);
00462     if (!ox.isValid()) return 0; // no observation for this osensor
00463     const Rotation& R_GO = getObservation(ox);
00464     if (!R_GO.isFinite()) return 0; // NaN in observation; error is ignored
00465     const Rotation R_GS = findCurrentOSensorOrientation(mx);
00466     const Rotation R_SO = ~R_GS*R_GO; // orientation error, in S
00467     const Vec4 aa_SO = R_SO.convertRotationToAngleAxis();
00468     return std::abs(aa_SO[0]);
00469 }
00474 //------------------------------------------------------------------------------
00478 int initializeCondition() const OVERRIDE_11;
00479 void uninitializeCondition() const OVERRIDE_11;
00480 int calcErrors(const State& state, Vector& err) const OVERRIDE_11;
00481 int calcErrorJacobian(const State& state, Matrix& jacobian) const OVERRIDE_11;
00482 int getNumErrors(const State& state) const OVERRIDE_11;
00483 int calcGoal(const State& state, Real& goal) const OVERRIDE_11;
00484 int calcGoalGradient(const State& state, Vector& grad) const OVERRIDE_11;
00487 //------------------------------------------------------------------------------
00488                                     private:
00489 //------------------------------------------------------------------------------
00490 const OSensor& getOSensor(OSensorIx i) const {return osensors[i];}
00491 OSensor& updOSensor(OSensorIx i) {uninitializeAssembler(); return osensors[i];}
00492 
00493                                 // data members                               
00494                                
00495 // OSensor definition. Any change here except a quantitative change to the
00496 // osensor's weight uninitializes the Assembler.
00497 Array_<OSensor,OSensorIx>       osensors;
00498 std::map<String,OSensorIx>      osensorsByName;
00499 
00500 // Observation-osensor corresondence specification. Any change here 
00501 // uninitializes the Assembler.
00502 Array_<OSensorIx,ObservationIx> observation2osensor;
00503 
00504 // For convience in mapping from an osensor to its corresponding observation.
00505 // ObservationIx will be invalid if a particular osensor has no associated
00506 // observation.
00507 Array_<ObservationIx,OSensorIx> osensor2observation;
00508 
00509 // This is the current set of osensor orientation observations, one per entry in 
00510 // the observation2osensor array. Changing the values here does not uninitialize
00511 // the Assembler.            
00512 Array_<Rotation,ObservationIx>  observations;
00513 
00514 // After initialize, this groups the osensors by body and weeds out
00515 // any zero-weighted osensors. TODO: skip low-weighted osensors, at
00516 // least at the start of the assembly.
00517 typedef std::map<MobilizedBodyIndex,Array_<OSensorIx> > PerBodyOSensors;
00518 mutable PerBodyOSensors         bodiesWithOSensors;
00519 };
00520 
00521 } // namespace SimTK
00522 
00523 #endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines