AssemblyCondition_Markers.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_
#define SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_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) 2010-14 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 {

//------------------------------------------------------------------------------
//                                  MARKERS
//------------------------------------------------------------------------------
class SimTK_SIMBODY_EXPORT Markers : public AssemblyCondition {

// This is a private class used in the implementation below but not
// accessible through the API.
struct Marker {
    Marker(const String& name, MobilizedBodyIndex bodyB, 
           const Vec3& markerInB, Real weight = 1)
    :   name(name), bodyB(bodyB), markerInB(markerInB), weight(weight) 
    { assert(weight >= 0); }

    Marker(MobilizedBodyIndex bodyB, const Vec3& markerInB, Real weight=1)
    :   name(""), bodyB(bodyB), markerInB(markerInB), weight(weight) 
    { assert(weight >= 0); }

    String              name;
    MobilizedBodyIndex  bodyB;
    Vec3                markerInB;
    Real                weight; 
};

public:

SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers,MarkerIx);
SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Markers,ObservationIx);



//------------------------------------------------------------------------------

Markers() : AssemblyCondition("Markers") {}

MarkerIx addMarker(const String& name, MobilizedBodyIndex bodyB, 
                   const Vec3& markerInB, Real weight=1)
{   SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 
        "Markers::addMarker()", "Illegal marker weight %g.", weight);
    uninitializeAssembler();
    // Forget any previously-established observation/marker correspondence.
    observation2marker.clear(); marker2observation.clear(); 
    observations.clear();
    const MarkerIx ix(markers.size());
    String nm = String::trimWhiteSpace(name);
    if (nm.empty())
        nm = String("_UNNAMED_") + String(ix);

    std::pair< std::map<String,MarkerIx>::iterator, bool >
        found = markersByName.insert(std::make_pair(nm,ix));
    SimTK_ERRCHK2_ALWAYS(found.second, // true if insertion was done
        "Markers::addMarker()",
        "Marker name '%s' was already use for Marker %d.",
        nm.c_str(), (int)found.first->second); 

    markers.push_back(Marker(nm,bodyB,markerInB,weight));
    return ix; 
}

MarkerIx addMarker(MobilizedBodyIndex bodyB, const Vec3& markerInB,
                   Real weight=1)
{   return addMarker("", bodyB, markerInB, weight); }


void defineObservationOrder(const Array_<MarkerIx>& observationOrder) {
    uninitializeAssembler();
    if (observationOrder.empty()) {
        observation2marker.resize(markers.size());
        for (MarkerIx mx(0); mx < markers.size(); ++mx)
            observation2marker[ObservationIx(mx)] = mx;
    } else 
        observation2marker = observationOrder;
    marker2observation.clear(); 
    // We might need to grow this more, but this is an OK starting guess.
    marker2observation.resize(observation2marker.size()); // all invalid
    for (ObservationIx ox(0); ox < observation2marker.size(); ++ox) {
        const MarkerIx mx = observation2marker[ox];
        if (!mx.isValid()) continue;

        if (marker2observation.size() <= mx)
            marker2observation.resize(mx+1);
        SimTK_ERRCHK4_ALWAYS(!marker2observation[mx].isValid(),
            "Markers::defineObservationOrder()", 
            "An attempt was made to associate Marker %d (%s) with" 
            " Observations %d and %d; only one Observation per Marker"
            " is permitted.",
            (int)mx, getMarkerName(mx).c_str(), 
            (int)marker2observation[mx], (int)ox);

        marker2observation[mx] = ox;
    }
    // Make room for marker observations.
    observations.clear();
    observations.resize(observation2marker.size(),Vec3(NaN));
}

void defineObservationOrder(const Array_<String>& observationOrder) 
{   Array_<MarkerIx> markerIxs(observationOrder.size());
    for (ObservationIx ox(0); ox < observationOrder.size(); ++ox)
        markerIxs[ox] = getMarkerIx(observationOrder[ox]);
    defineObservationOrder(markerIxs); }

// 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_<MarkerIx> markerIxs(n);
    for (ObservationIx ox(0); ox < n; ++ox)
        markerIxs[ox] = getMarkerIx(String(observationOrder[ox]));
    defineObservationOrder(markerIxs); }
//------------------------------------------------------------------------------

int getNumMarkers() const {return markers.size();}

const String& getMarkerName(MarkerIx ix) 
{   return markers[ix].name; }
const MarkerIx getMarkerIx(const String& name) 
{   std::map<String,MarkerIx>::const_iterator p = markersByName.find(name);
    return p == markersByName.end() ? MarkerIx() : p->second; }

