Assembler.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_ASSEMBLER_H_
#define SimTK_SIMBODY_ASSEMBLER_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-12 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/MultibodySystem.h"
#include "simbody/internal/SimbodyMatterSubsystem.h"

#include <set>
#include <map>
#include <cassert>
#include <cmath>

namespace SimTK {

SimTK_DEFINE_UNIQUE_INDEX_TYPE(AssemblyConditionIndex);

class AssemblyCondition;

class SimTK_SIMBODY_EXPORT Assembler : public Study {
    typedef std::set<MobilizedBodyIndex>            LockedMobilizers;
    typedef std::set<MobilizerQIndex>               QSet;
    typedef std::map<MobilizedBodyIndex, QSet>      LockedQs;
    typedef std::map<MobilizerQIndex, Vec2>         QRanges;
    typedef std::map<MobilizedBodyIndex, QRanges>   RestrictedQs;
public:

SimTK_DEFINE_UNIQUE_LOCAL_INDEX_TYPE(Assembler,FreeQIndex);

class AssembleFailed;
class TrackFailed;

explicit Assembler(const MultibodySystem& system);

Assembler& setErrorTolerance(Real tolerance=0) {
    SimTK_ERRCHK1_ALWAYS(0 <= tolerance,
        "Assembler::setTolerance()", "The requested error tolerance %g"
        " is illegal; we require 0 <= tolerance, with 0 indicating that"
        " the default tolerance (accuracy/10) is to be used.", tolerance);
    this->tolerance = tolerance;
    return *this;
}
Real getErrorToleranceInUse() const {   
    return tolerance > 0 ? tolerance 
           : (accuracy > 0 ? accuracy/10 : Real(0.1)/OODefaultAccuracy); 
}

Assembler& setAccuracy(Real accuracy=0) {
    SimTK_ERRCHK2_ALWAYS(0 <= accuracy && accuracy < 1,
        "Assembler::setAccuracy()", "The requested accuracy %g is illegal;"
        " we require 0 <= accuracy < 1, with 0 indicating that the default"
        " accuracy (%g) is to be used.", Real(1)/OODefaultAccuracy, accuracy);
    this->accuracy = accuracy;
    return *this;
}
Real getAccuracyInUse() const 
{   return accuracy > 0 ? accuracy : Real(1)/OODefaultAccuracy; }


Assembler& setSystemConstraintsWeight(Real weight)
{   assert(systemConstraints.isValid());
    setAssemblyConditionWeight(systemConstraints,weight);
    return *this; }

Real getSystemConstraintsWeight() const
{   assert(systemConstraints.isValid());
    return getAssemblyConditionWeight(systemConstraints); }

Assembler& setAssemblyConditionWeight(AssemblyConditionIndex condition, 
                                      Real                   weight) {
    SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
        "Assembler::setAssemblyConditionWeight()");
    SimTK_ERRCHK1_ALWAYS(weight >= 0, "Assembler::setAssemblyConditionWeight()",
        "Illegal weight %g; weight must be nonnegative.", weight);
    uninitialize();
    weights[condition] = weight;
    return *this;
}

Real getAssemblyConditionWeight(AssemblyConditionIndex condition) const {
    SimTK_INDEXCHECK_ALWAYS(condition, conditions.size(),
        "Assembler::getAssemblyConditionWeight()");
    return weights[condition];
}

AssemblyConditionIndex 
    adoptAssemblyError(AssemblyCondition* p);
AssemblyConditionIndex 
    adoptAssemblyGoal(AssemblyCondition* p, Real weight=1);


Assembler& setInternalState(const State& state) {
    uninitialize();
    getMatterSubsystem().convertToEulerAngles(state, internalState);
    system.realizeModel(internalState);
    return *this;
}
void initialize() const;
void initialize(const State& state)
{   setInternalState(state); initialize(); }

Real assemble();

Real track(Real frameTime = -1);

Real assemble(State& state) {
    setInternalState(state);
    Real achievedCost = assemble(); // throws if it fails
    updateFromInternalState(state);
    return achievedCost;
}


Real calcCurrentGoal() const;
Real calcCurrentErrorNorm() const;


void updateFromInternalState(State& state) const {
    system.realizeModel(state); // allocates q's if they haven't been yet
    if (!getMatterSubsystem().getUseEulerAngles(state)) {
        State tempState;
        getMatterSubsystem().convertToQuaternions(getInternalState(),
                                                  tempState);
        state.updQ() = tempState.getQ();
    } else 
        state.updQ() = getInternalState().getQ();
}

void lockMobilizer(MobilizedBodyIndex mbx)
{   uninitialize(); userLockedMobilizers.insert(mbx); }
void unlockMobilizer(MobilizedBodyIndex mbx) 
{   uninitialize(); userLockedMobilizers.erase(mbx); }

void lockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
{   uninitialize(); userLockedQs[mbx].insert(qx); }

void unlockQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
{   LockedQs::iterator p = userLockedQs.find(mbx);
    if (p == userLockedQs.end()) return;
    QSet& qs = p->second;
    if (qs.erase(qx)) { // returns 0 if nothing erased
        uninitialize();
        if (qs.empty())
            userLockedQs.erase(p); // remove the whole mobilized body
    }
}

void restrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx,
               Real lowerBound, Real upperBound)
{   SimTK_ERRCHK2_ALWAYS(lowerBound <= upperBound, "Assembler::restrictQ()", 
        "The given range [%g,%g] is illegal because the lower bound is"
        " greater than the upper bound.", lowerBound, upperBound);
    if (lowerBound == -Infinity && upperBound == Infinity)
    {   unrestrictQ(mbx,qx); return; }
    uninitialize(); 
    userRestrictedQs[mbx][qx] = Vec2(lowerBound,upperBound); 
}


