[delta][control] add rosparam for nonlinear optimization constraints

jsk-ros-pkg · Apr 13, 2024 · 5f3b17e · 5f3b17e
1 parent e577ecf
commit 5f3b17e
Show file tree

Hide file tree

Showing 5 changed files with 45 additions and 29 deletions.
diff --git a/robots/delta/config/20240319_mesh/RollingControl.yaml b/robots/delta/config/20240319_mesh/RollingControl.yaml
@@ -18,7 +18,13 @@ controller:
   rolling_baselink_roll_converged_thresh: 0.6
   steering_mu: 3.0
   full_lambda_mode: true
+
+  lambda_weight: 1.0
+  d_gimbal_center_weight: 2.0
+  d_gimbal_weight: 1.0
   gimbal_d_theta_max: 0.3
+  d_lambda_max: 5.0
+  ipopt_max_iter: 30
 
   controlled_axis: [1, 1, 1, 1, 1, 1]
 

diff --git a/robots/delta/config/20240319_mesh/RollingControl_sim.yaml b/robots/delta/config/20240319_mesh/RollingControl_sim.yaml
@@ -18,7 +18,13 @@ controller:
   rolling_baselink_roll_converged_thresh: 0.6
   steering_mu: 2.0
   full_lambda_mode: true
-  gimbal_d_theta_max: 0.3
+
+  lambda_weight: 1.0
+  d_gimbal_center_weight: 2.0
+  d_gimbal_weight: 2.0
+  gimbal_d_theta_max: 0.7
+  d_lambda_max: 10.0
+  ipopt_max_iter: 100
 
   controlled_axis: [1, 1, 1, 1, 1, 1]
 

diff --git a/robots/delta/include/delta/control/delta_controller.h b/robots/delta/include/delta/control/delta_controller.h
@@ -41,7 +41,6 @@ namespace aerial_robot_control
 
     boost::shared_ptr<aerial_robot_model::RobotModel> getRobotModel() {return robot_model_;}
     boost::shared_ptr<RollingRobotModel> getRollingRobotModel() {return rolling_robot_model_;}
-    boost::shared_ptr<RollingRobotModel> getRollingRobotModelForOpt() {return rolling_robot_model_for_opt_;}
 
   private:
     ros::Publisher rpy_gain_pub_;                     // for spinal
@@ -71,7 +70,6 @@ namespace aerial_robot_control
     boost::shared_ptr<aerial_robot_navigation::RollingNavigator> rolling_navigator_;
     boost::shared_ptr<aerial_robot_model::PinocchioRobotModel> pinocchio_robot_model_;
     boost::shared_ptr<RollingRobotModel> rolling_robot_model_;
-    boost::shared_ptr<RollingRobotModel> rolling_robot_model_for_opt_;
     boost::shared_ptr<aerial_robot_model::RobotModel> robot_model_for_control_;
 
     /* common part */
@@ -120,6 +118,11 @@ namespace aerial_robot_control
     double gravity_compensate_ratio_;
     double rolling_minimum_lateral_force_;
     double gimbal_d_theta_max_;
+    double d_lambda_max_;
+    double lambda_weight_;
+    double d_gimbal_center_weight_;
+    double d_gimbal_weight_;
+    int ipopt_max_iter_;
     std::vector<double> prev_opt_gimbal_;
     std::vector<double> prev_opt_lambda_;
 

