From 6da91428ae1a2ef9c680b8fbbb8748a0e54c4459 Mon Sep 17 00:00:00 2001 From: CzJaewan Date: Tue, 16 Feb 2021 19:05:20 +0900 Subject: [PATCH 1/7] add holonomic kinematics --- .../models/holonomic_kinematic_model.h | 69 +++++--- mpc_ros/include/models/kinematic_model.h | 3 +- mpc_ros/include/mpc_plannner.h | 2 + mpc_ros/include/mpc_plannner_ros.h | 2 +- mpc_ros/src/MPC.cpp | 1 + mpc_ros/src/mpc_plannner.cpp | 89 +++++++--- mpc_ros/src/mpc_plannner_ros.cpp | 165 ++++++++++++++---- 7 files changed, 248 insertions(+), 83 deletions(-) diff --git a/mpc_ros/include/models/holonomic_kinematic_model.h b/mpc_ros/include/models/holonomic_kinematic_model.h index 960baa4..029b18f 100644 --- a/mpc_ros/include/models/holonomic_kinematic_model.h +++ b/mpc_ros/include/models/holonomic_kinematic_model.h @@ -46,15 +46,17 @@ class HolonomicKinematicModel : public KinematicModel _w_angvel_d = 0; _w_accel_d = 0; - _mpc_steps = 40; _x_start = 0; _y_start = _x_start + _mpc_steps; _theta_start = _y_start + _mpc_steps; - _v_start = _theta_start + _mpc_steps; - _cte_start = _v_start + _mpc_steps; + _vx_start = _theta_start + _mpc_steps; + _vy_start = _vx_start + _mpc_steps; + + _cte_start = _vy_start + _mpc_steps; _etheta_start = _cte_start + _mpc_steps; _angvel_start = _etheta_start + _mpc_steps; - _a_start = _angvel_start + _mpc_steps - 1; + _ax_start = _angvel_start + _mpc_steps - 1; + _ay_start = _ax_start + _mpc_steps - 1; } void loadParams(const std::map ¶ms) @@ -76,11 +78,14 @@ class HolonomicKinematicModel : public KinematicModel _x_start = 0; _y_start = _x_start + _mpc_steps; _theta_start = _y_start + _mpc_steps; - _v_start = _theta_start + _mpc_steps; - _cte_start = _v_start + _mpc_steps; + _vx_start = _theta_start + _mpc_steps; + _vy_start = _vx_start + _mpc_steps; + + _cte_start = _vy_start + _mpc_steps; _etheta_start = _cte_start + _mpc_steps; _angvel_start = _etheta_start + _mpc_steps; - _a_start = _angvel_start + _mpc_steps - 1; + _ax_start = _angvel_start + _mpc_steps - 1; + _ay_start = _ax_start + _mpc_steps - 1; //cout << "\n!! FG_eval Obj parameters updated !! " << _mpc_steps << endl; } @@ -91,29 +96,35 @@ class HolonomicKinematicModel : public KinematicModel fg[0] = 0; cost_cte = 0; cost_etheta = 0; - cost_vel = 0; + cost_vel_x = 0; + cost_vel_y = 0; for (int i = 0; i < _mpc_steps; i++) { fg[0] += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); // cross deviation error fg[0] += _w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2); // heading error - fg[0] += _w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2); // speed error + fg[0] += _w_vel * CppAD::pow(vars[_vx_start + i] - _ref_vel, 2); // speed_x error + fg[0] += _w_vel * CppAD::pow(vars[_vy_start + i] - _ref_vel, 2); // speed_y error cost_cte += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); cost_etheta += (_w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2)); - cost_vel += (_w_vel * CppAD::pow(vars[_v_start + i] - _ref_vel, 2)); + cost_vel_x += (_w_vel * CppAD::pow(vars[_vx_start + i] - _ref_vel, 2)); + cost_vel_y += (_w_vel * CppAD::pow(vars[_vy_start + i] - _ref_vel, 2)); + } // Minimize the use of actuators. for (int i = 0; i < _mpc_steps - 1; i++) { fg[0] += _w_angvel * CppAD::pow(vars[_angvel_start + i], 2); - fg[0] += _w_accel * CppAD::pow(vars[_a_start + i], 2); - } + fg[0] += _w_accel * CppAD::pow(vars[_ax_start + i], 2); + fg[0] += _w_accel * CppAD::pow(vars[_ay_start + i], 2); //error + } // Minimize the value gap between sequential actuations. for (int i = 0; i < _mpc_steps - 2; i++) { fg[0] += _w_angvel_d * CppAD::pow(vars[_angvel_start + i + 1] - vars[_angvel_start + i], 2); - fg[0] += _w_accel_d * CppAD::pow(vars[_a_start + i + 1] - vars[_a_start + i], 2); + fg[0] += _w_accel_d * CppAD::pow(vars[_ax_start + i + 1] - vars[_ax_start + i], 2); + fg[0] += _w_accel_d * CppAD::pow(vars[_ay_start + i + 1] - vars[_ay_start + i], 2); } @@ -123,7 +134,8 @@ class HolonomicKinematicModel : public KinematicModel fg[1 + _x_start] = vars[_x_start]; fg[1 + _y_start] = vars[_y_start]; fg[1 + _theta_start] = vars[_theta_start]; - fg[1 + _v_start] = vars[_v_start]; + fg[1 + _vx_start] = vars[_vx_start]; + fg[1 + _vy_start] = vars[_vy_start]; fg[1 + _cte_start] = vars[_cte_start]; fg[1 + _etheta_start] = vars[_etheta_start]; @@ -134,7 +146,9 @@ class HolonomicKinematicModel : public KinematicModel AD x1 = vars[_x_start + i + 1]; AD y1 = vars[_y_start + i + 1]; AD theta1 = vars[_theta_start + i + 1]; - AD v1 = vars[_v_start + i + 1]; + AD vx1 = vars[_vx_start + i + 1]; + AD vy1 = vars[_vy_start + i + 1]; + AD cte1 = vars[_cte_start + i + 1]; AD etheta1 = vars[_etheta_start + i + 1]; @@ -142,15 +156,17 @@ class HolonomicKinematicModel : public KinematicModel AD x0 = vars[_x_start + i]; AD y0 = vars[_y_start + i]; AD theta0 = vars[_theta_start + i]; - AD v0 = vars[_v_start + i]; + AD vx0 = vars[_vx_start + i]; + AD vy0 = vars[_vy_start + i]; + AD cte0 = vars[_cte_start + i]; AD etheta0 = vars[_etheta_start + i]; // Only consider the actuation at time t. //AD angvel0 = vars[_angvel_start + i]; AD w0 = vars[_angvel_start + i]; - AD a0 = vars[_a_start + i]; - + AD ax0 = vars[_ax_start + i]; + AD ay0 = vars[_ay_start + i]; //AD f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3); AD f0 = 0.0; @@ -174,13 +190,16 @@ class HolonomicKinematicModel : public KinematicModel // This is also CppAD can compute derivatives and pass // these to the solver. // TODO: Setup the rest of the model constraints - fg[2 + _x_start + i] = x1 - (x0 + v0 * CppAD::cos(theta0) * _dt); - fg[2 + _y_start + i] = y1 - (y0 + v0 * CppAD::sin(theta0) * _dt); - fg[2 + _theta_start + i] = theta1 - (theta0 + w0 * _dt); - fg[2 + _v_start + i] = v1 - (v0 + a0 * _dt); - fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(etheta0) * _dt)); - //fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt);//theta0-trj_grad0)->etheta : it can have more curvature prediction, but its gradient can be only adjust positive plan. - fg[2 + _etheta_start + i] = etheta1 - (etheta0 + w0 * _dt); + fg[2 + _x_start + i] = x1 - (x0 - (vy0 * CppAD::sin(theta0) - vx0 * CppAD::cos(theta0)) * _dt); + fg[2 + _y_start + i] = y1 - (y0 + (vy0 * CppAD::cos(theta0) + vx0 * CppAD::sin(theta0)) * _dt); + fg[2 + _theta_start + i] = theta1 - (theta0 + w0 * _dt); + fg[2 + _vx_start + i] = vx1 - (vx0 + ax0 * _dt); //ax0 * CppAD::sin(theta0) * dt + ay0 * CppAD::cos(theta0) * dt + fg[2 + _vy_start + i] = vy1 - (vy0 + ay0 * _dt); //ax0 * CppAD::cos(theta0) * dt + ay0 * CppAD::sin(theta0) * dt + + //fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (vx0 * CppAD::sin(etheta0) * _dt) + (vy0 * CppAD::cos(etheta0) * _dt)); + fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (vx0 * CppAD::sin(etheta0) * _dt)); + fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt); + } } }; diff --git a/mpc_ros/include/models/kinematic_model.h b/mpc_ros/include/models/kinematic_model.h index 85cf763..86a5e15 100644 --- a/mpc_ros/include/models/kinematic_model.h +++ b/mpc_ros/include/models/kinematic_model.h @@ -40,9 +40,10 @@ class KinematicModel double _dt, _ref_cte, _ref_etheta, _ref_vel; double _w_cte, _w_etheta, _w_vel, _w_angvel, _w_accel, _w_angvel_d, _w_accel_d; int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, _etheta_start, _angvel_start, _a_start; + int _vx_start, _vy_start, _ax_start, _ay_start; AD cost_cte, cost_etheta, cost_vel; - + AD cost_vel_x, cost_vel_y; // Constructor KinematicModel(){} diff --git a/mpc_ros/include/mpc_plannner.h b/mpc_ros/include/mpc_plannner.h index 4872cab..db64980 100644 --- a/mpc_ros/include/mpc_plannner.h +++ b/mpc_ros/include/mpc_plannner.h @@ -49,6 +49,8 @@ class MPC // Parameters for mpc solver double _max_angvel, _max_throttle, _bound_value; int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, _etheta_start, _angvel_start, _a_start; + int _vx_start, _vy_start, _cte_h_start, _etheta_h_start, _angvel_h_start, _ax_start, _ay_start; + std::map _params; std::string model_type; diff --git a/mpc_ros/include/mpc_plannner_ros.h b/mpc_ros/include/mpc_plannner_ros.h index 91e6b77..cdebe28 100644 --- a/mpc_ros/include/mpc_plannner_ros.h +++ b/mpc_ros/include/mpc_plannner_ros.h @@ -165,7 +165,7 @@ namespace mpc_ros{ _w_angvel, _w_accel, _w_angvel_d, _w_accel_d, max_vel_theta, acc_lim_trans, _bound_value; double controller_freq; //double _Lf; - double _dt, _w, _throttle, _speed, max_vel_trans; + double _dt, _w, _throttle_x, _throttle_y, _speed_x, _speed_y, max_vel_trans; double _pathLength, _goalRadius, _waypointsDist; int _downSampling; bool _debug_info, _delay_mode; diff --git a/mpc_ros/src/MPC.cpp b/mpc_ros/src/MPC.cpp index c911a89..2e36828 100644 --- a/mpc_ros/src/MPC.cpp +++ b/mpc_ros/src/MPC.cpp @@ -40,6 +40,7 @@ class FG_eval int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, _etheta_start, _angvel_start, _a_start; AD cost_cte, cost_etheta, cost_vel; + // Constructor FG_eval(Eigen::VectorXd coeffs) { diff --git a/mpc_ros/src/mpc_plannner.cpp b/mpc_ros/src/mpc_plannner.cpp index 752cd5e..1cad2e5 100644 --- a/mpc_ros/src/mpc_plannner.cpp +++ b/mpc_ros/src/mpc_plannner.cpp @@ -36,15 +36,31 @@ MPC::MPC() _max_throttle = 1.0; // Maximal throttle accel _bound_value = 1.0e3; // Bound value for other variables +//---------------------------------------------------- _x_start = 0; _y_start = _x_start + _mpc_steps; _theta_start = _y_start + _mpc_steps; - _v_start = _theta_start + _mpc_steps; + +//--------DDMR & Ackermann------------------------- + + _v_start = _theta_start + _mpc_steps; _cte_start = _v_start + _mpc_steps; + _etheta_start = _cte_start + _mpc_steps; _angvel_start = _etheta_start + _mpc_steps; _a_start = _angvel_start + _mpc_steps - 1; +//--------Holonomic-------------------------------- + + _vx_start = _theta_start + _mpc_steps; + _vy_start = _vx_start + _mpc_steps; + _cte_h_start = _vy_start + _mpc_steps; + _etheta_h_start = _cte_h_start + _mpc_steps; + _angvel_h_start = _etheta_h_start + _mpc_steps; + _ax_start = _angvel_h_start + _mpc_steps - 1; + _ay_start = _ax_start + _mpc_steps - 1; + + } void MPC::LoadParams(const std::map ¶ms) @@ -56,15 +72,31 @@ void MPC::LoadParams(const std::map ¶ms) _max_throttle = _params.find("MAXTHR") != _params.end() ? _params.at("MAXTHR") : _max_throttle; _bound_value = _params.find("BOUND") != _params.end() ? _params.at("BOUND") : _bound_value; + + //---------------------------------------------------- _x_start = 0; _y_start = _x_start + _mpc_steps; _theta_start = _y_start + _mpc_steps; - _v_start = _theta_start + _mpc_steps; + + //--------DDMR & Ackermann------------------------- + + _v_start = _theta_start + _mpc_steps; _cte_start = _v_start + _mpc_steps; + _etheta_start = _cte_start + _mpc_steps; _angvel_start = _etheta_start + _mpc_steps; _a_start = _angvel_start + _mpc_steps - 1; + //--------Holonomic-------------------------------- + _vx_start = _theta_start + _mpc_steps; + _vy_start = _vx_start + _mpc_steps; + _cte_h_start = _vy_start + _mpc_steps; + _etheta_h_start = _cte_h_start + _mpc_steps; + _angvel_h_start = _etheta_h_start + _mpc_steps; + _ax_start = _angvel_h_start + _mpc_steps - 1; + _ay_start = _ax_start + _mpc_steps - 1; + + cout << "\n!! MPC Obj parameters updated !! " << endl; } @@ -216,6 +248,7 @@ vector MPC::bicycleModelSolve(Eigen::VectorXd state, Eigen::VectorXd coe const double y = state[1]; const double theta = state[2]; const double v = state[3]; + const double cte = state[4]; const double etheta = state[5]; @@ -355,15 +388,16 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c const double x = state[0]; const double y = state[1]; const double theta = state[2]; - const double v = state[3]; - const double cte = state[4]; - const double etheta = state[5]; + const double v_x = state[3]; + const double v_y = state[4]; + const double cte = state[5]; + const double etheta = state[6]; // Set the number of model variables (includes both states and inputs). // For example: If the state is a 4 element vector, the actuators is a 2 // element vector and there are 10 timesteps. The number of variables is: // 4 * 10 + 2 * 9 - size_t n_vars = _mpc_steps * 6 + (_mpc_steps - 1) * 2; + size_t n_vars = _mpc_steps * 7 + (_mpc_steps - 1) * 3; // Set the number of constraints size_t n_constraints = _mpc_steps * 6; @@ -380,9 +414,10 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c vars[_x_start] = x; vars[_y_start] = y; vars[_theta_start] = theta; - vars[_v_start] = v; - vars[_cte_start] = cte; - vars[_etheta_start] = etheta; + vars[_vx_start] = v_x; + vars[_vy_start] = v_y; + vars[_cte_h_start] = cte; + vars[_etheta_h_start] = etheta; // Set lower and upper limits for variables. Dvector vars_lowerbound(n_vars); @@ -390,20 +425,27 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c // Set all non-actuators upper and lowerlimits // to the max negative and positive values. - for (int i = 0; i < _angvel_start; i++) + for (int i = 0; i < _angvel_h_start; i++) { vars_lowerbound[i] = -_bound_value; vars_upperbound[i] = _bound_value; } // The upper and lower limits of angvel are set to -25 and 25 // degrees (values in radians). - for (int i = _angvel_start; i < _a_start; i++) + for (int i = _angvel_h_start; i < _ax_start; i++) { vars_lowerbound[i] = -_max_angvel; vars_upperbound[i] = _max_angvel; } // Acceleration/decceleration upper and lower limits - for (int i = _a_start; i < n_vars; i++) + + for (int i = _ax_start; i < _ay_start; i++) + { + vars_lowerbound[i] = -_max_throttle; + vars_upperbound[i] = _max_throttle; + } + + for (int i = _ay_start; i < n_vars; i++) { vars_lowerbound[i] = -_max_throttle; vars_upperbound[i] = _max_throttle; @@ -422,15 +464,20 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c constraints_lowerbound[_x_start] = x; constraints_lowerbound[_y_start] = y; constraints_lowerbound[_theta_start] = theta; - constraints_lowerbound[_v_start] = v; - constraints_lowerbound[_cte_start] = cte; - constraints_lowerbound[_etheta_start] = etheta; + constraints_lowerbound[_vx_start] = v_x; + constraints_lowerbound[_vy_start] = v_y; + + constraints_lowerbound[_cte_h_start] = cte; + constraints_lowerbound[_etheta_h_start] = etheta; + constraints_upperbound[_x_start] = x; constraints_upperbound[_y_start] = y; constraints_upperbound[_theta_start] = theta; - constraints_upperbound[_v_start] = v; - constraints_upperbound[_cte_start] = cte; - constraints_upperbound[_etheta_start] = etheta; + constraints_upperbound[_vx_start] = v_x; + constraints_upperbound[_vy_start] = v_y; + + constraints_upperbound[_cte_h_start] = cte; + constraints_upperbound[_etheta_h_start] = etheta; // object that computes objective and constraints HolonomicKinematicModel model(coeffs); @@ -481,8 +528,10 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c } vector result; - result.push_back(solution.x[_angvel_start]); - result.push_back(solution.x[_a_start]); + result.push_back(solution.x[_angvel_h_start]); + result.push_back(solution.x[_ax_start]); + result.push_back(solution.x[_ay_start]); + return result; } } \ No newline at end of file diff --git a/mpc_ros/src/mpc_plannner_ros.cpp b/mpc_ros/src/mpc_plannner_ros.cpp index f996930..31e1c12 100644 --- a/mpc_ros/src/mpc_plannner_ros.cpp +++ b/mpc_ros/src/mpc_plannner_ros.cpp @@ -72,9 +72,11 @@ namespace mpc_ros{ //Init variables - _throttle = 0.0; + _throttle_x = 0.0; + _throttle_y = 0.0; _w = 0.0; - _speed = 0.0; + _speed_x = 0.0; + _speed_y = 0.0; //_ackermann_msg = ackermann_msgs::AckermannDriveStamped(); _twist_msg = geometry_msgs::Twist(); @@ -398,11 +400,15 @@ namespace mpc_ros{ tf::Pose pose; tf::poseMsgToTF(base_odom.pose.pose, pose); double theta = tf::getYaw(pose.getRotation()); - const double v = base_odom.twist.twist.linear.x; //twist: body fixed frame + const double v_x = base_odom.twist.twist.linear.x; //twist: body fixed frame + const double v_y = base_odom.twist.twist.linear.y; //twist: body fixed frame + // Update system inputs: U=[w, throttle] const double w = _w; // steering -> w //const double steering = _steering; // radian - const double throttle = _throttle; // accel: >0; brake: <0 + const double throttle_x = _throttle_x; // accel: >0; brake: <0 + const double throttle_y = _throttle_y; // accel: >0; brake: <0 + const double dt = _dt; //Update path waypoints (conversion to odom frame) @@ -526,60 +532,145 @@ namespace mpc_ros{ cout << "x_err:"<< x_err << ", y_err:"<< y_err << endl; - VectorXd state(6); - if(_delay_mode) - { - // Kinematic model is used to predict vehicle state at the actual moment of control (current time + delay dt) - const double px_act = v * dt; - const double py_act = 0; - const double theta_act = w * dt; //(steering) theta_act = v * steering * dt / Lf; - const double v_act = v + throttle * dt; //v = v + a * dt - - const double cte_act = cte + v * sin(etheta) * dt; - const double etheta_act = etheta - theta_act; - - state << px_act, py_act, theta_act, v_act, cte_act, etheta_act; - } - else - { - state << 0, 0, 0, v, cte, etheta; - } - - // Solve MPC Problem ros::Time begin = ros::Time::now(); vector mpc_results; + if(model_type == "unicycle") + { + VectorXd state(6); + if(_delay_mode) + { + // Kinematic model is used to predict vehicle state at the actual moment of control (current time + delay dt) + const double px_act = v_x * dt; + const double py_act = 0; + const double theta_act = w * dt; //(steering) theta_act = v * steering * dt / Lf; + const double vx_act = v_x + throttle_x * dt; //v = v + a * dt + + const double cte_act = cte + v_x * sin(etheta) * dt; + const double etheta_act = etheta - theta_act; + + state << px_act, py_act, theta_act, vx_act, cte_act, etheta_act; + } + else + { + state << 0, 0, 0, v_x, cte, etheta; + } + // Solve MPC Problem mpc_results = _mpc.unicycleModelSolve(state, coeffs); + + } + else if(model_type == "bicycle") + { + VectorXd state(6); + if(_delay_mode) + { + // Kinematic model is used to predict vehicle state at the actual moment of control (current time + delay dt) + const double px_act = v_x * dt; + const double py_act = 0; + const double theta_act = w * dt; //(steering) theta_act = v * steering * dt / Lf; + const double vx_act = v_x + throttle_x * dt; //v = v + a * dt + + const double cte_act = cte + v_x * sin(etheta) * dt; + const double etheta_act = etheta - theta_act; + + state << px_act, py_act, theta_act, vx_act, cte_act, etheta_act; + } + else + { + state << 0, 0, 0, v_x, cte, etheta; + } + // Solve MPC Problem mpc_results = _mpc.bicycleModelSolve(state, coeffs); + + } + else if(model_type == "holonimic") + { + VectorXd state(7); + if(_delay_mode) + { + // Kinematic model is used to predict vehicle state at the actual moment of control (current time + delay dt) + const double px_act = v_x * dt; + const double py_act = v_y * dt; + const double theta_act = w * dt; //(steering) theta_act = v * steering * dt / Lf; + const double vx_act = v_x + throttle_x * dt; //v = v + a * dt + const double vy_act = v_y + throttle_y * dt; //v = v + a * dt + + //const double cte_act = cte + vx * sin(etheta) * dt + vy * cos(etheta) * dt; + const double cte_act = cte + v_x * sin(etheta) * dt; + const double etheta_act = etheta - theta_act; + + state << px_act, py_act, theta_act, vx_act, vy_act, cte_act, etheta_act; + } + else + { + state << 0, 0, 0, v_x, v_y, cte, etheta; + } + + // Solve MPC Problem mpc_results = _mpc.holonomicModelSolve(state, coeffs); + + } + ros::Time end = ros::Time::now(); cout << "Duration: " << end.sec << "." << end.nsec << endl << begin.sec<< "." << begin.nsec << endl; // MPC result (all described in car frame), output = (acceleration, w) - _w = mpc_results[0]; // radian/sec, angular velocity - _throttle = mpc_results[1]; // acceleration + if(model_type != "holonimic") + { + _w = mpc_results[0]; // radian/sec, angular velocity + _throttle_x = mpc_results[1]; // acceleration + _throttle_y = 0; // acceleration - _speed = v + _throttle * dt; // speed - if (_speed >= max_vel_trans) - _speed = max_vel_trans; - if(_speed <= 0.0) - _speed = 0.0; + _speed_x = v_x + _throttle_x * dt; // speed + _speed_x = 0; // speed + + if(_speed_x >= max_vel_trans) + _speed_x = max_vel_trans; + if(_speed_x <= 0.0) + _speed_x = 0.0; + } + + else + { + _w = mpc_results[0]; // radian/sec, angular velocity + _throttle_x = mpc_results[1]; // acceleration + _throttle_y = mpc_results[2]; // acceleration + + _speed_x = v_x + _throttle_x * dt; // speed + _speed_y = v_y + _throttle_y * dt; // speed + + if(_speed_x >= max_vel_trans) + _speed_x = max_vel_trans; + if(_speed_x <= -max_vel_trans) + _speed_x = -max_vel_trans; + + if (_speed_y >= max_vel_trans) + _speed_y = max_vel_trans; + if(_speed_y <= -max_vel_trans) + _speed_y = -max_vel_trans; + } if(_debug_info) { cout << "\n\nDEBUG" << endl; cout << "theta: " << theta << endl; - cout << "V: " << v << endl; + cout << "V_x: " << v_x << endl; + cout << "V_y: " << v_y << endl; + //cout << "odom_path: \n" << odom_path << endl; //cout << "x_points: \n" << x_veh << endl; //cout << "y_points: \n" << y_veh << endl; cout << "coeffs: \n" << coeffs << endl; cout << "_w: \n" << _w << endl; - cout << "_throttle: \n" << _throttle << endl; - cout << "_speed: \n" << _speed << endl; + cout << "_throttle_x: \n" << _throttle_x << endl; + cout << "_throttle_y: \n" << _throttle_y << endl; + + cout << "_speed_x: \n" << _speed_x << endl; + cout << "_speed_y: \n" << _speed_y << endl; } + // Display the MPC predicted trajectory _mpc_traj = nav_msgs::Path(); _mpc_traj.header.frame_id = _base_frame; // points in car coordinate @@ -612,9 +703,10 @@ namespace mpc_ros{ drive_velocities.pose.orientation.y = 0; drive_velocities.pose.orientation.z = 0; } + else{ - drive_velocities.pose.position.x = _speed; - drive_velocities.pose.position.y = 0; + drive_velocities.pose.position.x = _speed_x; + drive_velocities.pose.position.y = _speed_y; drive_velocities.pose.position.z = 0; tf2::Quaternion q; q.setRPY(0, 0, _w); @@ -670,6 +762,7 @@ namespace mpc_ros{ if(latchedStopRotateController_.isGoalReached(&planner_util_, odom_helper_, current_pose_)) { ROS_INFO("Goal reached"); return true; + } else { return false; } From dee3996565f55a2e195deaf0ac52c39124bde6c1 Mon Sep 17 00:00:00 2001 From: CzJaewan Date: Tue, 16 Feb 2021 19:25:38 +0900 Subject: [PATCH 2/7] add holonomic launch --- .../launch/holonomic_mpc_local_planner.launch | 2 +- mpc_ros/params/mpc_holonomic_params.yaml | 72 +++++++++++++++++++ mpc_ros/src/mpc_plannner_ros.cpp | 6 +- 3 files changed, 76 insertions(+), 4 deletions(-) create mode 100644 mpc_ros/params/mpc_holonomic_params.yaml diff --git a/mpc_ros/launch/holonomic_mpc_local_planner.launch b/mpc_ros/launch/holonomic_mpc_local_planner.launch index 046dad3..ad396e9 100644 --- a/mpc_ros/launch/holonomic_mpc_local_planner.launch +++ b/mpc_ros/launch/holonomic_mpc_local_planner.launch @@ -76,7 +76,7 @@ diff --git a/mpc_ros/params/mpc_holonomic_params.yaml b/mpc_ros/params/mpc_holonomic_params.yaml new file mode 100644 index 0000000..61c2e2d --- /dev/null +++ b/mpc_ros/params/mpc_holonomic_params.yaml @@ -0,0 +1,72 @@ + + +MPCPlannerROS: +# Robot Configuration Parameters + map_frame: "map" + odom_frame: "odom" + base_frame: "base_footprint" + model_type: "holonomic" + + max_vel_trans: 1.0 + min_vel_trans: 0.01 + + max_vel_x: 0.5 + min_vel_x: -0.05 + + max_vel_y: 0.0 + min_vel_y: 0.0 + + # The velocity when robot is moving in a straight line + trans_stopped_vel: 0.01 + + max_vel_theta: 1.5 + min_vel_theta: 0.1 + theta_stopped_vel: 0.01 + + acc_lim_x: 2.0 + acc_lim_y: 0.0 + acc_lim_theta: 3.0 + + prune_plan: true + +# Goal Tolerance Parametes + yaw_goal_tolerance: 1.0 + xy_goal_tolerance: 0.5 + latch_xy_goal_tolerance: false + +# Trajectory Scoring Parameters + path_distance_bias: 32.0 + goal_distance_bias: 20.0 + occdist_scale: 0.02 + forward_point_distance: 0.325 # when 0 is, off map warn disappear + stop_time_buffer: 0.2 + scaling_speed: 0.25 + max_scaling_factor: 0.2 + +# Oscillation Prevention Parameters + oscillation_reset_dist: 0.05 + +# Debugging + publish_traj_pc : true + publish_cost_grid_pc: true + # global_frame_id: odom + + # Parameters for control loop + debug_info: false + delay_mode: true + waypoints_dist: -1.0 # unit: m, set < 0 means computed by node + path_length: 5.0 # unit: m + + # Parameter for MPC solver + steps: 10.0 + ref_cte: 0.0 + ref_vel: 1.0 #1.0 + ref_etheta: 0.0 + w_cte: 1000.0 + w_etheta: 0.0 + w_vel: 10.0 #100.0 + w_angvel: 2000.0 #100.0 + w_angvel_d: 0.0 + w_accel: 50.0 + w_accel_d: 10.0 + bound_value: 1.0e3 # Bound value for other variables diff --git a/mpc_ros/src/mpc_plannner_ros.cpp b/mpc_ros/src/mpc_plannner_ros.cpp index 31e1c12..d13ff6c 100644 --- a/mpc_ros/src/mpc_plannner_ros.cpp +++ b/mpc_ros/src/mpc_plannner_ros.cpp @@ -585,7 +585,7 @@ namespace mpc_ros{ } - else if(model_type == "holonimic") + else if(model_type == "holonoimic") { VectorXd state(7); if(_delay_mode) @@ -617,7 +617,7 @@ namespace mpc_ros{ cout << "Duration: " << end.sec << "." << end.nsec << endl << begin.sec<< "." << begin.nsec << endl; // MPC result (all described in car frame), output = (acceleration, w) - if(model_type != "holonimic") + if(model_type != "holonoimic") { _w = mpc_results[0]; // radian/sec, angular velocity _throttle_x = mpc_results[1]; // acceleration @@ -703,7 +703,7 @@ namespace mpc_ros{ drive_velocities.pose.orientation.y = 0; drive_velocities.pose.orientation.z = 0; } - + else{ drive_velocities.pose.position.x = _speed_x; drive_velocities.pose.position.y = _speed_y; From ba06cf52815be1b4e3f5b1d72ead2ccd36c29061 Mon Sep 17 00:00:00 2001 From: CzJaewan Date: Tue, 16 Feb 2021 19:46:25 +0900 Subject: [PATCH 3/7] modify non-holonomic model err --- mpc_ros/include/models/holonomic_kinematic_model.h | 1 - mpc_ros/launch/holonomic_mpc_local_planner.launch | 2 +- mpc_ros/src/mpc_plannner_ros.cpp | 9 ++++++--- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/mpc_ros/include/models/holonomic_kinematic_model.h b/mpc_ros/include/models/holonomic_kinematic_model.h index 029b18f..4f84728 100644 --- a/mpc_ros/include/models/holonomic_kinematic_model.h +++ b/mpc_ros/include/models/holonomic_kinematic_model.h @@ -127,7 +127,6 @@ class HolonomicKinematicModel : public KinematicModel fg[0] += _w_accel_d * CppAD::pow(vars[_ay_start + i + 1] - vars[_ay_start + i], 2); } - // fg[x] for constraints // Initial constraints diff --git a/mpc_ros/launch/holonomic_mpc_local_planner.launch b/mpc_ros/launch/holonomic_mpc_local_planner.launch index ad396e9..4f4a7dc 100644 --- a/mpc_ros/launch/holonomic_mpc_local_planner.launch +++ b/mpc_ros/launch/holonomic_mpc_local_planner.launch @@ -66,7 +66,7 @@ - + diff --git a/mpc_ros/src/mpc_plannner_ros.cpp b/mpc_ros/src/mpc_plannner_ros.cpp index d13ff6c..7770d21 100644 --- a/mpc_ros/src/mpc_plannner_ros.cpp +++ b/mpc_ros/src/mpc_plannner_ros.cpp @@ -531,6 +531,7 @@ namespace mpc_ros{ const double goal_err = sqrt(x_err*x_err + y_err*y_err); cout << "x_err:"<< x_err << ", y_err:"<< y_err << endl; + cout << model_type << endl; ros::Time begin = ros::Time::now(); vector mpc_results; @@ -587,6 +588,7 @@ namespace mpc_ros{ else if(model_type == "holonoimic") { + VectorXd state(7); if(_delay_mode) { @@ -624,7 +626,7 @@ namespace mpc_ros{ _throttle_y = 0; // acceleration _speed_x = v_x + _throttle_x * dt; // speed - _speed_x = 0; // speed + _speed_y = 0; // speed if(_speed_x >= max_vel_trans) _speed_x = max_vel_trans; @@ -634,6 +636,7 @@ namespace mpc_ros{ else { + _w = mpc_results[0]; // radian/sec, angular velocity _throttle_x = mpc_results[1]; // acceleration _throttle_y = mpc_results[2]; // acceleration @@ -643,12 +646,12 @@ namespace mpc_ros{ if(_speed_x >= max_vel_trans) _speed_x = max_vel_trans; - if(_speed_x <= -max_vel_trans) + if(_speed_x < -max_vel_trans) _speed_x = -max_vel_trans; if (_speed_y >= max_vel_trans) _speed_y = max_vel_trans; - if(_speed_y <= -max_vel_trans) + if(_speed_y < -max_vel_trans) _speed_y = -max_vel_trans; } From f8512a5a7f5e5e760e87e7456d0fe820c1c42ea4 Mon Sep 17 00:00:00 2001 From: CzJaewan Date: Wed, 17 Feb 2021 19:40:02 +0900 Subject: [PATCH 4/7] modify holonomic model err --- .../models/holonomic_kinematic_model.h | 2 +- .../launch/holonomic_mpc_local_planner.launch | 2 +- mpc_ros/params/mpc_holonomic_params.yaml | 12 ++++++------ mpc_ros/src/mpc_plannner.cpp | 19 ++++++++++++++++++- mpc_ros/src/mpc_plannner_ros.cpp | 11 +++++++++-- 5 files changed, 35 insertions(+), 11 deletions(-) diff --git a/mpc_ros/include/models/holonomic_kinematic_model.h b/mpc_ros/include/models/holonomic_kinematic_model.h index 4f84728..bf97f1d 100644 --- a/mpc_ros/include/models/holonomic_kinematic_model.h +++ b/mpc_ros/include/models/holonomic_kinematic_model.h @@ -195,7 +195,7 @@ class HolonomicKinematicModel : public KinematicModel fg[2 + _vx_start + i] = vx1 - (vx0 + ax0 * _dt); //ax0 * CppAD::sin(theta0) * dt + ay0 * CppAD::cos(theta0) * dt fg[2 + _vy_start + i] = vy1 - (vy0 + ay0 * _dt); //ax0 * CppAD::cos(theta0) * dt + ay0 * CppAD::sin(theta0) * dt - //fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (vx0 * CppAD::sin(etheta0) * _dt) + (vy0 * CppAD::cos(etheta0) * _dt)); + //fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (vx0 * CppAD::sin(etheta0) * _dt)) - ((f0 - x0) + (vy0 * CppAD::cos(etheta0) * _dt)); fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (vx0 * CppAD::sin(etheta0) * _dt)); fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt); diff --git a/mpc_ros/launch/holonomic_mpc_local_planner.launch b/mpc_ros/launch/holonomic_mpc_local_planner.launch index 4f4a7dc..b62ed1b 100644 --- a/mpc_ros/launch/holonomic_mpc_local_planner.launch +++ b/mpc_ros/launch/holonomic_mpc_local_planner.launch @@ -12,7 +12,7 @@ - + diff --git a/mpc_ros/params/mpc_holonomic_params.yaml b/mpc_ros/params/mpc_holonomic_params.yaml index 61c2e2d..8e96bcb 100644 --- a/mpc_ros/params/mpc_holonomic_params.yaml +++ b/mpc_ros/params/mpc_holonomic_params.yaml @@ -8,13 +8,13 @@ MPCPlannerROS: model_type: "holonomic" max_vel_trans: 1.0 - min_vel_trans: 0.01 + min_vel_trans: -1.0 max_vel_x: 0.5 - min_vel_x: -0.05 + min_vel_x: -0.5 - max_vel_y: 0.0 - min_vel_y: 0.0 + max_vel_y: 0.5 + min_vel_y: -0.5 # The velocity when robot is moving in a straight line trans_stopped_vel: 0.01 @@ -24,7 +24,7 @@ MPCPlannerROS: theta_stopped_vel: 0.01 acc_lim_x: 2.0 - acc_lim_y: 0.0 + acc_lim_y: 2.0 acc_lim_theta: 3.0 prune_plan: true @@ -65,7 +65,7 @@ MPCPlannerROS: w_cte: 1000.0 w_etheta: 0.0 w_vel: 10.0 #100.0 - w_angvel: 2000.0 #100.0 + w_angvel: 1000.0 #100.0 w_angvel_d: 0.0 w_accel: 50.0 w_accel_d: 10.0 diff --git a/mpc_ros/src/mpc_plannner.cpp b/mpc_ros/src/mpc_plannner.cpp index 1cad2e5..0b697c8 100644 --- a/mpc_ros/src/mpc_plannner.cpp +++ b/mpc_ros/src/mpc_plannner.cpp @@ -56,6 +56,7 @@ MPC::MPC() _vy_start = _vx_start + _mpc_steps; _cte_h_start = _vy_start + _mpc_steps; _etheta_h_start = _cte_h_start + _mpc_steps; + _angvel_h_start = _etheta_h_start + _mpc_steps; _ax_start = _angvel_h_start + _mpc_steps - 1; _ay_start = _ax_start + _mpc_steps - 1; @@ -382,6 +383,8 @@ vector MPC::bicycleModelSolve(Eigen::VectorXd state, Eigen::VectorXd coe vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd coeffs) { + cout << "Start holonomic solve" << endl; + bool ok = true; size_t i; typedef CPPAD_TESTVECTOR(double) Dvector; @@ -400,7 +403,7 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c size_t n_vars = _mpc_steps * 7 + (_mpc_steps - 1) * 3; // Set the number of constraints - size_t n_constraints = _mpc_steps * 6; + size_t n_constraints = _mpc_steps * 7; // Initial value of the independent variables. // SHOULD BE 0 besides initial state. @@ -479,10 +482,19 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c constraints_upperbound[_cte_h_start] = cte; constraints_upperbound[_etheta_h_start] = etheta; + cout << "constraint set" << endl; + + // object that computes objective and constraints HolonomicKinematicModel model(coeffs); + + cout << "holonomic model" << endl; + model.loadParams(_params); + cout << "holonomic model loadParam" << endl; + + // options for IPOPT solver std::string options; // Uncomment this if you'd like more print information @@ -498,6 +510,9 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c // Change this as you see fit. options += "Numeric max_cpu_time 0.5\n"; + + cout << "set option" << endl; + // place to return solution CppAD::ipopt::solve_result solution; @@ -509,6 +524,8 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c // Check some of the solution values ok &= solution.status == CppAD::ipopt::solve_result::success; + cout << "get solve" << endl; + // Cost auto cost = solution.obj_value; std::cout << "------------ Total Cost(solution): " << cost << "------------" << std::endl; diff --git a/mpc_ros/src/mpc_plannner_ros.cpp b/mpc_ros/src/mpc_plannner_ros.cpp index 7770d21..ca3379e 100644 --- a/mpc_ros/src/mpc_plannner_ros.cpp +++ b/mpc_ros/src/mpc_plannner_ros.cpp @@ -586,7 +586,7 @@ namespace mpc_ros{ } - else if(model_type == "holonoimic") + else if(model_type == "holonomic") { VectorXd state(7); @@ -619,7 +619,7 @@ namespace mpc_ros{ cout << "Duration: " << end.sec << "." << end.nsec << endl << begin.sec<< "." << begin.nsec << endl; // MPC result (all described in car frame), output = (acceleration, w) - if(model_type != "holonoimic") + if(model_type != "holonomic") { _w = mpc_results[0]; // radian/sec, angular velocity _throttle_x = mpc_results[1]; // acceleration @@ -636,6 +636,7 @@ namespace mpc_ros{ else { + cout << "holonomic results" << endl; _w = mpc_results[0]; // radian/sec, angular velocity _throttle_x = mpc_results[1]; // acceleration @@ -644,6 +645,12 @@ namespace mpc_ros{ _speed_x = v_x + _throttle_x * dt; // speed _speed_y = v_y + _throttle_y * dt; // speed + cout << "thorottle_x" << _throttle_x <= max_vel_trans) _speed_x = max_vel_trans; if(_speed_x < -max_vel_trans) From a0485d1e0a1fd27d89c17f9e3f7410b9dadfa617 Mon Sep 17 00:00:00 2001 From: Geonhee Date: Thu, 18 Feb 2021 08:30:35 +0900 Subject: [PATCH 5/7] Delete MPC.cpp --- mpc_ros/src/MPC.cpp | 399 -------------------------------------------- 1 file changed, 399 deletions(-) delete mode 100644 mpc_ros/src/MPC.cpp diff --git a/mpc_ros/src/MPC.cpp b/mpc_ros/src/MPC.cpp deleted file mode 100644 index 2e36828..0000000 --- a/mpc_ros/src/MPC.cpp +++ /dev/null @@ -1,399 +0,0 @@ - -/* -# Copyright 2018 HyphaROS Workshop. -# Developer: HaoChih, LIN (hypha.ros@gmail.com) -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -*/ - -#include "MPC.h" -//#include -#include -#include - -// The program use fragments of code from -// https://github.com/udacity/CarND-MPC-Quizzes - -using CppAD::AD; - -// ========================================= -// FG_eval class definition implementation. -// ========================================= -class FG_eval -{ - public: - // Fitted polynomial coefficients - Eigen::VectorXd coeffs; - - double _dt, _ref_cte, _ref_etheta, _ref_vel; - double _w_cte, _w_etheta, _w_vel, _w_angvel, _w_accel, _w_angvel_d, _w_accel_d; - int _mpc_steps, _x_start, _y_start, _theta_start, _v_start, _cte_start, _etheta_start, _angvel_start, _a_start; - - AD cost_cte, cost_etheta, cost_vel; - - // Constructor - FG_eval(Eigen::VectorXd coeffs) - { - this->coeffs = coeffs; - - // Set default value - _dt = 0.1; // in sec - _ref_cte = 0; - _ref_etheta = 0; - _ref_vel = 0.5; // m/s - _w_cte = 100; - _w_etheta = 100; - _w_vel = 1; - _w_angvel = 100; - _w_accel = 50; - _w_angvel_d = 0; - _w_accel_d = 0; - - _mpc_steps = 40; - _x_start = 0; - _y_start = _x_start + _mpc_steps; - _theta_start = _y_start + _mpc_steps; - _v_start = _theta_start + _mpc_steps; - _cte_start = _v_start + _mpc_steps; - _etheta_start = _cte_start + _mpc_steps; - _angvel_start = _etheta_start + _mpc_steps; - _a_start = _angvel_start + _mpc_steps - 1; - } - - // Load parameters for constraints - void LoadParams(const std::map ¶ms) - { - _dt = params.find("DT") != params.end() ? params.at("DT") : _dt; - _mpc_steps = params.find("STEPS") != params.end() ? params.at("STEPS") : _mpc_steps; - _ref_cte = params.find("REF_CTE") != params.end() ? params.at("REF_CTE") : _ref_cte; - _ref_etheta = params.find("REF_ETHETA") != params.end() ? params.at("REF_ETHETA") : _ref_etheta; - _ref_vel = params.find("REF_V") != params.end() ? params.at("REF_V") : _ref_vel; - - _w_cte = params.find("W_CTE") != params.end() ? params.at("W_CTE") : _w_cte; - _w_etheta = params.find("W_EPSI") != params.end() ? params.at("W_EPSI") : _w_etheta; - _w_vel = params.find("W_V") != params.end() ? params.at("W_V") : _w_vel; - _w_angvel = params.find("W_ANGVEL") != params.end() ? params.at("W_ANGVEL") : _w_angvel; - _w_accel = params.find("W_A") != params.end() ? params.at("W_A") : _w_accel; - _w_angvel_d = params.find("W_DANGVEL") != params.end() ? params.at("W_DANGVEL") : _w_angvel_d; - _w_accel_d = params.find("W_DA") != params.end() ? params.at("W_DA") : _w_accel_d; - - _x_start = 0; - _y_start = _x_start + _mpc_steps; - _theta_start = _y_start + _mpc_steps; - _v_start = _theta_start + _mpc_steps; - _cte_start = _v_start + _mpc_steps; - _etheta_start = _cte_start + _mpc_steps; - _angvel_start = _etheta_start + _mpc_steps; - _a_start = _angvel_start + _mpc_steps - 1; - - //cout << "\n!! FG_eval Obj parameters updated !! " << _mpc_steps << endl; - } - - // MPC implementation (cost func & constraints) - typedef CPPAD_TESTVECTOR(AD) ADvector; - // fg: function that evaluates the objective and constraints using the syntax - void operator()(ADvector& fg, const ADvector& vars) - { - // fg[0] for cost function - fg[0] = 0; - cost_cte = 0; - cost_etheta = 0; - cost_vel = 0; - - /* - for (int i = 0; i < _mpc_steps; i++) - { - cout << i << endl; - cout << "_x_start" << vars[_x_start + i] < x1 = vars[_x_start + i + 1]; - AD y1 = vars[_y_start + i + 1]; - AD theta1 = vars[_theta_start + i + 1]; - AD v1 = vars[_v_start + i + 1]; - AD cte1 = vars[_cte_start + i + 1]; - AD etheta1 = vars[_etheta_start + i + 1]; - - // The state at time t. - AD x0 = vars[_x_start + i]; - AD y0 = vars[_y_start + i]; - AD theta0 = vars[_theta_start + i]; - AD v0 = vars[_v_start + i]; - AD cte0 = vars[_cte_start + i]; - AD etheta0 = vars[_etheta_start + i]; - - // Only consider the actuation at time t. - //AD angvel0 = vars[_angvel_start + i]; - AD w0 = vars[_angvel_start + i]; - AD a0 = vars[_a_start + i]; - - - //AD f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3); - AD f0 = 0.0; - for (int i = 0; i < coeffs.size(); i++) - { - f0 += coeffs[i] * CppAD::pow(x0, i); - } - - //AD trj_grad0 = CppAD::atan(coeffs[1] + 2 * coeffs[2] * x0 + 3 * coeffs[3] * CppAD::pow(x0, 2)); - AD trj_grad0 = 0.0; - for (int i = 1; i < coeffs.size(); i++) - { - trj_grad0 += i*coeffs[i] * CppAD::pow(x0, i-1); // f'(x0) - } - trj_grad0 = CppAD::atan(trj_grad0); - - - // Here's `x` to get you started. - // The idea here is to constraint this value to be 0. - // - // NOTE: The use of `AD` and use of `CppAD`! - // This is also CppAD can compute derivatives and pass - // these to the solver. - // TODO: Setup the rest of the model constraints - fg[2 + _x_start + i] = x1 - (x0 + v0 * CppAD::cos(theta0) * _dt); - fg[2 + _y_start + i] = y1 - (y0 + v0 * CppAD::sin(theta0) * _dt); - fg[2 + _theta_start + i] = theta1 - (theta0 + w0 * _dt); - fg[2 + _v_start + i] = v1 - (v0 + a0 * _dt); - - fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (v0 * CppAD::sin(etheta0) * _dt)); - fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt); - } - } -}; - -// ==================================== -// MPC class definition implementation. -// ==================================== -MPC::MPC() -{ - // Set default value - _mpc_steps = 20; - _max_angvel = 3.0; // Maximal angvel radian (~30 deg) - _max_throttle = 1.0; // Maximal throttle accel - _bound_value = 1.0e3; // Bound value for other variables - - _x_start = 0; - _y_start = _x_start + _mpc_steps; - _theta_start = _y_start + _mpc_steps; - _v_start = _theta_start + _mpc_steps; - _cte_start = _v_start + _mpc_steps; - _etheta_start = _cte_start + _mpc_steps; - _angvel_start = _etheta_start + _mpc_steps; - _a_start = _angvel_start + _mpc_steps - 1; - -} - -void MPC::LoadParams(const std::map ¶ms) -{ - _params = params; - //Init parameters for MPC object - _mpc_steps = _params.find("STEPS") != _params.end() ? _params.at("STEPS") : _mpc_steps; - _max_angvel = _params.find("ANGVEL") != _params.end() ? _params.at("ANGVEL") : _max_angvel; - _max_throttle = _params.find("MAXTHR") != _params.end() ? _params.at("MAXTHR") : _max_throttle; - _bound_value = _params.find("BOUND") != _params.end() ? _params.at("BOUND") : _bound_value; - - _x_start = 0; - _y_start = _x_start + _mpc_steps; - _theta_start = _y_start + _mpc_steps; - _v_start = _theta_start + _mpc_steps; - _cte_start = _v_start + _mpc_steps; - _etheta_start = _cte_start + _mpc_steps; - _angvel_start = _etheta_start + _mpc_steps; - _a_start = _angvel_start + _mpc_steps - 1; - - cout << "\n!! MPC Obj parameters updated !! " << endl; -} - - -vector MPC::Solve(Eigen::VectorXd state, Eigen::VectorXd coeffs) -{ - bool ok = true; - size_t i; - typedef CPPAD_TESTVECTOR(double) Dvector; - const double x = state[0]; - const double y = state[1]; - const double theta = state[2]; - const double v = state[3]; - const double cte = state[4]; - const double etheta = state[5]; - - // Set the number of model variables (includes both states and inputs). - // For example: If the state is a 4 element vector, the actuators is a 2 - // element vector and there are 10 timesteps. The number of variables is: - // 4 * 10 + 2 * 9 - size_t n_vars = _mpc_steps * 6 + (_mpc_steps - 1) * 2; - - // Set the number of constraints - size_t n_constraints = _mpc_steps * 6; - - // Initial value of the independent variables. - // SHOULD BE 0 besides initial state. - Dvector vars(n_vars); - for (int i = 0; i < n_vars; i++) - { - vars[i] = 0; - } - - // Set the initial variable values - vars[_x_start] = x; - vars[_y_start] = y; - vars[_theta_start] = theta; - vars[_v_start] = v; - vars[_cte_start] = cte; - vars[_etheta_start] = etheta; - - // Set lower and upper limits for variables. - Dvector vars_lowerbound(n_vars); - Dvector vars_upperbound(n_vars); - - // Set all non-actuators upper and lowerlimits - // to the max negative and positive values. - for (int i = 0; i < _angvel_start; i++) - { - vars_lowerbound[i] = -_bound_value; - vars_upperbound[i] = _bound_value; - } - - // The upper and lower limits of angvel are set to -25 and 25 - // degrees (values in radians). - for (int i = _angvel_start; i < _a_start; i++) - { - vars_lowerbound[i] = -_max_angvel; - vars_upperbound[i] = _max_angvel; - } - // Acceleration/decceleration upper and lower limits - for (int i = _a_start; i < n_vars; i++) - { - vars_lowerbound[i] = -_max_throttle; - vars_upperbound[i] = _max_throttle; - } - - - // Lower and upper limits for the constraints - // Should be 0 besides initial state. - Dvector constraints_lowerbound(n_constraints); - Dvector constraints_upperbound(n_constraints); - for (int i = 0; i < n_constraints; i++) - { - constraints_lowerbound[i] = 0; - constraints_upperbound[i] = 0; - } - constraints_lowerbound[_x_start] = x; - constraints_lowerbound[_y_start] = y; - constraints_lowerbound[_theta_start] = theta; - constraints_lowerbound[_v_start] = v; - constraints_lowerbound[_cte_start] = cte; - constraints_lowerbound[_etheta_start] = etheta; - constraints_upperbound[_x_start] = x; - constraints_upperbound[_y_start] = y; - constraints_upperbound[_theta_start] = theta; - constraints_upperbound[_v_start] = v; - constraints_upperbound[_cte_start] = cte; - constraints_upperbound[_etheta_start] = etheta; - - // object that computes objective and constraints - FG_eval fg_eval(coeffs); - fg_eval.LoadParams(_params); - - - // options for IPOPT solver - std::string options; - // Uncomment this if you'd like more print information - options += "Integer print_level 0\n"; - // NOTE: Setting sparse to true allows the solver to take advantage - // of sparse routines, this makes the computation MUCH FASTER. If you - // can uncomment 1 of these and see if it makes a difference or not but - // if you uncomment both the computation time should go up in orders of - // magnitude. - options += "Sparse true forward\n"; - options += "Sparse true reverse\n"; - // NOTE: Currently the solver has a maximum time limit of 0.5 seconds. - // Change this as you see fit. - options += "Numeric max_cpu_time 0.5\n"; - - // place to return solution - CppAD::ipopt::solve_result solution; - - // solve the problem - CppAD::ipopt::solve( - options, vars, vars_lowerbound, vars_upperbound, constraints_lowerbound, - constraints_upperbound, fg_eval, solution); - - // Check some of the solution values - ok &= solution.status == CppAD::ipopt::solve_result::success; - - // Cost - auto cost = solution.obj_value; - std::cout << "------------ Total Cost(solution): " << cost << "------------" << std::endl; - cout << "-----------------------------------------------" <mpc_x = {}; - this->mpc_y = {}; - for (int i = 0; i < _mpc_steps; i++) - { - this->mpc_x.push_back(solution.x[_x_start + i]); - this->mpc_y.push_back(solution.x[_y_start + i]); - } - vector result; - result.push_back(solution.x[_angvel_start]); - result.push_back(solution.x[_a_start]); - return result; -} From 5208438319b087ad5c17844cd4fe5fd745afd5ae Mon Sep 17 00:00:00 2001 From: CzJaewan Date: Fri, 5 Mar 2021 09:15:18 +0900 Subject: [PATCH 6/7] 2021_3_5 --- .../ipopt_install/ipopt_x86_install_tutorial.md | 2 ++ mpc_ros/CMakeLists.txt | 4 ++-- .../include/models/holonomic_kinematic_model.h | 8 ++++++-- mpc_ros/launch/holonomic_mpc_local_planner.launch | 8 ++++---- mpc_ros/launch/mpc_local_planner.launch | 4 ++-- mpc_ros/params/mpc_holonomic_params.yaml | 10 ++++------ mpc_ros/src/mpc_plannner.cpp | 1 + mpc_ros/src/mpc_plannner_ros.cpp | 15 +++++++++------ 8 files changed, 30 insertions(+), 22 deletions(-) diff --git a/assets/document/ipopt_install/ipopt_x86_install_tutorial.md b/assets/document/ipopt_install/ipopt_x86_install_tutorial.md index 71c8f9e..d968c33 100644 --- a/assets/document/ipopt_install/ipopt_x86_install_tutorial.md +++ b/assets/document/ipopt_install/ipopt_x86_install_tutorial.md @@ -36,3 +36,5 @@ cd CUSTOM_PATH/Ipopt-3.12.8/build sudo cp -a include/* /usr/include/. sudo cp -a lib/* /usr/lib/. ``` + + diff --git a/mpc_ros/CMakeLists.txt b/mpc_ros/CMakeLists.txt index 90eb204..18e3e2b 100644 --- a/mpc_ros/CMakeLists.txt +++ b/mpc_ros/CMakeLists.txt @@ -52,8 +52,8 @@ catkin_package( ########### # General MPC_Node -ADD_EXECUTABLE( MPC_Node src/MPC.cpp src/MPC_Node.cpp ) -TARGET_LINK_LIBRARIES(MPC_Node ipopt ${catkin_LIBRARIES} ) +# ADD_EXECUTABLE( MPC_Node src/MPC.cpp src/MPC_Node.cpp ) +# TARGET_LINK_LIBRARIES(MPC_Node ipopt ${catkin_LIBRARIES} ) # Total navigation with MPC_Node ADD_EXECUTABLE( nav_mpc src/navMPC.cpp src/navMPCNode.cpp ) diff --git a/mpc_ros/include/models/holonomic_kinematic_model.h b/mpc_ros/include/models/holonomic_kinematic_model.h index bf97f1d..3fc1700 100644 --- a/mpc_ros/include/models/holonomic_kinematic_model.h +++ b/mpc_ros/include/models/holonomic_kinematic_model.h @@ -27,7 +27,6 @@ namespace mpc_ros { class HolonomicKinematicModel : public KinematicModel { public: - HolonomicKinematicModel(){} HolonomicKinematicModel(Eigen::VectorXd coeffs) { @@ -46,6 +45,8 @@ class HolonomicKinematicModel : public KinematicModel _w_angvel_d = 0; _w_accel_d = 0; + _mpc_steps = 40; + _x_start = 0; _y_start = _x_start + _mpc_steps; _theta_start = _y_start + _mpc_steps; @@ -55,6 +56,7 @@ class HolonomicKinematicModel : public KinematicModel _cte_start = _vy_start + _mpc_steps; _etheta_start = _cte_start + _mpc_steps; _angvel_start = _etheta_start + _mpc_steps; + _ax_start = _angvel_start + _mpc_steps - 1; _ay_start = _ax_start + _mpc_steps - 1; } @@ -103,11 +105,13 @@ class HolonomicKinematicModel : public KinematicModel { fg[0] += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); // cross deviation error fg[0] += _w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2); // heading error + fg[0] += _w_vel * CppAD::pow(vars[_vx_start + i] - _ref_vel, 2); // speed_x error fg[0] += _w_vel * CppAD::pow(vars[_vy_start + i] - _ref_vel, 2); // speed_y error cost_cte += _w_cte * CppAD::pow(vars[_cte_start + i] - _ref_cte, 2); cost_etheta += (_w_etheta * CppAD::pow(vars[_etheta_start + i] - _ref_etheta, 2)); + cost_vel_x += (_w_vel * CppAD::pow(vars[_vx_start + i] - _ref_vel, 2)); cost_vel_y += (_w_vel * CppAD::pow(vars[_vy_start + i] - _ref_vel, 2)); @@ -198,7 +202,7 @@ class HolonomicKinematicModel : public KinematicModel //fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (vx0 * CppAD::sin(etheta0) * _dt)) - ((f0 - x0) + (vy0 * CppAD::cos(etheta0) * _dt)); fg[2 + _cte_start + i] = cte1 - ((f0 - y0) + (vx0 * CppAD::sin(etheta0) * _dt)); fg[2 + _etheta_start + i] = etheta1 - ((theta0 - trj_grad0) + w0 * _dt); - + } } }; diff --git a/mpc_ros/launch/holonomic_mpc_local_planner.launch b/mpc_ros/launch/holonomic_mpc_local_planner.launch index b62ed1b..b9c0430 100644 --- a/mpc_ros/launch/holonomic_mpc_local_planner.launch +++ b/mpc_ros/launch/holonomic_mpc_local_planner.launch @@ -12,14 +12,14 @@ - - + + - + @@ -36,7 +36,7 @@ - + diff --git a/mpc_ros/launch/mpc_local_planner.launch b/mpc_ros/launch/mpc_local_planner.launch index ab36d99..f31034b 100644 --- a/mpc_ros/launch/mpc_local_planner.launch +++ b/mpc_ros/launch/mpc_local_planner.launch @@ -19,7 +19,7 @@ - + @@ -36,7 +36,7 @@ - + diff --git a/mpc_ros/params/mpc_holonomic_params.yaml b/mpc_ros/params/mpc_holonomic_params.yaml index 8e96bcb..9cf9a34 100644 --- a/mpc_ros/params/mpc_holonomic_params.yaml +++ b/mpc_ros/params/mpc_holonomic_params.yaml @@ -1,5 +1,3 @@ - - MPCPlannerROS: # Robot Configuration Parameters map_frame: "map" @@ -20,7 +18,7 @@ MPCPlannerROS: trans_stopped_vel: 0.01 max_vel_theta: 1.5 - min_vel_theta: 0.1 + min_vel_theta: -1.5 theta_stopped_vel: 0.01 acc_lim_x: 2.0 @@ -63,10 +61,10 @@ MPCPlannerROS: ref_vel: 1.0 #1.0 ref_etheta: 0.0 w_cte: 1000.0 - w_etheta: 0.0 - w_vel: 10.0 #100.0 + w_etheta: 10.0 + w_vel: 100.0 #100.0 w_angvel: 1000.0 #100.0 - w_angvel_d: 0.0 + w_angvel_d: 10.0 w_accel: 50.0 w_accel_d: 10.0 bound_value: 1.0e3 # Bound value for other variables diff --git a/mpc_ros/src/mpc_plannner.cpp b/mpc_ros/src/mpc_plannner.cpp index 0b697c8..c10a43d 100644 --- a/mpc_ros/src/mpc_plannner.cpp +++ b/mpc_ros/src/mpc_plannner.cpp @@ -459,6 +459,7 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c // Should be 0 besides initial state. Dvector constraints_lowerbound(n_constraints); Dvector constraints_upperbound(n_constraints); + for (int i = 0; i < n_constraints; i++) { constraints_lowerbound[i] = 0; diff --git a/mpc_ros/src/mpc_plannner_ros.cpp b/mpc_ros/src/mpc_plannner_ros.cpp index ca3379e..3ded7b7 100644 --- a/mpc_ros/src/mpc_plannner_ros.cpp +++ b/mpc_ros/src/mpc_plannner_ros.cpp @@ -64,6 +64,7 @@ namespace mpc_ros{ private_nh.param("base_frame", _base_frame, "base_footprint"); private_nh.param("model_type", model_type, "unicycle"); + cout << "model_type : " << model_type << endl; //Publishers and Subscribers _sub_odom = _nh.subscribe("odom", 1, &MPCPlannerROS::odomCB, this); @@ -653,11 +654,13 @@ namespace mpc_ros{ if(_speed_x >= max_vel_trans) _speed_x = max_vel_trans; + if(_speed_x < -max_vel_trans) _speed_x = -max_vel_trans; if (_speed_y >= max_vel_trans) _speed_y = max_vel_trans; + if(_speed_y < -max_vel_trans) _speed_y = -max_vel_trans; } @@ -672,13 +675,13 @@ namespace mpc_ros{ //cout << "odom_path: \n" << odom_path << endl; //cout << "x_points: \n" << x_veh << endl; //cout << "y_points: \n" << y_veh << endl; - cout << "coeffs: \n" << coeffs << endl; - cout << "_w: \n" << _w << endl; - cout << "_throttle_x: \n" << _throttle_x << endl; - cout << "_throttle_y: \n" << _throttle_y << endl; + cout << "coeffs : \n" << coeffs << endl; + cout << "_w : \n" << _w << endl; + cout << "_throttle_x : \n" << _throttle_x << endl; + cout << "_throttle_y : \n" << _throttle_y << endl; - cout << "_speed_x: \n" << _speed_x << endl; - cout << "_speed_y: \n" << _speed_y << endl; + cout << "_speed_x : \n" << _speed_x << endl; + cout << "_speed_y : \n" << _speed_y << endl; } // Display the MPC predicted trajectory From 5ae50eee6f42bc48a8359447276d613c4d5428b2 Mon Sep 17 00:00:00 2001 From: CzJaewan Date: Fri, 5 Mar 2021 14:11:13 +0900 Subject: [PATCH 7/7] 21.03.05 --- .../models/holonomic_kinematic_model.h | 1 + .../launch/holonomic_mpc_local_planner.launch | 6 +-- mpc_ros/src/mpc_plannner.cpp | 38 +++++++++++++++++++ 3 files changed, 42 insertions(+), 3 deletions(-) diff --git a/mpc_ros/include/models/holonomic_kinematic_model.h b/mpc_ros/include/models/holonomic_kinematic_model.h index 3fc1700..7ecaf17 100644 --- a/mpc_ros/include/models/holonomic_kinematic_model.h +++ b/mpc_ros/include/models/holonomic_kinematic_model.h @@ -122,6 +122,7 @@ class HolonomicKinematicModel : public KinematicModel fg[0] += _w_angvel * CppAD::pow(vars[_angvel_start + i], 2); fg[0] += _w_accel * CppAD::pow(vars[_ax_start + i], 2); fg[0] += _w_accel * CppAD::pow(vars[_ay_start + i], 2); //error + } // Minimize the value gap between sequential actuations. diff --git a/mpc_ros/launch/holonomic_mpc_local_planner.launch b/mpc_ros/launch/holonomic_mpc_local_planner.launch index b9c0430..4f9d012 100644 --- a/mpc_ros/launch/holonomic_mpc_local_planner.launch +++ b/mpc_ros/launch/holonomic_mpc_local_planner.launch @@ -12,14 +12,14 @@ - + - + @@ -36,7 +36,7 @@ - + diff --git a/mpc_ros/src/mpc_plannner.cpp b/mpc_ros/src/mpc_plannner.cpp index c10a43d..6719d73 100644 --- a/mpc_ros/src/mpc_plannner.cpp +++ b/mpc_ros/src/mpc_plannner.cpp @@ -219,6 +219,7 @@ vector MPC::unicycleModelSolve(Eigen::VectorXd state, Eigen::VectorXd co // Cost auto cost = solution.obj_value; std::cout << "------------ Total Cost(solution): " << cost << "------------" << std::endl; + cout << "max_angvel:" << _max_angvel < MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c constraints_lowerbound[_cte_h_start] = cte; constraints_lowerbound[_etheta_h_start] = etheta; + constraints_upperbound[_x_start] = x; constraints_upperbound[_y_start] = y; constraints_upperbound[_theta_start] = theta; @@ -529,7 +531,9 @@ vector MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c // Cost auto cost = solution.obj_value; + std::cout << "------------ Total Cost(solution): " << cost << "------------" << std::endl; + cout << "max_angvel:" << _max_angvel < MPC::holonomicModelSolve(Eigen::VectorXd state, Eigen::VectorXd c this->mpc_x = {}; this->mpc_y = {}; this->mpc_theta = {}; + + cout << solution.x[0] <mpc_x.push_back(solution.x[_x_start + i]); + + cout << "_x_start:" << i <mpc_y.push_back(solution.x[_y_start + i]); + + cout << "_y_start:" << i <mpc_theta.push_back(solution.x[_theta_start + i]); + + cout << "_theta_start:" << i < result; + result.push_back(solution.x[_angvel_h_start]); + cout << "_angvel_h_start:" << i <