Skip to content

Commit

Permalink
Provide access to the list of prescribed Q and U variables at the Sys…
Browse files Browse the repository at this point in the history 
…tem level for integrators or other solvers that may need to know.

(cherry picked from commit 37edc20)
  • Loading branch information
@sherm1
sherm1 committed Sep 26, 2013
1 parent 806a0a6 commit 296a5c1
Show file tree
Hide file tree
Showing 4 changed files with 111 additions and 36 deletions.
14 changes: 14 additions & 0 deletions SimTKcommon/Simulation/include/SimTKcommon/internal/System.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -527,6 +527,20 @@ remaining udots, lambdas, taus, and all the zdots.
@return \c true if any change was made to \a state.
@see prescribe(), prescribeQ() **/
bool prescribeU(State& state) const;

/** Return the indices of the q's in the given \a state that are free; that is,
they are not affected by prescribeQ(). The indices are copied into the return
argument \a freeQs in ascending order; missing ones are prescribed. \a freeQs is
resized if necessary to the number of free q's, nfq. \a state must be realized
through Stage::Instance. **/
void getFreeQIndex(const State& state, Array_<SystemQIndex>& freeQs) const;

/** Return the indices of the u's in the given \a state that are free; that is,
they are not affected by prescribeU(). The indices are copied into the return
argument \a freeUs in ascending order; missing ones are prescribed. \a freeUs is
resized if necessary to the number of free u's, nfu. \a state must be realized
through Stage::Instance. **/
void getFreeUIndex(const State& state, Array_<SystemUIndex>& freeUs) const;
/**@}**/