diff --git a/robots/delta/src/control/delta_controller_common.cpp b/robots/delta/src/control/delta_controller_common.cpp
@@ -20,7 +20,6 @@ void RollingController::initialize(ros::NodeHandle nh, ros::NodeHandle nhp,
   rolling_navigator_ = boost::dynamic_pointer_cast<aerial_robot_navigation::RollingNavigator>(navigator_);
   pinocchio_robot_model_ = boost::make_shared<aerial_robot_model::PinocchioRobotModel>();
   rolling_robot_model_ = boost::dynamic_pointer_cast<RollingRobotModel>(robot_model_);
-  rolling_robot_model_for_opt_ = boost::make_shared<RollingRobotModel>();
   robot_model_for_control_ = boost::make_shared<aerial_robot_model::RobotModel>();
 
   rotor_tilt_.resize(motor_num_);
@@ -123,7 +122,6 @@ void RollingController::rosParamInit()
   getParam<double>(control_nh, "rolling_minimum_lateral_force", rolling_minimum_lateral_force_, 0.0);
 
   rolling_robot_model_->setCircleRadius(circle_radius_);
-  rolling_robot_model_for_opt_->setCircleRadius(circle_radius_);
 
   getParam<bool>(control_nh, "standing_baselink_pitch_update", standing_baselink_pitch_update_, false);
   getParam<double>(control_nh, "standing_baselink_ref_pitch_update_thresh", standing_baselink_ref_pitch_update_thresh_, 1.0);
@@ -132,6 +130,11 @@ void RollingController::rosParamInit()
   getParam<double>(control_nh, "steering_mu", steering_mu_, 0.0);
   getParam<bool>(control_nh, "full_lambda_mode", full_lambda_mode_, true);
   getParam<double>(control_nh, "gimbal_d_theta_max", gimbal_d_theta_max_, 0.0);
+  getParam<double>(control_nh, "d_lambda_max",   d_lambda_max_, 0.0);
+  getParam<double>(control_nh, "lambda_weight", lambda_weight_, 1.0);
+  getParam<double>(control_nh, "d_gimbal_center_weight", d_gimbal_center_weight_, 1.0);
+  getParam<double>(control_nh, "d_gimbal_weight", d_gimbal_weight_, 1.0);
+  getParam<int>(control_nh, "ipopt_max_iter", ipopt_max_iter_, 100);
 
   getParam<string>(nhp_, "tf_prefix", tf_prefix_, std::string(""));
 
@@ -354,7 +357,7 @@ void RollingController::wrenchAllocation()
 
   for(int i = 0; i < motor_num_; i++)
     {
-      prev_opt_gimbal_.at(i) = angles::normalize_angle(target_gimbal_angles_.at(i));
+      prev_opt_gimbal_.at(i) = angles::normalize_angle(current_gimbal_angles_.at(i));
     }
 
   /* update robot model by calculated gimbal angle */

diff --git a/robots/delta/src/control/delta_ground_controller.cpp b/robots/delta/src/control/delta_ground_controller.cpp
@@ -79,6 +79,7 @@ void RollingController::standingPlanning()
       ROS_WARN_ONCE("start roll/pitch I control");
     }
 
