Simbody
3.5
|
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_