diff --git a/robots/delta/config/20240319_mesh/RollingControl.yaml b/robots/delta/config/20240319_mesh/RollingControl.yaml index a72f812bf..e9a495330 100644 --- 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 index e63720a2c..eb6ded624 100644 --- 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 index d18f2045f..33ac334dd 100644 --- 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 getRobotModel() {return robot_model_;} boost::shared_ptr getRollingRobotModel() {return rolling_robot_model_;} - boost::shared_ptr 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 rolling_navigator_; boost::shared_ptr pinocchio_robot_model_; boost::shared_ptr rolling_robot_model_; - boost::shared_ptr rolling_robot_model_for_opt_; boost::shared_ptr 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 prev_opt_gimbal_; std::vector prev_opt_lambda_; diff --git a/robots/delta/src/control/delta_controller_common.cpp b/robots/delta/src/control/delta_controller_common.cpp index 9eb250351..ee6f88d18 100644 --- 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(navigator_); pinocchio_robot_model_ = boost::make_shared(); rolling_robot_model_ = boost::dynamic_pointer_cast(robot_model_); - rolling_robot_model_for_opt_ = boost::make_shared(); robot_model_for_control_ = boost::make_shared(); rotor_tilt_.resize(motor_num_); @@ -123,7 +122,6 @@ void RollingController::rosParamInit() getParam(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(control_nh, "standing_baselink_pitch_update", standing_baselink_pitch_update_, false); getParam(control_nh, "standing_baselink_ref_pitch_update_thresh", standing_baselink_ref_pitch_update_thresh_, 1.0); @@ -132,6 +130,11 @@ void RollingController::rosParamInit() getParam(control_nh, "steering_mu", steering_mu_, 0.0); getParam(control_nh, "full_lambda_mode", full_lambda_mode_, true); getParam(control_nh, "gimbal_d_theta_max", gimbal_d_theta_max_, 0.0); + getParam(control_nh, "d_lambda_max", d_lambda_max_, 0.0); + getParam(control_nh, "lambda_weight", lambda_weight_, 1.0); + getParam(control_nh, "d_gimbal_center_weight", d_gimbal_center_weight_, 1.0); + getParam(control_nh, "d_gimbal_weight", d_gimbal_weight_, 1.0); + getParam(control_nh, "ipopt_max_iter", ipopt_max_iter_, 100); getParam(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 index f122a1068..f5149993b 100644 --- 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,9 +343,9 @@ 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 { @@ -352,25 +353,25 @@ void RollingController::nonlinearQP() 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,7 +384,7 @@ 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; @@ -391,10 +392,7 @@ void RollingController::nonlinearQP() 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);