void unrestrictQ(MobilizedBodyIndex mbx, MobilizerQIndex qx)
{   RestrictedQs::iterator p = userRestrictedQs.find(mbx);
    if (p == userRestrictedQs.end()) return;
    QRanges& qranges = p->second;
    if (qranges.erase(qx)) { // returns 0 if nothing erased
        uninitialize();
        if (qranges.empty())
            userRestrictedQs.erase(p); // remove the whole mobilized body
    }
}
int getNumGoalEvals()  const;
int getNumErrorEvals() const;
int getNumGoalGradientEvals()   const;
int getNumErrorJacobianEvals()   const;
int getNumAssemblySteps() const;
int getNumInitializations() const;
void resetStats() const;

void setForceNumericalGradient(bool yesno)
{   forceNumericalGradient = yesno; }
void setForceNumericalJacobian(bool yesno)
{   forceNumericalJacobian = yesno; }

void setUseRMSErrorNorm(bool yesno)
{   useRMSErrorNorm = yesno; }
bool isUsingRMSErrorNorm() const {return useRMSErrorNorm;}

void uninitialize() const;
bool isInitialized() const {return alreadyInitialized;}

const State& getInternalState() const {return internalState;}

void addReporter(const EventReporter& reporter) {
    reporters.push_back(&reporter);
}

int getNumFreeQs() const 
{   return freeQ2Q.size(); }

QIndex getQIndexOfFreeQ(FreeQIndex freeQIndex) const
{   return freeQ2Q[freeQIndex]; }

FreeQIndex getFreeQIndexOfQ(QIndex qx) const 
{   return q2FreeQ[qx]; }

Vec2 getFreeQBounds(FreeQIndex freeQIndex) const {
    if (!lower.size()) return Vec2(-Infinity, Infinity);
    else return Vec2(lower[freeQIndex], upper[freeQIndex]);
}

const MultibodySystem& getMultibodySystem() const 
{   return system; }
const SimbodyMatterSubsystem& getMatterSubsystem() const
{   return system.getMatterSubsystem(); }
~Assembler();



//------------------------------------------------------------------------------
                           private: // methods
//------------------------------------------------------------------------------
void setInternalStateFromFreeQs(const Vector& freeQs) {
    assert(freeQs.size() == getNumFreeQs());
    Vector& q = internalState.updQ();
    for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx)
        q[getQIndexOfFreeQ(fx)] = freeQs[fx];
    system.realize(internalState, Stage::Position);
}

Vector getFreeQsFromInternalState() const {
    Vector freeQs(getNumFreeQs());
    const Vector& q = internalState.getQ();
    for (FreeQIndex fx(0); fx < getNumFreeQs(); ++fx)
        freeQs[fx] = q[getQIndexOfFreeQ(fx)];
    return freeQs;
}

void reinitializeWithExtraQsLocked
    (const Array_<QIndex>& toBeLocked) const;



//------------------------------------------------------------------------------
                           private: // data members 
//------------------------------------------------------------------------------
const MultibodySystem&          system;
Array_<const EventReporter*>    reporters; // just references; don't delete

// These members affect the behavior of the assembly algorithm.
static const int OODefaultAccuracy = 1000; // 1/accuracy if acc==0
Real    accuracy;               // 0 means use 1/OODefaultAccuracy
Real    tolerance;              // 0 means use accuracy/10
bool    forceNumericalGradient; // ignore analytic gradient methods
bool    forceNumericalJacobian; // ignore analytic Jacobian methods
bool    useRMSErrorNorm;        // what norm defines success?

