From 73619fb81bead9214f5103919da5fa2a43ceb663 Mon Sep 17 00:00:00 2001 From: Keenon Werling Date: Mon, 2 Dec 2024 14:01:03 -0800 Subject: [PATCH] Fixing build errors after merge --- dart/neural/WithRespectToDamping.cpp | 20 +- dart/neural/WithRespectToDamping.hpp | 10 +- dart/neural/WithRespectToSpring.cpp | 20 +- dart/neural/WithRespectToSpring.hpp | 10 +- dart/realtime/SSID.cpp | 16 +- dart/realtime/iLQRLocal.cpp | 445 ++++++++++-------- dart/realtime/saved_data/plotter.py | 45 ++ dart/realtime/saved_data/plotter2d.py | 40 ++ unittests/comprehensive/CMakeLists.txt | 4 + unittests/comprehensive/test_iLQRCartpole.cpp | 405 ++++++++++++++++ unittests/comprehensive/test_iLQRRealtime.cpp | 295 ++++++------ 11 files changed, 951 insertions(+), 359 deletions(-) create mode 100644 dart/realtime/saved_data/plotter.py create mode 100644 dart/realtime/saved_data/plotter2d.py create mode 100644 unittests/comprehensive/test_iLQRCartpole.cpp diff --git a/dart/neural/WithRespectToDamping.cpp b/dart/neural/WithRespectToDamping.cpp index f675c7538..0be17dd17 100644 --- a/dart/neural/WithRespectToDamping.cpp +++ b/dart/neural/WithRespectToDamping.cpp @@ -15,7 +15,10 @@ WithRespectToDamping::WithRespectToDamping() //============================================================================== WrtDampingJointEntry::WrtDampingJointEntry( - std::string jointName, WrtDampingJointEntryType type, int dofs, Eigen::VectorXi worldDofs) + std::string jointName, + WrtDampingJointEntryType type, + int dofs, + Eigen::VectorXi worldDofs) : jointName(jointName), type(type), mDofs(dofs), mWorldDofs(worldDofs) { } @@ -31,13 +34,11 @@ void WrtDampingJointEntry::set( dynamics::Skeleton* skel, const Eigen::Ref& value) { dynamics::Joint* joint = skel->getJoint(jointName); - for(int i = 0; i < joint->getNumDofs(); i++) + for (int i = 0; i < joint->getNumDofs(); i++) { joint->setDampingCoefficient(i, value(i)); } return; - - } //============================================================================== @@ -45,7 +46,7 @@ void WrtDampingJointEntry::get( dynamics::Skeleton* skel, Eigen::Ref out) { dynamics::Joint* joint = skel->getJoint(jointName); - for(int i = 0; i < joint->getNumDofs(); i++) + for (int i = 0; i < joint->getNumDofs(); i++) { out(i) = joint->getDampingCoefficient(i); } @@ -69,7 +70,8 @@ WrtDampingJointEntry& WithRespectToDamping::registerJoint( { std::string skelName = joint->getSkeleton()->getName(); std::vector& skelEntries = mEntries[skelName]; - skelEntries.emplace_back(joint->getName(), type, joint->getNumDofs(), dofs_index); + skelEntries.emplace_back( + joint->getName(), type, joint->getNumDofs(), dofs_index); WrtDampingJointEntry& entry = skelEntries[skelEntries.size() - 1]; @@ -121,6 +123,12 @@ WrtDampingJointEntry& WithRespectToDamping::getJoint(dynamics::Joint* joint) throw std::runtime_error{"Execution should never reach this point"}; } +//============================================================================== +std::string WithRespectToDamping::name() +{ + return "DAMPING"; +} + //============================================================================== /// This returns this WRT from the world as a vector Eigen::VectorXs WithRespectToDamping::get(simulation::World* world) diff --git a/dart/neural/WithRespectToDamping.hpp b/dart/neural/WithRespectToDamping.hpp index 0b4510dff..b253d35f8 100644 --- a/dart/neural/WithRespectToDamping.hpp +++ b/dart/neural/WithRespectToDamping.hpp @@ -35,7 +35,11 @@ struct WrtDampingJointEntry int mDofs; Eigen::VectorXi mWorldDofs; - WrtDampingJointEntry(std::string jointName, WrtDampingJointEntryType type, int dofs, Eigen::VectorXi worldDofs); + WrtDampingJointEntry( + std::string jointName, + WrtDampingJointEntryType type, + int dofs, + Eigen::VectorXi worldDofs); int dim(); @@ -55,7 +59,7 @@ class WithRespectToDamping : public WithRespectTo /// way in this differentiation /// Need to set the dofs WrtDampingJointEntry& registerJoint( - dynamics::Joint* joint, + dynamics::Joint* joint, WrtDampingJointEntryType type, Eigen::VectorXi dofs_index, Eigen::VectorXs upperBound, @@ -69,6 +73,8 @@ class WithRespectToDamping : public WithRespectTo // Implement all the methods we need ////////////////////////////////////////////////////////////// + std::string name() override; + /// This returns this WRT from the world as a vector Eigen::VectorXs get(simulation::World* world) override; diff --git a/dart/neural/WithRespectToSpring.cpp b/dart/neural/WithRespectToSpring.cpp index fb155f6e4..c0a19b42e 100644 --- a/dart/neural/WithRespectToSpring.cpp +++ b/dart/neural/WithRespectToSpring.cpp @@ -15,7 +15,10 @@ WithRespectToSpring::WithRespectToSpring() //============================================================================== WrtSpringJointEntry::WrtSpringJointEntry( - std::string jointName, WrtSpringJointEntryType type, int dofs, Eigen::VectorXi worldDofs) + std::string jointName, + WrtSpringJointEntryType type, + int dofs, + Eigen::VectorXi worldDofs) : jointName(jointName), type(type), mDofs(dofs), mWorldDofs(worldDofs) { } @@ -31,13 +34,11 @@ void WrtSpringJointEntry::set( dynamics::Skeleton* skel, const Eigen::Ref& value) { dynamics::Joint* joint = skel->getJoint(jointName); - for(int i = 0; i < joint->getNumDofs(); i++) + for (int i = 0; i < joint->getNumDofs(); i++) { joint->setSpringStiffness(i, value(i)); } return; - - } //============================================================================== @@ -45,7 +46,7 @@ void WrtSpringJointEntry::get( dynamics::Skeleton* skel, Eigen::Ref out) { dynamics::Joint* joint = skel->getJoint(jointName); - for(int i = 0; i < joint->getNumDofs(); i++) + for (int i = 0; i < joint->getNumDofs(); i++) { out(i) = joint->getSpringStiffness(i); } @@ -69,7 +70,8 @@ WrtSpringJointEntry& WithRespectToSpring::registerJoint( { std::string skelName = joint->getSkeleton()->getName(); std::vector& skelEntries = mEntries[skelName]; - skelEntries.emplace_back(joint->getName(), type, joint->getNumDofs(), dofs_index); + skelEntries.emplace_back( + joint->getName(), type, joint->getNumDofs(), dofs_index); WrtSpringJointEntry& entry = skelEntries[skelEntries.size() - 1]; @@ -121,6 +123,12 @@ WrtSpringJointEntry& WithRespectToSpring::getJoint(dynamics::Joint* joint) throw std::runtime_error{"Execution should never reach this point"}; } +//============================================================================== +std::string WithRespectToSpring::name() +{ + return "SPRING"; +} + //============================================================================== /// This returns this WRT from the world as a vector Eigen::VectorXs WithRespectToSpring::get(simulation::World* world) diff --git a/dart/neural/WithRespectToSpring.hpp b/dart/neural/WithRespectToSpring.hpp index af4cc5532..4627932d1 100644 --- a/dart/neural/WithRespectToSpring.hpp +++ b/dart/neural/WithRespectToSpring.hpp @@ -35,7 +35,11 @@ struct WrtSpringJointEntry int mDofs; Eigen::VectorXi mWorldDofs; - WrtSpringJointEntry(std::string jointName, WrtSpringJointEntryType type, int dofs, Eigen::VectorXi worldDofs); + WrtSpringJointEntry( + std::string jointName, + WrtSpringJointEntryType type, + int dofs, + Eigen::VectorXi worldDofs); int dim(); @@ -55,7 +59,7 @@ class WithRespectToSpring : public WithRespectTo /// way in this differentiation /// Need to set the dofs WrtSpringJointEntry& registerJoint( - dynamics::Joint* joint, + dynamics::Joint* joint, WrtSpringJointEntryType type, Eigen::VectorXi dofs_index, Eigen::VectorXs upperBound, @@ -69,6 +73,8 @@ class WithRespectToSpring : public WithRespectTo // Implement all the methods we need ////////////////////////////////////////////////////////////// + std::string name() override; + /// This returns this WRT from the world as a vector Eigen::VectorXs get(simulation::World* world) override; diff --git a/dart/realtime/SSID.cpp b/dart/realtime/SSID.cpp index 53baddb15..9e3c7e0bc 100644 --- a/dart/realtime/SSID.cpp +++ b/dart/realtime/SSID.cpp @@ -214,8 +214,6 @@ void SSID::runInference(long startTime) registerLock(); // Eigen::MatrixXs forceHistory = mControlLog.getValues( // startTime - mPlanningHistoryMillis, steps, millisPerStep); - Eigen::MatrixXs forceHistory - = mControlLog.getRecentValuesBefore(startTime, steps + 1); Eigen::MatrixXs forceHistory = mControlLog.getRecentValuesBefore(startTime, steps + 1); for (int i = 0; i < steps; i++) @@ -230,10 +228,6 @@ void SSID::runInference(long startTime) // Eigen::MatrixXs velHistory = mSensorLogs[1].getValues( // startTime - mPlanningHistoryMillis, steps, millisPerStep // ); - Eigen::MatrixXs poseHistory - = mSensorLogs[0].getRecentValuesBefore(startTime, steps + 1); - Eigen::MatrixXs velHistory - = mSensorLogs[1].getRecentValuesBefore(startTime, steps + 1); Eigen::MatrixXs poseHistory = mSensorLogs[0].getRecentValuesBefore(startTime, steps + 1); Eigen::MatrixXs velHistory @@ -278,12 +272,10 @@ void SSID::runInference(long startTime) } } -Eigen::VectorXs SSID::runPlotting( +// Run plotting function should be idenpotent +// Here it only support 1 body node plotting, but that make sense +std::pair SSID::runPlotting( long startTime, s_t upper, s_t lower, int samples) - // Run plotting function should be idenpotent - // Here it only support 1 body node plotting, but that make sense - std::pair SSID::runPlotting( - long startTime, s_t upper, s_t lower, int samples) { int millisPerStep = static_cast(ceil(mScale * mWorld->getTimeStep() * 1000.0)); @@ -341,8 +333,6 @@ Eigen::VectorXs SSID::runPlotting( losses(0) = loss; } - return losses; - std::pair result; result.first = losses; result.second = poseHistory; diff --git a/dart/realtime/iLQRLocal.cpp b/dart/realtime/iLQRLocal.cpp index 602c19a0e..2eceb574b 100644 --- a/dart/realtime/iLQRLocal.cpp +++ b/dart/realtime/iLQRLocal.cpp @@ -5,6 +5,8 @@ #include #include +#include "dart/neural/BackpropSnapshot.hpp" +#include "dart/neural/NeuralUtils.hpp" #include "dart/performance/PerformanceLog.hpp" #include "dart/proto/SerializeEigen.hpp" #include "dart/realtime/Millis.hpp" @@ -14,8 +16,6 @@ #include "dart/trajectory/LossFn.hpp" #include "dart/trajectory/MultiShot.hpp" #include "dart/trajectory/Solution.hpp" -#include "dart/neural/NeuralUtils.hpp" -#include "dart/neural/BackpropSnapshot.hpp" #include "signal.h" @@ -25,21 +25,20 @@ using namespace trajectory; namespace realtime { -// For initialization the steps size need to be created to the maximum possible steps +// For initialization the steps size need to be created to the maximum possible +// steps LQRBuffer::LQRBuffer( - int steps, - size_t nDofs, - size_t nControls, - Extrapolate_Method extrapolate) + int steps, size_t nDofs, size_t nControls, Extrapolate_Method extrapolate) { - std::cout << "LQRBuffer Initializing ..." < existK; - for(int i = 0; i < mMaxSteps; i++) + for (int i = 0; i < mMaxSteps; i++) { existK.push_back(Eigen::MatrixXs::Zero(control_dim, state_dim)); } @@ -108,7 +107,7 @@ void LQRBuffer::readNewActionPlan(long timestamp, RealTimeControlBuffer buffer) buffer.getPlannedStateStartingAt(timestamp, existx); size_t existSteps = buffer.getRemainSteps(timestamp); size_t i = 0; - for(;i < existSteps; i++) + for (; i < existSteps; i++) { U[i] = existForce.col(i); k[i] = existk.col(i); @@ -116,10 +115,10 @@ void LQRBuffer::readNewActionPlan(long timestamp, RealTimeControlBuffer buffer) X[i] = existx.col(i); } size_t last = i; - switch(ext) + switch (ext) { case ZERO: - for(; i < nsteps; i++) + for (; i < nsteps; i++) { U[i] = Eigen::VectorXs::Zero(control_dim); k[i] = Eigen::VectorXs::Zero(control_dim); @@ -129,9 +128,9 @@ void LQRBuffer::readNewActionPlan(long timestamp, RealTimeControlBuffer buffer) X[nsteps] = Eigen::VectorXs::Zero(state_dim); break; case LAST: - if(last==0) + if (last == 0) { - for(; i < nsteps; i++) + for (; i < nsteps; i++) { U[i] = Eigen::VectorXs::Zero(control_dim); k[i] = Eigen::VectorXs::Zero(control_dim); @@ -143,18 +142,19 @@ void LQRBuffer::readNewActionPlan(long timestamp, RealTimeControlBuffer buffer) } else { - for(; i < nsteps; i++) + for (; i < nsteps; i++) { U[i].segment(0, control_dim) = U[last]; k[i].segment(0, control_dim) = Eigen::VectorXs::Zero(control_dim); - K[i].block(0, 0, control_dim, state_dim) = Eigen::MatrixXs::Zero(control_dim, state_dim); + K[i].block(0, 0, control_dim, state_dim) + = Eigen::MatrixXs::Zero(control_dim, state_dim); X[i].segment(0, state_dim) = X[last]; } X[nsteps].segment(0, state_dim) = X[last]; break; } case RANDOM: - for(; i < nsteps; i++) + for (; i < nsteps; i++) { U[i] = 0.1 * Eigen::VectorXs::Random(control_dim); k[i] = Eigen::VectorXs::Zero(control_dim); @@ -169,9 +169,12 @@ void LQRBuffer::readNewActionPlan(long timestamp, RealTimeControlBuffer buffer) } } -void LQRBuffer::updateL(std::vector Lx_new, std::vector Lu_new, - std::vector Lxx_new, std::vector Luu_new, - std::vector Lux_new) +void LQRBuffer::updateL( + std::vector Lx_new, + std::vector Lu_new, + std::vector Lxx_new, + std::vector Luu_new, + std::vector Lux_new) { Lx = Lx_new; Lu = Lu_new; @@ -185,15 +188,12 @@ void LQRBuffer::setNewActionPlan(long timestamp, RealTimeControlBuffer* buffer) // Need to compute how many step in U has been invalid long current_time = timeSinceEpochMillis(); Eigen::MatrixXs control_force = Eigen::MatrixXs::Zero(control_dim, nsteps); - for(int i = 0; i < nsteps; i++) + for (int i = 0; i < nsteps; i++) { control_force.col(i) = U[i]; } // This assume the vector indicate plan right after timestamp - buffer->setControlForcePlan( - timestamp, - current_time, - control_force); + buffer->setControlForcePlan(timestamp, current_time, control_force); } void LQRBuffer::setNewControlLaw(long timestamp, RealTimeControlBuffer* buffer) @@ -202,17 +202,13 @@ void LQRBuffer::setNewControlLaw(long timestamp, RealTimeControlBuffer* buffer) long current_time = timeSinceEpochMillis(); // This assume the vector indicate plan right after timestamp - // Till this point, we can guarantee that X is the best trajectory rollout from forward - buffer->setControlLawPlan( - timestamp, - current_time, - k, - K, - X, - alpha); + // Till this point, we can guarantee that X is the best trajectory rollout + // from forward + buffer->setControlLawPlan(timestamp, current_time, k, K, X, alpha); } -void LQRBuffer::updateF(std::vector Fx_new, std::vector Fu_new) +void LQRBuffer::updateF( + std::vector Fx_new, std::vector Fu_new) { Fx = Fx_new; Fu = Fu_new; @@ -220,9 +216,9 @@ void LQRBuffer::updateF(std::vector Fx_new, std::vector(); - if(err > 1.0) + err = (X[i + 1] - X[i]).lpNorm(); + if (err > 1.0) { - if(mVerbose) + if (mVerbose) { - std::cout << "State: \n" << X[i+1] << "\n" << X[i] << "\n" << i << std::endl; - } + std::cout << "State: \n" + << X[i + 1] << "\n" + << X[i] << "\n" + << i << std::endl; + } flag = false; } } @@ -280,14 +279,15 @@ iLQRLocal::iLQRLocal( mRecordIterations(false), mPlanningHorizonMillis(planningHorizonMillis), mPlanningCandidateHorizonMillis(planningHorizonMillis), - mMillisPerStep(scale*1000 * world->getTimeStep()), + mMillisPerStep(scale * 1000 * world->getTimeStep()), mSteps((int)ceil((s_t)planningHorizonMillis / mMillisPerStep)), mMaxSteps(mSteps), mShotLength(50), mMaxIterations(5), mMillisInAdvanceToPlan(0), mLastOptimizedTime(0L), - mBuffer(RealTimeControlBuffer(nControls, mSteps, mMillisPerStep, world->getNumDofs() * 2)), + mBuffer(RealTimeControlBuffer( + nControls, mSteps, mMillisPerStep, world->getNumDofs() * 2)), mSilent(false), // Below are iLQR related information mAlpha_reset_value(0.2), @@ -298,14 +298,20 @@ iLQRLocal::iLQRLocal( mDelta0(2.0), mDelta(2.0), mMU_MIN(1e-6), - mMU(100.*1e-6), - mMU_reset_value(100.*1e-6), + mMU(100. * 1e-6), + mMU_reset_value(100. * 1e-6), mCost(std::numeric_limits::max()), - mlqrBuffer(LQRBuffer(mSteps, world->getNumDofs(), nControls, Extrapolate_Method::ZERO)), + mlqrBuffer(LQRBuffer( + mSteps, world->getNumDofs(), nControls, Extrapolate_Method::ZERO)), mLast_U(Eigen::VectorXs::Zero(nControls)), mActionDim(nControls), mStateDim(world->getNumDofs() * 2), - mRollout(createRollout(mSteps, world->getNumDofs(), world->getMassDims(), world->getDampingDims(), world->getSpringDims())) + mRollout(createRollout( + mSteps, + world->getNumDofs(), + world->getMassDims(), + world->getDampingDims(), + world->getSpringDims())) { } @@ -314,7 +320,12 @@ void iLQRLocal::setPlanningHorizon(size_t planningHorizonMillis) { mSteps = (int)ceil((s_t)planningHorizonMillis / mMillisPerStep); mPlanningHorizonMillis = planningHorizonMillis; - mRollout = createRollout(mSteps, mWorld->getNumDofs(), mWorld->getMassDims(), mWorld->getDampingDims(), mWorld->getSpringDims()); + mRollout = createRollout( + mSteps, + mWorld->getNumDofs(), + mWorld->getMassDims(), + mWorld->getDampingDims(), + mWorld->getSpringDims()); mlqrBuffer.setNumSteps(mSteps); } @@ -330,7 +341,8 @@ void iLQRLocal::setCostFn(std::shared_ptr costFn) mLoss = mCostFn->getLossFn(); } -void iLQRLocal::setMappedCostFn(std::shared_ptr costFn) +void iLQRLocal::setMappedCostFn( + std::shared_ptr costFn) { mMappedCostFn = costFn; mLoss = mMappedCostFn->getLossFn(); @@ -349,8 +361,8 @@ std::shared_ptr iLQRLocal::getOptimizer() return mOptimizer; } -/// This sets the problem that iLQRLocal will use. This will override the default -/// problem. This should be called before start(). +/// This sets the problem that iLQRLocal will use. This will override the +/// default problem. This should be called before start(). void iLQRLocal::setProblem(std::shared_ptr problem) { mProblem = problem; @@ -389,11 +401,11 @@ Eigen::VectorXs iLQRLocal::computeForce(Eigen::VectorXs state, long now) s_t alpha = getControlAlpha(now); Eigen::VectorXs stateErr = state - x; Eigen::VectorXs action; - if(stateErr.norm() < 0.1) + if (stateErr.norm() < 0.1) { - action = (u + alpha*k - + K * (state - x) - ).cwiseMin(mActionBound).cwiseMax(-mActionBound); + action = (u + alpha * k + K * (state - x)) + .cwiseMin(mActionBound) + .cwiseMax(-mActionBound); } else { @@ -401,7 +413,8 @@ Eigen::VectorXs iLQRLocal::computeForce(Eigen::VectorXs state, long now) } // Eigen::VectorXs action = u; // std::cout << "Last Time Buffer: " << mBuffer.getLastWriteBufferTime() - // << " Law Buffer: " << mBuffer.getLastWriteBufferLawTime() <<" Action: " << a << std::endl; + // << " Law Buffer: " << mBuffer.getLastWriteBufferLawTime() <<" + // Action: " << a << std::endl; // std::cout << "State Error: \n" << stateErr << std::endl; // std::cout << "State: \n" << state << std::endl; // std::cout << "Hypothetical State: \n" << x << std::endl; @@ -432,7 +445,6 @@ void iLQRLocal::setEnableLineSearch(bool enabled) mEnableLinesearch = enabled; } - void iLQRLocal::disableAdaptiveTime() { mAdaptiveTime = false; @@ -468,8 +480,8 @@ int iLQRLocal::getMaxIterations() } /// This sets the current maximum number of iterations that IPOPT will be -/// allowed to run during an optimization. iLQRLocal reserves the right to change -/// this value during runtime depending on timing and performance values +/// allowed to run during an optimization. iLQRLocal reserves the right to +/// change this value during runtime depending on timing and performance values /// observed during running. void iLQRLocal::setMaxIterations(int maxIters) { @@ -519,7 +531,7 @@ void iLQRLocal::optimizePlan(long startTime) ipoptOptimizer->setCheckDerivatives(false); ipoptOptimizer->setSuppressOutput(true); ipoptOptimizer->setRecoverBest(mEnableOptimizationGuards); - ipoptOptimizer->setTolerance(1e-3); //1e-3 + ipoptOptimizer->setTolerance(1e-3); // 1e-3 ipoptOptimizer->setIterationLimit(mMaxIterations); ipoptOptimizer->setDisableLinesearch(!mEnableLinesearch); ipoptOptimizer->setRecordFullDebugInfo(false); @@ -543,20 +555,18 @@ void iLQRLocal::optimizePlan(long startTime) } PerformanceLog* optimizeTrack = log->startRun("Optimize"); - //std::cout<<"MPC Optimization Start"<optimize(mProblem.get()); - //std::cout<<"MPC Optimization end"<end(); mLastOptimizedTime = startTime; // Here mBuffer should have access to mapping of control force - Eigen::MatrixXs actions = worldClone->mapToActionSpace(mProblem->getRolloutCache(worldClone)->getControlForcesConst()); + Eigen::MatrixXs actions = worldClone->mapToActionSpace( + mProblem->getRolloutCache(worldClone)->getControlForcesConst()); // No need to pad here automatically paded inside - mBuffer.setControlForcePlan( - startTime, - timeSinceEpochMillis(), - actions); + mBuffer.setControlForcePlan(startTime, timeSinceEpochMillis(), actions); log->end(); @@ -589,7 +599,8 @@ void iLQRLocal::optimizePlan(long startTime) mBuffer.estimateWorldStateAt( worldClone, &mObservationLog, roundedStartTime); - // Make sure that in the force buffer, the pointer of starting point is pointed to current + // Make sure that in the force buffer, the pointer of starting point is + // pointed to current mProblem->advanceSteps( worldClone, worldClone->getPositions(), @@ -603,13 +614,12 @@ void iLQRLocal::optimizePlan(long startTime) // std::endl; // Update the control force plan from current time stamp. - // The new optimization problem is started at start time, however since the replanning - // Process take an non trivial amount of time, the force to be set is from now. - Eigen::MatrixXs actions = worldClone->mapToActionSpace(mProblem->getRolloutCache(worldClone)->getControlForcesConst()); - mBuffer.setControlForcePlan( - startTime, - timeSinceEpochMillis(), - actions); + // The new optimization problem is started at start time, however since the + // replanning Process take an non trivial amount of time, the force to be + // set is from now. + Eigen::MatrixXs actions = worldClone->mapToActionSpace( + mProblem->getRolloutCache(worldClone)->getControlForcesConst()); + mBuffer.setControlForcePlan(startTime, timeSinceEpochMillis(), actions); long computeDurationWallTime = timeSinceEpochMillis() - startComputeWallTime; @@ -634,8 +644,9 @@ void iLQRLocal::optimizePlan(long startTime) << " factor of safety), and it took us " << computeDurationWallTime << "ms" << std::endl; } - // Here is when optimization finished, supposingly this time should equals to now - // And the real world can immediately get forced that has been planned + // Here is when optimization finished, supposingly this time should equals + // to now And the real world can immediately get forced that has been + // planned mLastOptimizedTime = roundedStartTime; } } @@ -650,12 +661,18 @@ void iLQRLocal::optimizePlan(long startTime) /// it may cause serious problems accordingly void iLQRLocal::adjustPerformance(long lastOptimizeTimeMillis) { - mMillisInAdvanceToPlan = 1.2 * lastOptimizeTimeMillis; - std::cout << "Estimated Planning time: " << mMillisInAdvanceToPlan << "From: " << lastOptimizeTimeMillis << std::endl; - if (mMillisInAdvanceToPlan > 200) - mMillisInAdvanceToPlan = 200; - if(!mAdaptiveTime) - mMillisInAdvanceToPlan = 20; // Which will be problematic when facing the change of parameters + (void)lastOptimizeTimeMillis; + // mMillisInAdvanceToPlan = 1.2 * lastOptimizeTimeMillis; + // std::cout << "Estimated Planning time: " << mMillisInAdvanceToPlan << + // "From: " << lastOptimizeTimeMillis << std::endl; if (mMillisInAdvanceToPlan + // > 200) + // mMillisInAdvanceToPlan = 200; + // if(!mAdaptiveTime) + // mMillisInAdvanceToPlan = 20; // Which will be problematic when facing the + // change of parameters + + mMillisInAdvanceToPlan + = 0; // Which will be problematic when facing the change of parameters } /// This starts our main thread and begins running optimizations @@ -674,7 +691,8 @@ void iLQRLocal::ilqrstart() return; mRunning = true; // mBuffer.setiLQRFlag(true); - mOptimizationThread = std::thread(&iLQRLocal::iLQRoptimizationThreadLoop, this); + mOptimizationThread + = std::thread(&iLQRLocal::iLQRoptimizationThreadLoop, this); } /// This stops our main thread, waits for it to finish, and then returns @@ -689,7 +707,7 @@ void iLQRLocal::stop() /// This stops our main thread, waits for it to finish, and then returns void iLQRLocal::ilqrstop() { - if(!mRunning) + if (!mRunning) return; mRunning = false; mOptimizationThread.join(); @@ -719,14 +737,13 @@ bool iLQRLocal::ilqrForward(simulation::WorldPtr world) std::vector Luu; std::vector Lux; - - while(mPatience > 0) + while (mPatience > 0) { // Reset Control Force // Let New State and control force equals to old one. // Which is equivalent to deep copy. mlqrBuffer.resetXUNew(); - + // set control force and initial condition world->setPositions(pos); world->setVelocities(vel); @@ -735,7 +752,12 @@ bool iLQRLocal::ilqrForward(simulation::WorldPtr world) mlqrBuffer.Xnew[0].segment(world->getNumDofs(), world->getNumDofs()) = vel; // Rollout Trajectory - TrajectoryRolloutReal rollout = createRollout(mSteps, world->getNumDofs(), world->getMassDims(), world->getDampingDims(), world->getSpringDims()); + TrajectoryRolloutReal rollout = createRollout( + mSteps, + world->getNumDofs(), + world->getMassDims(), + world->getDampingDims(), + world->getSpringDims()); s_t loss = 0; // Executing the trajectory according to control law @@ -743,32 +765,32 @@ bool iLQRLocal::ilqrForward(simulation::WorldPtr world) // Require states be stored in rollout // Assume loss except the final loss are multiplied by dt - if(mCostFn!=nullptr) + if (mCostFn != nullptr) { - Lx = mCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::X); - Lu = mCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::U); - Lxx = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::XX); - Luu = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UU); - Lux = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UX); + Lx = mCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::X); + Lu = mCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::U); + Lxx = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::XX); + Luu = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UU); + Lux = mCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UX); } else { - Lx = mMappedCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::X); - Lu = mMappedCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::U); - Lxx = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::XX); - Luu = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UU); - Lux = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UX); + Lx = mMappedCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::X); + Lu = mMappedCostFn->ilqrGradientEstimator(&rollout, loss, WRTFLAG::U); + Lxx = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::XX); + Luu = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UU); + Lux = mMappedCostFn->ilqrHessianEstimator(&rollout, WRTFLAG::UX); } // Sanity Check - if(!mlqrBuffer.validateXnew()) + if (!mlqrBuffer.validateXnew()) { mAlpha *= 0.5; mPatience--; mNaN_Flag = true; continue; } - else if((mCost - loss) < 0) + else if ((mCost - loss) < 0) { mAlpha *= 0.5; mPatience--; @@ -777,7 +799,7 @@ bool iLQRLocal::ilqrForward(simulation::WorldPtr world) } else // Good to leave, Guarantee the loss is lower than prev_cost { - mCost = loss; //Update cost with current loss + mCost = loss; // Update cost with current loss mPatience = mPatience_reset_value; mAlpha_opt = mAlpha; mAlpha = mAlpha_reset_value; @@ -787,11 +809,11 @@ bool iLQRLocal::ilqrForward(simulation::WorldPtr world) break; } } - if(mPatience == 0) + if (mPatience == 0) { - // std::cout << "Forward Pass Run Out of Patience, exiting ..." << std::endl; - // The out of patience is due to nan - if(mNaN_Flag) + // std::cout << "Forward Pass Run Out of Patience, exiting ..." << + // std::endl; The out of patience is due to nan + if (mNaN_Flag) { std::cout << "nan_detected" << std::endl; return false; @@ -820,42 +842,52 @@ bool iLQRLocal::ilqrBackward() bool done = false; bool early_termination = false; // The loop should exit either done or early terminate - while(!(early_termination || done)) + while (!(early_termination || done)) { Eigen::VectorXs Vx = mlqrBuffer.Lx[mSteps]; - Eigen::MatrixXs Vxx = mlqrBuffer.Lxx[mSteps]; - for(int cursor = mSteps - 1; cursor >= 0; cursor--) + Eigen::MatrixXs Vxx = mlqrBuffer.Lxx[mSteps]; + for (int cursor = mSteps - 1; cursor >= 0; cursor--) { // Build up Q matrices - Eigen::VectorXs Qx = mlqrBuffer.Lx[cursor] + mlqrBuffer.Fx[cursor].transpose() * Vx; - Eigen::VectorXs Qu = mlqrBuffer.Lu[cursor] + mlqrBuffer.Fu[cursor].transpose() * Vx; - Eigen::MatrixXs Qxx = mlqrBuffer.Lxx[cursor] - + mlqrBuffer.Fx[cursor].transpose() * Vxx * mlqrBuffer.Fx[cursor]; - Eigen::MatrixXs Qux = mlqrBuffer.Lux[cursor].transpose() - + mlqrBuffer.Fu[cursor].transpose() * Vxx * mlqrBuffer.Fx[cursor]; - Eigen::MatrixXs Quu = mlqrBuffer.Luu[cursor] - + mlqrBuffer.Fu[cursor].transpose() * Vxx * mlqrBuffer.Fu[cursor]; - Eigen::MatrixXs I = Eigen::MatrixXs::Identity(Vxx.rows(),Vxx.cols()); - Eigen::MatrixXs Quubar = mlqrBuffer.Luu[cursor] - + mlqrBuffer.Fu[cursor].transpose() * (Vxx + mMU * I) * mlqrBuffer.Fu[cursor]; - Eigen::MatrixXs Quxbar = mlqrBuffer.Lux[cursor] - + (mlqrBuffer.Fu[cursor].transpose() * (Vxx + mMU * I) * mlqrBuffer.Fx[cursor]).transpose(); + Eigen::VectorXs Qx + = mlqrBuffer.Lx[cursor] + mlqrBuffer.Fx[cursor].transpose() * Vx; + Eigen::VectorXs Qu + = mlqrBuffer.Lu[cursor] + mlqrBuffer.Fu[cursor].transpose() * Vx; + Eigen::MatrixXs Qxx + = mlqrBuffer.Lxx[cursor] + + mlqrBuffer.Fx[cursor].transpose() * Vxx * mlqrBuffer.Fx[cursor]; + Eigen::MatrixXs Qux + = mlqrBuffer.Lux[cursor].transpose() + + mlqrBuffer.Fu[cursor].transpose() * Vxx * mlqrBuffer.Fx[cursor]; + Eigen::MatrixXs Quu + = mlqrBuffer.Luu[cursor] + + mlqrBuffer.Fu[cursor].transpose() * Vxx * mlqrBuffer.Fu[cursor]; + Eigen::MatrixXs I = Eigen::MatrixXs::Identity(Vxx.rows(), Vxx.cols()); + Eigen::MatrixXs Quubar = mlqrBuffer.Luu[cursor] + + mlqrBuffer.Fu[cursor].transpose() + * (Vxx + mMU * I) * mlqrBuffer.Fu[cursor]; + Eigen::MatrixXs Quxbar = mlqrBuffer.Lux[cursor] + + (mlqrBuffer.Fu[cursor].transpose() + * (Vxx + mMU * I) * mlqrBuffer.Fx[cursor]) + .transpose(); if (abs((Quubar).determinant()) < 1e-20) { - if(mPatience == 0) + if (mPatience == 0) { early_termination = true; - // If the Q matrix kept non-invertible then it should be treated as NaN - std::cout << "Regularize Patience limit met, exiting... " << std::endl; + // If the Q matrix kept non-invertible then it should be treated as + // NaN + std::cout << "Regularize Patience limit met, exiting... " + << std::endl; break; } else { - std::cout << "Warning Singular Quu, iteration: "<< cursor - <<" -repeating backward pass with increased mu. Patience: " - < mDelta0) + if (mDelta * mDelta0 > mDelta0) { mDelta *= mDelta0; } @@ -863,7 +895,7 @@ bool iLQRLocal::ilqrBackward() { mDelta = mDelta0; } - if(mMU * mDelta > mMU_MIN) + if (mMU * mDelta > mMU_MIN) { mMU *= mDelta; } @@ -878,41 +910,43 @@ bool iLQRLocal::ilqrBackward() else { // Compute K matrix - //std::cout << "Reached Correct Backward pass"<< cursor << std::endl; + // std::cout << "Reached Correct Backward pass"<< cursor << std::endl; Eigen::MatrixXs Quubar_inv = Quubar.inverse(); mlqrBuffer.K[cursor] = -Quubar_inv * Quxbar.transpose(); mlqrBuffer.k[cursor] = -Quubar_inv * Qu; // Update Vxx and Vx - Vx = Qx + mlqrBuffer.K[cursor].transpose() * Quu * mlqrBuffer.k[cursor] - + mlqrBuffer.K[cursor].transpose() * Qu - + Qux.transpose() * mlqrBuffer.k[cursor]; - Vxx = Qxx + mlqrBuffer.K[cursor].transpose() * Quu * mlqrBuffer.K[cursor] - + mlqrBuffer.K[cursor].transpose() * Qux - + Qux.transpose() * mlqrBuffer.K[cursor]; - if(cursor == 0) + Vx = Qx + mlqrBuffer.K[cursor].transpose() * Quu * mlqrBuffer.k[cursor] + + mlqrBuffer.K[cursor].transpose() * Qu + + Qux.transpose() * mlqrBuffer.k[cursor]; + Vxx = Qxx + + mlqrBuffer.K[cursor].transpose() * Quu * mlqrBuffer.K[cursor] + + mlqrBuffer.K[cursor].transpose() * Qux + + Qux.transpose() * mlqrBuffer.K[cursor]; + if (cursor == 0) done = true; } } } mPatience = mPatience_reset_value; - // The only reason for early termination is that the backward process is keep getting NaN - if(early_termination) + // The only reason for early termination is that the backward process is keep + // getting NaN + if (early_termination) { mNaN_Flag = true; return false; } // Progress Delta for next iteration // Decrease mu - if(1.0/mDelta0 > mDelta / mDelta0) + if (1.0 / mDelta0 > mDelta / mDelta0) { mDelta /= mDelta0; } else { - mDelta = 1.0/mDelta0; + mDelta = 1.0 / mDelta0; } - if(mMU * mDelta > mMU_MIN) + if (mMU * mDelta > mMU_MIN) { mMU = mMU * mDelta; } @@ -927,30 +961,37 @@ bool iLQRLocal::ilqrBackward() /// This function rollout the trajectory and save all the Jacobians to Buffer /// as well as record the loss /// This function automatically update the trajectory -void iLQRLocal::getTrajectory(simulation::WorldPtr world, - TrajectoryRollout* rollout, - LQRBuffer* lqrBuffer) +void iLQRLocal::getTrajectory( + simulation::WorldPtr world, + TrajectoryRollout* rollout, + LQRBuffer* lqrBuffer) { // Need to store the state and actions in rollout Eigen::MatrixXs actions = Eigen::MatrixXs::Zero(mActionDim, mSteps); - for(int i = 0; i < mSteps; i++) + for (int i = 0; i < mSteps; i++) { // Compute New actions - lqrBuffer->Unew[i] = (lqrBuffer->Unew[i] + mAlpha * lqrBuffer->k[i] - + lqrBuffer->K[i] * (lqrBuffer->Xnew[i] - lqrBuffer->X[i])).cwiseMin(mActionBound).cwiseMax(-mActionBound); + lqrBuffer->Unew[i] + = (lqrBuffer->Unew[i] + mAlpha * lqrBuffer->k[i] + + lqrBuffer->K[i] * (lqrBuffer->Xnew[i] - lqrBuffer->X[i])) + .cwiseMin(mActionBound) + .cwiseMax(-mActionBound); // Record state and action on rollout // TODO: Need to make this robot skeleton rollout->getPoses().col(i) = world->getPositions(); rollout->getVels().col(i) = world->getVelocities(); actions.col(i) = lqrBuffer->Unew[i]; - + // Set the control force just computed from control law world->setAction(actions.col(i)); // Step the world - std::shared_ptr snapshot = neural::forwardPass(world, false); + std::shared_ptr snapshot + = neural::forwardPass(world, false); // set new state - lqrBuffer->Xnew[i+1].segment(0, world->getNumDofs()) = snapshot->getPostStepPosition(); - lqrBuffer->Xnew[i+1].segment(world->getNumDofs(), world->getNumDofs()) = snapshot->getPostStepVelocity(); + lqrBuffer->Xnew[i + 1].segment(0, world->getNumDofs()) + = snapshot->getPostStepPosition(); + lqrBuffer->Xnew[i + 1].segment(world->getNumDofs(), world->getNumDofs()) + = snapshot->getPostStepVelocity(); // set new Jacobian for linearization lqrBuffer->Fx[i] = world->getStateJacobian(); // Need to reassemble the matrix by actuated dofs @@ -970,16 +1011,17 @@ bool iLQRLocal::ilqroptimizePlan(long startTime) startTime = mLastOptimizedTime; } // Get action from mBuffer according to time - if(mPlanningCandidateHorizonMillis!=mPlanningHorizonMillis) + if (mPlanningCandidateHorizonMillis != mPlanningHorizonMillis) { setPlanningHorizon(mPlanningCandidateHorizonMillis); std::cout << "<<<<<<<<<<<<<<<<<<<<<<<" << std::endl; - std::cout << "Horizon Changed! " << mPlanningCandidateHorizonMillis <::max(); mlqrBuffer.readNewActionPlan(startTime, mBuffer); - + // Copy the world std::shared_ptr worldClone = mWorld->clone(); @@ -989,12 +1031,12 @@ bool iLQRLocal::ilqroptimizePlan(long startTime) s_t last_cost = mCost; // This Process takes non trivial amount of time // By the time is finished or failed the real world has taken lots of steps - for(int iter = 0; iter < mMaxIterations; iter++) + for (int iter = 0; iter < mMaxIterations; iter++) { // This function will forward world to the current point mBuffer.estimateWorldStateAt(worldClone, &mObservationLog, startTime); forward_flag = ilqrForward(worldClone); - if(!forward_flag || iter == mMaxIterations - 1) + if (!forward_flag || iter == mMaxIterations - 1) { break; } @@ -1002,32 +1044,32 @@ bool iLQRLocal::ilqroptimizePlan(long startTime) { backward_flag = ilqrBackward(); } - - if(!backward_flag) + + if (!backward_flag) { std::cout << "Backward Terminated, Exiting ..." << std::endl; break; } - if(abs(last_cost - mCost) <= mTolerence) + if (abs(last_cost - mCost) <= mTolerence) { break; } } - // Here inside the forward loop there actually mechanism which guarantee, the cost - // function should always decrease for MPC since the trajectory may be different and starting - // state is different, it may not be the case - // we need to enable mechanism that each optimization it will return the correspoding control laws - // that have lowest cost, and across optimization the lowest cost will be reset - // Hence in this case: - // If NaN detected in forward pass or non-invertible matrix detected in backward pass, the system - // need to be helted since the simulator has already crashed... - // If the forward pass the cost does not decay it should break the iteration whlie not executing the + // Here inside the forward loop there actually mechanism which guarantee, the + // cost function should always decrease for MPC since the trajectory may be + // different and starting state is different, it may not be the case we need + // to enable mechanism that each optimization it will return the correspoding + // control laws that have lowest cost, and across optimization the lowest cost + // will be reset Hence in this case: If NaN detected in forward pass or + // non-invertible matrix detected in backward pass, the system need to be + // helted since the simulator has already crashed... If the forward pass the + // cost does not decay it should break the iteration whlie not executing the // backward pass. - if(!mNaN_Flag) + if (!mNaN_Flag) { // Update the mBuffer according to current time bool flag = mlqrBuffer.detectXContinuity(); - if(!flag && mlqrBuffer.getVerbose()) + if (!flag && mlqrBuffer.getVerbose()) { std::cout << "=====================" << std::endl; std::cout << "X is not Continuous!!" << std::endl; @@ -1042,7 +1084,8 @@ bool iLQRLocal::ilqroptimizePlan(long startTime) mDelta = mDelta0; mAlpha = mAlpha_reset_value; // Invoke callback functions - long computeDurationWallTime = timeSinceEpochMillis() - startComputeWallTime; + long computeDurationWallTime + = timeSinceEpochMillis() - startComputeWallTime; // Here mRollout is the last valid rollout from forward pass for (auto listener : mReplannedListeners) { @@ -1091,12 +1134,17 @@ s_t iLQRLocal::getMU() return mMU; } -TrajectoryRolloutReal iLQRLocal::createRollout(size_t steps, size_t dofs, size_t mass_dim, size_t damping_dim, size_t spring_dim) +TrajectoryRolloutReal iLQRLocal::createRollout( + size_t steps, + size_t dofs, + size_t mass_dim, + size_t damping_dim, + size_t spring_dim) { - Eigen::MatrixXs pos = Eigen::MatrixXs::Zero(dofs, steps+1); + Eigen::MatrixXs pos = Eigen::MatrixXs::Zero(dofs, steps + 1); std::unordered_map pos_map; pos_map.emplace("identity", pos); - Eigen::MatrixXs vel = Eigen::MatrixXs::Zero(dofs, steps+1); + Eigen::MatrixXs vel = Eigen::MatrixXs::Zero(dofs, steps + 1); std::unordered_map vel_map; vel_map.emplace("identity", vel); Eigen::MatrixXs forces = Eigen::MatrixXs::Zero(dofs, steps); @@ -1106,7 +1154,8 @@ TrajectoryRolloutReal iLQRLocal::createRollout(size_t steps, size_t dofs, size_t Eigen::VectorXs damping = Eigen::VectorXs::Zero(damping_dim); Eigen::VectorXs spring = Eigen::VectorXs::Zero(spring_dim); std::unordered_map meta; - TrajectoryRolloutReal rollout = TrajectoryRolloutReal(pos_map, vel_map, force_map, mass, damping, spring, meta); + TrajectoryRolloutReal rollout = TrajectoryRolloutReal( + pos_map, vel_map, force_map, mass, damping, spring, meta); return rollout; } @@ -1125,13 +1174,15 @@ Eigen::VectorXs iLQRLocal::getControlState(long now) return mBuffer.getPlannedState(now); } -void iLQRLocal::setTargetReachingCostFn(std::shared_ptr costFn) +void iLQRLocal::setTargetReachingCostFn( + std::shared_ptr costFn) { mCostFn = costFn; mLoss = costFn->getLossFn(); } -/// Set action hard bound that any action computed by control law will be clipped to this. +/// Set action hard bound that any action computed by control law will be +/// clipped to this. void iLQRLocal::setActionBound(s_t actionBound) { assert(actionBound >= 0); @@ -1148,11 +1199,11 @@ Eigen::MatrixXs iLQRLocal::assembleJacobianMatrix(Eigen::MatrixXs B) { size_t ndim = mActuatedJoint.size(); Eigen::MatrixXs Bnew = Eigen::MatrixXs::Zero(ndim, ndim); - for(size_t i = 0; i < Bnew.cols(); i++) + for (size_t i = 0; i < Bnew.cols(); i++) { - for(size_t j = 0; j < Bnew.cols(); j++) + for (size_t j = 0; j < Bnew.cols(); j++) { - Bnew(i,j) = B((size_t)mActuatedJoint(i),(size_t)mActuatedJoint(j)); + Bnew(i, j) = B((size_t)mActuatedJoint(i), (size_t)mActuatedJoint(j)); } } return Bnew; @@ -1305,7 +1356,8 @@ void iLQRLocal::optimizationThreadLoop() long startTime = timeSinceEpochMillis(); optimizePlan(startTime + mMillisInAdvanceToPlan); long endTime = timeSinceEpochMillis(); - // std::cout <<"Time Per Run: "<< (float)(endTime - startTime)/1000 << std::endl; + // std::cout <<"Time Per Run: "<< (float)(endTime - startTime)/1000 << + // std::endl; adjustPerformance(endTime - startTime); } } @@ -1321,19 +1373,20 @@ void iLQRLocal::iLQRoptimizationThreadLoop() sigaddset(&sigset, SIGTERM); pthread_sigmask(SIG_BLOCK, &sigset, nullptr); - while(mRunning) + while (mRunning) { long startTime = timeSinceEpochMillis(); bool status = ilqroptimizePlan(startTime + mMillisInAdvanceToPlan); - //std::cout << "Planning Continue..." << std::endl; - if(!status) + // std::cout << "Planning Continue..." << std::endl; + if (!status) { std::cout << "Simulator Crashed by giving NaN..." << std::endl; // mRunning = false; // break; } long endTime = timeSinceEpochMillis(); - // std::cout <<"Time Per Run: "<< (float)(endTime - startTime)/1000 << std::endl; + // std::cout <<"Time Per Run: "<< (float)(endTime - startTime)/1000 << + // std::endl; adjustPerformance(endTime - startTime); } } @@ -1345,7 +1398,7 @@ bool iLQRLocal::variableChange() void iLQRLocal::setParameterChange(Eigen::VectorXs params) { - if(mInitialized == false) + if (mInitialized == false) { mPre_parameter = params; mInitialized = true; @@ -1354,7 +1407,7 @@ void iLQRLocal::setParameterChange(Eigen::VectorXs params) else { assert(params.size() == mPre_parameter.size()); - if((params-mPre_parameter).norm()>0.001) + if ((params - mPre_parameter).norm() > 0.001) { mVarchange = true; } diff --git a/dart/realtime/saved_data/plotter.py b/dart/realtime/saved_data/plotter.py new file mode 100644 index 000000000..2c2a67efe --- /dev/null +++ b/dart/realtime/saved_data/plotter.py @@ -0,0 +1,45 @@ +import numpy as np +import matplotlib.pyplot as plt +import argparse +import os + +def load_np_array(file,samples): + lines = file.readlines() + n_lines = len(lines) + data = np.zeros((n_lines,samples)) + for i,line in enumerate(lines): + entries = line.split(' ') + entries[-1] = entries[-1][:-1] + cur = 0 + for e in entries: + if e != '': + data[i,cur] = float(e) + cur += 1 + return data +parser = argparse.ArgumentParser() +parser.add_argument('--loss',type=str,default='Losses') +parser.add_argument('--solution',type=str,default='Solutions') +parser.add_argument('--exp_name',type=str,default=None,required=True) +parser.add_argument('--xlabel',type=str,default='mass') +parser.add_argument('--upper',type=float,default= 2.0) +parser.add_argument('--lower',type=float,default=-2.0) +args = parser.parse_args() + + +file = open(f'raw_data/{args.loss}.txt') +np_data = load_np_array(file,200) +file.close() +file = open(f'raw_data/{args.solution}.txt') +solutions = load_np_array(file,1) +file.close() +np.savez(f'np_files/{args.exp_name}.npz',loss=np_data,solution=solutions) + +x = np.linspace(start=args.lower,stop=args.upper,num=200) +os.makedirs(f'images/{args.exp_name}',exist_ok=True) +for i in range(np_data.shape[0]): + plt.clf() + plt.plot(x,np_data[i][::-1]) + plt.axvline(x=float(solutions[i]),color='red') + plt.xlabel(args.xlabel) + plt.ylabel('loss') + plt.savefig(f'images/{args.exp_name}/fig{i}.png',dpi=200) diff --git a/dart/realtime/saved_data/plotter2d.py b/dart/realtime/saved_data/plotter2d.py new file mode 100644 index 000000000..1bb0eb8ed --- /dev/null +++ b/dart/realtime/saved_data/plotter2d.py @@ -0,0 +1,40 @@ +import numpy as np +import matplotlib.pyplot as plt +import argparse +import os + +parser = argparse.ArgumentParser() +parser.add_argument('--loss',type=str,default='Losses') +parser.add_argument('--solution',type=str,default='Solutions') +parser.add_argument('--exp_name',type=str,default=None,required=True) +parser.add_argument('--rest_dim',type=int,default=2) +parser.add_argument('--x_upper',type=float,default= 2.0) +parser.add_argument('--x_lower',type=float,default= -2.0) +parser.add_argument('--y_upper',type=float,default= 2.0) +parser.add_argument('--y_lower',type=float,default= -2.0) +args = parser.parse_args() + +np_data = [] +csv_files = os.listdir(f'raw_data/{args.loss}/') +for f in csv_files: + np_data.append(np.genfromtxt(f'raw_data/{args.loss}/{f}',delimiter=',').T) + +solutions = np.genfromtxt(f'raw_data/{args.solution}.csv',delimiter=',') + +np.savez(f'np_files/{args.exp_name}_2d.npz',loss=np_data,solution=solutions) + +os.makedirs(f'images/{args.exp_name}',exist_ok=True) +for i in range(len(np_data)): + plt.clf() + mean = np_data[i].mean() + plt.imshow(np_data[i].clip(0,mean), + origin='lower', + extent=[args.x_lower,args.x_upper, + args.y_lower,args.y_upper], + cmap='hot') + cold_index = [0,1,2] + cold_index.remove(args.rest_dim) + plt.plot(solutions[i][cold_index[0]], + solutions[i][cold_index[1]], + 'bo') + plt.savefig(f'images/{args.exp_name}/fig{i}.png',dpi=200) diff --git a/unittests/comprehensive/CMakeLists.txt b/unittests/comprehensive/CMakeLists.txt index 6be71f070..c9a1cd3d4 100644 --- a/unittests/comprehensive/CMakeLists.txt +++ b/unittests/comprehensive/CMakeLists.txt @@ -25,6 +25,7 @@ dart_add_test("comprehensive" test_DampGradient) dart_add_test("comprehensive" test_SpringGradient) dart_add_test("comprehensive" test_iLQRRealtime) dart_add_test("comprehensive" test_iLQRHalfCheetah) +dart_add_test("comprehensive" test_iLQRCartpole) dart_add_test("comprehensive" test_cartpoleRealtime) dart_add_test("comprehensive" test_cartpoleSSID) dart_add_test("comprehensive" test_iLQR) @@ -33,6 +34,7 @@ dart_add_test("comprehensive" test_FullContactCartpole) dart_add_test("comprehensive" test_FullAcrobot) dart_add_test("comprehensive" test_FullDoublependulum) dart_add_test("comprehensive" test_FullWhip) + # target_link_libraries(test_Gradients dart-collision-bullet) # target_link_libraries(test_Gradients dart-collision-ode) if(TARGET dart-optimizer-pagmo) @@ -98,6 +100,8 @@ if(TARGET dart-utils) target_link_libraries(test_HalfCheetahRealtime dart-utils-urdf) target_link_libraries(test_iLQRHalfCheetah dart-utils) target_link_libraries(test_iLQRHalfCheetah dart-utils-urdf) + target_link_libraries(test_iLQRCartpole dart-utils) + target_link_libraries(test_iLQRCartpole dart-utils-urdf) target_link_libraries(test_KR5Trajectory dart-utils) target_link_libraries(test_KR5Trajectory dart-utils-urdf) target_link_libraries(test_Cartpole dart-utils) diff --git a/unittests/comprehensive/test_iLQRCartpole.cpp b/unittests/comprehensive/test_iLQRCartpole.cpp new file mode 100644 index 000000000..76a6c1709 --- /dev/null +++ b/unittests/comprehensive/test_iLQRCartpole.cpp @@ -0,0 +1,405 @@ +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include "dart/neural/RestorableSnapshot.hpp" +#include "dart/realtime/MPC.hpp" +#include "dart/realtime/SSID.hpp" +#include "dart/realtime/Ticker.hpp" +#include "dart/realtime/iLQRLocal.hpp" +#include "dart/server/GUIWebsocketServer.hpp" +#include "dart/simulation/World.hpp" +#include "dart/trajectory/IPOptOptimizer.hpp" +#include "dart/trajectory/LossFn.hpp" +#include "dart/trajectory/MultiShot.hpp" +#include "dart/trajectory/TrajectoryRollout.hpp" + +#include "TestHelpers.hpp" +#include "stdio.h" + +// #define ALL_TESTS + +using namespace dart; +using namespace math; +using namespace dynamics; +using namespace simulation; +using namespace neural; +using namespace realtime; +using namespace trajectory; +using namespace server; + +/* +TEST(REALTIME, CARTPOLE_ILQR) +{ + // Create the world + WorldPtr world = World::create(); + world->setGravity(Eigen::Vector3s(0.0, -9.81, 0.0)); + SkeletonPtr cartpole = Skeleton::create("cartpole"); + + std::pair sledPair + = cartpole->createJointAndBodyNodePair(nullptr); + sledPair.first->setAxis(Eigen::Vector3s(1, 0, 0)); + std::shared_ptr sledShapeBox( + new BoxShape(Eigen::Vector3s(0.5, 0.1, 0.1))); + ShapeNode* sledShape + = sledPair.second->createShapeNodeWith(sledShapeBox); + sledShape->getVisualAspect()->setColor(Eigen::Vector3s(0.5, 0.5, 0.5)); + + std::pair armPair + = cartpole->createJointAndBodyNodePair(sledPair.second); + armPair.first->setAxis(Eigen::Vector3s(0, 0, 1)); + std::shared_ptr armShapeBox( + new BoxShape(Eigen::Vector3s(0.1, 1.0, 0.1))); + ShapeNode* armShape + = armPair.second->createShapeNodeWith(armShapeBox); + armShape->getVisualAspect()->setColor(Eigen::Vector3s(0.7, 0.7, 0.7)); + + Eigen::Isometry3s armOffset = Eigen::Isometry3s::Identity(); + armOffset.translation() = Eigen::Vector3s(0, -0.5, 0); + armPair.first->setTransformFromChildBodyNode(armOffset); + + world->addSkeleton(cartpole); + + cartpole->setControlForceUpperLimit(0, 20); + cartpole->setControlForceLowerLimit(0, -20); + cartpole->setVelocityUpperLimit(0, 1000); + cartpole->setVelocityLowerLimit(0, -1000); + cartpole->setPositionUpperLimit(0, 10); + cartpole->setPositionLowerLimit(0, -10); + // The second DOF cannot be controlled + cartpole->setControlForceUpperLimit(1, 0); + cartpole->setControlForceLowerLimit(1, 0); + cartpole->setVelocityUpperLimit(1, 1000); + cartpole->setVelocityLowerLimit(1, -1000); + cartpole->setPositionUpperLimit(1, 10); + cartpole->setPositionLowerLimit(1, -10); + + cartpole->setPosition(0, 1.0); + cartpole->setPosition(1, 3.1415); + + world->setTimeStep(1.0 / 100); + + // Create iLQR instance + int steps = 400; + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = steps * millisPerTimestep; + + world->removeDofFromActionSpace(1); + // Create Goal + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * 2); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Zero(1); + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Zero(2 * 2); + finalStateWeight(0) = 10.0; + finalStateWeight(1) = 50.0; + finalStateWeight(2) = 10.0; + finalStateWeight(3) = 10.0; + runningActionWeight(0) = 0.01; + + std::shared_ptr costFn + = std::make_shared(runningStateWeight, + runningActionWeight, + finalStateWeight, + world); + costFn->setTimeStep(world->getTimeStep()); + Eigen::VectorXs goal = Eigen::VectorXs::Zero(4); + goal(0) = 0.5; + costFn->setTarget(goal); + std::cout << "Before MPC Local Initialization" << std::endl; + std::cout << "Planning Millis: " << planningHorizonMillis << std::endl; + iLQRLocal ilqr = iLQRLocal( + world, costFn, 1, planningHorizonMillis, 1.0); + + Eigen::VectorXs init_state = world->getState(); + + std::cout << "mpcLocal Created Successfully" << std::endl; + int maxIter = 10; + ilqr.setSilent(true); + ilqr.setMaxIterations(maxIter); + ilqr.setAlpha(0.5); + ilqr.setPatience(1); + ilqr.setActionBound(100.0); + // Create Server for rendering + GUIWebsocketServer server; + server.serve(8070); + // Initialize a fresh rollout for loss computation + TrajectoryRolloutReal rollout = ilqr.createRollout(steps, world->getNumDofs(), +world->getMassDims()); + + // Define a lambda function for simulate traj + auto simulate_traj = [&](std::vector X, +std::vector U, bool render) + { + world->setState(init_state); + if(render) + { + server.renderWorld(world); + for(int i = 0; i < steps-1; i++) + { + rollout.getPoses().col(i) = world->getPositions(); + rollout.getVels().col(i) = world->getVelocities(); + rollout.getControlForces().col(i) = world->mapToForceSpaceVector(U[i]); + world->setAction(U[i]); + world->step(); + X[i+1] = world->getState(); + server.renderWorld(world); + usleep(10000); + } + rollout.getPoses().col(steps-1) = world->getPositions(); + rollout.getVels().col(steps-1) = world->getVelocities(); + s_t loss = costFn->computeLoss(&rollout); + return loss; + } + else + { + for(int i = 0; i < steps-1; i++) + { + rollout.getPoses().col(i) = world->getPositions(); + rollout.getVels().col(i) = world->getVelocities(); + rollout.getControlForces().col(i) = world->mapToForceSpaceVector(U[i]); + world->setAction(U[i]); + world->step(); + X[i+1] = world->getState(); + } + rollout.getPoses().col(steps-1) = world->getPositions(); + rollout.getVels().col(steps-1) = world->getVelocities(); + s_t loss = costFn->computeLoss(&rollout); + return loss; + } + }; + // Instead of starting a single thread, iLQR Trajectory optimization from +starting state s_t init_cost = simulate_traj(ilqr.getStatesFromiLQRBuffer(), +ilqr.getActionsFromiLQRBuffer(), false); std::cout << "Initial Cost: " << +init_cost << std::endl; ilqr.setCurrentCost(init_cost); s_t prev_cost = 1e10; + s_t threshold = 0.01; + + // Print out current parameters settings + std::cout << "Alpha: " << ilqr.getAlpha() << "\n" + << "MU: " << ilqr.getMU() << std::endl; + + int iter = 0; + while(iter < maxIter) + { + // Set the world to initial state + world->setState(init_state); + + bool forwardFlag = ilqr.ilqrForward(world); + bool backwardFlag = false; + if(!forwardFlag) + { + std::cout << "Optimization Terminated, Exiting ..." <setGravity(Eigen::Vector3s(0, -9.81, 0)); + + // Create World of cartpole + SkeletonPtr cartpole = Skeleton::create("cartpole"); + + std::pair sledPair + = cartpole->createJointAndBodyNodePair(nullptr); + sledPair.first->setAxis(Eigen::Vector3s(1, 0, 0)); + std::shared_ptr sledShapeBox( + new BoxShape(Eigen::Vector3s(0.5, 0.1, 0.1))); + ShapeNode* sledShape + = sledPair.second->createShapeNodeWith(sledShapeBox); + sledShape->getVisualAspect()->setColor(Eigen::Vector3s(0.5, 0.5, 0.5)); + + std::pair armPair + = cartpole->createJointAndBodyNodePair(sledPair.second); + armPair.first->setAxis(Eigen::Vector3s(0, 0, 1)); + std::shared_ptr armShapeBox( + new BoxShape(Eigen::Vector3s(0.1, 1.0, 0.1))); + ShapeNode* armShape + = armPair.second->createShapeNodeWith(armShapeBox); + armShape->getVisualAspect()->setColor(Eigen::Vector3s(0.7, 0.7, 0.7)); + + Eigen::Isometry3s armOffset = Eigen::Isometry3s::Identity(); + armOffset.translation() = Eigen::Vector3s(0, -0.5, 0); + armPair.first->setTransformFromChildBodyNode(armOffset); + + world->addSkeleton(cartpole); + + cartpole->setControlForceUpperLimit(0, 15); + cartpole->setControlForceLowerLimit(0, -15); + cartpole->setVelocityUpperLimit(0, 1000); + cartpole->setVelocityLowerLimit(0, -1000); + cartpole->setPositionUpperLimit(0, 10); + cartpole->setPositionLowerLimit(0, -10); + // The second DOF cannot be controlled + cartpole->setControlForceUpperLimit(1, 0); + cartpole->setControlForceLowerLimit(1, 0); + cartpole->setVelocityUpperLimit(1, 1000); + cartpole->setVelocityLowerLimit(1, -1000); + cartpole->setPositionUpperLimit(1, 10); + cartpole->setPositionLowerLimit(1, -10); + + cartpole->setPosition(0, 0); + cartpole->setPosition(1, 30.0 / 180.0 * 3.1415); + cartpole->computeForwardDynamics(); + cartpole->integrateVelocities(world->getTimeStep()); + + world->setTimeStep(1.0 / 100); + + int millisPerTimestep = world->getTimeStep() * 1000; + int planningHorizonMillis = 500 * millisPerTimestep; + + world->removeDofFromActionSpace(1); + // Create Goal + Eigen::VectorXs runningStateWeight = Eigen::VectorXs::Zero(2 * 2); + Eigen::VectorXs runningActionWeight = Eigen::VectorXs::Zero(1); + Eigen::VectorXs finalStateWeight = Eigen::VectorXs::Zero(2 * 2); + finalStateWeight(0) = 10.0; + finalStateWeight(1) = 50.0; + finalStateWeight(2) = 50.0; + finalStateWeight(3) = 50.0; + runningActionWeight(0) = 0.01; + + std::shared_ptr costFn + = std::make_shared( + runningStateWeight, runningActionWeight, finalStateWeight, world); + + Eigen::VectorXs goal = Eigen::VectorXs::Zero(4); + goal(0) = 1.0; + costFn->setTarget(goal); + std::cout << "Before MPC Local Initialization" << std::endl; + iLQRLocal mpcLocal = iLQRLocal(world, 1, planningHorizonMillis, 1.0); + + std::cout << "mpcLocal Created Successfully" << std::endl; + + mpcLocal.setCostFn(costFn); + mpcLocal.setSilent(true); + mpcLocal.setMaxIterations(5); + mpcLocal.setEnableLineSearch(false); + mpcLocal.setEnableOptimizationGuards(true); + mpcLocal.setPredictUsingFeedback(false); + mpcLocal.setPatience(3); + mpcLocal.setActionBound(20.0); + mpcLocal.setAlpha(1); + + std::shared_ptr realtimeUnderlyingWorld = world->clone(); + GUIWebsocketServer server; + + server.createSphere( + "goal", + 0.1, + Eigen::Vector3s(goal(0), 1.0, 0), + Eigen::Vector4s(1.0, 0.0, 0.0, 1.0)); + server.registerDragListener( + "goal", + [&](Eigen::Vector3s dragTo) { + goal(0) = dragTo(0); + dragTo(1) = 1.0; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal", dragTo); + }, + [&]() { + // end drag + }); + std::cout << "Reach Here Before Ticker" << std::endl; + Ticker ticker = Ticker(2 * realtimeUnderlyingWorld->getTimeStep()); + + auto sledBodyVisual = realtimeUnderlyingWorld->getSkeleton("cartpole") + ->getBodyNodes()[0] + ->getShapeNodesWith()[0] + ->getVisualAspect(); + Eigen::Vector3s originalColor = sledBodyVisual->getColor(); + long total_steps = 0; + ticker.registerTickListener([&](long now) { + Eigen::VectorXs mpcforces + = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + std::cout << "Force:\n" << mpcforces << std::endl; + // Eigen::VectorXs mpcforces = mpcLocal.getControlForce(now); + realtimeUnderlyingWorld->setControlForces(mpcforces); + if (server.getKeysDown().count("a")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = -15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(1, 0, 0)); + } + else if (server.getKeysDown().count("e")) + { + Eigen::VectorXs perturbedForces + = realtimeUnderlyingWorld->getControlForces(); + perturbedForces(0) = 15.0; + realtimeUnderlyingWorld->setControlForces(perturbedForces); + sledBodyVisual->setColor(Eigen::Vector3s(0, 1, 0)); + } + else + { + sledBodyVisual->setColor(originalColor); + } + realtimeUnderlyingWorld->step(); + mpcLocal.recordGroundTruthState( + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); + + if (total_steps % 5 == 0) + { + server.renderWorld(realtimeUnderlyingWorld); + total_steps = 0; + } + total_steps++; + }); + + // Should only work when trajectory opt + mpcLocal.registerReplanningListener( + [&](long, const trajectory::TrajectoryRollout* rollout, long) { + server.renderTrajectoryLines(world, rollout->getPosesConst()); + }); + + server.registerConnectionListener([&]() { + ticker.start(); + // mpcLocal.start(); + mpcLocal.ilqrstart(); + }); + server.registerShutdownListener([&]() { mpcLocal.stop(); }); + server.serve(8070); + server.blockWhileServing(); +} +// #endif \ No newline at end of file diff --git a/unittests/comprehensive/test_iLQRRealtime.cpp b/unittests/comprehensive/test_iLQRRealtime.cpp index b6ad89a4e..a6f759239 100644 --- a/unittests/comprehensive/test_iLQRRealtime.cpp +++ b/unittests/comprehensive/test_iLQRRealtime.cpp @@ -1,23 +1,22 @@ #include +#include #include #include -#include -#include #include -#include #include -#include - +#include +#include +#include +#include #include -#include -#include +#include #include "dart/neural/RestorableSnapshot.hpp" #include "dart/realtime/MPC.hpp" -#include "dart/realtime/iLQRLocal.hpp" #include "dart/realtime/SSID.hpp" #include "dart/realtime/Ticker.hpp" +#include "dart/realtime/iLQRLocal.hpp" #include "dart/server/GUIWebsocketServer.hpp" #include "dart/simulation/World.hpp" #include "dart/trajectory/IPOptOptimizer.hpp" @@ -44,13 +43,15 @@ std::shared_ptr getSSIDPosLoss() { TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); - Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorPositions + = rawPos.block(0, 1, rawPos.rows(), rawPos.cols() - 1); int steps = rollout->getPosesConst().cols(); Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; - //std::cout << "Pos In Buffer: " << std::endl << sensorPositions << std::endl; - //std::cout << "Pos In Traj : " << std::endl << rollout->getPosesConst() << std::endl; + // std::cout << "Pos In Buffer: " << std::endl << sensorPositions << + // std::endl; std::cout << "Pos In Traj : " << std::endl << + // rollout->getPosesConst() << std::endl; s_t sum = 0.0; for (int i = 0; i < steps; i++) @@ -69,7 +70,8 @@ std::shared_ptr getSSIDPosLoss() int steps = rollout->getPosesConst().cols(); Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); - Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); + Eigen::MatrixXs sensorPositions + = rawPos.block(0, 1, rawPos.rows(), rawPos.cols() - 1); Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; @@ -88,11 +90,12 @@ std::shared_ptr getSSIDPosLoss() std::shared_ptr getSSIDVelPosLoss() { TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { - Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); - Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); - Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + Eigen::MatrixXs sensorPositions + = rawPos.block(0, 1, rawPos.rows(), rawPos.cols() - 1); + Eigen::MatrixXs sensorVelocities + = rawVel.block(0, 1, rawVel.rows(), rawVel.cols() - 1); int steps = rollout->getPosesConst().cols(); Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; @@ -119,8 +122,10 @@ std::shared_ptr getSSIDVelPosLoss() Eigen::MatrixXs rawPos = rollout->getMetadata("sensors"); Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); - Eigen::MatrixXs sensorPositions = rawPos.block(0,1,rawPos.rows(),rawPos.cols()-1); - Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + Eigen::MatrixXs sensorPositions + = rawPos.block(0, 1, rawPos.rows(), rawPos.cols() - 1); + Eigen::MatrixXs sensorVelocities + = rawVel.block(0, 1, rawVel.rows(), rawVel.cols() - 1); Eigen::MatrixXs posError = rollout->getPosesConst() - sensorPositions; Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; @@ -143,7 +148,8 @@ std::shared_ptr getSSIDVelLoss() { TrajectoryLossFn loss = [](const TrajectoryRollout* rollout) { Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); - Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + Eigen::MatrixXs sensorVelocities + = rawVel.block(0, 1, rawVel.rows(), rawVel.cols() - 1); int steps = rollout->getVelsConst().cols(); Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; @@ -167,7 +173,8 @@ std::shared_ptr getSSIDVelLoss() int steps = rollout->getVelsConst().cols(); Eigen::MatrixXs rawVel = rollout->getMetadata("velocities"); - Eigen::MatrixXs sensorVelocities = rawVel.block(0,1,rawVel.rows(),rawVel.cols()-1); + Eigen::MatrixXs sensorVelocities + = rawVel.block(0, 1, rawVel.rows(), rawVel.cols() - 1); Eigen::MatrixXs velError = rollout->getVelsConst() - sensorVelocities; @@ -185,12 +192,13 @@ std::shared_ptr getSSIDVelLoss() WorldPtr createWorld(s_t timestep) { - WorldPtr world = dart::utils::UniversalLoader::loadWorld("dart://sample/skel/cartpole.skel"); + WorldPtr world = dart::utils::UniversalLoader::loadWorld( + "dart://sample/skel/cartpole.skel"); world->setState(Eigen::Vector4s(0, 175.0 / 180 * 3.14, 0, 0)); world->removeDofFromActionSpace(1); - + world->setTimeStep(timestep); - + return world; } @@ -201,12 +209,13 @@ std::mt19937 initializeRandom() return gen; } -Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) +Eigen::VectorXs rand_normal( + size_t length, s_t mean, s_t stddev, std::mt19937 random_gen) { Eigen::VectorXs result = Eigen::VectorXs::Zero(length); std::normal_distribution<> dist{mean, stddev}; - - for(int i = 0; i < length; i++) + + for (int i = 0; i < length; i++) { result(i) += (s_t)(dist(random_gen)); } @@ -216,7 +225,7 @@ Eigen::VectorXs rand_normal(size_t length, s_t mean, s_t stddev, std::mt19937 ra std::vector convert2stdvec(Eigen::VectorXs vec) { std::vector stdvec; - for(int i = 0; i < vec.size(); i++) + for (int i = 0; i < vec.size(); i++) { stdvec.push_back(vec(i)); } @@ -227,24 +236,32 @@ void recordObs(size_t now, SSID* ssid, WorldPtr realtimeWorld) { ssid->registerLock(); ssid->registerControls(now, realtimeWorld->getControlForces()); - ssid->registerSensors(now, realtimeWorld->getPositions(),0); - ssid->registerSensors(now, realtimeWorld->getVelocities(),1); + ssid->registerSensors(now, realtimeWorld->getPositions(), 0); + ssid->registerSensors(now, realtimeWorld->getVelocities(), 1); ssid->registerUnlock(); } -void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t noise_scale, std::mt19937 random_gen) +void recordObsWithNoise( + size_t now, + SSID* ssid, + WorldPtr realtimeWorld, + s_t noise_scale, + std::mt19937 random_gen) { ssid->registerLock(); Eigen::VectorXs control_force = realtimeWorld->getControlForces(); - // Eigen::VectorXs force_eps = rand_normal(control_force.size(), 0, noise_scale, random_gen); + // Eigen::VectorXs force_eps = rand_normal(control_force.size(), 0, + // noise_scale, random_gen); ssid->registerControls(now, control_force); Eigen::VectorXs position = realtimeWorld->getPositions(); - Eigen::VectorXs position_eps = rand_normal(position.size(), 0, noise_scale, random_gen); + Eigen::VectorXs position_eps + = rand_normal(position.size(), 0, noise_scale, random_gen); ssid->registerSensors(now, position + position_eps, 0); Eigen::VectorXs velocity = realtimeWorld->getVelocities(); - Eigen::VectorXs velocity_eps = rand_normal(velocity.size(), 0, noise_scale, random_gen); + Eigen::VectorXs velocity_eps + = rand_normal(velocity.size(), 0, noise_scale, random_gen); ssid->registerSensors(now, velocity + velocity_eps, 1); ssid->registerUnlock(); } @@ -252,9 +269,11 @@ void recordObsWithNoise(size_t now, SSID* ssid, WorldPtr realtimeWorld, s_t nois Eigen::MatrixXs std2eigen(std::vector record) { size_t num_record = record.size(); - Eigen::MatrixXs eigen_record = Eigen::MatrixXs::Zero(num_record, record[0].size()); - std::cout << "Size: " << eigen_record.cols() << " " << eigen_record.rows() << std::endl; - for(int i = 0; i < num_record; i++) + Eigen::MatrixXs eigen_record + = Eigen::MatrixXs::Zero(num_record, record[0].size()); + std::cout << "Size: " << eigen_record.cols() << " " << eigen_record.rows() + << std::endl; + for (int i = 0; i < num_record; i++) { eigen_record.row(i) = record[i]; } @@ -284,38 +303,39 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) std::shared_ptr ssidWorld = world->clone(); ssidWorld->tuneMass( - world->getBodyNodeByIndex(ssid_index), - WrtMassBodyNodeEntryType::INERTIA_MASS, - Eigen::VectorXs::Ones(1) * 5.0, - Eigen::VectorXs::Ones(1) * 0.2); + world->getBodyNodeByIndex(ssid_index), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); ssidWorld->tuneMass( - world->getBodyNodeByIndex(ssid_index2), - WrtMassBodyNodeEntryType::INERTIA_MASS, - Eigen::VectorXs::Ones(1) * 5.0, - Eigen::VectorXs::Ones(1) * 0.2); - + world->getBodyNodeByIndex(ssid_index2), + WrtMassBodyNodeEntryType::INERTIA_MASS, + Eigen::VectorXs::Ones(1) * 5.0, + Eigen::VectorXs::Ones(1) * 0.2); + ssidWorld->tuneDamping( - world->getJointIndex(1), - WrtDampingJointEntryType::DAMPING, - Eigen::VectorXi::Ones(1), - Eigen::VectorXs::Ones(1), - Eigen::VectorXs::Ones(1) * 0.01); + world->getJointIndex(1), + WrtDampingJointEntryType::DAMPING, + Eigen::VectorXi::Ones(1), + Eigen::VectorXs::Ones(1), + Eigen::VectorXs::Ones(1) * 0.01); Eigen::Vector2s sensorDims(world->getNumDofs(), world->getNumDofs()); - Eigen::Vector2i ssid_idx(0,1); - - SSID ssid = SSID(ssidWorld, - getSSIDPosLoss(), - inferenceHistoryMillis, - sensorDims, - inferenceSteps, - scale); + Eigen::Vector2i ssid_idx(0, 1); + + SSID ssid = SSID( + ssidWorld, + getSSIDPosLoss(), + inferenceHistoryMillis, + sensorDims, + inferenceSteps, + scale); std::mutex lock; std::mutex param_lock; ssid.attachMutex(lock); ssid.attachParamMutex(param_lock); - ssid.setSSIDMassIndex(Eigen::Vector2i(0, 1)); + ssid.setSSIDMassIndex(Eigen::Vector2i(0, 1)); ssid.setSSIDDampIndex(Eigen::VectorXi::Ones(1)); ssid.useConfidence(); ssid.useHeuristicWeight(); @@ -323,19 +343,12 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) ssid.setTemperature(Eigen::Vector3s(0.5, 5, 2)); // ssid.setThreshs(0.1, 0.5); // As in paper ssid.setEWiseThreshs(Eigen::Vector3s(0.1, 0.1, 0.02), 0.5); - ssid.setInitialPosEstimator( - [](Eigen::MatrixXs sensors, long) - { - return sensors.col(0); - }); + [](Eigen::MatrixXs sensors, long) { return sensors.col(0); }); ssid.setInitialVelEstimator( - [](Eigen::MatrixXs sensors, long) - { - return sensors.col(0); - }); + [](Eigen::MatrixXs sensors, long) { return sensors.col(0); }); world->clearTunableMassThisInstance(); // Create Goal @@ -349,10 +362,8 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) runningActionWeight(0) = 0.01; std::shared_ptr costFn - = std::make_shared(runningStateWeight, - runningActionWeight, - finalStateWeight, - world); + = std::make_shared( + runningStateWeight, runningActionWeight, finalStateWeight, world); costFn->setSSIDMassNodeIndex(ssid_idx); // costFn->enableSSIDLoss(0.01); @@ -361,8 +372,7 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) goal(0) = 1.0; costFn->setTarget(goal); std::cout << "Before MPC Local Initialization" << std::endl; - iLQRLocal mpcLocal = iLQRLocal( - world, 1, planningHorizonMillis, 1.0); + iLQRLocal mpcLocal = iLQRLocal(world, 1, planningHorizonMillis, 1.0); mpcLocal.setCostFn(costFn); std::cout << "mpcLocal Created Successfully" << std::endl; @@ -375,7 +385,7 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) mpcLocal.setActionBound(20.0); mpcLocal.setAlpha(1); - //mpcLocal.disableAdaptiveTime(); + // mpcLocal.disableAdaptiveTime(); // bool init_flag = true; @@ -383,38 +393,45 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) Eigen::VectorXs, Eigen::VectorXs, Eigen::VectorXs params, - long, - bool steadyFound){ - // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will cause problem ... But Why - // If there are more than one type of parameters will this work? - if(steadyFound) + long, + bool steadyFound) { + // mpcLocal.recordGroundTruthState(time, pos, vel, mass); //TODO: This will + // cause problem ... But Why If there are more than one type of parameters + // will this work? + if (steadyFound) { mpcLocal.setCandidateHorizon(planningHorizonMillis); } else { - mpcLocal.setCandidateHorizon((size_t)(1*planningHorizonMillis)); + mpcLocal.setCandidateHorizon((size_t)(1 * planningHorizonMillis)); } world->setLinkMassIndex(params(0), ssid_index); world->setLinkMassIndex(params(1), ssid_index2); // The rest of segment should be damping - world->setJointDampingCoeffIndex(params.segment(2,1), 1); + world->setJointDampingCoeffIndex(params.segment(2, 1), 1); }); - std::shared_ptr realtimeUnderlyingWorld = world->clone(); GUIWebsocketServer server; - server.createSphere("goal", 0.1, - Eigen::Vector3s(goal(0), 0.7, 0), - Eigen::Vector4s(1.0, 0.0, 0.0, 1.0)); - server.registerDragListener("goal", [&](Eigen::Vector3s dragTo){ - goal(0) = dragTo(0); - dragTo(1) = 0.3; - dragTo(2) = 0.0; - costFn->setTarget(goal); - server.setObjectPosition("goal", dragTo); - }); + server.createSphere( + "goal", + 0.1, + Eigen::Vector3s(goal(0), 0.7, 0), + Eigen::Vector4s(1.0, 0.0, 0.0, 1.0)); + server.registerDragListener( + "goal", + [&](Eigen::Vector3s dragTo) { + goal(0) = dragTo(0); + dragTo(1) = 0.3; + dragTo(2) = 0.0; + costFn->setTarget(goal); + server.setObjectPosition("goal", dragTo); + }, + [&]() { + // end drag + }); std::string key = "mass"; @@ -432,25 +449,27 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) realtimeUnderlyingWorld->setLinkMassIndex(params(0), ssid_index); realtimeUnderlyingWorld->setLinkMassIndex(params(1), ssid_index2); - realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(2,1), 1); + realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(2, 1), 1); ssidWorld->setLinkMassIndex(id_params(0), ssid_index); ssidWorld->setLinkMassIndex(id_params(1), ssid_index2); - ssidWorld->setJointDampingCoeffIndex(id_params.segment(2,1),1); + ssidWorld->setJointDampingCoeffIndex(id_params.segment(2, 1), 1); world->setLinkMassIndex(id_params(0), ssid_index); world->setLinkMassIndex(id_params(1), ssid_index2); - world->setJointDampingCoeffIndex(id_params.segment(2,1),1); + world->setJointDampingCoeffIndex(id_params.segment(2, 1), 1); int cnt = 0; int filecnt = 0; bool renderIsReady = false; bool record = false; std::vector real_record; - std::vector id_record; + std::vector id_record; ticker.registerTickListener([&](long now) { - if(renderIsReady) + if (renderIsReady) { - Eigen::VectorXs mpcforces = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); - Eigen::VectorXs force_eps = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); + Eigen::VectorXs mpcforces + = mpcLocal.computeForce(realtimeUnderlyingWorld->getState(), now); + Eigen::VectorXs force_eps + = rand_normal(mpcforces.size(), 0, noise_scale, rand_gen); realtimeUnderlyingWorld->setControlForces(mpcforces + force_eps); } if (server.getKeysDown().count("a")) @@ -487,36 +506,45 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) params(0) = 1.0; params(2) = 0.1; realtimeUnderlyingWorld->setLinkMassIndex(params(0), ssid_index); - realtimeUnderlyingWorld->setJointDampingCoeffIndex(params.segment(2,1), 1); + realtimeUnderlyingWorld->setJointDampingCoeffIndex( + params.segment(2, 1), 1); } - else if(server.getKeysDown().count("s")) + else if (server.getKeysDown().count("s")) { renderIsReady = true; record = true; ssid.start(); - //ssid.startSlow(); // Which should be useless + // ssid.startSlow(); // Which should be useless mpcLocal.setPredictUsingFeedback(false); mpcLocal.ilqrstart(); } - else if(server.getKeysDown().count("r")) + else if (server.getKeysDown().count("r")) { record = true; } - else if(server.getKeysDown().count("p") || cnt == 940) + else if (server.getKeysDown().count("p") || cnt == 940) { - if(record) + if (record) { Eigen::MatrixXs real_params = std2eigen(real_record); Eigen::MatrixXs sysid_params = std2eigen(id_record); std::cout << "Converted!" << std::endl; - ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/cartpole_10_real_"+std::to_string(filecnt)+".csv", real_params); - ssid.saveCSVMatrix("/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/cartpole_10_identified_"+std::to_string(filecnt)+".csv", sysid_params); - filecnt ++; + ssid.saveCSVMatrix( + "/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/" + "cartpole_10_real_" + + std::to_string(filecnt) + ".csv", + real_params); + ssid.saveCSVMatrix( + "/workspaces/nimblephysics/dart/realtime/saved_data/timeplots/" + "cartpole_10_identified_" + + std::to_string(filecnt) + ".csv", + sysid_params); + filecnt++; } record = false; } // recordObs(now, &ssid, realtimeUnderlyingWorld); - if(renderIsReady) + if (renderIsReady) { // if((realtimeUnderlyingWorld->getState()-goal).norm()<0.2) // { @@ -527,55 +555,54 @@ TEST(REALTIME, CARTPOLE_MPC_MASS) // //mpcLocal.stop(); // //exit(1); // } - recordObsWithNoise(now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); + recordObsWithNoise( + now, &ssid, realtimeUnderlyingWorld, noise_scale, rand_gen); realtimeUnderlyingWorld->step(); cnt++; } id_params(0) = world->getLinkMassIndex(ssid_index); id_params(1) = world->getLinkMassIndex(ssid_index2); id_params(2) = world->getJointDampingCoeffIndex(1)(0); - if(renderIsReady) + if (renderIsReady) { mpcLocal.recordGroundTruthState( - now, - realtimeUnderlyingWorld->getPositions(), - realtimeUnderlyingWorld->getVelocities(), - realtimeUnderlyingWorld->getMasses()); // TODO: Why only mass + now, + realtimeUnderlyingWorld->getPositions(), + realtimeUnderlyingWorld->getVelocities(), + realtimeUnderlyingWorld->getMasses()); // TODO: Why only mass } - - if(total_steps % 5 == 0) + + if (total_steps % 5 == 0) { server.renderWorld(realtimeUnderlyingWorld); - server.createText(key, - "Cur Params: "+std::to_string(id_params(0))+" "+std::to_string(id_params(1))+" "+std::to_string(id_params(2)) - +"Real Params: "+std::to_string(params(0))+" "+std::to_string(params(1))+" "+std::to_string(params(2)), - Eigen::Vector2i(100,100), - Eigen::Vector2i(200,200)); - if(record && renderIsReady) + server.createText( + key, + "Cur Params: " + std::to_string(id_params(0)) + " " + + std::to_string(id_params(1)) + " " + + std::to_string(id_params(2)) + + "Real Params: " + std::to_string(params(0)) + " " + + std::to_string(params(1)) + " " + std::to_string(params(2)), + Eigen::Vector2i(100, 100), + Eigen::Vector2i(200, 200)); + if (record && renderIsReady) { id_record.push_back(id_params); real_record.push_back(params); } total_steps = 0; } - total_steps ++; - - + total_steps++; }); // Should only work when trajectory opt // We can always feed the trajectory used in forward pass here mpcLocal.registerReplanningListener( - [&](long , - const trajectory::TrajectoryRollout* rollout, - long ) { + [&](long, const trajectory::TrajectoryRollout* rollout, long) { server.renderTrajectoryLines(world, rollout->getPosesConst()); }); - - server.registerConnectionListener([&](){ - ticker.start(); - }); - server.registerShutdownListener([&]() {mpcLocal.stop(); }); + + server.registerConnectionListener([&]() { ticker.start(); }); + server.registerShutdownListener([&]() { mpcLocal.stop(); }); server.serve(8070); server.blockWhileServing(); }