| #include <chrono> |
| #include <cmath> |
| #include <thread> |
| |
| #include <ct/optcon/optcon.h> |
| #include <Eigen/Eigenvalues> |
| |
| #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, 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. |
| |
| // 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() {} |
| |
| 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: |
| // 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 = 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, |
| 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(BoundsCheck *bounds_check) : bounds_check_(bounds_check) {} |
| |
| MyTermStateBarrier(const MyTermStateBarrier &arg) |
| : bounds_check_(arg.bounds_check_) {} |
| |
| static constexpr double kEpsilon = 5.0e-6; |
| |
| virtual ~MyTermStateBarrier() {} |
| |
| MyTermStateBarrier<STATE_DIM, CONTROL_DIM, SCALAR_EVAL, SCALAR> *clone() |
| const override { |
| 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 { |
| Eigen::Matrix<SCALAR, 2, 1> norm = Eigen::Matrix<SCALAR, 2, 1>::Zero(); |
| SCALAR min_distance = distance(x, u, t, &norm); |
| |
| // 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; |
| } |
| |
| 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( |
| 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 += 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; |
| } |
| 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; |
| } |
| */ |
| |
| BoundsCheck *bounds_check_; |
| }; |
| |
| 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; |
| |
| ::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)); |
| |
| 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 / (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>> |
| 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)); |
| |
| ::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<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); |
| |
| // 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 << FLAGS_theta0, 0.0, FLAGS_theta1, 0.0; |
| |
| 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.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 = 40; |
| ilqr_settings.min_cost_improvement = FLAGS_convergance; |
| ilqr_settings.nlocp_algorithm = |
| //::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 = |
| ::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); |
| } |
| |
| 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( |
| 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> |
| 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 = 40; |
| // and we limited the printouts, too. |
| 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; |
| 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 = FLAGS_seconds / kDt; |
| ::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; |
| |
| ::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 |
| 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(); |
| { |
| 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 |
| ::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(); |
| // 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; |
| |
| ::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; |
| |
| 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(); |
| |
| 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> 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::show(); |
| |
| return 0; |
| } |
| |
| } // namespace control_loops |
| } // namespace y2018 |
| |
| int main(int argc, char **argv) { |
| gflags::ParseCommandLineFlags(&argc, &argv, false); |
| return ::y2018::control_loops::Main(); |
| } |