Finish up the ARM MPC as far as I could get before abandoning
Turns out this is a bit of a dead end due to the solver employed. I'll
have to revisit it in the future.
Change-Id: Ib75a053395afa6f31dee3ba6c20a236c7c0b433f
diff --git a/y2018/control_loops/python/arm_mpc.cc b/y2018/control_loops/python/arm_mpc.cc
index 2727545..a032cae 100644
--- a/y2018/control_loops/python/arm_mpc.cc
+++ b/y2018/control_loops/python/arm_mpc.cc
@@ -3,14 +3,112 @@
#include <thread>
#include <ct/optcon/optcon.h>
+#include <Eigen/Eigenvalues>
#include "third_party/gflags/include/gflags/gflags.h"
#include "third_party/matplotlib-cpp/matplotlibcpp.h"
+#include "y2018/control_loops/python/arm_bounds.h"
+#include "y2018/control_loops/python/dlqr.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(boundary_scalar, 1500.0, "Test command-line flag");
+DEFINE_double(velocity_boundary_scalar, 10.0, "Test command-line flag");
+DEFINE_double(boundary_rate, 20.0, "Sigmoid rate");
+DEFINE_bool(linear, false, "If true, linear, else see sigmoid.");
+DEFINE_bool(sigmoid, false, "If true, sigmoid, else exponential.");
DEFINE_double(round_corner, 0.0, "Corner radius of the constraint box.");
+DEFINE_double(convergance, 1e-12, "Residual before finishing the solver.");
+DEFINE_double(position_allowance, 5.0,
+ "Distance to Velocity at which we have 0 penalty conversion.");
+DEFINE_double(bounds_offset, 0.02, "Offset the quadratic boundary in by this");
+DEFINE_double(linear_bounds_offset, 0.00,
+ "Offset the linear boundary in by this");
+DEFINE_double(yrange, 1.0,
+ "+- y max for saturating out the state for the cost function.");
+DEFINE_bool(debug_print, false, "Print the debugging print from the solver.");
+DEFINE_bool(print_starting_summary, true,
+ "Print the summary on the pre-solution.");
+DEFINE_bool(print_summary, false, "Print the summary on each iteration.");
+DEFINE_bool(quadratic, true, "If true, quadratic bounds penalty.");
+
+DEFINE_bool(reset_every_cycle, false,
+ "If true, reset the initial guess every cycle.");
+
+DEFINE_double(seconds, 1.5, "The number of seconds to simulate.");
+
+DEFINE_double(theta0, 1.0, "Starting theta0");
+DEFINE_double(theta1, 0.9, "Starting theta1");
+
+DEFINE_double(goal_theta0, -0.5, "Starting theta0");
+DEFINE_double(goal_theta1, -0.5, "Starting theta1");
+
+DEFINE_double(qpos1, 0.2, "qpos1");
+DEFINE_double(qvel1, 4.0, "qvel1");
+DEFINE_double(qpos2, 0.2, "qpos2");
+DEFINE_double(qvel2, 4.0, "qvel2");
+
+DEFINE_double(u_over_linear, 0.0, "Linear penalty for too much U.");
+DEFINE_double(u_over_quadratic, 4.0, "Quadratic penalty for too much U.");
+
+DEFINE_double(time_horizon, 0.75, "MPC time horizon");
+
+DEFINE_bool(only_print_eigenvalues, false,
+ "If true, stop after computing the final eigenvalues");
+
+DEFINE_bool(plot_xy, false, "If true, plot the xy trajectory of the end of the arm.");
+DEFINE_bool(plot_cost, false, "If true, plot the cost function.");
+DEFINE_bool(plot_state_cost, false,
+ "If true, plot the state portion of the cost function.");
+DEFINE_bool(plot_states, false, "If true, plot the states.");
+DEFINE_bool(plot_u, false, "If true, plot the control signal.");
+
+static constexpr double kDt = 0.00505;
+
+namespace y2018 {
+namespace control_loops {
+
+::Eigen::Matrix<double, 4, 4> NumericalJacobianX(
+ ::Eigen::Matrix<double, 4, 1> (*fn)(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt),
+ ::Eigen::Matrix<double, 4, 1> X, ::Eigen::Matrix<double, 2, 1> U, double dt,
+ const double kEpsilon = 1e-4) {
+ constexpr int num_states = 4;
+ ::Eigen::Matrix<double, 4, 4> answer = ::Eigen::Matrix<double, 4, 4>::Zero();
+
+ // It's more expensive, but +- epsilon will be more reliable
+ for (int i = 0; i < num_states; ++i) {
+ ::Eigen::Matrix<double, 4, 1> dX_plus = X;
+ dX_plus(i, 0) += kEpsilon;
+ ::Eigen::Matrix<double, 4, 1> dX_minus = X;
+ dX_minus(i, 0) -= kEpsilon;
+ answer.block<4, 1>(0, i) =
+ (fn(dX_plus, U, dt) - fn(dX_minus, U, dt)) / kEpsilon / 2.0;
+ }
+ return answer;
+}
+
+::Eigen::Matrix<double, 4, 2> NumericalJacobianU(
+ ::Eigen::Matrix<double, 4, 1> (*fn)(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt),
+ ::Eigen::Matrix<double, 4, 1> X, ::Eigen::Matrix<double, 2, 1> U, double dt,
+ const double kEpsilon = 1e-4) {
+ constexpr int num_states = 4;
+ constexpr int num_inputs = 2;
+ ::Eigen::Matrix<double, num_states, num_inputs> answer =
+ ::Eigen::Matrix<double, num_states, num_inputs>::Zero();
+
+ // It's more expensive, but +- epsilon will be more reliable
+ for (int i = 0; i < num_inputs; ++i) {
+ ::Eigen::Matrix<double, 2, 1> dU_plus = U;
+ dU_plus(i, 0) += kEpsilon;
+ ::Eigen::Matrix<double, 2, 1> dU_minus = U;
+ dU_minus(i, 0) -= kEpsilon;
+ answer.block<4, 1>(0, i) =
+ (fn(X, dU_plus, dt) - fn(X, dU_minus, dt)) / kEpsilon / 2.0;
+ }
+ return answer;
+}
// 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.
@@ -38,6 +136,29 @@
}
virtual ~MySecondOrderSystem() {}
+ static constexpr SCALAR l1 = 46.25 * 0.0254;
+ static constexpr SCALAR l2 = 41.80 * 0.0254;
+
+ static constexpr SCALAR m1 = 9.34 / 2.2;
+ static constexpr SCALAR m2 = 9.77 / 2.2;
+
+ static constexpr SCALAR J1 = 2957.05 * 0.0002932545454545454;
+ static constexpr SCALAR J2 = 2824.70 * 0.0002932545454545454;
+
+ static constexpr SCALAR r1 = 21.64 * 0.0254;
+ static constexpr SCALAR r2 = 26.70 * 0.0254;
+
+ static constexpr SCALAR G1 = 140.0;
+ static constexpr SCALAR G2 = 90.0;
+
+ static constexpr SCALAR stall_torque = 1.41;
+ static constexpr SCALAR free_speed = (5840.0 / 60.0) * 2.0 * M_PI;
+ static constexpr SCALAR stall_current = 89.0;
+ static constexpr SCALAR R = 12.0 / stall_current;
+
+ static constexpr SCALAR Kv = free_speed / 12.0;
+ static constexpr SCALAR Kt = stall_torque / stall_current;
+
// Evaluate the system dynamics.
//
// Args:
@@ -49,11 +170,312 @@
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);
+ derivative = Dynamics(state, control);
}
+
+ static ::Eigen::Matrix<double, 4, 1> Dynamics(
+ const ::ct::core::StateVector<4, SCALAR> &X,
+ const ::ct::core::ControlVector<2, SCALAR> &U) {
+ ::ct::core::StateVector<4, SCALAR> derivative;
+ const SCALAR alpha = J1 + r1 * r1 * m1 + l1 * l1 * m2;
+ const SCALAR beta = l1 * r2 * m2;
+ const SCALAR gamma = J2 + r2 * r2 * m2;
+
+ const SCALAR s = sin(X(0) - X(2));
+ const SCALAR c = cos(X(0) - X(2));
+
+ // K1 * d^2 theta/dt^2 + K2 * d theta/dt = torque
+ ::Eigen::Matrix<SCALAR, 2, 2> K1;
+ K1(0, 0) = alpha;
+ K1(1, 0) = K1(0, 1) = c * beta;
+ K1(1, 1) = gamma;
+
+ ::Eigen::Matrix<SCALAR, 2, 2> K2 = ::Eigen::Matrix<SCALAR, 2, 2>::Zero();
+ K2(0, 1) = s * beta * X(3);
+ K2(1, 0) = -s * beta * X(1);
+
+ const SCALAR kNumDistalMotors = 2.0;
+ ::Eigen::Matrix<SCALAR, 2, 1> torque;
+ torque(0, 0) = G1 * (U(0) * Kt / R - X(1) * G1 * Kt / (Kv * R));
+ torque(1, 0) = G2 * (U(1) * kNumDistalMotors * Kt / R -
+ X(3) * G2 * Kt * kNumDistalMotors / (Kv * R));
+
+ ::Eigen::Matrix<SCALAR, 2, 1> velocity;
+ velocity(0, 0) = X(0);
+ velocity(1, 0) = X(2);
+
+ const ::Eigen::Matrix<SCALAR, 2, 1> accel =
+ K1.inverse() * (torque - K2 * velocity);
+
+ derivative(0) = X(1);
+ derivative(1) = accel(0);
+ derivative(2) = X(3);
+ derivative(3) = accel(1);
+
+ return derivative;
+ }
+
+ // Runge-Kutta.
+ static ::Eigen::Matrix<double, 4, 1> DiscreteDynamics(
+ ::Eigen::Ref<::Eigen::Matrix<double, 4, 1>> X,
+ ::Eigen::Ref<::Eigen::Matrix<double, 2, 1>> U, double dt) {
+ const double half_dt = dt * 0.5;
+ ::Eigen::Matrix<double, 4, 1> k1 = Dynamics(X, U);
+ ::Eigen::Matrix<double, 4, 1> k2 = Dynamics(X + half_dt * k1, U);
+ ::Eigen::Matrix<double, 4, 1> k3 = Dynamics(X + half_dt * k2, U);
+ ::Eigen::Matrix<double, 4, 1> k4 = Dynamics(X + dt * k3, U);
+ return X + dt / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4);
+ }
+};
+
+template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double,
+ typename SCALAR = SCALAR_EVAL>
+class ObstacleAwareQuadraticCost
+ : 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;
+
+ ObstacleAwareQuadraticCost(const ::Eigen::Matrix<double, 2, 2> &R,
+ const ::Eigen::Matrix<double, 4, 4> &Q)
+ : R_(R), Q_(Q) {}
+
+ ObstacleAwareQuadraticCost(const ObstacleAwareQuadraticCost &arg)
+ : R_(arg.R_), Q_(arg.Q_) {}
+ static constexpr double kEpsilon = 1.0e-5;
+
+ virtual ~ObstacleAwareQuadraticCost() {}
+
+ ObstacleAwareQuadraticCost<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR>
+ *clone() const override {
+ return new ObstacleAwareQuadraticCost(*this);
+ }
+
+ double SaturateX(double x, double yrange) {
+ return 2.0 * ((1.0 / (1.0 + ::std::exp(-x * 2.0 / yrange)) - 0.5)) * yrange;
+ }
+
+ SCALAR distance(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/) {
+ constexpr double kCornerNewUpper0 = 0.35;
+ // constexpr double kCornerUpper1 = 3.13;
+ // Push it up a bit further (non-real) until we have an actual path cost.
+ constexpr double kCornerNewUpper1 = 3.39;
+ constexpr double kCornerNewUpper0_far = 10.0;
+
+ // Push it up a bit further (non-real) until we have an actual path cost.
+ // constexpr double kCornerUpper0 = 0.315;
+ constexpr double kCornerUpper0 = 0.310;
+ // constexpr double kCornerUpper1 = 3.13;
+ constexpr double kCornerUpper1 = 3.25;
+ constexpr double kCornerUpper0_far = 10.0;
+
+ constexpr double kCornerLower0 = 0.023;
+ constexpr double kCornerLower1 = 1.57;
+ constexpr double kCornerLower0_far = 10.0;
+
+ const Segment new_upper_segment(
+ Point(kCornerNewUpper0, kCornerNewUpper1),
+ Point(kCornerNewUpper0_far, kCornerNewUpper1));
+ const Segment upper_segment(Point(kCornerUpper0, kCornerUpper1),
+ Point(kCornerUpper0_far, kCornerUpper1));
+ const Segment lower_segment(Point(kCornerLower0, kCornerLower1),
+ Point(kCornerLower0_far, kCornerLower1));
+
+ Point current_point(x(0, 0), x(2, 0));
+
+ SCALAR result = 0.0;
+ if (intersects(new_upper_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerNewUpper0,
+ current_point.y() - kCornerNewUpper1);
+ current_point = Point(kCornerNewUpper0, kCornerNewUpper1);
+ }
+
+ if (intersects(upper_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerUpper0,
+ current_point.y() - kCornerUpper1);
+ current_point = Point(kCornerUpper0, kCornerUpper1);
+ }
+
+ if (intersects(lower_segment,
+ Segment(current_point,
+ Point(FLAGS_goal_theta0, FLAGS_goal_theta1)))) {
+ result += hypot(current_point.x() - kCornerLower0,
+ current_point.y() - kCornerLower1);
+ current_point = Point(kCornerLower0, kCornerLower1);
+ }
+ result += hypot(current_point.x() - FLAGS_goal_theta0,
+ current_point.y() - FLAGS_goal_theta1);
+ return result;
+ }
+
+ virtual SCALAR evaluate(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> &u,
+ const SCALAR & /*t*/) override {
+ // Positive means violation.
+ Eigen::Matrix<SCALAR, STATE_DIM, 1> saturated_x = x;
+ SCALAR d = distance(x, u);
+ saturated_x(0, 0) = d;
+ saturated_x(2, 0) = 0.0;
+
+ saturated_x(0, 0) = SaturateX(saturated_x(0, 0), FLAGS_yrange);
+ saturated_x(2, 0) = 0.0;
+
+ //SCALAR saturation_scalar = saturated_x(0, 0) / d;
+ //saturated_x(1, 0) *= saturation_scalar;
+ //saturated_x(3, 0) *= saturation_scalar;
+
+ SCALAR result = (saturated_x.transpose() * Q_ * saturated_x +
+ u.transpose() * R_ * u)(0, 0);
+
+ if (::std::abs(u(0, 0)) > 11.0) {
+ result += (::std::abs(u(0, 0)) - 11.0) * FLAGS_u_over_linear;
+ result += (::std::abs(u(0, 0)) - 11.0) * (::std::abs(u(0, 0)) - 11.0) *
+ FLAGS_u_over_quadratic;
+ }
+ if (::std::abs(u(1, 0)) > 11.0) {
+ result += (::std::abs(u(1, 0)) - 11.0) * FLAGS_u_over_linear;
+ result += (::std::abs(u(1, 0)) - 11.0) * (::std::abs(u(1, 0)) - 11.0) *
+ FLAGS_u_over_quadratic;
+ }
+ return result;
+ }
+
+ 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();
+
+ // Perterb x for both position axis and return the result.
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
+ 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);
+
+ // Perterb x a second time.
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
+ 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;
+ }
+ //::std::cout << "Q_numeric " << result << " endQ" << ::std::endl;
+ 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 {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> result =
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL>::Zero();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ // Perterb x a second time.
+ for (size_t i = 0; i < CONTROL_DIM; i += 1) {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> plus_perterbed_u = u;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> minus_perterbed_u = u;
+ plus_perterbed_u[i] += epsilon;
+ minus_perterbed_u[i] -= epsilon;
+ SCALAR delta = (evaluate(x, plus_perterbed_u, t) -
+ evaluate(x, minus_perterbed_u, t)) /
+ (epsilon * 2.0);
+
+ result[i] = delta;
+ }
+ //::std::cout << "cd " << result(0, 0) << " " << result(1, 0) << " endcd"
+ //<< ::std::endl;
+
+ return result;
+ }
+
+ 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 {
+ // No coupling here, so let's not bother to calculate it.
+ 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();
+
+ SCALAR epsilon = SCALAR(kEpsilon);
+
+ //static int j = 0;
+ //::std::this_thread::sleep_for(::std::chrono::milliseconds(j % 10));
+ //int k = ++j;
+ // Perterb x a second time.
+ for (size_t i = 0; i < CONTROL_DIM; i += 1) {
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> plus_perterbed_u = u;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> minus_perterbed_u = u;
+ plus_perterbed_u[i] += epsilon;
+ minus_perterbed_u[i] -= epsilon;
+ ct::core::ControlVector<CONTROL_DIM, SCALAR_EVAL> delta =
+ (controlDerivative(x, plus_perterbed_u, t) -
+ controlDerivative(x, minus_perterbed_u, t)) /
+ (epsilon * 2.0);
+
+ //::std::cout << "delta: " << delta(0, 0) << " " << delta(1, 0) << " k "
+ //<< k << ::std::endl;
+ result.col(i) = delta;
+ }
+ //::std::cout << "R_numeric " << result << " endR 0.013888888888888888 k:" << k
+ //<< ::std::endl;
+ //::std::cout << "x " << x << " u " << u << " k " << k << ::std::endl;
+
+ return result;
+ }
+
+ private:
+ const ::Eigen::Matrix<double, 2, 2> R_;
+ const ::Eigen::Matrix<double, 4, 4> Q_;
};
template <size_t STATE_DIM, size_t CONTROL_DIM, typename SCALAR_EVAL = double,
@@ -75,11 +497,12 @@
typedef Eigen::Matrix<SCALAR_EVAL, CONTROL_DIM, STATE_DIM>
control_state_matrix_double_t;
- MyTermStateBarrier() {}
+ MyTermStateBarrier(BoundsCheck *bounds_check) : bounds_check_(bounds_check) {}
- MyTermStateBarrier(const MyTermStateBarrier & /*arg*/) {}
+ MyTermStateBarrier(const MyTermStateBarrier &arg)
+ : bounds_check_(arg.bounds_check_) {}
- static constexpr double kEpsilon = 1.0e-7;
+ static constexpr double kEpsilon = 5.0e-6;
virtual ~MyTermStateBarrier() {}
@@ -88,36 +511,57 @@
return new MyTermStateBarrier(*this);
}
+ SCALAR distance(const Eigen::Matrix<SCALAR, STATE_DIM, 1> &x,
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & /*u*/,
+ const SCALAR & /*t*/, Eigen::Matrix<SCALAR, 2, 1> *norm) {
+ return bounds_check_->min_distance(Point(x(0, 0), x(2, 0)), norm);
+ }
+
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;
+ const Eigen::Matrix<SCALAR, CONTROL_DIM, 1> & u,
+ const SCALAR & t) override {
+ Eigen::Matrix<SCALAR, 2, 1> norm = Eigen::Matrix<SCALAR, 2, 1>::Zero();
+ SCALAR min_distance = distance(x, u, t, &norm);
- // Round the corner by this amount.
- SCALAR round_corner = SCALAR(FLAGS_round_corner);
+ // Velocity component (+) towards the wall.
+ SCALAR velocity_penalty = -(x(1, 0) * norm(0, 0) + x(3, 0) * norm(1, 0));
+ if (min_distance + FLAGS_bounds_offset < 0.0) {
+ velocity_penalty = 0.0;
+ }
- // 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);
- }
+ SCALAR result;
+ //if (FLAGS_quadratic) {
+ result = FLAGS_boundary_scalar *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset) *
+ ::std::max(0.0, min_distance + FLAGS_bounds_offset) +
+ FLAGS_boundary_rate *
+ ::std::max(0.0, min_distance + FLAGS_linear_bounds_offset) +
+ FLAGS_velocity_boundary_scalar *
+ ::std::max(0.0, min_distance + FLAGS_linear_bounds_offset) *
+ ::std::max(0.0, velocity_penalty) *
+ ::std::max(0.0, velocity_penalty);
+ /*
+} else if (FLAGS_linear) {
+result =
+FLAGS_boundary_scalar * ::std::max(0.0, min_distance) +
+FLAGS_velocity_boundary_scalar * ::std::max(0.0, -velocity_penalty);
+} else if (FLAGS_sigmoid) {
+result = FLAGS_boundary_scalar /
+ (1.0 + ::std::exp(-min_distance * FLAGS_boundary_rate)) +
+FLAGS_velocity_boundary_scalar /
+ (1.0 + ::std::exp(-velocity_penalty * FLAGS_boundary_rate));
+} else {
+// Values of 4 and 15 work semi resonably.
+result = FLAGS_boundary_scalar *
+ ::std::exp(min_distance * FLAGS_boundary_rate) +
+FLAGS_velocity_boundary_scalar *
+ ::std::exp(velocity_penalty * FLAGS_boundary_rate);
+}
+if (result < 0.0) {
+printf("Result negative %f\n", result);
+}
+*/
+ return result;
}
::ct::core::StateVector<STATE_DIM, SCALAR_EVAL> stateDerivative(
@@ -152,7 +596,7 @@
SCALAR epsilon = SCALAR(kEpsilon);
// Perturb x a second time.
- for (size_t i = 0; i < STATE_DIM; i += 2) {
+ for (size_t i = 0; i < STATE_DIM; i += 1) {
::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;
@@ -202,11 +646,14 @@
return c;
}
*/
+
+ BoundsCheck *bounds_check_;
};
-int main(int argc, char **argv) {
- gflags::ParseCommandLineFlags(&argc, &argv, false);
- // PRELIMINIARIES, see example NLOC.cpp
+int Main() {
+ // PRELIMINIARIES
+ BoundsCheck arm_space = MakeClippedArmSpace();
+
constexpr size_t state_dim = MySecondOrderSystem<double>::STATE_DIM;
constexpr size_t control_dim = MySecondOrderSystem<double>::CONTROL_DIM;
@@ -217,32 +664,64 @@
ad_linearizer(new ::ct::core::SystemLinearizer<state_dim, control_dim>(
oscillator_dynamics));
- constexpr double kQPos = 0.5;
- constexpr double kQVel = 1.65;
+ const double kQPos1 = FLAGS_qpos1;
+ const double kQVel1 = FLAGS_qvel1;
+ const double kQPos2 = FLAGS_qpos2;
+ const double kQVel2 = FLAGS_qvel2;
+
::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);
+ Q_step << 1.0 / (kQPos1 * kQPos1), 0.0, 0.0, 0.0, 0.0,
+ 1.0 / (kQVel1 * kQVel1), 0.0, 0.0, 0.0, 0.0, 1.0 / (kQPos2 * kQPos2), 0.0,
+ 0.0, 0.0, 0.0, 1.0 / (kQVel2 * kQVel2);
::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));
+ ::std::shared_ptr<::ct::optcon::TermQuadratic<state_dim, control_dim>>
+ quadratic_intermediate_cost(
+ new ::ct::optcon::TermQuadratic<state_dim, control_dim>(Q_step,
+ R_step));
+ // TODO(austin): Move back to this with the new Q and R
+ ::std::shared_ptr<ObstacleAwareQuadraticCost<state_dim, control_dim>>
+ intermediate_cost(new ObstacleAwareQuadraticCost<4, 2>(R_step, Q_step));
- // TODO(austin): DARE for these.
- ::Eigen::Matrix<double, 4, 4> Q_final = Q_step;
- ::Eigen::Matrix<double, 2, 2> R_final = R_step;
+ ::Eigen::Matrix<double, 4, 4> final_A =
+ NumericalJacobianX(MySecondOrderSystem<double>::DiscreteDynamics,
+ Eigen::Matrix<double, 4, 1>::Zero(),
+ Eigen::Matrix<double, 2, 1>::Zero(), kDt);
+
+ ::Eigen::Matrix<double, 4, 2> final_B =
+ NumericalJacobianU(MySecondOrderSystem<double>::DiscreteDynamics,
+ Eigen::Matrix<double, 4, 1>::Zero(),
+ Eigen::Matrix<double, 2, 1>::Zero(), kDt);
+
+ ::Eigen::Matrix<double, 4, 4> S_lqr;
+ ::Eigen::Matrix<double, 2, 4> K_lqr;
+ ::frc971::controls::dlqr(final_A, final_B, Q_step, R_step, &K_lqr, &S_lqr);
+ ::std::cout << "A -> " << ::std::endl << final_A << ::std::endl;
+ ::std::cout << "B -> " << ::std::endl << final_B << ::std::endl;
+ ::std::cout << "K -> " << ::std::endl << K_lqr << ::std::endl;
+ ::std::cout << "S -> " << ::std::endl << S_lqr << ::std::endl;
+ ::std::cout << "Q -> " << ::std::endl << Q_step << ::std::endl;
+ ::std::cout << "R -> " << ::std::endl << R_step << ::std::endl;
+ ::std::cout << "Eigenvalues: " << (final_A - final_B * K_lqr).eigenvalues()
+ << ::std::endl;
+
+ ::Eigen::Matrix<double, 4, 4> Q_final = 0.5 * S_lqr;
+ ::Eigen::Matrix<double, 2, 2> R_final = ::Eigen::Matrix<double, 2, 2>::Zero();
::std::shared_ptr<ct::optcon::TermQuadratic<state_dim, control_dim>>
final_cost(new ::ct::optcon::TermQuadratic<state_dim, control_dim>(
Q_final, R_final));
+ if (FLAGS_only_print_eigenvalues) {
+ return 0;
+ }
- ::std::shared_ptr<ct::optcon::TermBase<state_dim, control_dim>> bounds_cost(
- new MyTermStateBarrier<4, 2>());
+ ::std::shared_ptr<MyTermStateBarrier<state_dim, control_dim>> bounds_cost(
+ new MyTermStateBarrier<4, 2>(&arm_space));
// 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(quadratic_intermediate_cost);
cost_function->addIntermediateTerm(intermediate_cost);
cost_function->addIntermediateTerm(bounds_cost);
cost_function->addFinalTerm(final_cost);
@@ -254,8 +733,8 @@
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;
+ //::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>>
@@ -275,42 +754,45 @@
// Starting point.
::ct::core::StateVector<state_dim> x0;
- x0 << 1.0, 0.0, 0.9, 0.0;
+ x0 << FLAGS_theta0, 0.0, FLAGS_theta1, 0.0;
- constexpr ::ct::core::Time kTimeHorizon = 1.5;
+ const ::ct::core::Time kTimeHorizon = FLAGS_time_horizon;
::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.nThreads = 4;
+ ilqr_settings.dt = kDt; // the control discretization in [sec]
ilqr_settings.integrator = ::ct::core::IntegrationType::RK4;
+ ilqr_settings.debugPrint = FLAGS_debug_print;
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.max_iterations = 40;
+ ilqr_settings.min_cost_improvement = FLAGS_convergance;
ilqr_settings.nlocp_algorithm =
- ::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ //::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::ILQR;
+ ::ct::optcon::NLOptConSettings::NLOCP_ALGORITHM::GNMS;
// 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;
+ ::ct::optcon::NLOptConSettings::LQOCP_SOLVER::GNRICCATI_SOLVER;
+ //ilqr_settings.lqocp_solver =
+ //::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER;
+ ilqr_settings.printSummary = FLAGS_print_starting_summary;
if (ilqr_settings.lqocp_solver ==
::ct::optcon::NLOptConSettings::LQOCP_SOLVER::HPIPM_SOLVER) {
- opt_con_problem.setBoxConstraints(box_constraints);
+ //opt_con_problem.setBoxConstraints(box_constraints);
}
- size_t K = ilqr_settings.computeK(kTimeHorizon);
- printf("Using %d steps\n", static_cast<int>(K));
+ const size_t num_steps = ilqr_settings.computeK(kTimeHorizon);
+ printf("Using %d steps\n", static_cast<int>(num_steps));
// Vector of feeback matricies.
::ct::core::FeedbackArray<state_dim, control_dim> u0_fb(
- K, ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
+ num_steps, ::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);
+ num_steps, ::ct::core::ControlVector<control_dim>::Zero());
+ ::ct::core::StateVectorArray<state_dim> x_ref_init(num_steps + 1, x0);
::ct::core::StateFeedbackController<state_dim, control_dim>
initial_controller(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
@@ -336,9 +818,9 @@
// 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;
+ ilqr_settings_mpc.max_iterations = 40;
// and we limited the printouts, too.
- ilqr_settings_mpc.printSummary = false;
+ ilqr_settings_mpc.printSummary = FLAGS_print_summary;
// 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;
@@ -368,7 +850,7 @@
///
auto start_time = ::std::chrono::high_resolution_clock::now();
// limit the maximum number of runs in this example
- size_t maxNumRuns = 400;
+ size_t maxNumRuns = FLAGS_seconds / kDt;
::std::cout << "Starting to run MPC" << ::std::endl;
::std::vector<double> time_array;
@@ -380,6 +862,11 @@
::std::vector<double> u0_array;
::std::vector<double> u1_array;
+ ::std::vector<double> x_array;
+ ::std::vector<double> y_array;
+
+ // TODO(austin): Plot x, y of the end of the arm.
+
for (size_t i = 0; i < maxNumRuns; i++) {
::std::cout << "Solving iteration " << i << ::std::endl;
// Time which has passed since start of MPC
@@ -389,6 +876,25 @@
::std::chrono::duration_cast<::std::chrono::microseconds>(current_time -
start_time)
.count();
+ {
+ if (FLAGS_reset_every_cycle) {
+ ::ct::core::FeedbackArray<state_dim, control_dim> u0_fb(
+ num_steps,
+ ::ct::core::FeedbackMatrix<state_dim, control_dim>::Zero());
+ ::ct::core::ControlVectorArray<control_dim> u0_ff(
+ num_steps, ::ct::core::ControlVector<control_dim>::Zero());
+ ::ct::core::StateVectorArray<state_dim> x_ref_init(num_steps + 1, x0);
+ ::ct::core::StateFeedbackController<state_dim, control_dim>
+ resolved_controller(x_ref_init, u0_ff, u0_fb, ilqr_settings.dt);
+
+ iLQR.setInitialGuess(initial_controller);
+ // we solve the optimal control problem and retrieve the solution
+ iLQR.solve();
+ resolved_controller = iLQR.getSolution();
+ ilqr_mpc.setInitialGuess(resolved_controller);
+ }
+ }
+
// prepare mpc iteration
ilqr_mpc.prepareIteration(t);
// new optimal policy
@@ -402,6 +908,9 @@
::std::chrono::duration_cast<::std::chrono::microseconds>(current_time -
start_time)
.count();
+ // TODO(austin): This is only iterating once... I need to fix that...
+ // NLOptConSolver::solve() runs for upto N iterations. This call runs
+ // runIteration() effectively once. (nlocAlgorithm_ is iLQR)
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;
@@ -427,28 +936,95 @@
ilqr_mpc.doForwardIntegration(0.0, ilqr_settings.dt, x0, newPolicy);
::std::cout << "Next X: " << x0.transpose() << ::std::endl;
+ x_array.push_back(MySecondOrderSystem<double>::l1 * sin(x0(0)) +
+ MySecondOrderSystem<double>::r2 * sin(x0(2)));
+ y_array.push_back(MySecondOrderSystem<double>::l1 * cos(x0(0)) +
+ MySecondOrderSystem<double>::r2 * cos(x0(2)));
+
// 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();
+ if (FLAGS_plot_states) {
+ // 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();
+ }
+
+ if (FLAGS_plot_xy) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(x_array, y_array, {{"label", "xy trajectory"}});
+ matplotlibcpp::legend();
+ }
+
+ if (FLAGS_plot_u) {
+ matplotlibcpp::figure();
+ matplotlibcpp::plot(time_array, u0_array, {{"label", "u0"}});
+ matplotlibcpp::plot(time_array, u1_array, {{"label", "u1"}});
+ matplotlibcpp::legend();
+ }
+
+ ::std::vector<::std::vector<double>> cost_x;
+ ::std::vector<::std::vector<double>> cost_y;
+ ::std::vector<::std::vector<double>> cost_z;
+ ::std::vector<::std::vector<double>> cost_state_z;
+
+ for (double x_coordinate = -0.5; x_coordinate < 1.2; x_coordinate += 0.05) {
+ ::std::vector<double> cost_x_row;
+ ::std::vector<double> cost_y_row;
+ ::std::vector<double> cost_z_row;
+ ::std::vector<double> cost_state_z_row;
+
+ for (double y_coordinate = -1.0; y_coordinate < 6.0; y_coordinate += 0.05) {
+ cost_x_row.push_back(x_coordinate);
+ cost_y_row.push_back(y_coordinate);
+ Eigen::Matrix<double, 4, 1> state_matrix;
+ state_matrix << x_coordinate, 0.0, y_coordinate, 0.0;
+ Eigen::Matrix<double, 2, 1> u_matrix =
+ Eigen::Matrix<double, 2, 1>::Zero();
+ cost_state_z_row.push_back(
+ intermediate_cost->distance(state_matrix, u_matrix));
+ cost_z_row.push_back(
+ ::std::min(bounds_cost->evaluate(state_matrix, u_matrix, 0.0), 50.0));
+ }
+ cost_x.push_back(cost_x_row);
+ cost_y.push_back(cost_y_row);
+ cost_z.push_back(cost_z_row);
+ cost_state_z.push_back(cost_state_z_row);
+ }
+
+ if (FLAGS_plot_cost) {
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_z);
+ }
+
+ if (FLAGS_plot_state_cost) {
+ matplotlibcpp::plot_surface(cost_x, cost_y, cost_state_z);
+ }
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"}});
+ ::std::vector<double> bounds_x;
+ ::std::vector<double> bounds_y;
+ for (const Point p : arm_space.points()) {
+ bounds_x.push_back(p.x());
+ bounds_y.push_back(p.y());
+ }
+ matplotlibcpp::plot(bounds_x, bounds_y, {{"label", "allowed region"}});
matplotlibcpp::legend();
- matplotlibcpp::figure();
- matplotlibcpp::plot(time_array, u0_array, {{"label", "u0"}});
- matplotlibcpp::plot(time_array, u1_array, {{"label", "u1"}});
- matplotlibcpp::legend();
matplotlibcpp::show();
+
+ return 0;
+}
+
+} // namespace control_loops
+} // namespace y2018
+
+int main(int argc, char **argv) {
+ gflags::ParseCommandLineFlags(&argc, &argv, false);
+ return ::y2018::control_loops::Main();
}