Squashed 'third_party/ct/' content from commit 0048d02
Change-Id: Ia7e5360cbb414f92ce4f118bd9613ea23597db52
git-subtree-dir: third_party/ct
git-subtree-split: 0048d027531b6cf1ea730da17b68a0b7ef9070b1
diff --git a/ct_models/examples/InvertedPendulum/NLOC_MPC.cpp b/ct_models/examples/InvertedPendulum/NLOC_MPC.cpp
new file mode 100644
index 0000000..298b813
--- /dev/null
+++ b/ct_models/examples/InvertedPendulum/NLOC_MPC.cpp
@@ -0,0 +1,194 @@
+/**********************************************************************************************************************
+This file is part of the Control Toolbox (https://adrlab.bitbucket.io/ct), copyright by ETH Zurich, Google Inc.
+Authors: Michael Neunert, Markus Giftthaler, Markus Stäuble, Diego Pardo, Farbod Farshidian
+Licensed under Apache2 license (see LICENSE file in main directory)
+**********************************************************************************************************************/
+
+#include <ct/optcon/optcon.h>
+#include <ct/rbd/rbd.h>
+#include "../exampleDir.h"
+
+#include <ct/models/InvertedPendulum/InvertedPendulum.h>
+
+using namespace ct::rbd;
+
+const size_t njoints = ct::rbd::InvertedPendulum::Kinematics::NJOINTS;
+const size_t actuator_state_dim = 1;
+
+typedef ct::rbd::InvertedPendulum::tpl::Dynamics<double> IPDynamics;
+typedef ct::rbd::FixBaseFDSystem<IPDynamics, actuator_state_dim, false> IPSystem;
+
+typedef ct::core::LinearSystem<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM, double> LinearSystem;
+
+using InvertedPendulumNLOC = FixBaseNLOC<IPSystem, actuator_state_dim, double>;
+
+class MPCSimulator : public ct::core::ControlSimulator<IPSystem>
+{
+public:
+ MPCSimulator(ct::core::Time sim_dt,
+ ct::core::Time control_dt,
+ const ct::core::StateVector<STATE_DIM>& x0,
+ std::shared_ptr<IPSystem> ip_system,
+ ct::optcon::MPC<ct::optcon::NLOptConSolver<STATE_DIM, CONTROL_DIM>>& mpc)
+ : ct::core::ControlSimulator<IPSystem>(sim_dt, control_dt, x0, ip_system), mpc_(mpc)
+ {
+ controller_.reset(new ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM>);
+ }
+
+ void finishSystemIteration(ct::core::Time sim_time) override
+ {
+ control_mtx_.lock();
+ system_->setController(controller_);
+ control_mtx_.unlock();
+ }
+
+ void prepareControllerIteration(ct::core::Time sim_time) override
+ {
+ mpc_.prepareIteration(sim_time);
+ }
+
+ void finishControllerIteration(ct::core::Time sim_time) override
+ {
+ state_mtx_.lock();
+ ct::core::StateVector<STATE_DIM> x_temp = x_;
+ state_mtx_.unlock();
+
+ std::shared_ptr<ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM>> new_controller(
+ new ct::core::StateFeedbackController<STATE_DIM, CONTROL_DIM>);
+ bool success = mpc_.finishIteration(x_temp, sim_time, *new_controller, controller_ts_);
+ if (!success) throw std::runtime_error("Failed to finish iteration.");
+
+ control_mtx_.lock();
+ controller_ = new_controller;
+ control_mtx_.unlock();
+ }
+
+private:
+ ct::optcon::MPC<ct::optcon::NLOptConSolver<STATE_DIM, CONTROL_DIM>>& mpc_;
+ ct::core::Time controller_ts_;
+};
+
+int main(int argc, char* argv[])
+{
+ const bool verbose = true;
+ try
+ {
+ std::string workingDirectory = ct::models::exampleDir + "/InvertedPendulum";
+
+ std::string configFile = workingDirectory + "/solver.info";
+ std::string costFunctionFile = workingDirectory + "/cost.info";
+
+ std::shared_ptr<ct::rbd::SEADynamicsFirstOrder<njoints, double>> actuatorDynamics(
+ new ct::rbd::SEADynamicsFirstOrder<njoints, double>(160.0));
+ std::shared_ptr<IPSystem> ipSystem(new IPSystem(actuatorDynamics));
+
+ // NLOC settings
+ ct::optcon::NLOptConSettings nloc_settings;
+ nloc_settings.load(configFile, verbose, "ilqr");
+
+ std::shared_ptr<ct::optcon::TermQuadratic<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM, double, double>>
+ termQuadInterm(new ct::optcon::TermQuadratic<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM, double, double>);
+ termQuadInterm->loadConfigFile(costFunctionFile, "term0", verbose);
+
+ std::shared_ptr<ct::optcon::TermQuadratic<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM, double, double>>
+ termQuadFinal(new ct::optcon::TermQuadratic<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM, double, double>);
+ termQuadFinal->loadConfigFile(costFunctionFile, "term1", verbose);
+
+ std::shared_ptr<ct::optcon::CostFunctionAnalytical<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM>> newCost(
+ new ct::optcon::CostFunctionAnalytical<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM>);
+ size_t intTermID = newCost->addIntermediateTerm(termQuadInterm);
+ size_t finalTermID = newCost->addFinalTerm(termQuadFinal);
+
+ ct::core::Time timeHorizon;
+ InvertedPendulumNLOC::FeedbackArray::value_type fbD;
+ ct::rbd::tpl::JointState<njoints, double> x0;
+ ct::rbd::tpl::JointState<njoints, double> xf;
+
+ ct::core::loadScalar(configFile, "timeHorizon", timeHorizon);
+ ct::core::loadMatrix(costFunctionFile, "K_init", fbD);
+ ct::core::loadMatrix(costFunctionFile, "x_0", x0.toImplementation());
+ ct::core::loadMatrix(costFunctionFile, "term1.weights.x_des", xf.toImplementation());
+ ct::core::StateVector<IPSystem::STATE_DIM> x0full = IPSystem::toFullState(x0.toImplementation());
+
+ std::shared_ptr<LinearSystem> linSystem = nullptr;
+ ct::optcon::OptConProblem<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM> optConProblem(
+ timeHorizon, x0full, ipSystem, newCost, linSystem);
+ InvertedPendulumNLOC nloc_solver(newCost, nloc_settings, ipSystem, verbose, linSystem);
+
+ int K = nloc_solver.getSettings().computeK(timeHorizon);
+
+ InvertedPendulumNLOC::StateVectorArray stateRefTraj(K + 1, x0full);
+ InvertedPendulumNLOC::FeedbackArray fbTrajectory(K, -fbD);
+ InvertedPendulumNLOC::ControlVectorArray ffTrajectory(K, InvertedPendulumNLOC::ControlVector::Zero());
+
+ int initType = 0;
+ ct::core::loadScalar(configFile, "initType", initType);
+
+ switch (initType)
+ {
+ case 0: // steady state
+ {
+ ct::core::ControlVector<IPSystem::CONTROL_DIM> uff_ref;
+ nloc_solver.initializeSteadyPose(x0, timeHorizon, K, uff_ref, -fbD);
+
+ std::vector<
+ std::shared_ptr<ct::optcon::CostFunctionQuadratic<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM>>>&
+ inst1 = nloc_solver.getSolver()->getCostFunctionInstances();
+
+ for (size_t i = 0; i < inst1.size(); i++)
+ {
+ inst1[i]->getIntermediateTermById(intTermID)->updateReferenceControl(uff_ref);
+ }
+ break;
+ }
+ case 1: // linear interpolation
+ {
+ nloc_solver.initializeDirectInterpolation(x0, xf, timeHorizon, K, -fbD);
+ break;
+ }
+ default:
+ {
+ throw std::runtime_error("illegal init type");
+ break;
+ }
+ }
+
+ std::cout << "waiting 1 second for begin" << std::endl;
+ std::this_thread::sleep_for(std::chrono::seconds(1));
+
+ nloc_solver.solve();
+ ct::core::StateFeedbackController<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM> initialSolution =
+ nloc_solver.getSolution();
+ InvertedPendulumNLOC::StateVectorArray x_nloc = initialSolution.x_ref();
+
+ // nloc_solver.retrieveLastRollout();
+
+ ct::optcon::NLOptConSettings ilqr_settings_mpc(nloc_solver.getSettings());
+ ilqr_settings_mpc.max_iterations = 1;
+ ilqr_settings_mpc.printSummary = false;
+
+ ct::optcon::mpc_settings mpc_settings;
+ mpc_settings.stateForwardIntegration_ = false;
+ mpc_settings.postTruncation_ = false;
+ mpc_settings.measureDelay_ = false;
+ mpc_settings.delayMeasurementMultiplier_ = 1.0;
+ mpc_settings.mpc_mode = ct::optcon::MPC_MODE::CONSTANT_RECEDING_HORIZON;
+ mpc_settings.coldStart_ = false;
+ mpc_settings.minimumTimeHorizonMpc_ = 3.0;
+
+ ct::optcon::MPC<ct::optcon::NLOptConSolver<IPSystem::STATE_DIM, IPSystem::CONTROL_DIM>> ilqr_mpc(
+ optConProblem, ilqr_settings_mpc, mpc_settings);
+ ilqr_mpc.setInitialGuess(initialSolution);
+
+ MPCSimulator mpc_sim(1e-3, 1e-2, x0full, ipSystem, ilqr_mpc);
+ std::cout << "simulating 3 seconds" << std::endl;
+ mpc_sim.simulate(3);
+ mpc_sim.finish();
+
+ ilqr_mpc.printMpcSummary();
+
+ } catch (std::runtime_error& e)
+ {
+ std::cout << "Exception caught: " << e.what() << std::endl;
+ }
+}
diff --git a/ct_models/examples/InvertedPendulum/cost.info b/ct_models/examples/InvertedPendulum/cost.info
new file mode 100644
index 0000000..df9b66c
--- /dev/null
+++ b/ct_models/examples/InvertedPendulum/cost.info
@@ -0,0 +1,114 @@
+term0
+{
+ name "intermediate cost"
+ kind "quadratic"
+ type 0 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ ; state weighting
+ Q
+ {
+ scaling 10
+
+ ; joint position
+ (0,0) 1.0
+
+ ; joint velocity
+ (1,1) 1.0
+
+ ; motor position
+ (2,2) 0.0
+ }
+
+ R
+ {
+ scaling 1
+
+ ; motor velocity
+
+ (0,0) 0.1
+ }
+
+ x_des
+ {
+ ; joint position
+ (0,0) 0.0
+
+ ; joint velocity
+ (1,0) 0.0
+
+ ; motor position
+ (2,0) 0.0
+ }
+ }
+}
+
+
+term1
+{
+ name "final cost"
+ kind "quadratic"
+ type 1 ; 0 = intermediate, 1 = final
+
+ weights
+ {
+ ; state weighting
+ Q
+ {
+ scaling 10
+
+ ; joint position
+ (0,0) 1.0
+
+ ; joint velocity
+ (1,1) 1.0
+
+ ; motor position
+ (2,2) 0.0
+ }
+
+ x_des
+ {
+ ; joint position
+ (0,0) 0.0
+
+ ; joint velocity
+ (1,0) 0.0
+
+ ; motor pos
+ (2,0) 0.0
+ }
+ }
+}
+
+
+; initial position
+x_0
+{
+ ; joint position
+ (0,0) -3.14
+
+ ; joint velocity
+ (1,0) 0.0
+
+ ; actuator position
+ (2,0) 0.0
+}
+
+
+K_init
+{
+ scaling 0.0
+
+ ; joint position
+ (0,0) 0.0
+
+ ; joint velocity
+ (1,0) 0.0
+}
+
+
+
+
+
diff --git a/ct_models/examples/InvertedPendulum/solver.info b/ct_models/examples/InvertedPendulum/solver.info
new file mode 100644
index 0000000..96d01ca
--- /dev/null
+++ b/ct_models/examples/InvertedPendulum/solver.info
@@ -0,0 +1,40 @@
+timeHorizon 3.0
+
+initType 0 ; 0 = steady state ID, 1 = linear, 2 = integrated with zero control action, 3 = random; 4=zero
+
+ilqr
+{
+ integrator RK4
+ useSensitivityIntegrator false
+ discretization Forward_euler
+ timeVaryingDiscretization false
+ dt 0.005
+ K_sim 1
+ K_shot 1
+ epsilon 0e-6
+ max_iterations 50
+ fixedHessianCorrection false
+ recordSmallestEigenvalue false
+ min_cost_improvement 1e-7
+ meritFunctionRho 0.0
+ maxDefectSum 1e-5
+ nThreads 1
+ nThreadsEigen 1
+ ;locp_solver HPIPM_SOLVER
+ locp_solver GNRICCATI_SOLVER
+ nlocp_algorithm GNMS
+ closedLoopShooting true
+ printSummary true
+ debugPrint false
+
+
+ line_search
+ {
+ active false ;
+ adaptive false ;
+ maxIterations 10 ;
+ alpha_0 1.0 ;
+ n_alpha 0.5 ;
+ debugPrint false
+ }
+}