// Changes to any of these data members set isInitialized()=false.
State                           internalState;

// These are the mobilizers that were set in lockMobilizer(). They are
// separate from those involved in individually-locked q's.
LockedMobilizers                userLockedMobilizers;
// These are locks placed on individual q's; they are independent of the
// locked mobilizer settings.
LockedQs                        userLockedQs;
// These are range restrictions placed on individual q's.
RestrictedQs                    userRestrictedQs;

// These are (condition,weight) pairs with weight==Infinity meaning
// constraint; weight==0 meaning currently ignored; and any other
// positive weight meaning a goal.
Array_<AssemblyCondition*,AssemblyConditionIndex> 
                                        conditions;
Array_<Real,AssemblyConditionIndex>     weights;

// We always have an assembly condition for the Constraints which are
// enabled in the System; this is the index which can be used to 
// retrieve that condition. The default weight is Infinity.
AssemblyConditionIndex                  systemConstraints;


// These are filled in when the Assembler is initialized.
mutable bool                            alreadyInitialized;

// These are extra q's we removed for numerical reasons.
mutable Array_<QIndex>                  extraQsLocked;

// These represent restrictions on the independent variables (q's).
mutable std::set<QIndex>                lockedQs;
mutable Array_<FreeQIndex,QIndex>       q2FreeQ;    // nq of these
mutable Array_<QIndex,FreeQIndex>       freeQ2Q;    // nfreeQ of these
// 0 length if no bounds; otherwise, index by FreeQIndex.
mutable Vector                          lower, upper;

// These represent the active assembly conditions.
mutable Array_<AssemblyConditionIndex>  errors;
mutable Array_<int>                     nTermsPerError;
mutable Array_<AssemblyConditionIndex>  goals;

class AssemblerSystem; // local class
mutable AssemblerSystem* asmSys;
mutable Optimizer*       optimizer;

mutable int nAssemblySteps;   // count assemble() and track() calls
mutable int nInitializations; // # times we had to reinitialize

friend class AssemblerSystem;
};



//------------------------------------------------------------------------------
//                            ASSEMBLY CONDITION
//------------------------------------------------------------------------------
class SimTK_SIMBODY_EXPORT AssemblyCondition {
public:

explicit AssemblyCondition(const String& name) 
:   name(name), assembler(0) {}

virtual ~AssemblyCondition() {}

virtual int initializeCondition() const {return 0;}

virtual void uninitializeCondition() const {}

virtual int calcErrors(const State& state, Vector& err) const
{   return -1; }

virtual int calcErrorJacobian(const State& state, Matrix& jacobian) const
{   return -1; }

virtual int getNumErrors(const State& state) const 
{   Vector err;
    const int status = calcErrors(state, err);
    if (status == 0)
        return err.size();
    SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::getNumErrors()",
        "The default implementation of getNumErrors() depends on"
        " calcErrors() but that method was not implemented for assembly"
        " condition '%s'.", name.c_str());
    SimTK_ERRCHK2_ALWAYS(status == 0,  "AssemblyCondition::getNumErrors()",
        "The default implementation of getNumErrors() uses calcErrors()"
        " which returned status %d (assembly condition '%s').", 
        status, name.c_str());
    return -1; // NOTREACHED
}

virtual int calcGoal(const State& state, Real& goal) const
{   static Vector err;
    const int status = calcErrors(state, err);
    if (status == 0)
    {   goal = err.normSqr() / std::max(1,err.size());
        return 0; }
    SimTK_ERRCHK1_ALWAYS(status != -1, "AssemblyCondition::calcGoal()",
        "The default implementation of calcGoal() depends on calcErrors()"
        " but that method was not implemented for assembly condition '%s'.",
        name.c_str());
    SimTK_ERRCHK2_ALWAYS(status == 0,  "AssemblyCondition::calcGoal()",
        "The default implementation of calcGoal() uses calcErrors() which"
        " returned status %d (assembly condition '%s').", 
        status, name.c_str());
    return -1; // NOTREACHED
}

virtual int calcGoalGradient(const State& state, Vector& gradient) const
{   return -1; }

const char* getName() const {return name.c_str();}

bool isInAssembler() const {return assembler != 0;}
const Assembler& getAssembler() const 
{   assert(assembler); return *assembler;}
AssemblyConditionIndex getAssemblyConditionIndex() const 
{   return myAssemblyConditionIndex; }

//------------------------------------------------------------------------------
                                 protected:
//------------------------------------------------------------------------------
// These are useful when writing concrete AssemblyConditions.

