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
//------------------------------------------------------------------------------
// Note that the internalState is realized to Stage::Position on return.
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;
};

} // namespace SimTK

#endif // SimTK_SIMBODY_ASSEMBLER_H_