Expand Down
20 changes: 20 additions & 0 deletions SimTKcommon/Simulation/include/SimTKcommon/internal/SystemGuts.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -150,6 +150,8 @@ class SimTK_SimTKCOMMON_EXPORT System::Guts {

bool prescribeQ(State&) const;
bool prescribeU(State&) const;
void getFreeQIndex(const State&, Array_<SystemQIndex>& freeQs) const;
void getFreeUIndex(const State&, Array_<SystemUIndex>& freeUs) const;

void projectQ(State&, Vector& qErrEst,
const ProjectOptions& options, ProjectResults& results) const;
Expand Down Expand Up @@ -208,6 +210,7 @@ class SimTK_SimTKCOMMON_EXPORT System::Guts {
virtual bool prescribeQImpl(State&) const {return false;}
virtual bool prescribeUImpl(State&) const {return false;}


// Defaults assume no constraints and return success meaning "all
// constraints satisfied".
virtual void projectQImpl(State& state, Vector& qErrEst,
Expand All @@ -234,6 +237,23 @@ class SimTK_SimTKCOMMON_EXPORT System::Guts {
(const State& state, Real& tNextEvent, Array_<EventId>& eventIds,
bool includeCurrentTime) const;


// Default is that all the state variables are free.
virtual void getFreeQIndexImpl
(const State& s, Array_<SystemQIndex>& freeQs) const {
const unsigned nq = (unsigned)s.getNQ();
freeQs.resize(nq);
for (unsigned i=0; i<nq; ++i)
freeQs[i] = SystemQIndex(i);
}
virtual void getFreeUIndexImpl
(const State& s, Array_<SystemUIndex>& freeUs) const {
const unsigned nu = (unsigned)s.getNU();
freeUs.resize(nu);
for (unsigned i=0; i<nu; ++i)
freeUs[i] = SystemUIndex(i);
}

private:
Guts& operator=(const Guts&); // suppress default copy assignment operator

Expand Down
22 changes: 21 additions & 1 deletion SimTKcommon/Simulation/src/System.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -186,6 +186,10 @@ bool System::prescribeQ(State& s) const
{ return getSystemGuts().prescribeQ(s); }
bool System::prescribeU(State& s) const
{ return getSystemGuts().prescribeU(s); }
void System::getFreeQIndex(const State& s, Array_<SystemQIndex>& freeQs) const
{ return getSystemGuts().getFreeQIndex(s,freeQs); }
void System::getFreeUIndex(const State& s, Array_<SystemUIndex>& freeUs) const
{ return getSystemGuts().getFreeUIndex(s,freeUs); }

void System::project(State& state, Real accuracy) const {
const ProjectOptions projOptions(accuracy);
Expand Down Expand Up @@ -598,7 +602,6 @@ bool System::Guts::prescribeQ(State& s) const {
}



//------------------------------------------------------------------------------
// PRESCRIBE U
//------------------------------------------------------------------------------
Expand All @@ -610,6 +613,23 @@ bool System::Guts::prescribeU(State& s) const {
}


//------------------------------------------------------------------------------
// GET FREE Q(U) INDEX
//------------------------------------------------------------------------------
void System::Guts::
getFreeQIndex(const State& s, Array_<SystemQIndex>& freeQs) const {
SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Instance,
"System::Guts::getFreeQIndex()");
return getFreeQIndexImpl(s, freeQs);
}
void System::Guts::
getFreeUIndex(const State& s, Array_<SystemUIndex>& freeUs) const {
SimTK_STAGECHECK_GE_ALWAYS(s.getSystemStage(), Stage::Instance,
"System::Guts::getFreeUIndex()");
return getFreeUIndexImpl(s, freeUs);
}



//------------------------------------------------------------------------------
// PROJECT Q
Expand Down
91 changes: 56 additions & 35 deletions Simbody/src/MultibodySystemRep.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -408,64 +408,85 @@ class MultibodySystemRep : public System::Guts {
}

// pure virtual
MultibodySystemRep* cloneImpl() const {return new MultibodySystemRep(*this);}
MultibodySystemRep* cloneImpl() const OVERRIDE_11
{ return new MultibodySystemRep(*this); }

// Override the SystemRep default implementations for these virtual methods.
int realizeTopologyImpl (State&) const;
int realizeModelImpl (State&) const;
int realizeInstanceImpl (const State&) const;
int realizeTimeImpl (const State&) const;
int realizePositionImpl (const State&) const;
int realizeVelocityImpl (const State&) const;
int realizeDynamicsImpl (const State&) const;
int realizeAccelerationImpl(const State&) const;
int realizeReportImpl (const State&) const;
int realizeTopologyImpl (State&) const OVERRIDE_11;
int realizeModelImpl (State&) const OVERRIDE_11;
int realizeInstanceImpl (const State&) const OVERRIDE_11;
int realizeTimeImpl (const State&) const OVERRIDE_11;
int realizePositionImpl (const State&) const OVERRIDE_11;
int realizeVelocityImpl (const State&) const OVERRIDE_11;
int realizeDynamicsImpl (const State&) const OVERRIDE_11;
int realizeAccelerationImpl(const State&) const OVERRIDE_11;
int realizeReportImpl (const State&) const OVERRIDE_11;


void multiplyByNImpl(const State& s, const Vector& u,
Vector& dq) const OVERRIDE_11 {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().multiplyByN(s,false,u,dq);
}
void multiplyByNTransposeImpl(const State& s, const Vector& fq,
Vector& fu) const OVERRIDE_11 {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().multiplyByN(s,true,fq,fu);
}
void multiplyByNPInvImpl(const State& s, const Vector& dq,
Vector& u) const OVERRIDE_11 {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().multiplyByNInv(s,false,dq,u);
}
void multiplyByNPInvTransposeImpl(const State& s, const Vector& fu,
Vector& fq) const OVERRIDE_11 {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().multiplyByNInv(s,true,fu,fq);
}

// Currently prescribe() and project() affect only the Matter subsystem.
bool prescribeQImpl(State& state) const {
bool prescribeQImpl(State& state) const OVERRIDE_11 {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
return mech.getRep().prescribeQ(state);
}
bool prescribeUImpl(State& state) const {
bool prescribeUImpl(State& state) const OVERRIDE_11 {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
return mech.getRep().prescribeU(state);
}

void projectQImpl(State& state, Vector& qErrEst,
const ProjectOptions& options, ProjectResults& results) const
{
const ProjectOptions& options,
ProjectResults& results) const OVERRIDE_11 {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().projectQ(state, qErrEst, options, results);
realize(state, Stage::Position); // realize the whole system now
}
void projectUImpl(State& state, Vector& uErrEst,
const ProjectOptions& options, ProjectResults& results) const
{
const ProjectOptions& options,
ProjectResults& results) const OVERRIDE_11 {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().projectU(state, uErrEst, options, results);
realize(state, Stage::Velocity); // realize the whole system now
}

void multiplyByNImpl(const State& s, const Vector& u, Vector& dq) const {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().multiplyByN(s,false,u,dq);
}
void multiplyByNTransposeImpl(const State& s, const Vector& fq,
Vector& fu) const {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().multiplyByN(s,true,fq,fu);
}
void multiplyByNPInvImpl(const State& s, const Vector& dq,
Vector& u) const {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().multiplyByNInv(s,false,dq,u);
void getFreeQIndexImpl
(const State& s, Array_<SystemQIndex>& freeQs) const OVERRIDE_11 {
const SimbodyMatterSubsystem& matter = getMatterSubsystem();
const SystemQIndex qStart = matter.getQStart(s);
const Array_<QIndex>& matterFreeQs = matter.getFreeQIndex(s);
freeQs.resize(matterFreeQs.size());
for (unsigned i=0; i < matterFreeQs.size(); ++i)
freeQs[i] = SystemQIndex(qStart + matterFreeQs[i]);
}
void getFreeUIndexImpl
(const State& s, Array_<SystemUIndex>& freeUs) const OVERRIDE_11 {
const SimbodyMatterSubsystem& matter = getMatterSubsystem();
const SystemUIndex uStart = matter.getUStart(s);
const Array_<UIndex>& matterFreeUs = matter.getFreeUIndex(s);
freeUs.resize(matterFreeUs.size());
for (unsigned i=0; i < matterFreeUs.size(); ++i)
freeUs[i] = SystemUIndex(uStart + matterFreeUs[i]);
}
void multiplyByNPInvTransposeImpl(const State& s, const Vector& fu,
Vector& fq) const {
const SimbodyMatterSubsystem& mech = getMatterSubsystem();
mech.getRep().multiplyByNInv(s,true,fu,fq);
}

/* TODO: not yet
virtual void handleEventsImpl
(State&, EventCause, const Array<EventId>& eventIds,
Expand Down

0 comments on commit 296a5c1

Please sign in to comment.