int getNumFreeQs() const {return getAssembler().getNumFreeQs();}
QIndex getQIndexOfFreeQ(Assembler::FreeQIndex fx) const
{   return getAssembler().getQIndexOfFreeQ(fx); }
Assembler::FreeQIndex getFreeQIndexOfQ(QIndex qx) const
{   return getAssembler().getFreeQIndexOfQ(qx); }
const MultibodySystem& getMultibodySystem() const
{   return getAssembler().getMultibodySystem(); }
const SimbodyMatterSubsystem& getMatterSubsystem() const
{   return getMultibodySystem().getMatterSubsystem(); }

void initializeAssembler() const {
    // The Assembler will in turn invoke initializeCondition().
    if (isInAssembler()) getAssembler().initialize();
    else                 initializeCondition();
}

void uninitializeAssembler() const {
    // The Assembler will in turn invoke uninitializeCondition().
    if (isInAssembler()) getAssembler().uninitialize();
    else                 uninitializeCondition();
}

//------------------------------------------------------------------------------
                                   private:
//------------------------------------------------------------------------------
// This method is used by the Assembler when the AssemblyCondition object 
// is adopted.
friend class Assembler;
void setAssembler(const Assembler& assembler, AssemblyConditionIndex acx) {
    assert(!this->assembler);
    this->assembler = &assembler;
    this->myAssemblyConditionIndex = acx;
}

String                  name; // assembly condition name
const Assembler*        assembler;
AssemblyConditionIndex  myAssemblyConditionIndex;
};



//------------------------------------------------------------------------------
//                                 Q VALUE
//------------------------------------------------------------------------------
class QValue : public AssemblyCondition {
public:
    QValue(MobilizedBodyIndex mbx, MobilizerQIndex qx,
           Real value)
    :   AssemblyCondition("QValue"), 
        mobodIndex(mbx), qIndex(qx), value(value) {}

    Real getValue() const {return value;}
    void setValue(Real newValue) {value=newValue;}

    // For constraint:
    int getNumEquations(const State&) const {return 1;}
    int calcErrors(const State& state, Vector& error) const {
        const SimbodyMatterSubsystem& matter = getMatterSubsystem();
        const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
        error.resize(1);
        error[0] = mobod.getOneQ(state, qIndex) - value;
        return 0;
    }
    // Error jacobian is a zero-row except for a 1 in this q's entry (if
    // this q is free).
    int calcErrorJacobian(const State& state, Matrix& J) const {
        const SimbodyMatterSubsystem& matter = getMatterSubsystem();
        const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
        J.resize(1, getNumFreeQs());
        J = 0; // will have at most one non-zero

        // Find the FreeQIndex corresponding to this q.
        const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex);
        const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx);

        // If this q isn't free then there is no way to affect the error
        // so the Jacobian stays all-zero.
        if (thisFreeIx.isValid())
            J(0,thisFreeIx) = 1;

        return 0;
    }

    // For goal: goal = (q-value)^2 / 2 (the /2 is for gradient beauty)
    int calcGoal(const State& state, Real& goal) const {
        const SimbodyMatterSubsystem& matter = getMatterSubsystem();
        const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
        goal = square(mobod.getOneQ(state, qIndex) - value) / 2;
        return 0;
    }
    // Return a gradient with only this q's entry non-zero (if
    // this q is free).
    int calcGoalGradient(const State& state, Vector& grad) const {
        const SimbodyMatterSubsystem& matter = getMatterSubsystem();
        const MobilizedBody& mobod = matter.getMobilizedBody(mobodIndex);
        grad.resize(getNumFreeQs());
        grad = 0; // will have at most one non-zero

        // Find the FreeQIndex corresponding to this q.
        const QIndex thisIx = QIndex(mobod.getFirstQIndex(state)+qIndex);
        const Assembler::FreeQIndex thisFreeIx = getFreeQIndexOfQ(thisIx);

        // If this q isn't free then there is no way to affect the goal
        // so the gradient stays all-zero.
        if (thisFreeIx.isValid())
            grad[thisFreeIx] = mobod.getOneQ(state, qIndex) - value;

        return 0;
    }

private:
    MobilizedBodyIndex mobodIndex;
    MobilizerQIndex    qIndex;
    Real               value;
};



//------------------------------------------------------------------------------
//                                  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 calcErrors(const State& state, Vector& err) const;
int calcErrorJacobian(const State& state, Matrix& jacobian) const;
int getNumErrors(const State& state) const;
int calcGoal(const State& state, Real& goal) const;
int calcGoalGradient(const State& state, Vector& grad) const;
int initializeCondition() const;
void uninitializeCondition() const;
//------------------------------------------------------------------------------
                                    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_ASSEMBLER_H_