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); }