Make CT build and add a double jointed arm optimization.
Add an arm move simulation which needs to avoid a box. It's a
starting point for future work.
Change-Id: I1d84a7749376d853acf72c9fb7b9a43af7caabfa
diff --git a/third_party/blasfeo/.gitignore b/third_party/blasfeo/.gitignore
index bd23910..1c36c52 100644
--- a/third_party/blasfeo/.gitignore
+++ b/third_party/blasfeo/.gitignore
@@ -2,7 +2,6 @@
*.s
*.o
*.out
-include/blasfeo_target.h
libblasfeo.a
libblasfeo.so
octave-workspace
diff --git a/third_party/blasfeo/BUILD b/third_party/blasfeo/BUILD
index 92be2d4..ba61061 100644
--- a/third_party/blasfeo/BUILD
+++ b/third_party/blasfeo/BUILD
@@ -13,6 +13,9 @@
"include/blasfeo_d_blas.h",
"include/blasfeo_m_aux.h",
"include/blasfeo_s_blas.h",
+ "include/blasfeo_v_aux_ext_dep.h",
+ "include/blasfeo_d_aux_ext_dep.h",
+ "include/blasfeo_i_aux_ext_dep.h",
],
includes = [
"include",
diff --git a/third_party/ct/BUILD b/third_party/ct/BUILD
new file mode 100644
index 0000000..6455466
--- /dev/null
+++ b/third_party/ct/BUILD
@@ -0,0 +1,277 @@
+licenses(["notice"]) # lgpl
+
+cc_library(
+ name = "ct",
+ includes = [
+ "ct_optcon/include",
+ "ct_core/include",
+ "ct_core/include/external",
+ ],
+ hdrs = [
+ "ct_optcon/include/ct/optcon/optcon.h",
+ "ct_core/include/ct/core/core.h",
+ "ct_core/include/ct/core/internal/autodiff/CppadParallel.h",
+ "ct_core/include/ct/core/Common",
+ "ct_core/include/ct/core/Common-impl",
+ "ct_core/include/ct/core/common/GaussianNoise.h",
+ "ct_core/include/ct/core/common/QuantizationNoise.h",
+ "ct_core/include/ct/core/common/InfoFileParser.h",
+ "ct_core/include/ct/core/common/Timer.h",
+ "ct_core/include/ct/core/common/ExternallyDrivenTimer.h",
+ "ct_core/include/ct/core/common/Interpolation.h",
+ "ct_core/include/ct/core/types/arrays/DiscreteArray.h",
+ "ct_core/include/ct/core/types/arrays/TimeArray.h",
+ "ct_core/include/ct/core/types/Time.h",
+ "ct_core/include/ct/core/common/linspace.h",
+ "ct_core/include/ct/core/types/arrays/ScalarArray.h",
+ "ct_core/include/ct/core/common/activations/Activations.h",
+ "ct_core/include/ct/core/common/activations/ActivationBase.hpp",
+ "ct_core/include/ct/core/common/activations/PeriodicActivation.hpp",
+ "ct_core/include/ct/core/internal/traits/TraitSelectorSpecs.h",
+ "ct_core/include/ct/core/internal/traits/CppADCodegenTrait.h",
+ "ct_core/include/ct/core/internal/traits/CppADDoubleTrait.h",
+ "ct_core/include/ct/core/internal/traits/TraitSelector.h",
+ "ct_core/include/ct/core/internal/traits/FloatTrait.h",
+ "ct_core/include/ct/core/internal/traits/DoubleTrait.h",
+ "ct_core/include/ct/core/common/activations/RBFGaussActivation.h",
+ "ct_core/include/ct/core/common/activations/SingleActivation.hpp",
+ "ct_core/include/ct/core/common/activations/BarrierActivation.hpp",
+ "ct_core/include/ct/core/common/activations/utilities/ActivationLoadMacros.h",
+ "ct_core/include/ct/core/Types",
+ "ct_core/include/ct/core/Types-impl",
+ "ct_core/include/ct/core/types/AutoDiff.h",
+ "ct_core/include/ct/core/types/ControlVector.h",
+ "ct_core/include/ct/core/types/StateVector.h",
+ "ct_core/include/ct/core/types/FeedbackMatrix.h",
+ "ct_core/include/ct/core/types/StateMatrix.h",
+ "ct_core/include/ct/core/types/ControlMatrix.h",
+ "ct_core/include/ct/core/types/StateControlMatrix.h",
+ "ct_core/include/ct/core/types/arrays/MatrixArrays.h",
+ "ct_core/include/ct/core/types/trajectories/DiscreteTrajectoryBase.h",
+ "ct_core/include/ct/core/types/trajectories/TrajectoryBase.h",
+ "ct_core/include/ct/core/types/trajectories/ScalarTrajectory.h",
+ "ct_core/include/ct/core/types/trajectories/MatrixTrajectories.h",
+ "ct_core/include/ct/core/Control",
+ "ct_core/include/ct/core/Control-impl",
+ "ct_core/include/ct/core/control/continuous_time/Controller.h",
+ "ct_core/include/ct/core/control/continuous_time/StateFeedbackController.h",
+ "ct_core/include/ct/core/control/continuous_time/ConstantController.h",
+ "ct_core/include/ct/core/control/continuous_time/ConstantStateFeedbackController.h",
+ "ct_core/include/ct/core/control/continuous_time/ConstantTrajectoryController.h",
+ "ct_core/include/ct/core/control/continuous_time/siso/PIDController.h",
+ "ct_core/include/ct/core/control/continuous_time/siso/PIDController-impl.h",
+ "ct_core/include/ct/core/control/continuous_time/siso/StepInputController.h",
+ "ct_core/include/ct/core/control/continuous_time/mimo/LinearFunctions.h",
+ "ct_core/include/ct/core/control/discrete_time/DiscreteController.h",
+ "ct_core/include/ct/core/Systems",
+ "ct_core/include/ct/core/Systems-impl",
+ "ct_core/include/ct/core/systems/continuous_time/System.h",
+ "ct_core/include/ct/core/systems/continuous_time/ControlledSystem.h",
+ "ct_core/include/ct/core/systems/continuous_time/SecondOrderSystem.h",
+ "ct_core/include/ct/core/systems/continuous_time/linear/LinearSystem.h",
+ "ct_core/include/ct/core/systems/continuous_time/linear/LTISystem.h",
+ "ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerNumDiff.h",
+ "ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerAD.h",
+ "ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerADBase.h",
+ "ct_core/include/ct/core/internal/autodiff/SparsityPattern.h",
+ "ct_core/include/ct/core/systems/linearizer/DynamicsLinearizerADCG.h",
+ "ct_core/include/ct/core/internal/autodiff/CGHelpers.h",
+ "ct_core/include/ct/core/systems/continuous_time/linear/SystemLinearizer.h",
+ "ct_core/include/ct/core/systems/continuous_time/linear/AutoDiffLinearizer.h",
+ "ct_core/include/ct/core/systems/continuous_time/linear/ADCodegenLinearizer.h",
+ "ct_core/include/ct/core/templateDir.h", # TODO(austin): Looks like this could be non-hermetic.
+ "ct_core/include/ct/core/systems/discrete_time/DiscreteControlledSystem.h",
+ "ct_core/include/ct/core/systems/discrete_time/DiscreteSystem.h",
+ "ct_core/include/ct/core/systems/discrete_time/SystemDiscretizer.h",
+ "ct_core/include/ct/core/integration/Integrator.h",
+ "ct_core/include/ct/core/integration/EventHandler.h",
+ "ct_core/include/ct/core/integration/Observer.h",
+ "ct_core/include/ct/core/integration/eigenIntegration.h",
+ "ct_core/include/ct/core/integration/internal/StepperBase.h",
+ "ct_core/include/ct/core/integration/internal/SteppersODEInt.h",
+ "ct_core/include/ct/core/integration/internal/SteppersODEIntDefinitions.h",
+ "ct_core/include/ct/core/integration/internal/SteppersCT.h",
+ "ct_core/include/ct/core/integration/IntegratorSymplectic.h",
+ "ct_core/include/ct/core/systems/continuous_time/SymplecticSystem.h",
+ "ct_core/include/ct/core/integration/EventHandlers/SubstepRecorder.h",
+ "ct_core/include/ct/core/systems/discrete_time/linear/DiscreteLinearSystem.h",
+ "ct_core/include/ct/core/systems/discrete_time/linear/DiscreteSystemLinearizer.h",
+ "ct_core/include/ct/core/systems/discrete_time/linear/DiscreteSystemLinearizerAD.h",
+ "ct_core/include/ct/core/systems/discrete_time/linear/DiscreteSystemLinearizerADCG.h",
+ "ct_core/include/ct/core/Integration",
+ "ct_core/include/ct/core/Integration-impl",
+ "ct_core/include/ct/core/integration/EventHandlers/KillIntegrationEventHandler.h",
+ "ct_core/include/ct/core/integration/EventHandlers/MaxStepsEventHandler.h",
+ "ct_core/include/ct/core/integration/sensitivity/Sensitivity.h",
+ "ct_core/include/ct/core/integration/sensitivity/SensitivityApproximation.h",
+ "ct_core/include/ct/core/integration/sensitivity/SensitivityIntegrator.h",
+ "ct_core/include/ct/core/Geometry",
+ "ct_core/include/ct/core/Geometry-impl",
+ "ct_core/include/ct/core/geometry/Ellipsoid.h",
+ "ct_core/include/ct/core/geometry/Plane.h",
+ "ct_core/include/ct/core/geometry/PlaneEstimator.h",
+ "ct_core/include/ct/core/Internal",
+ "ct_core/include/ct/core/Internal-impl",
+ "ct_core/include/ct/core/internal/autodiff/ADHelpers.h",
+ "ct_core/include/ct/core/Math",
+ "ct_core/include/ct/core/Math-impl",
+ "ct_core/include/ct/core/math/Derivatives.h",
+ "ct_core/include/ct/core/math/DerivativesCppadSettings.h",
+ "ct_core/include/ct/core/math/DerivativesNumDiff.h",
+ "ct_core/include/ct/core/math/DerivativesCppad.h",
+ "ct_core/include/ct/core/math/DerivativesCppadJIT.h",
+ "ct_core/include/ct/core/math/DerivativesCppadCG.h",
+ "ct_core/include/ct/core/Simulation",
+ "ct_core/include/ct/core/Simulation-impl",
+ "ct_core/include/ct/core/simulation/ControlSimulator.h",
+ "ct_core/include/ct/core/control/continuous_time/StateFeedbackController-impl.h",
+ "ct_core/include/ct/core/control/continuous_time/ConstantController-impl.h",
+ "ct_core/include/ct/core/systems/discrete_time/SystemDiscretizer-impl.h",
+ "ct_core/include/ct/core/integration/Observer-impl.h",
+ "ct_core/include/ct/core/integration/Integrator-impl.h",
+ "ct_core/include/ct/core/integration/IntegratorSymplectic-impl.h",
+ "ct_optcon/include/ct/optcon/costfunction/costfunction.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermBase.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermLinear.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermMixed.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunction.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadratic-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunctionAD.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunctionAD-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/utility/utilities.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermLoadMacros.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunctionQuadraticSimple-impl.hpp",
+ "ct_optcon/include/ct/optcon/constraint/constraint.h",
+ "ct_optcon/include/ct/optcon/constraint/term/ConstraintBase.h",
+ "ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase.h",
+ "ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint.h",
+ "ct_optcon/include/ct/optcon/constraint/term/ControlInputConstraint-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint.h",
+ "ct_optcon/include/ct/optcon/constraint/term/ObstacleConstraint-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/term/StateConstraint.h",
+ "ct_optcon/include/ct/optcon/constraint/term/StateConstraint-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint.h",
+ "ct_optcon/include/ct/optcon/constraint/term/TerminalConstraint-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase.h",
+ "ct_optcon/include/ct/optcon/constraint/ConstraintContainerBase-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer.h",
+ "ct_optcon/include/ct/optcon/constraint/LinearConstraintContainer-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD.h",
+ "ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical.h",
+ "ct_optcon/include/ct/optcon/constraint/ConstraintContainerAnalytical-impl.h",
+ "ct_optcon/include/ct/optcon/problem/OptConProblem.h",
+ "ct_optcon/include/ct/optcon/problem/OptConProblem-impl.h",
+ "ct_optcon/include/ct/optcon/problem/LQOCProblem.hpp",
+ "ct_optcon/include/ct/optcon/solver/OptConSolver.h",
+ "ct_optcon/include/ct/optcon/nloc/NLOCBackendBase.hpp",
+ "ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver.hpp",
+ "ct_optcon/include/ct/optcon/solver/lqp/LQOCSolver.hpp",
+ "ct_optcon/include/ct/optcon/solver/NLOptConSettings.hpp",
+ "ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface.hpp",
+ "ct_optcon/include/ct/optcon/nloc/NLOCResults.hpp",
+ "ct_optcon/include/ct/optcon/nloc/NLOCBackendST.hpp",
+ "ct_optcon/include/ct/optcon/nloc/NLOCBackendMP.hpp",
+ "ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS.hpp",
+ "ct_optcon/include/ct/optcon/nloc/NLOCAlgorithm.hpp",
+ "ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR.hpp",
+ "ct_optcon/include/ct/optcon/solver/NLOptConSolver.hpp",
+ "ct_optcon/include/ct/optcon/lqr/riccati/CARE.hpp",
+ "ct_optcon/include/ct/optcon/lqr/riccati/DARE.hpp",
+ "ct_optcon/include/ct/optcon/lqr/riccati/DynamicRiccatiEquation.hpp",
+ "ct_optcon/include/ct/optcon/lqr/FHDTLQR.hpp",
+ "ct_optcon/include/ct/optcon/lqr/LQR.hpp",
+ "ct_optcon/include/ct/optcon/dms/dms.h",
+ "ct_optcon/include/ct/optcon/dms/Constraints",
+ "ct_optcon/include/ct/optcon/dms/constraints/ConstraintDiscretizer.h",
+ "ct_optcon/include/ct/optcon/nlp/DiscreteConstraintBase.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/OptVectorDms.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/TimeGrid.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/DmsDimensions.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/spline/SplinerBase.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/spline/ZeroOrderHold/ZeroOrderHoldSpliner.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/spline/Linear/LinearSpliner.h",
+ "ct_optcon/include/ct/optcon/nlp/OptVector.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/DmsSettings.h",
+ "ct_optcon/include/ct/optcon/nlp/solver/NlpSolverSettings.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/implementation/OptVectorDms-impl.h",
+ "ct_optcon/include/ct/optcon/dms/constraints/ConstraintsContainerDms.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/ShotContainer.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/SensitivityIntegratorCT.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/ControllerDms.h",
+ "ct_optcon/include/ct/optcon/nlp/DiscreteConstraintContainerBase.h",
+ "ct_optcon/include/ct/optcon/dms/constraints/InitStateConstraint.h",
+ "ct_optcon/include/ct/optcon/dms/constraints/ContinuityConstraint.h",
+ "ct_optcon/include/ct/optcon/dms/constraints/TimeHorizonEqualityConstraint.h",
+ "ct_optcon/include/ct/optcon/dms/constraints/implementation/ConstraintsContainerDms-impl.h",
+ "ct_optcon/include/ct/optcon/dms/Dms",
+ "ct_optcon/include/ct/optcon/dms/dms_core/DmsSolver.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/DmsProblem.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h",
+ "ct_optcon/include/ct/optcon/nlp/DiscreteCostEvaluatorBase.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h",
+ "ct_optcon/include/ct/optcon/nlp/Nlp.h",
+ "ct_optcon/include/ct/optcon/nlp/Nlp",
+ "ct_optcon/include/ct/optcon/nlp/solver/NlpSolver.h",
+ "ct_optcon/include/ct/optcon/nlp/solver/IpoptSolver.h",
+ "ct_optcon/include/ct/optcon/nlp/solver/SnoptSolver.h",
+ "ct_optcon/include/ct/optcon/dms/dms_core/RKnDerivatives.h",
+ "ct_optcon/include/ct/optcon/mpc/MpcSettings.h",
+ "ct_optcon/include/ct/optcon/mpc/MPC.h",
+ "ct_optcon/include/ct/optcon/mpc/MpcTimeKeeper.h",
+ "ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon.h",
+ "ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler.h",
+ "ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler.h",
+ "ct_optcon/include/ct/optcon/costfunction/costfunction-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermBase-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermLinear-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermMixed-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermQuadratic-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermQuadMult-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermQuadTracking-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/term/TermStateBarrier-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunction-impl.hpp",
+ "ct_optcon/include/ct/optcon/costfunction/CostFunctionAnalytical-impl.hpp",
+ "ct_optcon/include/ct/optcon/constraint/constraint-impl.h",
+ "ct_optcon/include/ct/optcon/constraint/term/ConstraintBase-impl.h",
+ "ct_optcon/include/ct/optcon/problem/LQOCProblem-impl.hpp",
+ "ct_optcon/include/ct/optcon/solver/lqp/GNRiccatiSolver-impl.hpp",
+ "ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp",
+ "ct_optcon/include/ct/optcon/solver/NLOptConSolver-impl.hpp",
+ "ct_optcon/include/ct/optcon/lqr/riccati/CARE-impl.hpp",
+ "ct_optcon/include/ct/optcon/lqr/riccati/DARE-impl.hpp",
+ "ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp",
+ "ct_optcon/include/ct/optcon/lqr/LQR-impl.hpp",
+ "ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp",
+ "ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp",
+ "ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp",
+ "ct_optcon/include/ct/optcon/nloc/algorithms/gnms/GNMS-impl.hpp",
+ "ct_optcon/include/ct/optcon/nloc/algorithms/ilqr/iLQR-impl.hpp",
+ "ct_optcon/include/ct/optcon/mpc/MPC-impl.h",
+ "ct_optcon/include/ct/optcon/mpc/timehorizon/MpcTimeHorizon-impl.h",
+ "ct_optcon/include/ct/optcon/mpc/policyhandler/PolicyHandler-impl.h",
+ "ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h",
+ ] + glob(["ct_core/include/external/cppad/**/*.hpp"]),
+ deps = [
+ "//third_party/hpipm",
+ "//third_party/eigen",
+ ],
+ copts = [
+ "-Wno-pointer-arith",
+ "-Wno-unused-variable",
+ "-Wno-unused-parameter",
+ ],
+ defines = [
+ "HPIPM=1",
+ ],
+ visibility = ["//visibility:public"],
+ restricted_to = ["//tools:k8"],
+)
diff --git a/third_party/ct/ct_core/include/ct/core/math/DerivativesCppadJIT.h b/third_party/ct/ct_core/include/ct/core/math/DerivativesCppadJIT.h
index 639f218..70d8f16 100644
--- a/third_party/ct/ct_core/include/ct/core/math/DerivativesCppadJIT.h
+++ b/third_party/ct/ct_core/include/ct/core/math/DerivativesCppadJIT.h
@@ -266,8 +266,8 @@
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> sparsityMat(outputDim_, inputDim_);
assert((int)(sparsityVec.size()) == outputDim_ * inputDim_);
- for (size_t row = 0; row < outputDim_; ++row)
- for (size_t col = 0; col < inputDim_; ++col)
+ for (int row = 0; row < outputDim_; ++row)
+ for (int col = 0; col < inputDim_; ++col)
sparsityMat(row, col) = sparsityVec[col + row * inputDim_];
return sparsityMat;
@@ -286,8 +286,8 @@
Eigen::Matrix<bool, Eigen::Dynamic, Eigen::Dynamic> sparsityMat(inputDim_, inputDim_);
assert(sparsityVec.size() == inputDim_ * inputDim_);
- for (size_t row = 0; row < inputDim_; ++row)
- for (size_t col = 0; col < inputDim_; ++col)
+ for (int row = 0; row < inputDim_; ++row)
+ for (int col = 0; col < inputDim_; ++col)
{
// std::cout << "sparsityVec: " << sparsityRowsHessian_[col + row * this->inputDim_] << std::endl;
sparsityMat(row, col) = sparsityVec[col + row * inputDim_];
diff --git a/third_party/ct/ct_core/include/ct/core/simulation/ControlSimulator.h b/third_party/ct/ct_core/include/ct/core/simulation/ControlSimulator.h
index d0d655d..66e159f 100644
--- a/third_party/ct/ct_core/include/ct/core/simulation/ControlSimulator.h
+++ b/third_party/ct/ct_core/include/ct/core/simulation/ControlSimulator.h
@@ -46,13 +46,13 @@
std::shared_ptr<CONTROLLED_SYSTEM> controlled_system)
: sim_dt_(sim_dt),
control_dt_(control_dt),
- x0_(x0),
system_(controlled_system),
+ x0_(x0),
stop_(false)
{
system_->getController(controller_);
- if (sim_dt_ <= 0 || control_dt_ <= 0) throw std::runtime_error("Step sizes must be positive.");
- if (sim_dt_ > control_dt_) throw std::runtime_error("Simulation step must be smaller than the control step.");
+ if (sim_dt_ <= 0 || control_dt_ <= 0) throw "Step sizes must be positive.";
+ if (sim_dt_ > control_dt_) throw "Simulation step must be smaller than the control step.";
}
//! copy constructor
diff --git a/third_party/ct/ct_core/include/ct/core/templateDir.h b/third_party/ct/ct_core/include/ct/core/templateDir.h
new file mode 100644
index 0000000..34b631b
--- /dev/null
+++ b/third_party/ct/ct_core/include/ct/core/templateDir.h
@@ -0,0 +1,17 @@
+/**********************************************************************************************************************
+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)
+**********************************************************************************************************************/
+
+#pragma once
+
+namespace ct {
+namespace core {
+
+static const std::string CODEGEN_TEMPLATE_DIR = "./third_party/ct/ct_core/templates";
+static const std::string CODEGEN_OUTPUT_DIR = "./third_party/ct/ct_core/generated";
+
+}
+}
+
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h b/third_party/ct/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h
index 3668114..2b5e8f3 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h
+++ b/third_party/ct/ct_optcon/include/ct/optcon/constraint/ConstraintContainerAD-impl.h
@@ -434,9 +434,9 @@
size_t stateIndex = 0;
size_t inputIndex = 0;
- for (size_t i = 0; i < sparsityRows.rows(); ++i)
+ for (size_t i = 0; i < static_cast<size_t>(sparsityRows.rows()); ++i)
{
- if (sparsityCols(i) < STATE_DIM)
+ if (sparsityCols(i) < static_cast<ssize_t>(STATE_DIM))
{
sparsityStateIntermediateRows_(stateIndex) = sparsityRows(i);
sparsityStateIntermediateCols_(stateIndex) = sparsityCols(i);
@@ -499,9 +499,9 @@
size_t stateIndex = 0;
size_t inputIndex = 0;
- for (size_t i = 0; i < sparsityRows.rows(); ++i)
+ for (int i = 0; i < sparsityRows.rows(); ++i)
{
- if (sparsityCols(i) < STATE_DIM)
+ if (sparsityCols(i) < static_cast<int>(STATE_DIM))
{
sparsityStateTerminalRows_(stateIndex) = sparsityRows(i);
sparsityStateTerminalCols_(stateIndex) = sparsityCols(i);
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h b/third_party/ct/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h
index c143490..5b45748 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h
+++ b/third_party/ct/ct_optcon/include/ct/optcon/constraint/term/BoxConstraintBase-impl.h
@@ -97,7 +97,7 @@
const VectorXs& ub) const
{
// assert that the size of constraint vectors is equal to the computed/given number of constraints
- if (lb.rows() != nCon | ub.rows() != nCon)
+ if (lb.rows() != static_cast<ssize_t>(nCon) || ub.rows() != static_cast<ssize_t>(nCon))
{
std::cout << "no. Constraints: " << nCon << std::endl;
std::cout << "BoxConstraintBase: lb " << lb.transpose() << std::endl;
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h b/third_party/ct/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h
index 43d2a76..6a124a8 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h
+++ b/third_party/ct/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorFull.h
@@ -6,7 +6,7 @@
#pragma once
-#include <omp.h>
+//#include <omp.h>
#include <math.h>
#include <cmath>
#include <functional>
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h b/third_party/ct/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h
index d2b32f7..d7e70cf 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h
+++ b/third_party/ct/ct_optcon/include/ct/optcon/dms/dms_core/cost_evaluator/CostEvaluatorSimple.h
@@ -6,7 +6,7 @@
#pragma once
-#include <omp.h>
+//#include <omp.h>
#include <math.h>
#include <cmath>
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp b/third_party/ct/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp
index b098be2..c547c69 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp
+++ b/third_party/ct/ct_optcon/include/ct/optcon/lqr/FHDTLQR-impl.hpp
@@ -64,7 +64,7 @@
A.resize(N);
B.resize(N);
- for (int i = 0; i < N; i++)
+ for (size_t i = 0; i < N; i++)
{
A[i] = state_matrix_t::Identity() + dt * derivatives->getDerivativeState(x_trajectory[i], u_trajectory[i]);
B[i] = dt * derivatives->getDerivativeControl(x_trajectory[i], u_trajectory[i]);
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h b/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
index 81adecd..54742eb 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
+++ b/third_party/ct/ct_optcon/include/ct/optcon/mpc/MPC-impl.h
@@ -18,12 +18,12 @@
:
solver_(problem, solverSettings),
+ policyHandler_(new PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>()),
mpc_settings_(mpcsettings),
+ firstRun_(true),
dynamics_(problem.getNonlinearSystem()->clone()),
forwardIntegrator_(dynamics_, mpcsettings.stateForwardIntegratorType_),
- firstRun_(true),
- runCallCounter_(0),
- policyHandler_(new PolicyHandler<Policy_t, STATE_DIM, CONTROL_DIM, Scalar_t>())
+ runCallCounter_(0)
{
checkSettings(mpcsettings);
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h b/third_party/ct/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h
index 4eaa32b..8a3bc61 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h
+++ b/third_party/ct/ct_optcon/include/ct/optcon/mpc/policyhandler/default/StateFeedbackPolicyHandler-impl.h
@@ -64,7 +64,7 @@
{
//extend at back with constant value taken from last element
bool timeIsRelative = true;
- for (size_t i = 0; i < Kn_new - currentSize; i++)
+ for (int i = 0; i < Kn_new - currentSize; i++)
{
FeedbackTraj.push_back(FeedbackTraj.back(), dt_, timeIsRelative);
FeedForwardTraj.push_back(FeedForwardTraj.back(), dt_, timeIsRelative);
@@ -74,7 +74,7 @@
else if (Kn_new < currentSize)
{
// remove elements from back
- for (size_t i = 0; i < currentSize - Kn_new; i++)
+ for (int i = 0; i < currentSize - Kn_new; i++)
{
FeedbackTraj.pop_back();
FeedForwardTraj.pop_back();
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp b/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
index d68bcc9..9c79dd5 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
+++ b/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendBase-impl.hpp
@@ -433,6 +433,7 @@
std::shared_ptr<HPIPMInterface<STATE_DIM, CONTROL_DIM>>(new HPIPMInterface<STATE_DIM, CONTROL_DIM>());
#else
throw std::runtime_error("HPIPM selected but not built.");
+ #error
#endif
}
else
@@ -1169,8 +1170,9 @@
}
else
{
-#ifdef MATLAB // in case of no debug printing but still logging, need to compute them \
- //! compute l2 norms of state and control update
+#ifdef MATLAB
+ // in case of no debug printing but still logging, need to compute them
+ //! compute l2 norms of state and control update
lu_norm_ = computeDiscreteArrayNorm<ControlVectorArray, 2>(u_ff_prev_, uff_local);
lx_norm_ = computeDiscreteArrayNorm<StateVectorArray, 2>(x_prev_, x_);
#endif
@@ -1481,7 +1483,7 @@
lqocSolver_->setProblem(lqocProblem_);
//iterate backward up to first stage
- for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= startIndex; i--)
+ for (int i = this->lqocProblem_->getNumberOfStages() - 1; i >= static_cast<int>(startIndex); i--)
lqocSolver_->solveSingleStage(i);
}
else
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp b/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp
index d9f6c71..9a95591 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp
+++ b/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendMP-impl.hpp
@@ -214,7 +214,7 @@
size_t lastIndex)
{
// fill terminal cost
- if (lastIndex == (this->K_ - 1))
+ if (static_cast<int>(lastIndex) == (this->K_ - 1))
this->initializeCostToGo();
/*
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp b/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp
index bc0eaab..96a4fc8 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp
+++ b/third_party/ct/ct_optcon/include/ct/optcon/nloc/NLOCBackendST-impl.hpp
@@ -38,7 +38,7 @@
void NLOCBackendST<STATE_DIM, CONTROL_DIM, P_DIM, V_DIM, SCALAR>::computeLQApproximation(size_t firstIndex,
size_t lastIndex)
{
- if (lastIndex == this->K_ - 1)
+ if (static_cast<int>(lastIndex) == this->K_ - 1)
this->initializeCostToGo();
for (size_t k = firstIndex; k <= lastIndex; k++)
diff --git a/third_party/ct/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp b/third_party/ct/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
index 01255c0..ddc2d10 100644
--- a/third_party/ct/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
+++ b/third_party/ct/ct_optcon/include/ct/optcon/solver/lqp/HPIPMInterface-impl.hpp
@@ -375,7 +375,7 @@
std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
{
// stages 1 to N
- for (size_t i = 0; i < N_ + 1; i++)
+ for (int i = 0; i < N_ + 1; i++)
{
nb_[i] = lqocProblem->nb_[i];
@@ -411,7 +411,7 @@
void HPIPMInterface<STATE_DIM, CONTROL_DIM>::configureGeneralConstraints(
std::shared_ptr<LQOCProblem<STATE_DIM, CONTROL_DIM>> lqocProblem)
{
- for (size_t i = 0; i < N_ + 1; i++)
+ for (int i = 0; i < N_ + 1; i++)
{
// check dimensions
assert(lqocProblem->d_lb_[i].rows() == lqocProblem->d_ub_[i].rows());
diff --git a/third_party/matplotlib-cpp/BUILD b/third_party/matplotlib-cpp/BUILD
index 9d21f21..8eb8734 100644
--- a/third_party/matplotlib-cpp/BUILD
+++ b/third_party/matplotlib-cpp/BUILD
@@ -8,6 +8,7 @@
deps = [
"@usr_repo//:python2.7_lib",
],
+ visibility = ["//visibility:public"],
restricted_to = ["//tools:k8"],
)
diff --git a/y2018/control_loops/python/BUILD b/y2018/control_loops/python/BUILD
index 042c39e..a72c464 100644
--- a/y2018/control_loops/python/BUILD
+++ b/y2018/control_loops/python/BUILD
@@ -69,3 +69,16 @@
],
restricted_to = ['//tools:k8'],
)
+
+cc_binary(
+ name = "arm_mpc",
+ srcs = [
+ "arm_mpc.cc",
+ ],
+ deps = [
+ "//third_party/ct",
+ "//third_party/matplotlib-cpp",
+ "//third_party/gflags",
+ ],
+ restricted_to = ["//tools:k8"],
+)
diff --git a/y2018/control_loops/python/arm_mpc.cc b/y2018/control_loops/python/arm_mpc.cc
new file mode 100644
index 0000000..2727545
--- /dev/null
+++ b/y2018/control_loops/python/arm_mpc.cc
@@ -0,0 +1,454 @@
+#include <chrono>
+#include <cmath>
+#include <thread>
+
+#include <ct/optcon/optcon.h>
+
+#include "third_party/gflags/include/gflags/gflags.h"
+#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+
+DEFINE_double(boundary_scalar, 12.0, "Test command-line flag");
+DEFINE_double(boundary_rate, 25.0, "Sigmoid rate");
+DEFINE_bool(sigmoid, true, "If true, sigmoid, else exponential.");
+DEFINE_double(round_corner, 0.0, "Corner radius of the constraint box.");
+
+// This code is for analysis and simulation of a double jointed arm. It is an
+// attempt to see if a MPC could work for arm control under constraints.
+
+// Describes a double jointed arm.
+// A large chunk of this code comes from demos. Most of the raw pointer,
+// shared_ptr, and non-const &'s come from the library's conventions.
+template <typename SCALAR>
+class MySecondOrderSystem : public ::ct::core::ControlledSystem<4, 2, SCALAR> {
+ public:
+ static const size_t STATE_DIM = 4;
+ static const size_t CONTROL_DIM = 2;
+
+ MySecondOrderSystem(::std::shared_ptr<::ct::core::Controller<4, 2, SCALAR>>
+ controller = nullptr)
+ : ::ct::core::ControlledSystem<4, 2, SCALAR>(
+ controller, ::ct::core::SYSTEM_TYPE::GENERAL) {}
+
+ MySecondOrderSystem(const MySecondOrderSystem &arg)
+ : ::ct::core::ControlledSystem<4, 2, SCALAR>(arg) {}
+
+ // Deep copy
+ MySecondOrderSystem *clone() const override {
+ return new MySecondOrderSystem(*this);
+ }
+ virtual ~MySecondOrderSystem() {}
+
+ // Evaluate the system dynamics.
+ //
+ // Args:
+ // state: current state (position, velocity)
+ // t: current time (gets ignored)
+ // control: control action
+ // derivative: (velocity, acceleration)
+ virtual void computeControlledDynamics(
+ const ::ct::core::StateVector<4, SCALAR> &state, const SCALAR & /*t*/,
+ const ::ct::core::ControlVector<2, SCALAR> &control,
+ ::ct::core::StateVector<4, SCALAR> &derivative) override {
+ derivative(0) = state(1);
+ derivative(1) = control(0);
+ derivative(2) = state(3);
+ derivative(3) = control(1);
+ }
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double,
+ typename SCALAR = SCALAR_EVAL>
+class MyTermStateBarrier : public ::ct::optcon::TermBase<STATE_DIM, CONTROL_DIM,
+ SCALAR_EVAL, SCALAR> {
+ public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, 1> state_vector_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM> state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM> control_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
+ control_state_matrix_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, STATE_DIM, STATE_DIM>
+ state_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, CONTROL_DIM>
+ control_matrix_double_t;
+ typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
+ control_state_matrix_double_t;
+
+ MyTermStateBarrier() {}
+
+ MyTermStateBarrier(const MyTermStateBarrier & /*arg*/) {}
+
+ static constexpr double kEpsilon = 1.0e-7;
+
+ virtual ~MyTermStateBarrier() {}
+
+ MyTermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR> *clone()
+ const override {
+ return new MyTermStateBarrier(*this);
+ }
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/,
+ const SCALAR & /*t*/) override {
+ SCALAR min_distance;
+
+ // Round the corner by this amount.
+ SCALAR round_corner = SCALAR(FLAGS_round_corner);
+
+ // Positive means violation.
+ SCALAR theta0_distance = x(0, 0) - (0.5 + round_corner);
+ SCALAR theta1_distance = (0.8 - round_corner) - x(2, 0);
+ if (theta0_distance < SCALAR(0.0) && theta1_distance < SCALAR(0.0)) {
+ // Ok, both outside. Return corner distance.
+ min_distance = -hypot(theta1_distance, theta0_distance);
+ } else if (theta0_distance < SCALAR(0.0) && theta1_distance > SCALAR(0.0)) {
+ min_distance = theta0_distance;
+ } else if (theta0_distance > SCALAR(0.0) && theta1_distance < SCALAR(0.0)) {
+ min_distance = theta1_distance;
+ } else {
+ min_distance = ::std::min(theta0_distance, theta1_distance);
+ }
+ min_distance += round_corner;
+ if (FLAGS_sigmoid) {
+ return FLAGS_boundary_scalar /
+ (1.0 + ::std::exp(-min_distance * FLAGS_boundary_rate));
+ } else {
+ // Values of 4 and 15 work semi resonably.
+ return FLAGS_boundary_scalar *
+ ::std::exp(min_distance * FLAGS_boundary_rate);
+ }
+ }
+
+ ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(
+ const ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ::ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> result =
+ ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL>::Zero();
+
+ // Perturb x for both position axis and return the result.
+ for (size_t i = 0; i < STATE_DIM; i += 2) {
+ ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> plus_perterbed_x = x;
+ ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> minus_perterbed_x = x;
+ plus_perterbed_x[i] += epsilon;
+ minus_perterbed_x[i] -= epsilon;
+ result[i] = (evaluate(plus_perterbed_x, u, t) -
+ evaluate(minus_perterbed_x, u, t)) /
+ (epsilon * 2.0);
+ }
+ return result;
+ }
+
+ // Compute second order derivative of this cost term w.r.t. the state
+ state_matrix_t stateSecondDerivative(
+ const ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> &x,
+ const ::ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> &u,
+ const SCALAR_EVAL &t) override {
+ state_matrix_t result = state_matrix_t::Zero();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ // Perturb x a second time.
+ for (size_t i = 0; i < STATE_DIM; i += 2) {
+ ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> plus_perterbed_x = x;
+ ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> minus_perterbed_x = x;
+ plus_perterbed_x[i] += epsilon;
+ minus_perterbed_x[i] -= epsilon;
+ state_vector_t delta = (stateDerivative(plus_perterbed_x, u, t) -
+ stateDerivative(minus_perterbed_x, u, t)) /
+ (epsilon * 2.0);
+
+ result.col(i) = delta;
+ }
+ return result;
+ }
+
+ ::ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> controlDerivative(
+ const ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> & /*x*/,
+ const ::ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> & /*u*/,
+ const SCALAR_EVAL & /*t*/) override {
+ return ::ct::core::StateVector<CONTROL_DIM, SCALAR_EVAL>::Zero();
+ }
+
+ control_state_matrix_t stateControlDerivative(
+ const ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> & /*x*/,
+ const ::ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> & /*u*/,
+ const SCALAR_EVAL & /*t*/) override {
+ control_state_matrix_t result = control_state_matrix_t::Zero();
+
+ return result;
+ }
+
+ control_matrix_t controlSecondDerivative(
+ const ::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> & /*x*/,
+ const ::ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> & /*u*/,
+ const SCALAR_EVAL & /*t*/) override {
+ control_matrix_t result = control_matrix_t::Zero();
+ return result;
+ }
+
+ /*
+ // TODO(austin): Implement this for the automatic differentiation.
+ virtual ::ct::core::ADCGScalar evaluateCppadCg(
+ const ::ct::core::StateVector<STATE_DIM, ::ct::core::ADCGScalar> &x,
+ const ::ct::core::ControlVector<CONTROL_DIM, ::ct::core::ADCGScalar> &u,
+ ::ct::core::ADCGScalar t) override {
+ ::ct::core::ADCGScalar c = ::ct::core::ADCGScalar(0.0);
+ for (size_t i = 0; i < STATE_DIM; i++)
+ c += barriers_[i].computeActivation(x(i));
+ return c;
+ }
+ */
+};
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+ // PRELIMINIARIES, see example NLOC.cpp
+ constexpr size_t state_dim = MySecondOrderSystem<double>::STATE_DIM;
+ constexpr size_t control_dim = MySecondOrderSystem<double>::CONTROL_DIM;
+
+ ::std::shared_ptr<ct::core::ControlledSystem<state_dim, control_dim>>
+ oscillator_dynamics(new MySecondOrderSystem<double>());
+
+ ::std::shared_ptr<ct::core::SystemLinearizer<state_dim, control_dim>>
+ ad_linearizer(new ::ct::core::SystemLinearizer<state_dim, control_dim>(
+ oscillator_dynamics));
+
+ constexpr double kQPos = 0.5;
+ constexpr double kQVel = 1.65;
+ ::Eigen::Matrix<double, 4, 4> Q_step;
+ Q_step << 1.0 / (kQPos * kQPos), 0.0, 0.0, 0.0, 0.0, 1.0 / (kQVel * kQVel),
+ 0.0, 0.0, 0.0, 0.0, 1.0 / (kQPos * kQPos), 0.0, 0.0, 0.0, 0.0,
+ 1.0 / (kQVel * kQVel);
+ ::Eigen::Matrix<double, 2, 2> R_step;
+ R_step << 1.0 / (12.0 * 12.0), 0.0, 0.0, 1.0 / (12.0 * 12.0);
+ ::std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>>
+ intermediate_cost(new ::ct::optcon::TermQuadratic<state_dim, control_dim>(
+ Q_step, R_step));
+
+ // TODO(austin): DARE for these.
+ ::Eigen::Matrix<double, 4, 4> Q_final = Q_step;
+ ::Eigen::Matrix<double, 2, 2> R_final = R_step;
+ ::std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>>
+ final_cost(new ::ct::optcon::TermQuadratic<state_dim, control_dim>(
+ Q_final, R_final));
+
+ ::std::shared_ptr<ct::optcon::TermBase<state_dim, control_dim>> bounds_cost(
+ new MyTermStateBarrier<4, 2>());
+
+ // TODO(austin): Cost function needs constraints.
+ ::std::shared_ptr<::ct::optcon::CostFunctionQuadratic<state_dim, control_dim>>
+ cost_function(
+ new ::ct::optcon::CostFunctionAnalytical<state_dim, control_dim>());
+ cost_function->addIntermediateTerm(intermediate_cost);
+ cost_function->addIntermediateTerm(bounds_cost);
+ cost_function->addFinalTerm(final_cost);
+
+ // STEP 1-D: set up the box constraints for the control input
+ // input box constraint boundaries with sparsities in constraint toolbox
+ // format
+ Eigen::VectorXd u_lb(control_dim);
+ Eigen::VectorXd u_ub(control_dim);
+ u_ub.setConstant(12.0);
+ u_lb = -u_ub;
+ ::std::cout << "uub " << u_ub << ::std::endl;
+ ::std::cout << "ulb " << u_lb << ::std::endl;
+
+ // constraint terms
+ std::shared_ptr<::ct::optcon::ControlInputConstraint<state_dim, control_dim>>
+ controlConstraint(
+ new ::ct::optcon::ControlInputConstraint<state_dim, control_dim>(
+ u_lb, u_ub));
+ controlConstraint->setName("ControlInputConstraint");
+ // create constraint container
+ std::shared_ptr<
+ ::ct::optcon::ConstraintContainerAnalytical<state_dim, control_dim>>
+ box_constraints(
+ new ::ct::optcon::ConstraintContainerAnalytical<state_dim,
+ control_dim>());
+ // add and initialize constraint terms
+ box_constraints->addIntermediateConstraint(controlConstraint, true);
+ box_constraints->initialize();
+
+ // Starting point.
+ ::ct::core::StateVector<state_dim> x0;
+ x0 << 1.0, 0.0, 0.9, 0.0;
+
+ constexpr ::ct::core::Time kTimeHorizon = 1.5;
+ ::ct::optcon::OptConProblem<state_dim, control_dim> opt_con_problem(
+ kTimeHorizon, x0, oscillator_dynamics, cost_function, ad_linearizer);
+ ::ct::optcon::NLOptConSettings ilqr_settings;
+ ilqr_settings.dt = 0.00505; // the control discretization in [sec]
+ ilqr_settings.integrator = ::ct::core::IntegrationType::RK4;
+ ilqr_settings.discretization =
+ ::ct::optcon::NLOptConSettings::APPROXIMATION::FORWARD_EULER;
+ // ilqr_settings.discretization =
+ // NLOptConSettings::APPROXIMATION::MATRIX_EXPONENTIAL;
+ ilqr_settings.max_iterations = 20;
+ ilqr_settings.min_cost_improvement = 1.0e-11;
+ ilqr_settings.nlocp_algorithm =
+ ::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ // the LQ-problems are solved using a custom Gauss-Newton Riccati solver
+ // ilqr_settings.lqocp_solver =
+ // NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ ilqr_settings.lqocp_solver =
+ ::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER;
+ ilqr_settings.printSummary = true;
+ if (ilqr_settings.lqocp_solver ==
+ ::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER) {
+ opt_con_problem.setBoxConstraints(box_constraints);
+ }
+
+ size_t K = ilqr_settings.computeK(kTimeHorizon);
+ printf("Using %d steps\n", static_cast<int>(K));
+
+ // Vector of feeback matricies.
+ ::ct::core::FeedbackArray<state_dim, control_dim> u0_fb(
+ K, ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
+ ::ct::core::ControlVectorArray<control_dim> u0_ff(
+ K, ::ct::core::ControlVector<control_dim>::Zero());
+ ::ct::core::StateVectorArray<state_dim> x_ref_init(K + 1, x0);
+ ::ct::core::StateFeedbackController<state_dim, control_dim>
+ initial_controller(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+ // STEP 2-C: create an NLOptConSolver instance
+ ::ct::optcon::NLOptConSolver<state_dim, control_dim> iLQR(opt_con_problem,
+ ilqr_settings);
+ // Seed it with the initial guess
+ iLQR.setInitialGuess(initial_controller);
+ // we solve the optimal control problem and retrieve the solution
+ iLQR.solve();
+ ::ct::core::StateFeedbackController<state_dim, control_dim> initial_solution =
+ iLQR.getSolution();
+ // MPC-EXAMPLE
+ // we store the initial solution obtained from solving the initial optimal
+ // control problem, and re-use it to initialize the MPC solver in the
+ // following.
+
+ // STEP 1: first, we set up an MPC instance for the iLQR solver and configure
+ // it. Since the MPC class is wrapped around normal Optimal Control Solvers,
+ // we need to different kind of settings, those for the optimal control
+ // solver, and those specific to MPC:
+
+ // 1) settings for the iLQR instance used in MPC. Of course, we use the same
+ // settings as for solving the initial problem ...
+ ::ct::optcon::NLOptConSettings ilqr_settings_mpc = ilqr_settings;
+ ilqr_settings_mpc.max_iterations = 20;
+ // and we limited the printouts, too.
+ ilqr_settings_mpc.printSummary = false;
+ // 2) settings specific to model predictive control. For a more detailed
+ // description of those, visit ct/optcon/mpc/MpcSettings.h
+ ::ct::optcon::mpc_settings mpc_settings;
+ mpc_settings.stateForwardIntegration_ = true;
+ mpc_settings.postTruncation_ = false;
+ mpc_settings.measureDelay_ = false;
+ mpc_settings.fixedDelayUs_ = 5000 * 0; // Ignore the delay for now.
+ mpc_settings.delayMeasurementMultiplier_ = 1.0;
+ // mpc_settings.mpc_mode = ::ct::optcon::MPC_MODE::FIXED_FINAL_TIME;
+ mpc_settings.mpc_mode = ::ct::optcon::MPC_MODE::CONSTANT_RECEDING_HORIZON;
+ mpc_settings.coldStart_ = false;
+
+ // STEP 2 : Create the iLQR-MPC object, based on the optimal control problem
+ // and the selected settings.
+ ::ct::optcon::MPC<::ct::optcon::NLOptConSolver<state_dim, control_dim>>
+ ilqr_mpc(opt_con_problem, ilqr_settings_mpc, mpc_settings);
+ // initialize it using the previously computed initial controller
+ ilqr_mpc.setInitialGuess(initial_solution);
+ // STEP 3: running MPC
+ // Here, we run the MPC loop. Note that the general underlying idea is that
+ // you receive a state-estimate together with a time-stamp from your robot or
+ // system. MPC needs to receive both that time information and the state from
+ // your control system. Here, "simulate" the time measurement using
+ // ::std::chrono and wrap everything into a for-loop.
+ // The basic idea of operation is that after receiving time and state
+ // information, one executes the finishIteration() method of MPC.
+ ///
+ auto start_time = ::std::chrono::high_resolution_clock::now();
+ // limit the maximum number of runs in this example
+ size_t maxNumRuns = 400;
+ ::std::cout << "Starting to run MPC" << ::std::endl;
+
+ ::std::vector<double> time_array;
+ ::std::vector<double> theta1_array;
+ ::std::vector<double> omega1_array;
+ ::std::vector<double> theta2_array;
+ ::std::vector<double> omega2_array;
+
+ ::std::vector<double> u0_array;
+ ::std::vector<double> u1_array;
+
+ for (size_t i = 0; i < maxNumRuns; i++) {
+ ::std::cout << "Solving iteration " << i << ::std::endl;
+ // Time which has passed since start of MPC
+ auto current_time = ::std::chrono::high_resolution_clock::now();
+ ::ct::core::Time t =
+ 1e-6 *
+ ::std::chrono::duration_cast<::std::chrono::microseconds>(current_time -
+ start_time)
+ .count();
+ // prepare mpc iteration
+ ilqr_mpc.prepareIteration(t);
+ // new optimal policy
+ ::std::shared_ptr<ct::core::StateFeedbackController<state_dim, control_dim>>
+ newPolicy(
+ new ::ct::core::StateFeedbackController<state_dim, control_dim>());
+ // timestamp of the new optimal policy
+ ::ct::core::Time ts_newPolicy;
+ current_time = ::std::chrono::high_resolution_clock::now();
+ t = 1e-6 *
+ ::std::chrono::duration_cast<::std::chrono::microseconds>(current_time -
+ start_time)
+ .count();
+ bool success = ilqr_mpc.finishIteration(x0, t, *newPolicy, ts_newPolicy);
+ // we break the loop in case the time horizon is reached or solve() failed
+ if (ilqr_mpc.timeHorizonReached() | !success) break;
+
+ ::std::cout << "Solved for time " << newPolicy->time()[0] << " state "
+ << x0.transpose() << " next time " << newPolicy->time()[1]
+ << ::std::endl;
+ ::std::cout << " Solution: Uff " << newPolicy->uff()[0].transpose()
+ << " x_ref_ " << newPolicy->x_ref()[0].transpose()
+ << ::std::endl;
+
+ time_array.push_back(ilqr_settings.dt * i);
+ theta1_array.push_back(x0(0));
+ omega1_array.push_back(x0(1));
+ theta2_array.push_back(x0(2));
+ omega2_array.push_back(x0(3));
+
+ u0_array.push_back(newPolicy->uff()[0](0, 0));
+ u1_array.push_back(newPolicy->uff()[0](1, 0));
+
+ ::std::cout << "xref[1] " << newPolicy->x_ref()[1].transpose()
+ << ::std::endl;
+ ilqr_mpc.doForwardIntegration(0.0, ilqr_settings.dt, x0, newPolicy);
+ ::std::cout << "Next X: " << x0.transpose() << ::std::endl;
+
+ // TODO(austin): Re-use the policy. Maybe? Or maybe mpc already does that.
+ }
+ // The summary contains some statistical data about time delays, etc.
+ ilqr_mpc.printMpcSummary();
+
+ // Now plot our simulation.
+ matplotlibcpp::plot(time_array, theta1_array, {{"label", "theta1"}});
+ matplotlibcpp::plot(time_array, omega1_array, {{"label", "omega1"}});
+ matplotlibcpp::plot(time_array, theta2_array, {{"label", "theta2"}});
+ matplotlibcpp::plot(time_array, omega2_array, {{"label", "omega2"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(theta1_array, theta2_array, {{"label", "trajectory"}});
+ ::std::vector<double> box_x{0.5, 0.5, 1.0, 1.0};
+ ::std::vector<double> box_y{0.0, 0.8, 0.8, 0.0};
+ matplotlibcpp::plot(box_x, box_y, {{"label", "keepout zone"}});
+ matplotlibcpp::legend();
+
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(time_array, u0_array, {{"label", "u0"}});
+ matplotlibcpp::plot(time_array, u1_array, {{"label", "u1"}});
+ matplotlibcpp::legend();
+ matplotlibcpp::show();
+}