AssemblyCondition_OrientationSensors.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
#define SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_
 
/* -------------------------------------------------------------------------- *
 *                               Simbody(tm)                                  *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK biosimulation toolkit originating from           *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2014 Stanford University and the Authors.           *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */
 
#include "SimTKcommon.h"
#include "simbody/internal/common.h"
#include "simbody/internal/Assembler.h"
#include "simbody/internal/AssemblyCondition.h"
 
#include <map>
 
namespace SimTK {
 
//------------------------------------------------------------------------------
//                           ORIENTATION SENSORS
//------------------------------------------------------------------------------
class SimTK_SIMBODY_EXPORT OrientationSensors : public AssemblyCondition {
 
// This is a private class used in the implementation below but not
// accessible through the API.
struct OSensor {
    OSensor(const String& name, MobilizedBodyIndex bodyB, 
            const Rotation& orientationInB, Real weight = 1)
    :   name(name), bodyB(bodyB), orientationInB(orientationInB), weight(weight) 
    { assert(weight >= 0); }
 
    OSensor(MobilizedBodyIndex bodyB, const Rotation& orientationInB, 
            Real weight=1)
    :   name(""), bodyB(bodyB), orientationInB(orientationInB), weight(weight) 
    { assert(weight >= 0); }
 
    String              name;
    MobilizedBodyIndex  bodyB;
    Rotation            orientationInB;
    Real                weight; 
};
 
public:
 
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(OrientationSensors,OSensorIx);
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(OrientationSensors,ObservationIx);
 
 
 
//------------------------------------------------------------------------------
 
OrientationSensors() : AssemblyCondition("OrientationSensors") {}
 
OSensorIx addOSensor(const String& name, MobilizedBodyIndex bodyB, 
                     const Rotation& orientationInB, Real weight=1)
{   SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 
        "OrientationSensors::addOSensor()", 
        "Illegal orientation sensor weight %g.", weight);
    uninitializeAssembler();
    // Forget any previously-established observation/osensor correspondence.
    observation2osensor.clear(); osensor2observation.clear(); 
    observations.clear();
    const OSensorIx ix(osensors.size());
    String nm = String::trimWhiteSpace(name);
    if (nm.empty())
        nm = String("_UNNAMED_") + String(ix);
 
    std::pair< std::map<String,OSensorIx>::iterator, bool >
        found = osensorsByName.insert(std::make_pair(nm,ix));
    SimTK_ERRCHK2_ALWAYS(found.second, // true if insertion was done
        "OSensors::addOSensor()",
        "OSensor name '%s' was already use for OSensor %d.",
        nm.c_str(), (int)found.first->second); 
 
