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/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/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 new file mode 100644 index 000000000..52382eb10 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/mpc_controller.cpp @@ -0,0 +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)), + 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 new file mode 100644 index 000000000..371d1ffe0 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/b_spline_calculator.hpp @@ -0,0 +1,66 @@ +#pragma once + +#include +#include +#include +#include + +#include "./objective_function_base.hpp" + +namespace rmcs_core::controller::mpc::utils { + +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}; +#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_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_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; + } + +private: + Eigen::Matrix m_k_; + Eigen::Matrix u_trajectory; + 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 new file mode 100644 index 000000000..b61cf7ab7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/mpc/objective_function/objective_function_base.hpp @@ -0,0 +1,50 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +namespace rmcs_core::controller::mpc::utils { + +template class ObjectiveFunctionBase { +public: + ObjectiveFunctionBase() = default; + + virtual Eigen::Matrix + 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: + 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 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/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..430167b4a --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/car_model_base.hpp @@ -0,0 +1,27 @@ +#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 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; +}; +} // 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..b9fc0b7d7 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/vehicle_dynamics_model/four_wheel_steering_model.hpp @@ -0,0 +1,176 @@ +#pragma once +// \\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\\/ +// //////////// 警告 \\\\\\\\\\\\\\\\\/ +// //////////////////////////////////// +// 写这份代码的时候我快睡着了,大概,不, +// 肯定会有暗病,后面出问题了优先怀疑这里 +// 快速定位代码 ALRAY_ERROR +#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 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]; + 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_->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_->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_->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_->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(); + + 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/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 new file mode 100644 index 000000000..531a3e73f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/ruder_wheel.hpp @@ -0,0 +1,87 @@ +#pragma once + +#include + +#include +#include + +// todo : change later +#include "./wheel_model_base.hpp" + +namespace rmcs_core::controller::model::wheel { +class RuderWheelModel : public WheelBase { +public: + 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(); + 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(); + } + + void update(double alpha, double slip_rate) override { + + 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_; + aligning_torque_ = + my_d_ * + sin(my_c_ * atan(my_b_ * alpha - + my_e_ * (my_b_ * alpha - atan(my_b_ * alpha)))) + + my_s_v_; + lateral_force_ = + fz_d_ * + sin(fz_c_ * atan(fz_b_ * alpha - + fz_e_ * (fz_b_ * alpha - atan(fz_b_ * alpha)))) + + fz_s_v_; + } + + // 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_; }; + double mixed_force() const override { + return sqrt(lateral_force_ * lateral_force_ + + longitudinal_force_ * longitudinal_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 longitudinal_force_; + double aligning_torque_; + double lateral_force_; +}; +} // 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..2c80a7731 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/models/wheel_model/wheel_model_base.hpp @@ -0,0 +1,22 @@ +#pragma once + +#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)) { + } + + void unused(auto){}; + + 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; + virtual double mixed_force() const = 0; +}; +} // namespace rmcs_core::controller::model::wheel \ No newline at end of file