Feature ike controller linear velocity cost and angular velocity cost (…

…#84) * Add parameter target_velocity * Feature linear velocity cost and angular velocity cost
uhobeike · Nov 17, 2023 · 9a131b4 · 9a131b4
1 parent ffedddb
commit 9a131b4
Show file tree

Hide file tree

Showing 3 changed files with 19 additions and 7 deletions.
diff --git a/ike_controller/include/ike_controller/ike_controller.hpp b/ike_controller/include/ike_controller/ike_controller.hpp
@@ -64,6 +64,7 @@ class IkeController : public rclcpp::Node
 
   double dt_;
   int predictive_horizon_num_;
+  double target_velocity_;
   double lower_bound_linear_velocity_;
   double lower_bound_angular_velocity_;
   double upper_bound_linear_velocity_;
@@ -77,14 +78,15 @@ struct ObjectiveFunction
 {
   ObjectiveFunction(
     double init_pose_x, double init_pose_y, double init_pose_th, std::vector<double> path_x,
-    std::vector<double> path_y, double dt, int predictive_horizon_num)
+    std::vector<double> path_y, double dt, int predictive_horizon_num, double target_velocity)
   : init_pose_x_(init_pose_x),
     init_pose_y_(init_pose_y),
     init_pose_th_(init_pose_th),
     path_x_(path_x),
     path_y_(path_y),
     dt_(dt),
-    predictive_horizon_num_(predictive_horizon_num)
+    predictive_horizon_num_(predictive_horizon_num),
+    target_velocity_(target_velocity)
   {
   }
 
@@ -108,13 +110,15 @@ struct ObjectiveFunction
               parameters[1][i] * dt_;
       // clang-format on
 
-      T cost = pow((path_x_[i] - x), 2) + pow((path_y_[i] - y), 2);
+      T path_cost = pow((path_x_[i] - x), 2) + pow((path_y_[i] - y), 2);
+      T linear_velocity_cost = pow(((T)target_velocity_ - parameters[0][i]), 2);
+      T angular_velocity_cost = (i > 0) ? pow((parameters[1][i] - parameters[1][i - 1]), 2) : (T)0;
 
       xs[i + 1] = x;
       ys[i + 1] = y;
       ths[i + 1] = th;
 
-      residual[i] = cost;
+      residual[i] = path_cost + linear_velocity_cost + angular_velocity_cost;
     }
 
     return true;
@@ -126,6 +130,7 @@ struct ObjectiveFunction
   std::vector<double> path_y_;
   double dt_;
   int predictive_horizon_num_;
+  double target_velocity_;
 };
 
 }  // namespace ike_nav

diff --git a/ike_controller/src/ike_controller.cpp b/ike_controller/src/ike_controller.cpp
@@ -38,6 +38,7 @@ void IkeController::getParam()
 
   dt_ = this->params_.mpc.delta_time;
   predictive_horizon_num_ = this->params_.mpc.predictive_horizon_num;
+  target_velocity_ = this->params_.mpc.target_velocity;
   lower_bound_linear_velocity_ = this->params_.mpc.lower_bound_linear_velocity;
   lower_bound_angular_velocity_ = this->params_.mpc.lower_bound_angular_velocity;
   upper_bound_linear_velocity_ = this->params_.mpc.upper_bound_linear_velocity;
@@ -122,7 +123,7 @@ std::pair<std::vector<double>, std::vector<double>> IkeController::optimize(
     new ceres::DynamicAutoDiffCostFunction<ObjectiveFunction, MAX_PREDICTIVE_HORIZON_NUM>(
       new ObjectiveFunction(
         std::get<0>(robot_pose), std::get<1>(robot_pose), std::get<2>(robot_pose),
-        std::get<0>(path), std::get<1>(path), dt_, predictive_horizon_num_));
+        std::get<0>(path), std::get<1>(path), dt_, predictive_horizon_num_, target_velocity_));
 
   cost_function->SetNumResiduals(predictive_horizon_num_);
   cost_function->AddParameterBlock(predictive_horizon_num_);
@@ -253,8 +254,8 @@ geometry_msgs::msg::TwistStamped IkeController::convertTwist(
   twist.header.stamp = rclcpp::Time();
 
   // todo fix
-  twist.twist.linear.x = action.first[0] + action.first[1];
-  twist.twist.angular.z = action.second[0] + action.second[1];
+  twist.twist.linear.x = action.first[0];
+  twist.twist.angular.z = action.second[0];
 
   return twist;
 }

diff --git a/ike_nav_parameters/config/ike_controller_parameter.yaml b/ike_nav_parameters/config/ike_controller_parameter.yaml
@@ -12,6 +12,12 @@ ike_controller:
       description: "",
     }
 
+    target_velocity: {
+      type: double,
+      default_value: 0.3,
+      description: "",
+    }
+
     lower_bound_linear_velocity: {
       type: double,
       default_value: 0.0,