    osensors.push_back(OSensor(nm,bodyB,orientationInB,weight));
    return ix; 
}
 
OSensorIx addOSensor(MobilizedBodyIndex bodyB, const Rotation& orientationInB,
                     Real weight=1)
{   return addOSensor("", bodyB, orientationInB, weight); }
 
 
void defineObservationOrder(const Array_<OSensorIx>& observationOrder) {
    uninitializeAssembler();
    if (observationOrder.empty()) {
        observation2osensor.resize(osensors.size());
        for (OSensorIx mx(0); mx < osensors.size(); ++mx)
            observation2osensor[ObservationIx(mx)] = mx;
    } else 
        observation2osensor = observationOrder;
    osensor2observation.clear(); 
    // We might need to grow this more, but this is an OK starting guess.
    osensor2observation.resize(observation2osensor.size()); // all invalid
    for (ObservationIx ox(0); ox < observation2osensor.size(); ++ox) {
        const OSensorIx mx = observation2osensor[ox];
        if (!mx.isValid()) continue;
 
        if (osensor2observation.size() <= mx)
            osensor2observation.resize(mx+1);
        SimTK_ERRCHK4_ALWAYS(!osensor2observation[mx].isValid(),
            "OSensors::defineObservationOrder()", 
            "An attempt was made to associate OSensor %d (%s) with" 
            " Observations %d and %d; only one Observation per OSensor"
            " is permitted.",
            (int)mx, getOSensorName(mx).c_str(), 
            (int)osensor2observation[mx], (int)ox);
 
        osensor2observation[mx] = ox;
    }
    // Make room for osensor observations.
    observations.clear();
    observations.resize(observation2osensor.size(),
                        Rotation().setRotationToNaN());
}
 
void defineObservationOrder(const Array_<String>& observationOrder) 
{   Array_<OSensorIx> osensorIxs(observationOrder.size());
    for (ObservationIx ox(0); ox < observationOrder.size(); ++ox)
        osensorIxs[ox] = getOSensorIx(observationOrder[ox]);
    defineObservationOrder(osensorIxs); }
 
// no copy required
void defineObservationOrder(const std::vector<String>& observationOrder)
{   defineObservationOrder(ArrayViewConst_<String>(observationOrder)); }
 
 
// must copy
void defineObservationOrder(const Array_<std::string>& observationOrder) 
{   const Array_<String> observations(observationOrder); // copy
    defineObservationOrder(observations); }
 
// must copy
void defineObservationOrder(const std::vector<std::string>& observationOrder) 
{   const Array_<String> observations(observationOrder); // copy
    defineObservationOrder(observations); }
 
void defineObservationOrder(int n, const char* const observationOrder[]) 
{   Array_<OSensorIx> osensorIxs(n);
    for (ObservationIx ox(0); ox < n; ++ox)
        osensorIxs[ox] = getOSensorIx(String(observationOrder[ox]));
    defineObservationOrder(osensorIxs); }
//------------------------------------------------------------------------------
 
int getNumOSensors() const {return osensors.size();}
 
const String& getOSensorName(OSensorIx ix) 
{   return osensors[ix].name; }
 
const OSensorIx getOSensorIx(const String& name) 
{   std::map<String,OSensorIx>::const_iterator p = osensorsByName.find(name);
    return p == osensorsByName.end() ? OSensorIx() : p->second; }
 
Real getOSensorWeight(OSensorIx mx)
{   return osensors[mx].weight; }
 
MobilizedBodyIndex getOSensorBody(OSensorIx mx) const
{   return osensors[mx].bodyB; }
 
const Rotation& getOSensorStation(OSensorIx mx) const
{   return osensors[mx].orientationInB; }
 
int getNumObservations() const {return observation2osensor.size();}
 
ObservationIx getObservationIxForOSensor(OSensorIx mx) const 
{   return osensor2observation[mx]; }
 
bool hasObservation(OSensorIx mx) const 
{   return getObservationIxForOSensor(mx).isValid(); }
 
OSensorIx getOSensorIxForObservation(ObservationIx ox) const 
{   return observation2osensor[ox]; }
 
bool hasOSensor(ObservationIx ox) const 
{   return getOSensorIxForObservation(ox).isValid();}
 
const Array_<OSensorIx>& getOSensorsOnBody(MobilizedBodyIndex mbx) {
    static const Array_<OSensorIx> empty;
    SimTK_ERRCHK_ALWAYS(isInAssembler(), "OSensors::getOSensorsOnBody()",
        "This method can't be called until the OSensors object has been"
        " adopted by an Assembler.");
    initializeAssembler();
    PerBodyOSensors::const_iterator bodyp = bodiesWithOSensors.find(mbx);
    return bodyp == bodiesWithOSensors.end() ? empty : bodyp->second;
}
//------------------------------------------------------------------------------
 
void moveOneObservation(ObservationIx ox, const Rotation& observation) {
    SimTK_ERRCHK_ALWAYS(!observations.empty(), "Assembler::moveOneObservation()",
        "There are currently no observations defined. Either the Assembler"
        " needs to be initialized to get the default observation order, or you"
        " should call defineObservationOrder() explicitly.");
    SimTK_ERRCHK2_ALWAYS(ox.isValid() && ox < observations.size(),
        "Assembler::moveOneObservation()", "ObservationIx %d is invalid or"
        " out of range; there are %d observations currently defined. Use"
        " defineObservationOrder() to specify the set of observations and how"
        " they correspond to osensors.", 
        (int)ox, (int)observations.size()); 
    observations[ox] = observation; 
}
 
void moveAllObservations(const Array_<Rotation>& observations) {
    SimTK_ERRCHK2_ALWAYS(   (int)observations.size() 
                         == (int)observation2osensor.size(),
        "OSensors::moveAllObservations()",
        "Number of observations provided (%d) differs from the number of"
        " observations (%d) last defined with defineObservationOrder().",
        observations.size(), observation2osensor.size());
    this->observations = observations;
}
 
void changeOSensorWeight(OSensorIx mx, Real weight) {
   SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 
        "OSensors::changeOSensorWeight()", 
        "Illegal osensor weight %g.", weight);
 
