Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Holonomic modeling #19

Open
wants to merge 7 commits into
base: modeling
Choose a base branch
from
Open

Holonomic modeling #19

Show file tree
Hide file tree
Changes from 5 commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
6da9142
add holonomic kinematics
CzJaewan Feb 16, 2021
dee3996
add holonomic launch
CzJaewan Feb 16, 2021
ba06cf5
modify non-holonomic model err
CzJaewan Feb 16, 2021
f8512a5
modify holonomic model err
CzJaewan Feb 17, 2021
a0485d1
Delete MPC.cpp
Geonhee-LEE Feb 17, 2021
5208438
2021_3_5
CzJaewan Mar 5, 2021
5ae50ee
21.03.05
CzJaewan Mar 5, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
70 changes: 44 additions & 26 deletions mpc_ros/include/models/holonomic_kinematic_model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<string, double> &params)
Expand All @@ -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;
}
Expand All @@ -91,39 +96,45 @@ 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);
}



// fg[x] for constraints
// Initial constraints
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];

Expand All @@ -134,23 +145,27 @@ class HolonomicKinematicModel : public KinematicModel
AD<double> x1 = vars[_x_start + i + 1];
AD<double> y1 = vars[_y_start + i + 1];
AD<double> theta1 = vars[_theta_start + i + 1];
AD<double> v1 = vars[_v_start + i + 1];
AD<double> vx1 = vars[_vx_start + i + 1];
AD<double> vy1 = vars[_vy_start + i + 1];

AD<double> cte1 = vars[_cte_start + i + 1];
AD<double> etheta1 = vars[_etheta_start + i + 1];

// The state at time t.
AD<double> x0 = vars[_x_start + i];
AD<double> y0 = vars[_y_start + i];
AD<double> theta0 = vars[_theta_start + i];
AD<double> v0 = vars[_v_start + i];
AD<double> vx0 = vars[_vx_start + i];
AD<double> vy0 = vars[_vy_start + i];

AD<double> cte0 = vars[_cte_start + i];
AD<double> etheta0 = vars[_etheta_start + i];

// Only consider the actuation at time t.
//AD<double> angvel0 = vars[_angvel_start + i];
AD<double> w0 = vars[_angvel_start + i];
AD<double> a0 = vars[_a_start + i];

AD<double> ax0 = vars[_ax_start + i];
AD<double> ay0 = vars[_ay_start + i];

//AD<double> f0 = coeffs[0] + coeffs[1] * x0 + coeffs[2] * CppAD::pow(x0, 2) + coeffs[3] * CppAD::pow(x0, 3);
AD<double> f0 = 0.0;
Expand All @@ -174,13 +189,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)) - ((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);

}
}
};
Expand Down
3 changes: 2 additions & 1 deletion mpc_ros/include/models/kinematic_model.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<double> cost_cte, cost_etheta, cost_vel;

AD<double> cost_vel_x, cost_vel_y;
// Constructor
KinematicModel(){}

Expand Down
2 changes: 2 additions & 0 deletions mpc_ros/include/mpc_plannner.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<string, double> _params;
std::string model_type;

Expand Down
2 changes: 1 addition & 1 deletion mpc_ros/include/mpc_plannner_ros.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand Down
6 changes: 3 additions & 3 deletions mpc_ros/launch/holonomic_mpc_local_planner.launch
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -12,7 +12,7 @@
<arg name="z_pos" default="0.0"/>
<arg name="roll" default="0"/>
<arg name="pitch" default="0"/>
<arg name="yaw" default="-1.507"/>
<arg name="yaw" default="0"/>
<arg name="gui" default="false"/>

<!-- ************** Robot model *************** -->
Expand Down Expand Up @@ -66,7 +66,7 @@
<rosparam file="$(find mpc_ros)/params/global_planner_params.yaml" command="load" />

<!-- Local Planner -->
<rosparam file="$(find mpc_ros)/params/mpc_last_params.yaml" command="load" />
<rosparam file="$(find mpc_ros)/params/mpc_holonomic_params.yaml" command="load" />
<param name="base_local_planner" value="mpc_ros/MPCPlannerROS" if="$(eval controller == 'mpc')"/>
<param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" if="$(eval controller == 'dwa')"/>

Expand All @@ -76,7 +76,7 @@

<!-- ************** MPC Node ************** -->
<!--node name="nav_mpc" pkg="mpc_ros" type="nav_mpc" output="screen" if="$(eval controller == 'mpc')" >
<rosparam file="$(find mpc_ros)/params/mpc_last_params.yaml" command="load" />
<rosparam file="$(find mpc_ros)/params/mpc_holonomic_params.yaml" command="load" />
</node-->

<!-- ************** Pure Pursuit ************** -->
Expand Down
72 changes: 72 additions & 0 deletions mpc_ros/params/mpc_holonomic_params.yaml
View file
Open in desktop
Original file line number Diff line number Diff line change
@@ -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: -1.0

max_vel_x: 0.5
min_vel_x: -0.5

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

max_vel_theta: 1.5
min_vel_theta: 0.1
theta_stopped_vel: 0.01

acc_lim_x: 2.0
acc_lim_y: 2.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: 1000.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
Loading