void PerSubsystemInfo::
popAllocationStackBackToStage(Array_<T>& stack, const Stage& g) {
unsigned newSize = stack.size();
while (newSize > 0 && stack[newSize-1].getAllocationStage() > g)
stack[--newSize].deepDestruct(*m_stateImpl);
stack.resize(newSize);
}
//------------------------------------------------------------------------------
// REACHING AND GRAVITY COMPENSATION :: MAP MODEL TO REAL ROBOT
//------------------------------------------------------------------------------
// Fill in the q- and u-maps so we can correctly apply sampled joint angles
// to the model's equivalents. Assumes both model and real robot have been
// initialized so we can determine how many coordinates there are in each.
void ReachingAndGravityCompensation::
mapModelToRealRobot(const State& realState) {
const URDFJoints& modelJoints = m_modelRobot.getURDFRobot().joints;
const URDFJoints& realJoints = m_realRobot.getURDFRobot().joints;
m_model2realU.resize(m_modelState.getNU());
m_model2realQ.resize(m_modelState.getNQ());
for (int mj=0; mj < (int)modelJoints.size(); ++mj) {
const URDFJointInfo& modelInfo = modelJoints.getJoint(mj);
const URDFJointInfo& realInfo = realJoints.getJoint(modelInfo.name);
const MobilizedBody& modelMobod = modelInfo.mobod;
const MobilizedBody& realMobod = realInfo.mobod;
const int mnu = modelMobod.getNumU(m_modelState),
mnq = modelMobod.getNumQ(m_modelState),
mu0 = modelMobod.getFirstUIndex(m_modelState),
mq0 = modelMobod.getFirstQIndex(m_modelState);
if (mnu==0)
continue; // this is fixed in the model; might not be in real robot
const int rnu = realMobod.getNumU(realState),
rnq = realMobod.getNumQ(realState),
ru0 = realMobod.getFirstUIndex(realState),
rq0 = realMobod.getFirstQIndex(realState);
SimTK_ASSERT1_ALWAYS(mnu==rnu && mnq==rnq,
"ReachingAndGravityCompensation::mapModelToRealRobot(): "
"joint '%s' dof mismatch.", modelInfo.name.c_str());
for (int mu=0; mu < mnu; ++mu)
m_model2realU[mu0+mu] = ru0+mu;
for (int mq=0; mq < mnq; ++mq)
m_model2realQ[mq0+mq] = rq0+mq;
}
std::cout<<"m2rU="<<m_model2realU<<std::endl;
std::cout<<"m2rQ="<<m_model2realQ<<std::endl;
}
/** Same as above but for a given time series */
void InverseDynamicsSolver::solve(State &s, const FunctionSet &Qs, const Array_<double> ×, Array_<Vector> &genForceTrajectory)
{
int nq = getModel().getNumCoordinates();
int nt = times.size();
//Preallocate if not done already
genForceTrajectory.resize(nt, Vector(nq));
AnalysisSet& analysisSet = const_cast<AnalysisSet&>(getModel().getAnalysisSet());
//fill in results for each time
for(int i=0; i<nt; i++){
genForceTrajectory[i] = solve(s, Qs, times[i]);
analysisSet.step(s, i);
}
}
void PerSubsystemInfo::resizeAllocationStack(Array_<T>& stack, int newSize) {
assert(newSize >= 0);
for (int i = stack.size()-1; i >= newSize; --i)
stack[i].deepDestruct(*m_stateImpl);
stack.resize(newSize);
}
