From 6ee5cb691cd0e17482651b742adf0ab7fc140fea Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Sun, 16 Jun 2024 14:24:01 +0000 Subject: [PATCH 1/7] ruder wheel component complete --- .../src/models/wheel_model/ruder_wheel.hpp | 95 +++++++++++++++++++ 1 file changed, 95 insertions(+) create mode 100644 rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp new file mode 100644 index 000000000..30f0e7a69 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp @@ -0,0 +1,95 @@ +#include + +#include +#include + +namespace rmcs_core::controller::model::wheel { +class ruder_wheel_model : public rmcs_executor::Component, public rclcpp::Node { +public: + ruder_wheel_model() + : Node(get_component_name(), + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)) { + fx_c_ = get_parameter("c").as_double(); + fx_b_ = get_parameter("b").as_double(); + fx_d_ = get_parameter("d").as_double(); + fx_e_ = get_parameter("e").as_double(); + fx_s_v_ = get_parameter("s_v").as_double(); + + fz_c_ = get_parameter("c").as_double(); + fz_b_ = get_parameter("b").as_double(); + fz_d_ = get_parameter("d").as_double(); + fz_e_ = get_parameter("e").as_double(); + fz_s_v_ = get_parameter("s_v").as_double(); + + my_c_ = get_parameter("c").as_double(); + my_b_ = get_parameter("b").as_double(); + my_d_ = get_parameter("d").as_double(); + my_e_ = get_parameter("e").as_double(); + my_s_v_ = get_parameter("s_v").as_double(); + identify_ = get_parameter("wheel_id").as_string(); + + register_input("/chassis/" + identify_ + "_wheel/alpha", alpha_); + register_input("/chassis/" + identify_ + "_wheel/slip_rate", slip_rate_); + } + + void update() override { + auto alpha = *alpha_; + auto slip_rate = *slip_rate_; + + auto longitudinal_force = + fx_d_ * sin(fx_c_ * atan(fx_b_ * slip_rate - + fx_e_ * (fx_b_ * slip_rate - + atan(fx_b_ * slip_rate)))) + + fx_s_v_; + auto aligning_torque = + my_d_ * + sin(my_c_ * atan(my_b_ * alpha - + my_e_ * (my_b_ * alpha - atan(my_b_ * alpha)))) + + my_s_v_; + auto lateral_force = + fz_d_ * + sin(fz_c_ * atan(fz_b_ * alpha - + fz_e_ * (fz_b_ * alpha - atan(fz_b_ * alpha)))) + + fz_s_v_; + + *longitudinal_force_ = longitudinal_force; + *aligning_torque_ = aligning_torque; + *lateral_force_ = lateral_force; + } + +private: + double fx_c_; + double fx_b_; + double fx_d_; + double fx_e_; + double fx_s_v_; + + double fz_c_; + double fz_b_; + double fz_d_; + double fz_e_; + double fz_s_v_; + + double my_c_; + double my_b_; + double my_d_; + double my_e_; + double my_s_v_; + + // double fx_s_h_; + // double fz_s_h_; + // double my_s_h_; + // because we assume that F_z is a constant,so delta s_h is zero, + // param s_h is useless + + std::string identify_; + + InputInterface alpha_; // in rad + InputInterface slip_rate_; // in rad + + OutputInterface longitudinal_force_; + OutputInterface aligning_torque_; + OutputInterface lateral_force_; +}; +} // namespace rmcs_core::controller::model::wheel \ No newline at end of file From 0a81aa024084850c43d1266b12e65a7c0b41647a Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Sun, 16 Jun 2024 14:27:25 +0000 Subject: [PATCH 2/7] ruder wheel model component param_name fix --- .../src/models/wheel_model/ruder_wheel.hpp | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp index 30f0e7a69..bc1a30efe 100644 --- a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp @@ -10,23 +10,24 @@ class ruder_wheel_model : public rmcs_executor::Component, public rclcpp::Node { : Node(get_component_name(), rclcpp::NodeOptions{} .automatically_declare_parameters_from_overrides(true)) { - fx_c_ = get_parameter("c").as_double(); - fx_b_ = get_parameter("b").as_double(); - fx_d_ = get_parameter("d").as_double(); - fx_e_ = get_parameter("e").as_double(); - fx_s_v_ = get_parameter("s_v").as_double(); - - fz_c_ = get_parameter("c").as_double(); - fz_b_ = get_parameter("b").as_double(); - fz_d_ = get_parameter("d").as_double(); - fz_e_ = get_parameter("e").as_double(); - fz_s_v_ = get_parameter("s_v").as_double(); - - my_c_ = get_parameter("c").as_double(); - my_b_ = get_parameter("b").as_double(); - my_d_ = get_parameter("d").as_double(); - my_e_ = get_parameter("e").as_double(); - my_s_v_ = get_parameter("s_v").as_double(); + fx_c_ = get_parameter("fx_c").as_double(); + fx_b_ = get_parameter("fx_b").as_double(); + fx_d_ = get_parameter("fx_d").as_double(); + fx_e_ = get_parameter("fx_e").as_double(); + fx_s_v_ = get_parameter("fx_s_v").as_double(); + + fz_c_ = get_parameter("fz_c").as_double(); + fz_b_ = get_parameter("fz_b").as_double(); + fz_d_ = get_parameter("fz_d").as_double(); + fz_e_ = get_parameter("fz_e").as_double(); + fz_s_v_ = get_parameter("fz_s_v").as_double(); + + my_c_ = get_parameter("my_c").as_double(); + my_b_ = get_parameter("my_b").as_double(); + my_d_ = get_parameter("my_d").as_double(); + my_e_ = get_parameter("my_e").as_double(); + my_s_v_ = get_parameter("my_s_v").as_double(); + identify_ = get_parameter("wheel_id").as_string(); register_input("/chassis/" + identify_ + "_wheel/alpha", alpha_); From 1e47fa7d1ee8d5ee2c61148b69e50eb78055de5f Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Sun, 16 Jun 2024 15:33:53 +0000 Subject: [PATCH 3/7] change wheel model from component to func class --- .../controller/pid/error_pid_controller.cpp | 56 +++++++++---------- .../four_wheel_streering_model.hpp | 55 ++++++++++++++++++ .../src/models/wheel_model/ruder_wheel.hpp | 54 +++++++----------- .../models/wheel_model/wheel_model_base.hpp | 18 ++++++ 4 files changed, 120 insertions(+), 63 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_streering_model.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp index 6aa3e6451..79a3eae7b 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/pid/error_pid_controller.cpp @@ -7,42 +7,42 @@ namespace rmcs_core::controller::pid { -class ErrorPidController - : public rmcs_executor::Component - , public rclcpp::Node { +class ErrorPidController : public rmcs_executor::Component, + public rclcpp::Node { public: - ErrorPidController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , pid_calculator_( - get_parameter("kp").as_double(), get_parameter("ki").as_double(), - get_parameter("kd").as_double()) { - - register_input(get_parameter("measurement").as_string(), measurement_); - register_output(get_parameter("control").as_string(), control_); - - get_parameter("integral_min", pid_calculator_.integral_min); - get_parameter("integral_max", pid_calculator_.integral_max); - get_parameter("output_min", pid_calculator_.output_min); - get_parameter("output_max", pid_calculator_.output_max); - } - - void update() override { - auto err = *measurement_; - *control_ = pid_calculator_.update(err); - } + ErrorPidController() + : Node(get_component_name(), + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)), + pid_calculator_(get_parameter("kp").as_double(), + get_parameter("ki").as_double(), + get_parameter("kd").as_double()) { + + register_input(get_parameter("measurement").as_string(), measurement_); + register_output(get_parameter("control").as_string(), control_); + + get_parameter("integral_min", pid_calculator_.integral_min); + get_parameter("integral_max", pid_calculator_.integral_max); + get_parameter("output_min", pid_calculator_.output_min); + get_parameter("output_max", pid_calculator_.output_max); + } + + void update() override { + auto err = *measurement_; + *control_ = pid_calculator_.update(err); + } private: - PidCalculator pid_calculator_; + PidCalculator pid_calculator_; - InputInterface measurement_; + InputInterface measurement_; - OutputInterface control_; + OutputInterface control_; }; } // namespace rmcs_core::controller::pid #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::pid::ErrorPidController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::pid::ErrorPidController, + rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_streering_model.hpp b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_streering_model.hpp new file mode 100644 index 000000000..024bae151 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_streering_model.hpp @@ -0,0 +1,55 @@ +#include +#include +#include + +#include +#include +#include + +namespace rmcs_core::controller::model::car { +class four_wheel_steering_dynamics_model : public rmcs_executor::Component, + public rclcpp::Node { +public: + four_wheel_steering_dynamics_model() + : Node(get_component_name(), + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)) { + wheel_position_lf_ = + Eigen::Vector2d(get_parameter("wheel_position_lr").as_double_array()); + wheel_position_rf_ = + Eigen::Vector2d(get_parameter("wheel_position_rf").as_double_array()); + wheel_position_lb_ = + Eigen::Vector2d(get_parameter("wheel_position_lb").as_double_array()); + wheel_position_rf_ = + Eigen::Vector2d(get_parameter("wheel_position_rb").as_double_array()); + + wheel_angle_lf_ = acos(wheel_position_lf_.y() / wheel_position_lf_.x()); + wheel_angle_rf_ = acos(wheel_position_rf_.y() / wheel_position_rf_.x()); + wheel_angle_lb_ = acos(wheel_position_lb_.y() / wheel_position_lb_.x()); + wheel_angle_rb_ = acos(wheel_position_rb_.y() / wheel_position_rb_.x()); + } + void update() override {} + +private: + Eigen::Vector2d wheel_position_lf_; + Eigen::Vector2d wheel_position_rf_; + Eigen::Vector2d wheel_position_lb_; + Eigen::Vector2d wheel_position_rb_; + + double wheel_angle_lf_; + double wheel_angle_rf_; + double wheel_angle_lb_; + double wheel_angle_rb_; + + InputInterface chassis_angle_; + InputInterface steering_angle_lf_; + InputInterface steering_angle_lb_; + InputInterface steering_angle_rf_; + InputInterface steering_angle_rb_; +}; +} // namespace rmcs_core::controller::model::car + +#include +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::model::car::four_wheel_steering_dynamics_model, + rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp index bc1a30efe..147bddafb 100644 --- a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp @@ -3,13 +3,12 @@ #include #include +#include "wheel_model_base.hpp" + namespace rmcs_core::controller::model::wheel { -class ruder_wheel_model : public rmcs_executor::Component, public rclcpp::Node { +class RuderWheelModel : public WheelBase { public: - ruder_wheel_model() - : Node(get_component_name(), - rclcpp::NodeOptions{} - .automatically_declare_parameters_from_overrides(true)) { + RuderWheelModel() : WheelBase("ruder_wheel") { fx_c_ = get_parameter("fx_c").as_double(); fx_b_ = get_parameter("fx_b").as_double(); fx_d_ = get_parameter("fx_d").as_double(); @@ -27,38 +26,32 @@ class ruder_wheel_model : public rmcs_executor::Component, public rclcpp::Node { my_d_ = get_parameter("my_d").as_double(); my_e_ = get_parameter("my_e").as_double(); my_s_v_ = get_parameter("my_s_v").as_double(); - - identify_ = get_parameter("wheel_id").as_string(); - - register_input("/chassis/" + identify_ + "_wheel/alpha", alpha_); - register_input("/chassis/" + identify_ + "_wheel/slip_rate", slip_rate_); } - void update() override { - auto alpha = *alpha_; - auto slip_rate = *slip_rate_; + void Claculate(double alpha, double slip_rate) override { - auto longitudinal_force = + longitudinal_force_ = fx_d_ * sin(fx_c_ * atan(fx_b_ * slip_rate - fx_e_ * (fx_b_ * slip_rate - atan(fx_b_ * slip_rate)))) + fx_s_v_; - auto aligning_torque = + aligning_torque_ = my_d_ * sin(my_c_ * atan(my_b_ * alpha - my_e_ * (my_b_ * alpha - atan(my_b_ * alpha)))) + my_s_v_; - auto lateral_force = + lateral_force_ = fz_d_ * sin(fz_c_ * atan(fz_b_ * alpha - fz_e_ * (fz_b_ * alpha - atan(fz_b_ * alpha)))) + fz_s_v_; - - *longitudinal_force_ = longitudinal_force; - *aligning_torque_ = aligning_torque; - *lateral_force_ = lateral_force; } + // properties + double longitudinal_force() const override { return longitudinal_force_; }; + double aligning_torque() const override { return aligning_torque_; }; + double lateral_force() const override { return lateral_force_; }; + private: double fx_c_; double fx_b_; @@ -78,19 +71,10 @@ class ruder_wheel_model : public rmcs_executor::Component, public rclcpp::Node { double my_e_; double my_s_v_; - // double fx_s_h_; - // double fz_s_h_; - // double my_s_h_; - // because we assume that F_z is a constant,so delta s_h is zero, - // param s_h is useless - - std::string identify_; - - InputInterface alpha_; // in rad - InputInterface slip_rate_; // in rad - - OutputInterface longitudinal_force_; - OutputInterface aligning_torque_; - OutputInterface lateral_force_; + double longitudinal_force_; + double aligning_torque_; + double lateral_force_; }; -} // namespace rmcs_core::controller::model::wheel \ No newline at end of file +} // namespace rmcs_core::controller::model::wheel + +#include diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp new file mode 100644 index 000000000..77735ff36 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp @@ -0,0 +1,18 @@ + +#include +#include + +namespace rmcs_core::controller::model::wheel { +class WheelBase : public rclcpp::Node { +public: + explicit WheelBase(const std::string &name) + : Node(name, rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)) { + } + + virtual void Claculate(double, double){}; + virtual double longitudinal_force() const { return 0; }; + virtual double aligning_torque() const { return 0; }; + virtual double lateral_force() const { return 0; }; +}; +} // namespace rmcs_core::controller::model::wheel \ No newline at end of file From 170d9ea7b53dd1cfded73e27557ef011f6419137 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Sun, 16 Jun 2024 17:21:18 +0000 Subject: [PATCH 4/7] four wheel steering driver dynamic model complete --- .../controller/chassis/chassis_controller.cpp | 244 +++++++++--------- .../vehicle_dynamics_model/car_model_base.hpp | 28 ++ .../four_wheel_steering_model.hpp | 177 +++++++++++++ .../four_wheel_streering_model.hpp | 55 ---- .../two_steering_two_omni_model.hpp | 1 + .../src/models/wheel_model/mecanum_wheel.hpp | 1 + .../src/models/wheel_model/omni_wheel.hpp | 1 + .../src/models/wheel_model/ruder_wheel.hpp | 8 +- .../models/wheel_model/wheel_model_base.hpp | 12 +- 9 files changed, 347 insertions(+), 180 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp delete mode 100644 rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_streering_model.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/two_steering_two_omni_model.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/models/wheel_model/mecanum_wheel.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/models/wheel_model/omni_wheel.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp index d1d71d69f..0050e2b42 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp @@ -9,153 +9,157 @@ namespace rmcs_core::controller::chassis { -class ChassisController - : public rmcs_executor::Component - , public rclcpp::Node { +class ChassisController : public rmcs_executor::Component, public rclcpp::Node { public: - ChassisController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - - register_input("/remote/joystick/right", joystick_right_); - register_input("/remote/joystick/left", joystick_left_); - register_input("/remote/switch/right", switch_right_); - register_input("/remote/switch/left", switch_left_); - register_input("/remote/mouse/velocity", mouse_velocity_); - register_input("/remote/mouse", mouse_); - register_input("/remote/keyboard", keyboard_); - - register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); - - register_output( - "/chassis/left_front_wheel/control_velocity", left_front_control_velocity_, nan); - register_output( - "/chassis/left_back_wheel/control_velocity", left_back_control_velocity_, nan); - register_output( - "/chassis/right_back_wheel/control_velocity", right_back_control_velocity_, nan); - register_output( - "/chassis/right_front_wheel/control_velocity", right_front_control_velocity_, nan); - } + ChassisController() + : Node(get_component_name(), + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)) { + + register_input("/remote/joystick/right", joystick_right_); + register_input("/remote/joystick/left", joystick_left_); + register_input("/remote/switch/right", switch_right_); + register_input("/remote/switch/left", switch_left_); + register_input("/remote/mouse/velocity", mouse_velocity_); + register_input("/remote/mouse", mouse_); + register_input("/remote/keyboard", keyboard_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_); + + register_output("/chassis/left_front_wheel/control_velocity", + left_front_control_velocity_, nan); + register_output("/chassis/left_back_wheel/control_velocity", + left_back_control_velocity_, nan); + register_output("/chassis/right_back_wheel/control_velocity", + right_back_control_velocity_, nan); + register_output("/chassis/right_front_wheel/control_velocity", + right_front_control_velocity_, nan); + } + + void update() override { + using namespace rmcs_core::msgs; + + auto switch_right = *switch_right_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; + + do { + if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) || + (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { + reset_all_controls(); + break; + } + + if (switch_left != Switch::DOWN) { + if (switch_right == Switch::MIDDLE) { + spinning_mode_ |= keyboard.c; + spinning_mode_ &= !(keyboard.ctrl && keyboard.c); + } else if (last_switch_right_ == Switch::MIDDLE && + switch_right == Switch::DOWN) { + spinning_mode_ = !spinning_mode_; + } + } - void update() override { - using namespace rmcs_core::msgs; - - auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; - - do { - if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) - || (switch_left == Switch::DOWN && switch_right == Switch::DOWN)) { - reset_all_controls(); - break; - } - - if (switch_left != Switch::DOWN) { - if (switch_right == Switch::MIDDLE) { - spinning_mode_ |= keyboard.c; - spinning_mode_ &= !(keyboard.ctrl && keyboard.c); - } else if (last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) { - spinning_mode_ = !spinning_mode_; - } - } - - auto keyboard_move = - Eigen::Vector2d{0.5 * (keyboard.w - keyboard.s), 0.5 * (keyboard.a - keyboard.d)}; - update_wheel_velocities( - Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move)); - } while (false); - - last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - } + auto keyboard_move = Eigen::Vector2d{0.5 * (keyboard.w - keyboard.s), + 0.5 * (keyboard.a - keyboard.d)}; + update_wheel_velocities(Eigen::Rotation2Dd{*gimbal_yaw_angle_} * + (*joystick_right_ + keyboard_move)); + } while (false); - void reset_all_controls() { - spinning_mode_ = false; + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + } - *left_front_control_velocity_ = nan; - *left_back_control_velocity_ = nan; - *right_back_control_velocity_ = nan; - *right_front_control_velocity_ = nan; - } + void reset_all_controls() { + spinning_mode_ = false; - void update_wheel_velocities(Eigen::Vector2d move) { - constexpr double velocity_limit = 80; + *left_front_control_velocity_ = nan; + *left_back_control_velocity_ = nan; + *right_back_control_velocity_ = nan; + *right_front_control_velocity_ = nan; + } - if (move.norm() > 1) { - move.normalize(); - } + void update_wheel_velocities(Eigen::Vector2d move) { + constexpr double velocity_limit = 80; - double right_oblique = velocity_limit * (-move.y() * cos_45 + move.x() * sin_45); - double left_oblique = velocity_limit * (move.x() * cos_45 + move.y() * sin_45); + if (move.norm() > 1) { + move.normalize(); + } - double velocities[4] = {right_oblique, left_oblique, -right_oblique, -left_oblique}; + double right_oblique = + velocity_limit * (-move.y() * cos_45 + move.x() * sin_45); + double left_oblique = + velocity_limit * (move.x() * cos_45 + move.y() * sin_45); - if (spinning_mode_) { - double max_velocity = 0; - for (auto& velocity : velocities) { - max_velocity = std::max(std::abs(velocity), max_velocity); - } + double velocities[4] = {right_oblique, left_oblique, -right_oblique, + -left_oblique}; - auto spinning_velocity = velocity_limit - max_velocity; - if (spinning_velocity < spinning_min_ * velocity_limit) { - spinning_velocity = spinning_min_ * velocity_limit; + if (spinning_mode_) { + double max_velocity = 0; + for (auto &velocity : velocities) { + max_velocity = std::max(std::abs(velocity), max_velocity); + } - auto scale = (velocity_limit - spinning_velocity) / max_velocity; - for (auto& velocity : velocities) { - velocity *= scale; - } - } else if (spinning_velocity > spinning_max_ * velocity_limit) { - spinning_velocity = spinning_max_ * velocity_limit; - } + auto spinning_velocity = velocity_limit - max_velocity; + if (spinning_velocity < spinning_min_ * velocity_limit) { + spinning_velocity = spinning_min_ * velocity_limit; - for (auto& velocity : velocities) { - velocity += spinning_velocity; - } + auto scale = (velocity_limit - spinning_velocity) / max_velocity; + for (auto &velocity : velocities) { + velocity *= scale; } + } else if (spinning_velocity > spinning_max_ * velocity_limit) { + spinning_velocity = spinning_max_ * velocity_limit; + } - *left_front_control_velocity_ = velocities[0]; - *left_back_control_velocity_ = velocities[1]; - *right_back_control_velocity_ = velocities[2]; - *right_front_control_velocity_ = velocities[3]; + for (auto &velocity : velocities) { + velocity += spinning_velocity; + } } + *left_front_control_velocity_ = velocities[0]; + *left_back_control_velocity_ = velocities[1]; + *right_back_control_velocity_ = velocities[2]; + *right_front_control_velocity_ = velocities[3]; + } + private: - static constexpr double inf = std::numeric_limits::infinity(); - static constexpr double nan = std::numeric_limits::quiet_NaN(); + static constexpr double inf = std::numeric_limits::infinity(); + static constexpr double nan = std::numeric_limits::quiet_NaN(); - // Since sine and cosine function are not constexpr, we calculate once and cache them. - static inline const double sin_45 = std::sin(std::numbers::pi / 4.0); - static inline const double cos_45 = std::cos(std::numbers::pi / 4.0); + // Since sine and cosine function are not constexpr, we calculate once and + // cache them. + static inline const double sin_45 = std::sin(std::numbers::pi / 4.0); + static inline const double cos_45 = std::cos(std::numbers::pi / 4.0); - // Velocity scale in spinning mode - static inline const double spinning_max_ = 0.4; - static inline const double spinning_min_ = 0.2; + // Velocity scale in spinning mode + static inline const double spinning_max_ = 0.4; + static inline const double spinning_min_ = 0.2; - InputInterface joystick_right_; - InputInterface joystick_left_; - InputInterface switch_right_; - InputInterface switch_left_; - InputInterface mouse_velocity_; - InputInterface mouse_; - InputInterface keyboard_; + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; - InputInterface gimbal_yaw_angle_; + InputInterface gimbal_yaw_angle_; - rmcs_core::msgs::Switch last_switch_right_ = rmcs_core::msgs::Switch::UNKNOWN; - rmcs_core::msgs::Switch last_switch_left_ = rmcs_core::msgs::Switch::UNKNOWN; + rmcs_core::msgs::Switch last_switch_right_ = rmcs_core::msgs::Switch::UNKNOWN; + rmcs_core::msgs::Switch last_switch_left_ = rmcs_core::msgs::Switch::UNKNOWN; - bool spinning_mode_ = false; + bool spinning_mode_ = false; - OutputInterface left_front_control_velocity_; - OutputInterface left_back_control_velocity_; - OutputInterface right_back_control_velocity_; - OutputInterface right_front_control_velocity_; + OutputInterface left_front_control_velocity_; + OutputInterface left_back_control_velocity_; + OutputInterface right_back_control_velocity_; + OutputInterface right_front_control_velocity_; }; } // namespace rmcs_core::controller::chassis #include -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController, rmcs_executor::Component) \ No newline at end of file +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController, + rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp new file mode 100644 index 000000000..7fe2cfcc3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp @@ -0,0 +1,28 @@ +#pragma once +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::model::car { +class CarDynamicModelBase : public rclcpp::Node { +public: + explicit CarDynamicModelBase(const std::string &name) + : Node(name, rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)) { + } + + void unused(auto){}; + + virtual void claculate(const std::vector &steering_angles, + const std::vector &wheel_slippage_rate, + double yaw_angle, + const Eigen::Vector2d &linear_velocity, + double angular_velocity) = 0; + + virtual Eigen::Vector2d acceleration() const = 0; + virtual double angular_acceleration() const = 0; +}; +} // namespace rmcs_core::controller::model::car \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp new file mode 100644 index 000000000..189bf766c --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp @@ -0,0 +1,177 @@ +#pragma once +// \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\/ +// //////////// 警告 \\\\\\\\\\\\\\\\\/ +// //////////////////////////////////// +// 写这份代码的时候我快睡着了,大概,不, +// 肯定会有暗病,后面出问题了优先怀疑这里 +// 快速定位代码 ALRAY_ERROR +#include + +#include +#include + +#include +#include +#include +#include +#include + +// TODO: change later +#include "../wheel_model/ruder_wheel.hpp" +#include "../wheel_model/wheel_model_base.hpp" +#include "./car_model_base.hpp" + +namespace rmcs_core::controller::model::car { +class four_wheel_steering_dynamics_model : public CarDynamicModelBase { +public: + four_wheel_steering_dynamics_model() + : CarDynamicModelBase("four_wheel_steering_model") { + + param_car_rotational_inertia_ = + get_parameter("rotational_inertia").as_double(); + param_car_quality_ = get_parameter("quality").as_double(); + + param_wheel_position_lf_ = + Eigen::Vector2d(get_parameter("wheel_position_lr").as_double_array()); + param_wheel_position_rf_ = + Eigen::Vector2d(get_parameter("wheel_position_rf").as_double_array()); + param_wheel_position_lb_ = + Eigen::Vector2d(get_parameter("wheel_position_lb").as_double_array()); + param_wheel_position_rf_ = + Eigen::Vector2d(get_parameter("wheel_position_rb").as_double_array()); + + param_wheel_length_lf_ = param_wheel_position_lf_.norm(); + param_wheel_length_rf_ = param_wheel_position_rf_.norm(); + param_wheel_length_lb_ = param_wheel_position_lb_.norm(); + param_wheel_length_rb_ = param_wheel_position_rb_.norm(); + + param_wheel_angle_lf_ = + acos(param_wheel_position_lf_.y() / param_wheel_position_lf_.x()); + param_wheel_angle_rf_ = + acos(param_wheel_position_rf_.y() / param_wheel_position_rf_.x()); + param_wheel_angle_lb_ = + acos(param_wheel_position_lb_.y() / param_wheel_position_lb_.x()); + param_wheel_angle_rb_ = + acos(param_wheel_position_rb_.y() / param_wheel_position_rb_.x()); + } + + void claculate(const std::vector &steering_angles, + const std::vector &wheel_slippage_rate, + double yaw_angle, const Eigen::Vector2d &linear_velocity, + double angular_velocity) override { + steering_angle_lf_ = steering_angles[0]; + steering_angle_rf_ = steering_angles[1]; + steering_angle_lb_ = steering_angles[2]; + steering_angle_rb_ = steering_angles[3]; + + wheel_slippage_rate_lf_ = wheel_slippage_rate[0]; + wheel_slippage_rate_rf_ = wheel_slippage_rate[1]; + wheel_slippage_rate_lb_ = wheel_slippage_rate[2]; + wheel_slippage_rate_rb_ = wheel_slippage_rate[3]; + + yaw_angle_ = yaw_angle; + + linear_velocity_current_ = linear_velocity; + angular_velocity_current_ = angular_velocity; + + auto angle_velocity = + acos(linear_velocity_current_.y() / linear_velocity_current_.y()); + auto cos_angle_velocity = cos(angle_velocity); + auto sin_angle_velocity = sin(angle_velocity); + auto trans_matrix = Eigen::Matrix2d(cos_angle_velocity, -sin_angle_velocity, + sin_angle_velocity, cos_angle_velocity); + + wheel_model_->claculate((yaw_angle_ + steering_angle_lf_) - angle_velocity, + wheel_slippage_rate_lf_); + mixed_force_lf_ = Eigen::Vector2d(wheel_model_->longitudinal_force(), + wheel_model_->lateral_force()); + mixed_force_lf_ = trans_matrix * mixed_force_lf_.transpose().transpose(); + + wheel_model_->claculate((yaw_angle_ + steering_angle_lb_) - angle_velocity, + wheel_slippage_rate_lb_); + mixed_force_lb_ = Eigen::Vector2d(wheel_model_->longitudinal_force(), + wheel_model_->lateral_force()); + mixed_force_lb_ = trans_matrix * mixed_force_lb_.transpose().transpose(); + + wheel_model_->claculate((yaw_angle_ + steering_angle_rf_) - angle_velocity, + wheel_slippage_rate_rf_); + mixed_force_rf_ = Eigen::Vector2d(wheel_model_->longitudinal_force(), + wheel_model_->lateral_force()); + mixed_force_rf_ = trans_matrix * mixed_force_rf_.transpose().transpose(); + + wheel_model_->claculate((yaw_angle_ + steering_angle_rb_) - angle_velocity, + wheel_slippage_rate_rb_); + mixed_force_rb_ = Eigen::Vector2d(wheel_model_->longitudinal_force(), + wheel_model_->lateral_force()); + mixed_force_rb_ = trans_matrix * mixed_force_rb_.transpose().transpose(); + + mixed_force = + mixed_force_lb_ + mixed_force_lf_ + mixed_force_rb_ + mixed_force_rf_; + + torque = (sin(steering_angle_lf_ - param_wheel_angle_lf_) > 0 ? 1 : -1) * + mixed_force_lf_.dot(param_wheel_position_lf_); + + torque += (sin(steering_angle_lb_ - param_wheel_angle_lb_) > 0 ? 1 : -1) * + mixed_force_lb_.dot(param_wheel_position_lb_); + + torque += (sin(steering_angle_rf_ - param_wheel_angle_rf_) > 0 ? 1 : -1) * + mixed_force_rf_.dot(param_wheel_position_rf_); + + torque += (sin(steering_angle_rb_ - param_wheel_angle_rb_) > 0 ? 1 : -1) * + mixed_force_rb_.dot(param_wheel_position_rb_); + + acceleration_ = mixed_force * param_car_quality_; + angular_acceleration_ = torque * param_car_rotational_inertia_; + } + + Eigen::Vector2d acceleration() const override { return acceleration_; } + double angular_acceleration() const override { return angular_acceleration_; } + +private: + Eigen::Vector2d param_wheel_position_lf_; + Eigen::Vector2d param_wheel_position_rf_; + Eigen::Vector2d param_wheel_position_lb_; + Eigen::Vector2d param_wheel_position_rb_; + + double param_wheel_length_lf_; + double param_wheel_length_rf_; + double param_wheel_length_lb_; + double param_wheel_length_rb_; + + double param_wheel_angle_lf_; + double param_wheel_angle_rf_; + double param_wheel_angle_lb_; + double param_wheel_angle_rb_; + + double param_car_rotational_inertia_; + double param_car_quality_; + + double steering_angle_lf_; + double steering_angle_rf_; + double steering_angle_lb_; + double steering_angle_rb_; + + double wheel_slippage_rate_lf_; + double wheel_slippage_rate_rf_; + double wheel_slippage_rate_lb_; + double wheel_slippage_rate_rb_; + + Eigen::Vector2d mixed_force_lf_; + Eigen::Vector2d mixed_force_rf_; + Eigen::Vector2d mixed_force_lb_; + Eigen::Vector2d mixed_force_rb_; + + double yaw_angle_; + + std::shared_ptr wheel_model_; + + Eigen::Vector2d linear_velocity_current_; + double angular_velocity_current_; + + Eigen::Vector2d mixed_force; + double torque; + + Eigen::Vector2d acceleration_; + double angular_acceleration_; +}; +} // namespace rmcs_core::controller::model::car diff --git a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_streering_model.hpp b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_streering_model.hpp deleted file mode 100644 index 024bae151..000000000 --- a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_streering_model.hpp +++ /dev/null @@ -1,55 +0,0 @@ -#include -#include -#include - -#include -#include -#include - -namespace rmcs_core::controller::model::car { -class four_wheel_steering_dynamics_model : public rmcs_executor::Component, - public rclcpp::Node { -public: - four_wheel_steering_dynamics_model() - : Node(get_component_name(), - rclcpp::NodeOptions{} - .automatically_declare_parameters_from_overrides(true)) { - wheel_position_lf_ = - Eigen::Vector2d(get_parameter("wheel_position_lr").as_double_array()); - wheel_position_rf_ = - Eigen::Vector2d(get_parameter("wheel_position_rf").as_double_array()); - wheel_position_lb_ = - Eigen::Vector2d(get_parameter("wheel_position_lb").as_double_array()); - wheel_position_rf_ = - Eigen::Vector2d(get_parameter("wheel_position_rb").as_double_array()); - - wheel_angle_lf_ = acos(wheel_position_lf_.y() / wheel_position_lf_.x()); - wheel_angle_rf_ = acos(wheel_position_rf_.y() / wheel_position_rf_.x()); - wheel_angle_lb_ = acos(wheel_position_lb_.y() / wheel_position_lb_.x()); - wheel_angle_rb_ = acos(wheel_position_rb_.y() / wheel_position_rb_.x()); - } - void update() override {} - -private: - Eigen::Vector2d wheel_position_lf_; - Eigen::Vector2d wheel_position_rf_; - Eigen::Vector2d wheel_position_lb_; - Eigen::Vector2d wheel_position_rb_; - - double wheel_angle_lf_; - double wheel_angle_rf_; - double wheel_angle_lb_; - double wheel_angle_rb_; - - InputInterface chassis_angle_; - InputInterface steering_angle_lf_; - InputInterface steering_angle_lb_; - InputInterface steering_angle_rf_; - InputInterface steering_angle_rb_; -}; -} // namespace rmcs_core::controller::model::car - -#include -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::model::car::four_wheel_steering_dynamics_model, - rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/two_steering_two_omni_model.hpp b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/two_steering_two_omni_model.hpp new file mode 100644 index 000000000..08847e2ad --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/two_steering_two_omni_model.hpp @@ -0,0 +1 @@ +// to be continue \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/mecanum_wheel.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/mecanum_wheel.hpp new file mode 100644 index 000000000..08847e2ad --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/mecanum_wheel.hpp @@ -0,0 +1 @@ +// to be continue \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/omni_wheel.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/omni_wheel.hpp new file mode 100644 index 000000000..08847e2ad --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/omni_wheel.hpp @@ -0,0 +1 @@ +// to be continue \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp index 147bddafb..5fb4d3f04 100644 --- a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include @@ -28,7 +30,7 @@ class RuderWheelModel : public WheelBase { my_s_v_ = get_parameter("my_s_v").as_double(); } - void Claculate(double alpha, double slip_rate) override { + void claculate(double alpha, double slip_rate) override { longitudinal_force_ = fx_d_ * sin(fx_c_ * atan(fx_b_ * slip_rate - @@ -51,6 +53,10 @@ class RuderWheelModel : public WheelBase { double longitudinal_force() const override { return longitudinal_force_; }; double aligning_torque() const override { return aligning_torque_; }; double lateral_force() const override { return lateral_force_; }; + double mixed_force() const override { + return sqrt(lateral_force_ * lateral_force_ + + longitudinal_force_ * longitudinal_force_); + }; private: double fx_c_; diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp index 77735ff36..581838687 100644 --- a/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp @@ -1,3 +1,4 @@ +#pragma once #include #include @@ -10,9 +11,12 @@ class WheelBase : public rclcpp::Node { .automatically_declare_parameters_from_overrides(true)) { } - virtual void Claculate(double, double){}; - virtual double longitudinal_force() const { return 0; }; - virtual double aligning_torque() const { return 0; }; - virtual double lateral_force() const { return 0; }; + void unused(auto){}; + + virtual void claculate(double alpha, double slip_rate) = 0; + virtual double longitudinal_force() const = 0; + virtual double aligning_torque() const = 0; + virtual double lateral_force() const = 0; + virtual double mixed_force() const = 0; }; } // namespace rmcs_core::controller::model::wheel \ No newline at end of file From d0e41cd39f463a2ae444895ae3adc8b3e6bf4b48 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Mon, 17 Jun 2024 07:26:16 +0000 Subject: [PATCH 5/7] object funtion base, intermission --- .../src/controller/mpc/mpc_controller.cpp | 19 ++++++++++++ .../b_spline_calculator.hpp | 10 +++++++ .../objective_function_base.hpp | 24 +++++++++++++++ .../vehicle_dynamics_model/car_model_base.hpp | 9 +++--- .../four_wheel_steering_model.hpp | 29 +++++++++---------- .../src/models/wheel_model/ruder_wheel.hpp | 5 ++-- .../models/wheel_model/wheel_model_base.hpp | 2 +- 7 files changed, 75 insertions(+), 23 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp new file mode 100644 index 000000000..698828fd6 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp @@ -0,0 +1,19 @@ + +#include +#include +#include +#include + +#include "../../models/vehicle_dynamics_model/four_wheel_steering_model.hpp" + +namespace rmcs_core::controller::mpc { +class MpcController : public rmcs_executor::Component, public rclcpp::Node { +public: + MpcController() + : Node(get_component_name(), + rclcpp::NodeOptions{} + .automatically_declare_parameters_from_overrides(true)) {} + +private: +}; +} // namespace rmcs_core::controller::mpc \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp new file mode 100644 index 000000000..13d5d9ad0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp @@ -0,0 +1,10 @@ +#pragma once + +#include "./objective_function_base.hpp" + +namespace rmcs_core::controller::mpc::utils { +template +class BSplineCalculator : public ObjectiveFunctionBase { + BSplineCalculator() : ObjectiveFunctionBase() {} +}; +} // namespace rmcs_core::controller::mpc::utils \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp new file mode 100644 index 000000000..dd7763a32 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::mpc::utils { +template class ObjectiveFunctionBase { +public: + ObjectiveFunctionBase() = default; + +private: + Eigen::Matrix parameters_; + Eigen::Matrix control_points_; + std::vector knot_timeline_; + Eigen::Matrix timeline_parameters_; + Eigen::Matrix timeline_control_points_; +}; +} // namespace rmcs_core::controller::mpc::utils \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp index 7fe2cfcc3..430167b4a 100644 --- a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp +++ b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp @@ -16,11 +16,10 @@ class CarDynamicModelBase : public rclcpp::Node { void unused(auto){}; - virtual void claculate(const std::vector &steering_angles, - const std::vector &wheel_slippage_rate, - double yaw_angle, - const Eigen::Vector2d &linear_velocity, - double angular_velocity) = 0; + virtual void update(const std::vector &steering_angles, + const std::vector &wheel_slippage_rate, + double yaw_angle, const Eigen::Vector2d &linear_velocity, + double angular_velocity) = 0; virtual Eigen::Vector2d acceleration() const = 0; virtual double angular_acceleration() const = 0; diff --git a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp index 189bf766c..b9fc0b7d7 100644 --- a/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp +++ b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp @@ -6,15 +6,14 @@ // 肯定会有暗病,后面出问题了优先怀疑这里 // 快速定位代码 ALRAY_ERROR #include +#include +#include #include #include -#include -#include #include #include -#include // TODO: change later #include "../wheel_model/ruder_wheel.hpp" @@ -55,10 +54,10 @@ class four_wheel_steering_dynamics_model : public CarDynamicModelBase { acos(param_wheel_position_rb_.y() / param_wheel_position_rb_.x()); } - void claculate(const std::vector &steering_angles, - const std::vector &wheel_slippage_rate, - double yaw_angle, const Eigen::Vector2d &linear_velocity, - double angular_velocity) override { + void update(const std::vector &steering_angles, + const std::vector &wheel_slippage_rate, double yaw_angle, + const Eigen::Vector2d &linear_velocity, + double angular_velocity) override { steering_angle_lf_ = steering_angles[0]; steering_angle_rf_ = steering_angles[1]; steering_angle_lb_ = steering_angles[2]; @@ -81,26 +80,26 @@ class four_wheel_steering_dynamics_model : public CarDynamicModelBase { auto trans_matrix = Eigen::Matrix2d(cos_angle_velocity, -sin_angle_velocity, sin_angle_velocity, cos_angle_velocity); - wheel_model_->claculate((yaw_angle_ + steering_angle_lf_) - angle_velocity, - wheel_slippage_rate_lf_); + wheel_model_->update((yaw_angle_ + steering_angle_lf_) - angle_velocity, + wheel_slippage_rate_lf_); mixed_force_lf_ = Eigen::Vector2d(wheel_model_->longitudinal_force(), wheel_model_->lateral_force()); mixed_force_lf_ = trans_matrix * mixed_force_lf_.transpose().transpose(); - wheel_model_->claculate((yaw_angle_ + steering_angle_lb_) - angle_velocity, - wheel_slippage_rate_lb_); + wheel_model_->update((yaw_angle_ + steering_angle_lb_) - angle_velocity, + wheel_slippage_rate_lb_); mixed_force_lb_ = Eigen::Vector2d(wheel_model_->longitudinal_force(), wheel_model_->lateral_force()); mixed_force_lb_ = trans_matrix * mixed_force_lb_.transpose().transpose(); - wheel_model_->claculate((yaw_angle_ + steering_angle_rf_) - angle_velocity, - wheel_slippage_rate_rf_); + wheel_model_->update((yaw_angle_ + steering_angle_rf_) - angle_velocity, + wheel_slippage_rate_rf_); mixed_force_rf_ = Eigen::Vector2d(wheel_model_->longitudinal_force(), wheel_model_->lateral_force()); mixed_force_rf_ = trans_matrix * mixed_force_rf_.transpose().transpose(); - wheel_model_->claculate((yaw_angle_ + steering_angle_rb_) - angle_velocity, - wheel_slippage_rate_rb_); + wheel_model_->update((yaw_angle_ + steering_angle_rb_) - angle_velocity, + wheel_slippage_rate_rb_); mixed_force_rb_ = Eigen::Vector2d(wheel_model_->longitudinal_force(), wheel_model_->lateral_force()); mixed_force_rb_ = trans_matrix * mixed_force_rb_.transpose().transpose(); diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp index 5fb4d3f04..531a3e73f 100644 --- a/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp @@ -5,7 +5,8 @@ #include #include -#include "wheel_model_base.hpp" +// todo : change later +#include "./wheel_model_base.hpp" namespace rmcs_core::controller::model::wheel { class RuderWheelModel : public WheelBase { @@ -30,7 +31,7 @@ class RuderWheelModel : public WheelBase { my_s_v_ = get_parameter("my_s_v").as_double(); } - void claculate(double alpha, double slip_rate) override { + void update(double alpha, double slip_rate) override { longitudinal_force_ = fx_d_ * sin(fx_c_ * atan(fx_b_ * slip_rate - diff --git a/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp b/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp index 581838687..2c80a7731 100644 --- a/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp @@ -13,7 +13,7 @@ class WheelBase : public rclcpp::Node { void unused(auto){}; - virtual void claculate(double alpha, double slip_rate) = 0; + virtual void update(double alpha, double slip_rate) = 0; virtual double longitudinal_force() const = 0; virtual double aligning_torque() const = 0; virtual double lateral_force() const = 0; From 11578fc9c60d0eef94e1c19efcb343a6ebff0731 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Mon, 17 Jun 2024 08:55:49 +0000 Subject: [PATCH 6/7] bspline calculator --- .../b_spline_calculator.hpp | 60 ++++++++++++++++++- .../objective_function_base.hpp | 16 +++-- 2 files changed, 67 insertions(+), 9 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp index 13d5d9ad0..5cd86ddeb 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp @@ -1,10 +1,64 @@ #pragma once +#include +#include +#include +#include + #include "./objective_function_base.hpp" namespace rmcs_core::controller::mpc::utils { -template -class BSplineCalculator : public ObjectiveFunctionBase { - BSplineCalculator() : ObjectiveFunctionBase() {} + +template +requires BSplineConcept +class BSplineCalculator + : public ObjectiveFunctionBase { + BSplineCalculator() : ObjectiveFunctionBase() { +#if k == 3 + m_k_ = {0.5, 0.5, 0, -1, 1, 0, 0.5, -1, 0.5}; +#else + m_k_ = {1 / 6.0, 4 / 6.0, 1 / 6.0, 0, -3 / 6.0, 0, + 3 / 6.0, 0, 3 / 6.0, -6 / 6.0, 3 / 6.0, 0, + -1 / 6.0, 3 / 6.0, -3 / 6.0, 1 / 6.0}; +#endif + m_tk_ = {0.5, 0.5, 0, -1, 1, 0, 0.5, -1, 0.5}; + } + + Eigen::Matrix calculate(double t) override { + t = std::min(std::max(t, 0.0), this->knot_timeline_.back()); + auto i = 0; + while (this->knot_timeline_[i + 1] < t) + i++; + auto u = this->knot_timeline_[i + 1] - this->knot_timeline_[i]; + u_time_density = {0, 1, 2 * u}; + + double time_density = u_time_density * m_tk_ * + this->timeline_control_points_.block(i, 0, 3, 1); + + Eigen::Matrix time_density_vec = { + 1, time_density, time_density * time_density, + time_density * time_density * time_density, + time_density * time_density * time_density * time_density}; + + u_traj = {1, u, u * u, u * u * u, u * u * u * u, + 0, 1, 2 * u, 3 * u * u, 4 * u * u * u, + 0, 0, 2 * 1, 2 * 3 * u, 4 * 3 * u * u, + 0, 0, 2 * 0, 2 * 3 * 1, 4 * 3 * 2 * u, + 0, 0, 2 * 0, 2 * 3 * 0, 4 * 3 * 2 * 1}; + u_traj.array().colwise() /= time_density_vec.array().col(0); + + auto size = k - ret_dimension == k ? 0 : 1; + Eigen::Matrix target_matrix = + u_traj.block(0, 0, ret_dimension, size) * m_k_.block(0, 0, size, size) * + this->control_points_.block(i, 0, size, dimension); + + return target_matrix; + } + +private: + Eigen::Matrix m_k_; + Eigen::Matrix u_traj; + Eigen::Matrix m_tk_; + Eigen::Matrix u_time_density; }; } // namespace rmcs_core::controller::mpc::utils \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp index dd7763a32..a9d63925f 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp @@ -10,15 +10,19 @@ #include namespace rmcs_core::controller::mpc::utils { -template class ObjectiveFunctionBase { +template +concept BSplineConcept = std::isless() && std::isless(); + +template class ObjectiveFunctionBase { public: - ObjectiveFunctionBase() = default; + ObjectiveFunctionBase() = default; + + virtual Eigen::Matrix + calculate(float t) = 0; -private: - Eigen::Matrix parameters_; +protected: Eigen::Matrix control_points_; std::vector knot_timeline_; - Eigen::Matrix timeline_parameters_; - Eigen::Matrix timeline_control_points_; + Eigen::Matrix timeline_control_points_; }; } // namespace rmcs_core::controller::mpc::utils \ No newline at end of file From 9268d4765d7c0037caa60e5d259a8d86e600bf88 Mon Sep 17 00:00:00 2001 From: alray <1780284652@qq.com> Date: Mon, 17 Jun 2024 14:45:23 +0000 Subject: [PATCH 7/7] mpc complete --- .vscode/settings.json | 8 ++ .../controller/mpc/Optimizers/ceresinstall.sh | 7 ++ .../mpc/cost_function/four_wheel_cost.hpp | 74 +++++++++++++++++++ .../cost_function/four_wheel_lazy_cost.hpp | 65 ++++++++++++++++ .../src/controller/mpc/mpc_controller.cpp | 56 +++++++++++++- .../b_spline_calculator.hpp | 18 +++-- .../objective_function_base.hpp | 34 +++++++-- 7 files changed, 247 insertions(+), 15 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/mpc/Optimizers/ceresinstall.sh create mode 100644 rmcs_ws/src/rmcs_core/src/controller/mpc/cost_function/four_wheel_cost.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/mpc/cost_function/four_wheel_lazy_cost.hpp diff --git a/.vscode/settings.json b/.vscode/settings.json index 593784e4f..b6fd3a270 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -6,5 +6,13 @@ "python.analysis.extraPaths": [ "/opt/ros/humble/lib/python3.10/site-packages", "/opt/ros/humble/local/lib/python3.10/dist-packages" + ], + "cSpell.words": [ + "colwise", + "cwise", + "Eigen", + "rclcpp", + "rmcs", + "rowwise" ] } \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/Optimizers/ceresinstall.sh b/rmcs_ws/src/rmcs_core/src/controller/mpc/Optimizers/ceresinstall.sh new file mode 100644 index 000000000..d59b5bf91 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/Optimizers/ceresinstall.sh @@ -0,0 +1,7 @@ +cd ~ && git clone https://ceres-solver.googlesource.com/ceres-solver +cd ceres-solver +mkdir build && cd build && cmake .. +make -j4 +sudo make install + +sudo ln -s /usr/include/eigen3/Eigen /usr/include/Eigen \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/cost_function/four_wheel_cost.hpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/cost_function/four_wheel_cost.hpp new file mode 100644 index 000000000..b13aaf00f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/cost_function/four_wheel_cost.hpp @@ -0,0 +1,74 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "controller/mpc/objective_function/objective_function_base.hpp" +#include "models/vehicle_dynamics_model/car_model_base.hpp" +#include "models/vehicle_dynamics_model/four_wheel_steering_model.hpp" + +namespace rmcs_core::controller::mpc::utils { +struct FourWheelCost { +public: + explicit FourWheelCost( + const std::vector &lambdas, double time_interval, + std::shared_ptr &&car_model) { + lambdas_ = {lambdas[0], lambdas[1], lambdas[2]}; + time_interval_ = time_interval; + car_model_ = car_model; + }; + + [[nodiscard]] double + calculate_cost(const std::vector &control_vector) const { + car_model_->update( + std::vector(control_vector.begin(), control_vector.begin() + 4), + std::vector(control_vector.begin() + 4, + control_vector.begin() + 8), + yaw_angle_, current_velocity_, angular_velocity_); + auto angular_acceleration = car_model_->angular_acceleration(); + auto acceleration = car_model_->acceleration(); + auto velocity = current_velocity_ + acceleration * time_interval_; + auto position = + position_ + (velocity + current_velocity_) / 2 * time_interval_; + auto angular_velocity = + angular_velocity_ + angular_acceleration * time_interval_; + + Eigen::Matrix3d calc_matrix = Eigen::Matrix3d::Zero(); + calc_matrix.block(0, 0, 1, 2) = position; + calc_matrix.block(1, 0, 1, 2) = velocity; + calc_matrix(2, 2) = angular_velocity; + return (lambdas_ * + (target_matrix_ - calc_matrix).cwiseAbs().rowwise().sum())(0, 0); + } + + void set_parameters(double yaw_angle, const Eigen::Matrix3d &target_matrix, + const Eigen::Vector2d &linear_velocity, + const Eigen::Vector2d &position, + double angular_velocity) { + target_matrix_ = target_matrix; + yaw_angle_ = yaw_angle; + current_velocity_ = linear_velocity; + angular_velocity_ = angular_velocity; + position_ = position; + } + + bool operator()(const double *const x, double *residual) const { + residual[0] = calculate_cost(std::vector(x, x + 8)); + return true; + } + +private: + Eigen::Vector3d lambdas_; + double time_interval_; + double yaw_angle_; + std::vector omegas_; + Eigen::Matrix3d target_matrix_; + Eigen::Vector2d current_velocity_; + Eigen::Vector2d position_; + double angular_velocity_; + std::shared_ptr car_model_; +}; +} // namespace rmcs_core::controller::mpc::utils \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/cost_function/four_wheel_lazy_cost.hpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/cost_function/four_wheel_lazy_cost.hpp new file mode 100644 index 000000000..7d969caff --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/cost_function/four_wheel_lazy_cost.hpp @@ -0,0 +1,65 @@ +#pragma once + +#include +#include +#include +#include + +#include "controller/mpc/objective_function/objective_function_base.hpp" +#include "models/vehicle_dynamics_model/four_wheel_steering_model.hpp" + +namespace rmcs_core::controller::mpc::utils { +class FourWheelCost { +public: + explicit FourWheelCost(const std::vector &lambdas, + const std::vector &omegas, + double time_interval) { + lambdas_ = {lambdas[0], lambdas[1], lambdas[2]}; + omegas_ = omegas; + time_interval_ = time_interval; + }; + + [[nodiscard]] double calculate_cost( + const std::vector &control_vector, + const Eigen::Matrix3d &target_matrix, + model::car::four_wheel_steering_dynamics_model &car_model_) const { + car_model_.update( + std::vector(control_vector.begin(), control_vector.begin() + 4), + std::vector(control_vector.begin() + 4, + control_vector.begin() + 8), + yaw_angle_, current_velocity_, angular_velocity_); + auto angular_acceleration = car_model_.angular_acceleration(); + auto acceleration = car_model_.acceleration(); + auto velocity = current_velocity_ + acceleration * time_interval_; + auto position = + position_ + (velocity + current_velocity_) / 2 * time_interval_; + auto angular_velocity = + angular_velocity_ + angular_acceleration * time_interval_; + + Eigen::Matrix3d calc_matrix = Eigen::Matrix3d::Zero(); + calc_matrix.block(0, 0, 1, 2) = position; + calc_matrix.block(1, 0, 1, 2) = velocity; + calc_matrix(2, 2) = angular_velocity; + return (lambdas_ * + (target_matrix - calc_matrix).cwiseAbs().rowwise().sum())(0, 0); + } + + void set_parameters(double yaw_angle, const Eigen::Vector2d &linear_velocity, + const Eigen::Vector2d &position, + double angular_velocity) { + yaw_angle_ = yaw_angle; + current_velocity_ = linear_velocity; + angular_velocity_ = angular_velocity; + position_ = position; + } + +private: + Eigen::Vector3d lambdas_; + double time_interval_; + double yaw_angle_; + std::vector omegas_; + Eigen::Vector2d current_velocity_; + Eigen::Vector2d position_; + double angular_velocity_; +}; +} // namespace rmcs_core::controller::mpc::utils \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp index 698828fd6..52382eb10 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp @@ -1,19 +1,73 @@ +#include +#include +#include +#include + #include #include #include #include +#include +#include + +#include + #include "../../models/vehicle_dynamics_model/four_wheel_steering_model.hpp" +#include "./cost_function/four_wheel_cost.hpp" +#include "./objective_function/b_spline_calculator.hpp" +#include "models/vehicle_dynamics_model/car_model_base.hpp" namespace rmcs_core::controller::mpc { + class MpcController : public rmcs_executor::Component, public rclcpp::Node { public: MpcController() : Node(get_component_name(), rclcpp::NodeOptions{} - .automatically_declare_parameters_from_overrides(true)) {} + .automatically_declare_parameters_from_overrides(true)), + car_model_(), bspline_(), + cost_func_( + get_parameter("cost_lambdas").as_double_array(), + get_parameter("time_interval").as_double(), + std::make_shared(car_model_)), + options_() { + options_.linear_solver_type = ceres::DENSE_QR; + options_.minimizer_progress_to_stdout = true; + cost_function = std::make_shared< + ceres::AutoDiffCostFunction>((cost_func_)); + + register_input("measured_steering_angles", current_steering_angles_); + register_input("measured_slippage_rates", current_slippage_rates_); + register_output("expected_steering_angles", target_steering_angles_); + register_output("expected_slippage_rates", target_slippage_rates_); + } + + void update() override { + auto problem_ = ceres::Problem(); + + std::copy_n(*current_steering_angles_, 4, inputs_); + std::copy_n(*current_slippage_rates_, 4, inputs_ + 4); + + problem_.AddResidualBlock(cost_function.get(), nullptr, inputs_); + + std::copy_n(inputs_, 4, *target_steering_angles_); + std::copy_n(inputs_ + 4, 4, *target_slippage_rates_); + } private: + model::car::four_wheel_steering_dynamics_model car_model_; + utils::BSplineCalculator<3, 4, 3> bspline_; + utils::FourWheelCost cost_func_; + ceres::Solver::Options options_; + std::shared_ptr cost_function; + double inputs_[8]; + + InputInterface current_steering_angles_; + InputInterface current_slippage_rates_; + + OutputInterface target_steering_angles_; + OutputInterface target_slippage_rates_; }; } // namespace rmcs_core::controller::mpc \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp index 5cd86ddeb..371d1ffe0 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp @@ -13,6 +13,7 @@ template requires BSplineConcept class BSplineCalculator : public ObjectiveFunctionBase { +public: BSplineCalculator() : ObjectiveFunctionBase() { #if k == 3 m_k_ = {0.5, 0.5, 0, -1, 1, 0, 0.5, -1, 0.5}; @@ -40,16 +41,17 @@ class BSplineCalculator time_density * time_density * time_density, time_density * time_density * time_density * time_density}; - u_traj = {1, u, u * u, u * u * u, u * u * u * u, - 0, 1, 2 * u, 3 * u * u, 4 * u * u * u, - 0, 0, 2 * 1, 2 * 3 * u, 4 * 3 * u * u, - 0, 0, 2 * 0, 2 * 3 * 1, 4 * 3 * 2 * u, - 0, 0, 2 * 0, 2 * 3 * 0, 4 * 3 * 2 * 1}; - u_traj.array().colwise() /= time_density_vec.array().col(0); + u_trajectory = {1, u, u * u, u * u * u, u * u * u * u, + 0, 1, 2 * u, 3 * u * u, 4 * u * u * u, + 0, 0, 2 * 1, 2 * 3 * u, 4 * 3 * u * u, + 0, 0, 2 * 0, 2 * 3 * 1, 4 * 3 * 2 * u, + 0, 0, 2 * 0, 2 * 3 * 0, 4 * 3 * 2 * 1}; + u_trajectory.array().colwise() /= time_density_vec.array().col(0); auto size = k - ret_dimension == k ? 0 : 1; Eigen::Matrix target_matrix = - u_traj.block(0, 0, ret_dimension, size) * m_k_.block(0, 0, size, size) * + u_trajectory.block(0, 0, ret_dimension, size) * + m_k_.block(0, 0, size, size) * this->control_points_.block(i, 0, size, dimension); return target_matrix; @@ -57,7 +59,7 @@ class BSplineCalculator private: Eigen::Matrix m_k_; - Eigen::Matrix u_traj; + Eigen::Matrix u_trajectory; Eigen::Matrix m_tk_; Eigen::Matrix u_time_density; }; diff --git a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp index a9d63925f..b61cf7ab7 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp @@ -1,6 +1,8 @@ #pragma once +#include #include +#include #include #include @@ -10,19 +12,39 @@ #include namespace rmcs_core::controller::mpc::utils { -template -concept BSplineConcept = std::isless() && std::isless(); template class ObjectiveFunctionBase { public: ObjectiveFunctionBase() = default; virtual Eigen::Matrix - calculate(float t) = 0; + calculate(double t) = 0; + + void set_control_points( + Eigen::Matrix &&control_points) { + control_points_ = std::make_shared(control_points); + } + void set_knot_timeline(std::vector &&knot_timeline) { + knot_timeline_ = std::make_shared>(knot_timeline); + } + void set_timeline_control_points( + Eigen::Matrix &&timeline_control_points) { + timeline_control_points_ = + std::make_shared>( + timeline_control_points); + } protected: - Eigen::Matrix control_points_; - std::vector knot_timeline_; - Eigen::Matrix timeline_control_points_; + std::shared_ptr> + control_points_; + std::shared_ptr> knot_timeline_; + std::shared_ptr> + timeline_control_points_; +}; + +template +concept BSplineConcept = requires() { + ret_dimension < k &&k < 5; }; + } // namespace rmcs_core::controller::mpc::utils \ No newline at end of file