Real getMarkerWeight(MarkerIx mx)
{   return markers[mx].weight; }

MobilizedBodyIndex getMarkerBody(MarkerIx mx) const
{   return markers[mx].bodyB; }

const Vec3& getMarkerStation(MarkerIx mx) const
{   return markers[mx].markerInB; }

int getNumObservations() const {return observation2marker.size();}

ObservationIx getObservationIxForMarker(MarkerIx mx) const 
{ return marker2observation[mx]; }

bool hasObservation(MarkerIx mx) const 
{ return getObservationIxForMarker(mx).isValid(); }

MarkerIx getMarkerIxForObservation(ObservationIx ox) const 
{ return observation2marker[ox]; }

bool hasMarker(ObservationIx ox) const 
{ return getMarkerIxForObservation(ox).isValid();}

const Array_<MarkerIx>& getMarkersOnBody(MobilizedBodyIndex mbx) {
    static const Array_<MarkerIx> empty;
    SimTK_ERRCHK_ALWAYS(isInAssembler(), "Markers::getMarkersOnBody()",
        "This method can't be called until the Markers object has been"
        " adopted by an Assembler.");
    initializeAssembler();
    PerBodyMarkers::const_iterator bodyp = bodiesWithMarkers.find(mbx);
    return bodyp == bodiesWithMarkers.end() ? empty : bodyp->second;
}
//------------------------------------------------------------------------------

void moveOneObservation(ObservationIx ox, const Vec3& 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 markers.", 
        (int)ox, (int)observations.size()); 
    observations[ox] = observation; 
}

void moveAllObservations(const Array_<Vec3>& observations) 
{   SimTK_ERRCHK2_ALWAYS((int)observations.size() == (int)observation2marker.size(),
        "Markers::moveAllObservations()",
        "Number of observations provided (%d) differs from the number of"
        " observations (%d) last defined with defineObservationOrder().",
        observations.size(), observation2marker.size());
    this->observations = observations; }

void changeMarkerWeight(MarkerIx mx, Real weight) {
   SimTK_ERRCHK1_ALWAYS(isFinite(weight) && weight >= 0, 
        "Markers::changeMarkerWeight()", "Illegal marker weight %g.", weight);

    Marker& marker = markers[mx];
    if (marker.weight == weight)
        return;

    if (marker.weight == 0 || weight == 0)
        uninitializeAssembler(); // qualitative change

    marker.weight = weight;
}

const Vec3& getObservation(ObservationIx ox) const {return observations[ox];}
const Array_<Vec3,ObservationIx>& getAllObservations() const
{   return observations; }

Vec3 findCurrentMarkerLocation(MarkerIx mx) const;

Real findCurrentMarkerError(MarkerIx mx) const
{   return std::sqrt(findCurrentMarkerErrorSquared(mx)); }

Real findCurrentMarkerErrorSquared(MarkerIx mx) const {
    const ObservationIx ox = getObservationIxForMarker(mx);
    if (!ox.isValid()) return 0; // no observation for this marker
    const Vec3& loc = getObservation(ox);
    if (!loc.isFinite()) return 0; // NaN in observation; error is ignored
    return (findCurrentMarkerLocation(mx) - loc).normSqr();
}
//------------------------------------------------------------------------------
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 Marker& getMarker(MarkerIx i) const {return markers[i];}
Marker& updMarker(MarkerIx i) {uninitializeAssembler(); return markers[i];}

                                // data members                               
                               
// Marker definition. Any change here except a quantitative change to the
// marker's weight uninitializes the Assembler.
Array_<Marker,MarkerIx>         markers;
std::map<String,MarkerIx>       markersByName;

// Observation-marker corresondence specification. Any change here 
// uninitializes the Assembler.
Array_<MarkerIx,ObservationIx>  observation2marker;

// For convience in mapping from a marker to its corresponding observation.
// ObservationIx will be invalid if a particular marker has no associated
// observation.
Array_<ObservationIx,MarkerIx>  marker2observation;

// This is the current set of marker location observations, one per entry in 
// the observation2marker array. Changing the values here does not uninitialize
// the Assembler.            
Array_<Vec3,ObservationIx>      observations;

// After initialize, this groups the markers by body and weeds out
// any zero-weighted markers. TODO: skip low-weighted markers, at
// least at the start of the assembly.
typedef std::map<MobilizedBodyIndex,Array_<MarkerIx> > PerBodyMarkers;
mutable PerBodyMarkers          bodiesWithMarkers;
};

} // namespace SimTK

#endif // SimTK_SIMBODY_ASSEMBLY_CONDITION_MARKERS_H_