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"],
)