+  /* TODO change to use baselink roll angle not to switch during trajectory tracking */
   if(ground_navigation_mode_ == aerial_robot_navigation::STANDING_STATE && fabs(pid_msg_.roll.err_p) < standing_baselink_roll_converged_thresh_)
     {
       rolling_navigator_->setGroundNavigationMode(aerial_robot_navigation::ROLLING_STATE);
@@ -302,10 +303,10 @@ void RollingController::nonlinearQP()
   x_opt(4) = lambda(1);
   x_opt(5) = lambda(2);
 
-  casadi::SX cost = dot(lambda, lambda)
-    + 2.0 * pow(x_opt(0) - M_PI / 2.0, 2)
-    + 2.0 * pow(x_opt(1) - M_PI / 2.0, 2)
-    + 2.0 * pow(x_opt(2) - M_PI / 2.0, 2);
+  casadi::SX cost
+    = lambda_weight_ * dot(lambda, lambda)
+    + d_gimbal_center_weight_ * (pow(x_opt(0) - M_PI / 2.0, 2) + pow(x_opt(1) - M_PI / 2.0, 2) + 2.0 * pow(x_opt(2) - M_PI / 2.0, 2))
+    + d_gimbal_weight_ * (pow(x_opt(0) - prev_opt_gimbal_.at(0), 2) + pow(x_opt(1) - prev_opt_gimbal_.at(1), 2) + pow(x_opt(2) - prev_opt_gimbal_.at(2), 2));
 
   casadi::SX constraints = casadi::SX(8, 1);
   constraints(0) = wrench_cp(ROLL);
@@ -342,35 +343,35 @@ void RollingController::nonlinearQP()
 
   if(ground_navigation_mode_ == aerial_robot_navigation::ROLLING_STATE)
     {
-      lbx(0) = min(M_PI, max(0.0, prev_opt_gimbal_.at(0) - gimbal_d_theta_max_));
-      lbx(1) = min(M_PI, max(0.0, prev_opt_gimbal_.at(1) - gimbal_d_theta_max_));
-      lbx(2) = min(M_PI, max(0.0, prev_opt_gimbal_.at(2) - gimbal_d_theta_max_));
+      lbx(0) = min(max(0.0, prev_opt_gimbal_.at(0) - gimbal_d_theta_max_), M_PI);
+      lbx(1) = min(max(0.0, prev_opt_gimbal_.at(1) - gimbal_d_theta_max_), M_PI);
+      lbx(2) = min(max(0.0, prev_opt_gimbal_.at(2) - gimbal_d_theta_max_), M_PI);
     }
   else
     {
       lbx(0) = max(-M_PI, prev_opt_gimbal_.at(0) - gimbal_d_theta_max_);
       lbx(1) = max(-M_PI, prev_opt_gimbal_.at(1) - gimbal_d_theta_max_);
       lbx(2) = max(-M_PI, prev_opt_gimbal_.at(2) - gimbal_d_theta_max_);
     }
-  lbx(3) = 0.0;
-  lbx(4) = 0.0;
-  lbx(5) = 0.0;
+  lbx(3) = max(0.0, prev_opt_lambda_.at(0) - d_lambda_max_);
+  lbx(4) = max(0.0, prev_opt_lambda_.at(1) - d_lambda_max_);
+  lbx(5) = max(0.0, prev_opt_lambda_.at(2) - d_lambda_max_);
 
   if(ground_navigation_mode_ == aerial_robot_navigation::ROLLING_STATE)
     {
-      ubx(0) = max(0.0, min(M_PI, prev_opt_gimbal_.at(0) + gimbal_d_theta_max_));
-      ubx(1) = max(0.0, min(M_PI, prev_opt_gimbal_.at(1) + gimbal_d_theta_max_));
-      ubx(2) = max(0.0, min(M_PI, prev_opt_gimbal_.at(2) + gimbal_d_theta_max_));
+      ubx(0) = max(0.0, min(prev_opt_gimbal_.at(0) + gimbal_d_theta_max_, M_PI));
+      ubx(1) = max(0.0, min(prev_opt_gimbal_.at(1) + gimbal_d_theta_max_, M_PI));
+      ubx(2) = max(0.0, min(prev_opt_gimbal_.at(2) + gimbal_d_theta_max_, M_PI));
     }
   else
     {
-      ubx(0) = min(M_PI, prev_opt_gimbal_.at(0) + gimbal_d_theta_max_);
-      ubx(1) = min(M_PI, prev_opt_gimbal_.at(1) + gimbal_d_theta_max_);
-      ubx(2) = min(M_PI, prev_opt_gimbal_.at(2) + gimbal_d_theta_max_);
+      ubx(0) = min(prev_opt_gimbal_.at(0) + gimbal_d_theta_max_, M_PI);
+      ubx(1) = min(prev_opt_gimbal_.at(1) + gimbal_d_theta_max_, M_PI);
+      ubx(2) = min(prev_opt_gimbal_.at(2) + gimbal_d_theta_max_, M_PI);
     }
-  ubx(3) = robot_model_->getThrustUpperLimit();
-  ubx(4) = robot_model_->getThrustUpperLimit();
-  ubx(5) = robot_model_->getThrustUpperLimit();
+  ubx(3) = min(prev_opt_lambda_.at(0) + d_lambda_max_, robot_model_->getThrustUpperLimit());
+  ubx(4) = min(prev_opt_lambda_.at(1) + d_lambda_max_, robot_model_->getThrustUpperLimit());
+  ubx(5) = min(prev_opt_lambda_.at(2) + d_lambda_max_, robot_model_->getThrustUpperLimit());
 
   ROS_INFO_STREAM_ONCE("x_opt: \n" << x_opt << "\n");
   ROS_INFO_STREAM_ONCE("cost: \n" << cost << "\n");
@@ -383,18 +384,15 @@ void RollingController::nonlinearQP()
   casadi::SXDict nlp = { {"x", x_opt}, {"f", cost}, {"g", constraints} };
 
   casadi::Dict opt_dict = casadi::Dict();
-  opt_dict["ipopt.max_iter"] = 30;
+  opt_dict["ipopt.max_iter"] = ipopt_max_iter_;
   opt_dict["ipopt.print_level"] = 0;
   opt_dict["ipopt.sb"] = "yes";
   opt_dict["print_time"] = 0;
 
   casadi::Function S = casadi::nlpsol("S", "ipopt", nlp, opt_dict);
   casadi::DM initial_x = casadi::DM({prev_opt_gimbal_.at(0), prev_opt_gimbal_.at(1), prev_opt_gimbal_.at(2), prev_opt_lambda_.at(0), prev_opt_lambda_.at(1), prev_opt_lambda_.at(2)});
 
-  // double start = ros::Time::now().toSec();
   auto res = S(casadi::DMDict{ {"x0", initial_x}, {"lbg",lbg}, {"ubg", ubg}, {"lbx", lbx}, {"ubx", ubx} });
-  // std::cout << "time: " << ros::Time::now().toSec() - start << std::endl;
-  // std::cout << res.at("x") << std::endl;
 
   prev_opt_gimbal_.at(0) = (double)res.at("x")(0);
   prev_opt_gimbal_.at(1) = (double)res.at("x")(1);