Skip to content
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
8 changes: 8 additions & 0 deletions .vscode/settings.json
Original file line number Diff line number Diff line change
Expand Up @@ -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"
]
}
244 changes: 124 additions & 120 deletions rmcs_ws/src/rmcs_core/src/controller/chassis/chassis_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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<double>::infinity();
static constexpr double nan = std::numeric_limits<double>::quiet_NaN();
static constexpr double inf = std::numeric_limits<double>::infinity();
static constexpr double nan = std::numeric_limits<double>::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<Eigen::Vector2d> joystick_right_;
InputInterface<Eigen::Vector2d> joystick_left_;
InputInterface<rmcs_core::msgs::Switch> switch_right_;
InputInterface<rmcs_core::msgs::Switch> switch_left_;
InputInterface<Eigen::Vector2d> mouse_velocity_;
InputInterface<rmcs_core::msgs::Mouse> mouse_;
InputInterface<rmcs_core::msgs::Keyboard> keyboard_;
InputInterface<Eigen::Vector2d> joystick_right_;
InputInterface<Eigen::Vector2d> joystick_left_;
InputInterface<rmcs_core::msgs::Switch> switch_right_;
InputInterface<rmcs_core::msgs::Switch> switch_left_;
InputInterface<Eigen::Vector2d> mouse_velocity_;
InputInterface<rmcs_core::msgs::Mouse> mouse_;
InputInterface<rmcs_core::msgs::Keyboard> keyboard_;

InputInterface<double> gimbal_yaw_angle_;
InputInterface<double> 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<double> left_front_control_velocity_;
OutputInterface<double> left_back_control_velocity_;
OutputInterface<double> right_back_control_velocity_;
OutputInterface<double> right_front_control_velocity_;
OutputInterface<double> left_front_control_velocity_;
OutputInterface<double> left_back_control_velocity_;
OutputInterface<double> right_back_control_velocity_;
OutputInterface<double> right_front_control_velocity_;
};

} // namespace rmcs_core::controller::chassis

#include <pluginlib/class_list_macros.hpp>

PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController, rmcs_executor::Component)
PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::ChassisController,
rmcs_executor::Component)
Original file line number Diff line number Diff line change
@@ -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
Original file line number Diff line number Diff line change
@@ -0,0 +1,74 @@
#pragma once

#include <algorithm>
#include <eigen3/Eigen/Eigen>
#include <eigen3/Eigen/src/Core/Matrix.h>
#include <memory>
#include <vector>

#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<double> &lambdas, double time_interval,
std::shared_ptr<model::car::CarDynamicModelBase> &&car_model) {
lambdas_ = {lambdas[0], lambdas[1], lambdas[2]};
time_interval_ = time_interval;
car_model_ = car_model;
};

[[nodiscard]] double
calculate_cost(const std::vector<double> &control_vector) const {
car_model_->update(
std::vector<double>(control_vector.begin(), control_vector.begin() + 4),
std::vector<double>(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<double>(x, x + 8));
return true;
}

private:
Eigen::Vector3d lambdas_;
double time_interval_;
double yaw_angle_;
std::vector<double> omegas_;
Eigen::Matrix3d target_matrix_;
Eigen::Vector2d current_velocity_;
Eigen::Vector2d position_;
double angular_velocity_;
std::shared_ptr<model::car::CarDynamicModelBase> car_model_;
};
} // namespace rmcs_core::controller::mpc::utils
Loading