    OSensor& osensor = osensors[mx];
    if (osensor.weight == weight)
        return;
 
    if (osensor.weight == 0 || weight == 0)
        uninitializeAssembler(); // qualitative change
 
    osensor.weight = weight;
}
 
const Rotation& getObservation(ObservationIx ox) const 
{   return observations[ox]; }
 
const Array_<Rotation,ObservationIx>& getAllObservations() const
{   return observations; }
 
Rotation findCurrentOSensorOrientation(OSensorIx mx) const;
 
Real findCurrentOSensorError(OSensorIx mx) const {
    const ObservationIx ox = getObservationIxForOSensor(mx);
    if (!ox.isValid()) return 0; // no observation for this osensor
    const Rotation& R_GO = getObservation(ox);
    if (!R_GO.isFinite()) return 0; // NaN in observation; error is ignored
    const Rotation R_GS = findCurrentOSensorOrientation(mx);
    const Rotation R_SO = ~R_GS*R_GO; // orientation error, in S
    const Vec4 aa_SO = R_SO.convertRotationToAngleAxis();
    return std::abs(aa_SO[0]);
}
//------------------------------------------------------------------------------
int initializeCondition() const override;
void uninitializeCondition() const override;
int calcErrors(const State& state, Vector& err) const override;
int calcErrorJacobian(const State& state, Matrix& jacobian) const override;
int getNumErrors(const State& state) const override;
int calcGoal(const State& state, Real& goal) const override;
int calcGoalGradient(const State& state, Vector& grad) const override;
//------------------------------------------------------------------------------
                                    private:
//------------------------------------------------------------------------------
const OSensor& getOSensor(OSensorIx i) const {return osensors[i];}
OSensor& updOSensor(OSensorIx i) {uninitializeAssembler(); return osensors[i];}
 
                                // data members                               
                               
// OSensor definition. Any change here except a quantitative change to the
// osensor's weight uninitializes the Assembler.
Array_<OSensor,OSensorIx>       osensors;
std::map<String,OSensorIx>      osensorsByName;
 
// Observation-osensor corresondence specification. Any change here 
// uninitializes the Assembler.
Array_<OSensorIx,ObservationIx> observation2osensor;
 
// For convience in mapping from an osensor to its corresponding observation.
// ObservationIx will be invalid if a particular osensor has no associated
// observation.
Array_<ObservationIx,OSensorIx> osensor2observation;
 
// This is the current set of osensor orientation observations, one per entry in 
// the observation2osensor array. Changing the values here does not uninitialize
// the Assembler.            
Array_<Rotation,ObservationIx>  observations;
 
// After initialize, this groups the osensors by body and weeds out
// any zero-weighted osensors. TODO: skip low-weighted osensors, at
// least at the start of the assembly.
typedef std::map<MobilizedBodyIndex,Array_<OSensorIx> > PerBodyOSensors;
mutable PerBodyOSensors         bodiesWithOSensors;
};
 
} // namespace SimTK
 
#endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_ORIENTATION_SENSORS_H_