diff --git a/.env b/.env new file mode 100644 index 00000000..c28fb80f --- /dev/null +++ b/.env @@ -0,0 +1,3 @@ +CONTAINER_USER=ubuntu +CONTAINER_HOME=/home/${CONTAINER_USER} +HOST_NVIM_DIR=${HOME}/.config/nvim diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml new file mode 100644 index 00000000..71c5538f --- /dev/null +++ b/.github/workflows/update-image.yml @@ -0,0 +1,49 @@ +name: Build and Push RMCS Images + +on: + workflow_dispatch: + push: + paths: + - 'Dockerfile' + branches: + - main + +jobs: + build-and-push: + runs-on: ubuntu-latest + + steps: + - name: Checkout repository + uses: actions/checkout@v4 + + - name: Set up Docker Buildx + uses: docker/setup-buildx-action@v3 + + - name: Log in to Docker Hub + uses: docker/login-action@v3 + with: + username: ${{ secrets.DOCKERHUB_USERNAME }} + password: ${{ secrets.DOCKERHUB_TOKEN }} + + - name: Set up SSH keys + run: | + echo "${{ secrets.CONTAINER_ID_RSA }}" > .ssh/id_rsa + echo "${{ secrets.CONTAINER_ID_RSA_PUB }}" > .ssh/id_rsa.pub + chmod 600 .ssh/id_rsa + chmod 644 .ssh/id_rsa.pub + + - name: Build and push rmcs-develop:latest + uses: docker/build-push-action@v6 + with: + context: . + push: true + target: rmcs-develop + tags: qzhhhi/rmcs-develop:latest + + - name: Build and push rmcs-runtime:latest + uses: docker/build-push-action@v6 + with: + context: . + push: true + target: rmcs-runtime + tags: qzhhhi/rmcs-runtime:latest diff --git a/.gitmodules b/.gitmodules index 1938ee6e..d4d71be2 100644 --- a/.gitmodules +++ b/.gitmodules @@ -9,4 +9,4 @@ url = https://github.com/Alliance-Algorithm/ros2-hikcamera.git [submodule "rmcs_ws/src/serial"] path = rmcs_ws/src/serial - url = git@github.com:Alliance-Algorithm/ros2-serial.git + url = https://github.com/Alliance-Algorithm/ros2-serial.git diff --git a/.script/build-rmcs b/.script/build-rmcs index 08d7198e..ef5d85ff 100755 --- a/.script/build-rmcs +++ b/.script/build-rmcs @@ -2,6 +2,6 @@ source /opt/ros/jazzy/setup.bash -cd /workspaces/RMCS/rmcs_ws +cd ${RMCS_PATH}/rmcs_ws -colcon build --symlink-install --merge-install +colcon build --symlink-install --merge-install $@ diff --git a/.script/clean-rmcs b/.script/clean-rmcs index cab27019..3227fd82 100755 --- a/.script/clean-rmcs +++ b/.script/clean-rmcs @@ -1,3 +1,4 @@ #! /bin/bash -rm -rf /workspaces/RMCS/rmcs_ws/build /workspaces/RMCS/rmcs_ws/install /workspaces/RMCS/rmcs_ws/log \ No newline at end of file +rm -rf ${RMCS_PATH}/rmcs_ws/build ${RMCS_PATH}/rmcs_ws/install ${RMCS_PATH}/rmcs_ws/log + diff --git a/.script/complete/_play-autoaim b/.script/complete/_play-autoaim new file mode 100644 index 00000000..7365b535 --- /dev/null +++ b/.script/complete/_play-autoaim @@ -0,0 +1,6 @@ +#compdef play-autoaim + +_arguments \ + '--user[指定远程用户名]:用户名:' \ + '--remote[从设备拉取 SDP 到容器]' \ + '--no-copy[跳过容器到宿主机的拷贝]' diff --git a/.script/complete/_set-remote b/.script/complete/_set-remote new file mode 100644 index 00000000..e5599c3f --- /dev/null +++ b/.script/complete/_set-remote @@ -0,0 +1,22 @@ +#compdef set-remote + +_remote_hosts() { + local hosts=( + "169.254.233.233" + "alliance-sentry.local" + "alliance-infantry.local" + "alliance-hero.local" + ) + + # 从 ~/.ssh/config 提取最近使用的 HostName + if [[ -f ~/.ssh/config ]]; then + local extracted + extracted=(${(f)"$(grep -A1 'Host remote' ~/.ssh/config | grep HostName | awk '{print $2}')"}) + hosts+=(${extracted}) + fi + + compadd -- $hosts +} + +_arguments \ + '1:远程主机地址:_remote_hosts' diff --git a/.script/play-autoaim b/.script/play-autoaim new file mode 100755 index 00000000..8f367049 --- /dev/null +++ b/.script/play-autoaim @@ -0,0 +1,69 @@ +#!/bin/sh + +set -e + +MONITOR_USER="" +MONITOR_PLAYER="vlc" +MONITOR_HOST="localhost" + +SDP_PATH="/tmp/auto_aim.sdp" + +USE_REMOTE=false +SKIP_COPY=false + +# 参数解析 +while [ "$#" -gt 0 ]; do + case "$1" in + --user) + shift + [ -z "$1" ] && echo "❌ --user 参数缺失" && exit 1 + MONITOR_USER="$1" + ;; + --remote) + USE_REMOTE=true + ;; + --no-copy) + SKIP_COPY=true + ;; + *) + echo "未知参数: $1" + echo "用法: $0 --user <用户名> [--remote] [--no-copy]" + exit 1 + ;; + esac + shift +done + +if [ -z "$MONITOR_USER" ]; then + echo "❌ 缺少 --user 参数" + echo "用法: play-autoaim --user <用户名> [--remote] [--no-copy]" + exit 1 +fi + +if [ "$USE_REMOTE" = true ]; then + scp remote:${SDP_PATH} ${SDP_PATH} + if [ $? -ne 0 ]; then + echo " 从 remote 拷贝失败。是否继续？(y/n)" + read -r answer + case "$answer" in + [Yy]*) echo " 继续执行后续操作…" ;; + *) + echo " 已取消。" + exit 1 + ;; + esac + fi +fi + +if [ "$SKIP_COPY" = false ]; then + scp "${SDP_PATH}" "${MONITOR_USER}@${MONITOR_HOST}:${SDP_PATH}" + if [ $? -ne 0 ]; then + echo " scp 拷贝失败" + exit 1 + fi + echo " 文件已拷贝到宿主机：${SDP_PATH}" +else + echo " 跳过拷贝，直接打开远程 SDP" +fi + +ssh -f "${MONITOR_USER}@${MONITOR_HOST}" "DISPLAY=:0 ${MONITOR_PLAYER} '${SDP_PATH}' --network-caching=50" diff --git a/.script/sync-remote b/.script/sync-remote index 60a2b816..7d62782b 100755 --- a/.script/sync-remote +++ b/.script/sync-remote @@ -8,7 +8,7 @@ import subprocess from colorama import Fore, Style -SRC_DIR = "/workspaces/RMCS/rmcs_ws/install" +SRC_DIR = os.getenv("RMCS_PATH") + "/rmcs_ws/install" DST_DIR = "ssh://remote//rmcs_install" SOCKET_PATH = "/tmp/sync-remote" diff --git a/.script/template/env_setup.bash b/.script/template/env_setup.bash index aa904464..f8ab1dd2 100644 --- a/.script/template/env_setup.bash +++ b/.script/template/env_setup.bash @@ -1,13 +1,15 @@ #!/bin/bash export ROS_LOCALHOST_ONLY=1 +export RCUTILS_COLORIZED_OUTPUT=1 +export RMCS_PATH="/workspaces/RMCS" source /opt/ros/jazzy/setup.bash if [ -f "/rmcs_install/local_setup.bash" ]; then source /rmcs_install/local_setup.bash -elif [ -f "/workspaces/RMCS/rmcs_ws/install/local_setup.bash" ]; then - source /workspaces/RMCS/rmcs_ws/install/local_setup.bash +elif [ -f "${RMCS_PATH}/rmcs_ws/install/local_setup.bash" ]; then + source ${RMCS_PATH}/rmcs_ws/install/local_setup.bash fi export RMCS_ROBOT_TYPE="" diff --git a/.script/template/env_setup.zsh b/.script/template/env_setup.zsh index 518afb8f..2f7a0ee6 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -1,16 +1,22 @@ #!/bin/bash +export RCUTILS_COLORIZED_OUTPUT=1 export ROS_LOCALHOST_ONLY=1 +export RMCS_PATH="/workspaces/RMCS" source /opt/ros/jazzy/setup.zsh if [ -f "/rmcs_install/local_setup.zsh" ]; then source /rmcs_install/local_setup.zsh -elif [ -f "/workspaces/RMCS/rmcs_ws/install/local_setup.zsh" ]; then - source /workspaces/RMCS/rmcs_ws/install/local_setup.zsh +elif [ -f "${RMCS_PATH}/rmcs_ws/install/local_setup.zsh" ]; then + source ${RMCS_PATH}/rmcs_ws/install/local_setup.zsh fi eval "$(register-python-argcomplete ros2)" eval "$(register-python-argcomplete colcon)" export RMCS_ROBOT_TYPE="" + +fpath=(${RMCS_PATH}/.script/complete $fpath) +autoload -Uz compinit +compinit diff --git a/Dockerfile b/Dockerfile index 37bf1097..7fe52df3 100644 --- a/Dockerfile +++ b/Dockerfile @@ -18,6 +18,7 @@ RUN apt-get update && apt-get install -y --no-install-recommends \ vim wget curl unzip \ zsh screen tmux \ usbutils net-tools iputils-ping \ + gstreamer1.0-tools gstreamer1.0-plugins-base gstreamer1.0-plugins-good \ ripgrep htop fzf \ libusb-1.0-0-dev \ libeigen3-dev \ @@ -123,7 +124,7 @@ RUN sh -c "$(wget https://raw.githubusercontent.com/ohmyzsh/ohmyzsh/master/tools sed -i 's/ZSH_THEME=\"[a-z0-9\-]*\"/ZSH_THEME="af-magic"/g' ~/.zshrc && \ echo 'source ~/env_setup.zsh' >> ~/.zshrc && \ echo 'export PATH="${PATH}:/opt/nvim-linux-x86_64/bin"' >> ~/.zshrc && \ - echo 'export PATH=${PATH}:/workspaces/RMCS/.script' >> ~/.zshrc + echo 'export PATH="${PATH}:${RMCS_PATH}/.script"' >> ~/.zshrc # Copy environment setup scripts COPY --chown=1000:1000 .script/template/env_setup.bash env_setup.bash diff --git a/docker-compose.yml b/docker-compose.yml index 384717e7..d59afc5b 100644 --- a/docker-compose.yml +++ b/docker-compose.yml @@ -1,15 +1,18 @@ services: rmcs-develop: image: qzhhhi/rmcs-develop:latest - container_name: rmcs-develop - user: "developer" - hostname: "developer" + user: "${CONTAINER_USER}" privileged: true + command: > + bash -c " + sudo chown -R ${CONTAINER_USER}:${CONTAINER_USER} ${CONTAINER_HOME}/.config + exec bash + " volumes: - /dev:/dev:bind - /tmp/.X11-unix:/tmp/.X11-unix:bind - /run/user/1000/wayland-0:/run/user/1000/wayland-0 - - ~/.config/nvim/:/home/developer/.config/nvim/:bind + - ${HOST_NVIM_DIR}:${CONTAINER_HOME}/.config/nvim/:bind - .:/workspaces/RMCS:bind environment: - DISPLAY=${DISPLAY} diff --git a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml new file mode 100644 index 00000000..2d4ba216 --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml @@ -0,0 +1,159 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::WheelLegInfantry -> infantry_hardware + + - rmcs_core::broadcaster::ValueBroadcaster -> value_broadcaster + + # - rmcs_core::referee::Status -> referee_status + # - rmcs_core::referee::Command -> referee_command + + # - rmcs_core::referee::command::Interaction -> referee_interaction + # - rmcs_core::referee::command::interaction::Ui -> referee_ui + + # - rmcs_core::controller::gimbal::SimpleGimbalController -> gimbal_controller + # - rmcs_core::controller::pid::ErrorPidController -> yaw_angle_pid_controller + # - rmcs_core::controller::pid::ErrorPidController -> pitch_angle_pid_controller + + # - rmcs_core::controller::shooting::FrictionWheelController -> friction_wheel_controller + # - rmcs_core::controller::shooting::HeatController -> heat_controller + # - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller + # - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller + + - rmcs_core::controller::chassis::WheelLegController -> wheel_leg_controller + - rmcs_core::controller::chassis::WheelLegChassisController -> chassis_controller + # - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + + + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer + # - rmcs_auto_aim::AutoAimController -> auto_aim_controller + +infantry_hardware: + ros__parameters: + usb_pid_top_board: 0x93ac + usb_pid_bottom_board: 0xf30c + yaw_motor_zero_point: 32285 + pitch_motor_zero_point: 6321 + left_front_hip_motor_zero_point: 748 + left_back_hip_motor_zero_point: 32855 + right_back_hip_motor_zero_point: 32473 + right_front_hip_motor_zero_point: 65277 + +value_broadcaster: + ros__parameters: + forward_list: + - /chassis/yaw/angle + - /chassis/yaw/velocity + - /chassis/pitch/angle + - /chassis/pitch/velocity + - /chassis/left_wheel/control_torque + - /chassis/right_wheel/control_torque + + +yaw_angle_pid_controller: + ros__parameters: + measurement: /gimbal/yaw/control_angle_error + control: /gimbal/yaw/control_velocity + kp: 16.0 + ki: 0.0 + kd: 0.0 + +pitch_angle_pid_controller: + ros__parameters: + measurement: /gimbal/pitch/control_angle_error + control: /gimbal/pitch/control_velocity + kp: 10.00 + ki: 0.0 + kd: 0.0 + +friction_wheel_controller: + ros__parameters: + friction_wheels: + - /gimbal/left_friction + - /gimbal/right_friction + friction_velocities: + - 600.0 + - 600.0 + friction_soft_start_stop_time: 1.0 + +heat_controller: + ros__parameters: + heat_per_shot: 10000 + reserved_heat: 0 + +bullet_feeder_controller: + ros__parameters: + bullets_per_feeder_turn: 10.0 + shot_frequency: 24.0 + safe_shot_frequency: 10.0 + eject_frequency: 15.0 + eject_time: 0.15 + deep_eject_frequency: 15.0 + deep_eject_time: 0.20 + single_shot_max_stop_delay: 2.0 + +left_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/left_friction/velocity + setpoint: /gimbal/left_friction/control_velocity + control: /gimbal/left_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +right_friction_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/right_friction/velocity + setpoint: /gimbal/right_friction/control_velocity + control: /gimbal/right_friction/control_torque + kp: 0.003436926 + ki: 0.00 + kd: 0.009373434 + +bullet_feeder_velocity_pid_controller: + ros__parameters: + measurement: /gimbal/bullet_feeder/velocity + setpoint: /gimbal/bullet_feeder/control_velocity + control: /gimbal/bullet_feeder/control_torque + kp: 0.283 + ki: 0.0 + kd: 0.0 + +auto_aim_controller: + ros__parameters: + # capture + use_video: false # If true, use video stream instead of camera. + video_path: "/workspaces/RMCS/rmcs_ws/resources/1.avi" + exposure_time: 3 + invert_image: false + # identifier + armor_model_path: "/models/mlp.onnx" + # pnp + fx: 1.722231837421459e+03 + fy: 1.724876404292754e+03 + cx: 7.013056440882832e+02 + cy: 5.645821718351237e+02 + k1: -0.064232403853946 + k2: -0.087667493884102 + k3: 0.792381808294582 + # tracker + armor_predict_duration: 500 + # controller + gimbal_predict_duration: 100 + yaw_error: -0.02 + pitch_error: 0.01 + shoot_velocity: 21.0 + predict_sec: 0.050 + # etc + buff_predict_duration: 200 + buff_model_path: "/models/buff_nocolor_v6.onnx" + omni_exposure: 1000.0 + record_fps: 120 + debug: false # Setup in actual using.Debug mode is used when referee is not ready + debug_color: 0 # 0 For blue while 1 for red. mine + debug_robot_id: 4 + debug_buff_mode: false + record: true + raw_img_pub: false # Set false in actual use + image_viewer_type: 0 diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs index b82f2eaf..1347b210 160000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -1 +1 @@ -Subproject commit b82f2eafd21371a23d046e5f75884fe6c6e49124 +Subproject commit 1347b2109d0c93dbc3cfe5daaa6acd7754987f81 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index ac7d1368..2cb6b2bb 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -17,8 +17,14 @@ Test plugin. + + Test plugin. + Test plugin. + + + Test plugin. Test plugin. @@ -29,6 +35,9 @@ Steering wheel controller. + + Wheel leg controller. + Test plugin. 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 da70a96f..6b5b7539 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 @@ -58,8 +58,8 @@ class ChassisController using namespace rmcs_msgs; auto switch_right = *switch_right_; - auto switch_left = *switch_left_; - auto keyboard = *keyboard_; + auto switch_left = *switch_left_; + auto keyboard = *keyboard_; do { if ((switch_left == Switch::UNKNOWN || switch_right == Switch::UNKNOWN) @@ -74,14 +74,14 @@ class ChassisController if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::STEP_DOWN; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.c && keyboard.c) { if (mode == rmcs_msgs::ChassisMode::SPIN) { mode = rmcs_msgs::ChassisMode::AUTO; } else { - mode = rmcs_msgs::ChassisMode::SPIN; + mode = rmcs_msgs::ChassisMode::SPIN; spinning_forward_ = !spinning_forward_; } } else if (!last_keyboard_.x && keyboard.x) { @@ -100,8 +100,8 @@ class ChassisController } while (false); last_switch_right_ = switch_right; - last_switch_left_ = switch_left; - last_keyboard_ = keyboard; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; } void reset_all_controls() { @@ -112,7 +112,7 @@ class ChassisController void update_velocity_control() { auto translational_velocity = update_translational_velocity_control(); - auto angular_velocity = update_angular_velocity_control(); + auto angular_velocity = update_angular_velocity_control(); chassis_control_velocity_->vector << translational_velocity, angular_velocity; } @@ -133,7 +133,7 @@ class ChassisController } double update_angular_velocity_control() { - double angular_velocity = 0.0; + double angular_velocity = 0.0; double chassis_control_angle = nan; switch (*mode_) { @@ -171,7 +171,7 @@ class ChassisController angular_velocity = following_velocity_controller_.update(err); } break; } - *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; *chassis_control_angle_ = chassis_control_angle; return angular_velocity; @@ -201,8 +201,8 @@ class ChassisController static constexpr double nan = std::numeric_limits::quiet_NaN(); // Maximum control velocities - static constexpr double translational_velocity_max = 10.0; - static constexpr double angular_velocity_max = 16.0; + static constexpr double translational_velocity_max = 2.0; + static constexpr double angular_velocity_max = 16.0; InputInterface joystick_right_; InputInterface joystick_left_; @@ -214,8 +214,8 @@ class ChassisController InputInterface rotary_knob_; rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; - rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; OutputInterface chassis_angle_, chassis_control_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/README.md b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/README.md new file mode 100644 index 00000000..c301b00d --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/README.md @@ -0,0 +1,22 @@ +#Wheel-leg Controller + +desire state: [s ds phi d_phi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b] + +## reset +启动时收腿至最短后再启动lqr计算。 + +## Wheel-leg Mode +balanceless: 锁死关节，仅控轮子移动 +follow： yaw轴角度、角速度 +spin： yaw轴角速度 + +### balanceless +非平衡模式，关节以位置模式固定在0点，仅控制轮毂，不进行lqr计算。 + +### follow + +## Special Situation +rescue tip over: 关闭lqr，仅关节 +jump: 跳跃 +climb: 爬台阶 + diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_solver.hpp new file mode 100644 index 00000000..5abba902 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_solver.hpp @@ -0,0 +1,178 @@ +#pragma once + +#include "rmcs_msgs/wheel_leg_state.hpp" +#include + +namespace rmcs_core::controller { + +class DesireStateSolver { +public: + explicit DesireStateSolver(double desire_leg_length) + : initial_leg_length_(desire_leg_length) { + reset(); + } + + void reset() { + desire_state_ = reset_state(); + + desire_leg_length_ = initial_leg_length_; + desire_roll_angle_ = 0.0; + + jump_state_ = JumpState::IDLE; + } + + rmcs_msgs::WheelLegState update(Eigen::Vector3d control_velocity) { + // desire_leg_length_ += vz * leg_length_sensitivity_; + + if (std::abs(measure_state_.velocity) < 1e-2) { + park_mode_ = true; + } else { + park_mode_ = false; + } + + // distance :always 0 during velocity control + desire_state_.distance = 0.0; + desire_state_.velocity = 0.0; + + // yaw angle: + desire_state_.yaw_angle = 0.0; // else measure.yaw + desire_state_.yaw_velocity = 0.0; + + // leg tilt angle: + desire_state_.left_tilt_angle = 0.0; + desire_state_.left_tilt_velocity = 0.0; + + desire_state_.right_tilt_angle = 0.0; + desire_state_.right_tilt_velocity = 0.0; + + desire_state_.body_pitch_angle = 0.0; + desire_state_.body_pitch_velocity = 0.0; + + return desire_state_; + } + + void update_measure_state(rmcs_msgs::WheelLegState& measure_state) { + measure_state_ = measure_state; + } + + double + update_desire_leg_length(const bool is_leg_extended, const double min, const double max) { + if (is_leg_extended) { + desire_leg_length_ = 0.25; + } else { + desire_leg_length_ = 0.16; + } + desire_leg_length_ = std::clamp(desire_leg_length_, min, max); + + return desire_leg_length_; + } + + double update_desire_roll_angle() const { return desire_roll_angle_; } + + void update_jump_state(bool jump_active, double leg_length) { + switch (jump_state_) { + case JumpState::IDLE: { + if (jump_active) { + jump_state_ = JumpState::EXTEND_LEGS; + } + break; + } + case JumpState::EXTEND_LEGS: { + desire_leg_length_ = 0.35; + if (leg_length >= 0.33) { + jump_state_ = JumpState::CONTRACT_LEGS; + } + break; + } + case JumpState::CONTRACT_LEGS: { + desire_leg_length_ = 0.14; + if (leg_length <= 0.15) { + jump_state_ = JumpState::LEVITATE; + } + break; + } + case JumpState::LEVITATE: { + desire_leg_length_ = 0.17; + if (leg_length <= 0.18) { + jump_state_ = JumpState::LAND; + } + break; + } + case JumpState::LAND: { + jump_state_ = JumpState::IDLE; + break; + } + } + } + + void update_climb_state(bool climb_active, double leg_length) {} + + double desire_leg_length() const { return desire_leg_length_; } + double desire_roll_angle() const { return desire_roll_angle_; } + +private: + enum class JumpState { + IDLE = 0, + EXTEND_LEGS = 1, + CONTRACT_LEGS = 2, + LEVITATE = 3, + LAND = 4, + }; + + enum class ClimbState { + IDLE = 0, + DETECT = 1, + CONTRACT_LEGS = 2, + EXTEND_LEGS = 3, + LAND = 4, + }; + + void update_desire_state(Eigen::Vector3d control_velocity, double control_angle) { + // Update desire state based on control velocity and measure state + + // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity + auto& [vx, vz, wz] = control_velocity; + + // distance :always 0 during velocity control + desire_state_.distance = 0.0; + desire_state_.velocity = vx; + + desire_state_.yaw_angle = 0.0; // + desire_state_.yaw_velocity = wz; + + desire_state_.left_tilt_angle = 0.0; + desire_state_.left_tilt_velocity = 0.0; + desire_state_.right_tilt_angle = 0.0; + desire_state_.right_tilt_velocity = 0.0; + desire_state_.body_pitch_angle = 0.0; + desire_state_.body_pitch_velocity = 0.0; + } + + constexpr static rmcs_msgs::WheelLegState reset_state() { + return rmcs_msgs::WheelLegState{nan_, nan_, nan_, nan_, nan_, nan_, nan_, nan_, nan_, nan_}; + } + + constexpr static double nan_ = std::numeric_limits::quiet_NaN(); + + constexpr static double leg_length_sensitivity_ = 0.00001; + + constexpr static double dt_ = 0.001; + + // Maximum control velocities + static constexpr double translational_velocity_max_ = 2.0; + static constexpr double angular_velocity_max_ = 2.0; + + const double initial_leg_length_; + + bool park_mode_ = false; + + double desire_leg_length_; + double desire_roll_angle_; + + JumpState jump_state_ = JumpState::IDLE; + // ClimbState climb_state_ = ClimbState::IDLE; + + rmcs_msgs::WheelLegState measure_state_; + rmcs_msgs::WheelLegState desire_state_; +}; +} // namespace rmcs_core::controller \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/vmc_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/vmc_solver.hpp new file mode 100644 index 00000000..ab1bfed3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/vmc_solver.hpp @@ -0,0 +1,102 @@ +#pragma once + +#include +#include +#include + +namespace rmcs_core::controller::chassis { +class VmcSolver { +public: + VmcSolver(double l1, double l2, double l5) + : l1_(l1) + , l2_(l2) + , l3_(l2) + , l4_(l1) + , l5_(l5) { + reset(); + } + + void reset() { + tilt_angle_ = nan_; + leg_length_ = nan_; + } + + Eigen::Vector2d update(double phi1, double phi4) { + if (std::isnan(phi1) || std::isnan(phi4)) { + reset(); + return Eigen::Vector2d{leg_length_, tilt_angle_}; + } + + calculate_five_link_solution(phi1, phi4); + + return get_leg_posture(); + } + + Eigen::Vector2d update_joint_torque(double F, double Tp) { + return joint_torque_matrix_ * Eigen::Vector2d{F, Tp}; + } + + Eigen::Vector2d update_virtual_torque(double T1, double T2) { + return joint_torque_matrix_.inverse() * Eigen::Vector2d{T1, T2}; + } + +private: + void calculate_five_link_solution(double phi1, double phi4) { + // The coordinate system and variable definitions are referenced from the Zhihu article: + // https://zhuanlan.zhihu.com/p/613007726 + auto xb = l1_ * std::cos(phi1), yb = l1_ * std::sin(phi1); + auto xd = l5_ + l4_ * std::cos(phi4), yd = l4_ * std::sin(phi4); + + auto lbd = std::sqrt((xd - xb) * (xd - xb) + (yd - yb) * (yd - yb)); + + auto a = 2 * l2_ * (xd - xb), b = 2 * l2_ * (yd - yb), + c = l2_ * l2_ + lbd * lbd - l3_ * l3_; + + auto phi2 = 2 * std::atan2(b + std::sqrt(a * a + b * b - c * c), (a + c)), + phi3 = std::atan2((yb - yd) + l2_ * std::sin(phi2), (xb - xd) + l2_ * std::cos(phi2)); + + auto xc = l1_ * std::cos(phi1) + l2_ * std::cos(phi2), + yc = l1_ * std::sin(phi1) + l2_ * std::sin(phi2); + + auto phi0 = std::atan2(yc, xc - l5_ / 2.0); + + tilt_angle_ = pi_ / 2.0 - phi0; + leg_length_ = std::sqrt((xc - l5_ / 2.0) * (xc - l5_ / 2.0) + yc * yc); + + leg_posture_ = {leg_length_, tilt_angle_}; + + auto j11 = l1_ * std::sin(phi0 - phi3) * std::sin(phi1 - phi2) / std::sin(phi3 - phi2), + j12 = l4_ * std::sin(phi0 - phi2) * std::sin(phi3 - phi4) / std::sin(phi3 - phi2), + j21 = l1_ * std::cos(phi0 - phi3) * std::sin(phi1 - phi2) + / (leg_length_ * std::sin(phi3 - phi2)), + j22 = l4_ * std::cos(phi0 - phi2) * std::sin(phi3 - phi4) + / (leg_length_ * std::sin(phi3 - phi2)); + + auto jacobian_matrix = Eigen::Matrix2d{ + {j11, j12}, + {j21, j22} + }; + auto rotation_matrix = Eigen::Rotation2Dd(phi0 - pi_ / 2.0); + auto transform_matrix = Eigen::Matrix2d{ + {0, -1 / leg_length_}, + {1, 0} + }; + + joint_torque_matrix_ = jacobian_matrix.transpose() * rotation_matrix * transform_matrix; + } + + Eigen::Vector2d get_leg_posture() const { return leg_posture_; } + + static constexpr double inf_ = std::numeric_limits::infinity(); + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + + static constexpr double pi_ = std::numbers::pi; + + const double l1_, l2_, l3_, l4_, l5_; + + double leg_length_, tilt_angle_; + + Eigen::Vector2d leg_posture_; + Eigen::Matrix2d joint_torque_matrix_; +}; +} // namespace rmcs_core::controller::chassis \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_chassis_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_chassis_controller.cpp new file mode 100644 index 00000000..e5432c39 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_chassis_controller.cpp @@ -0,0 +1,276 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/pid/pid_calculator.hpp" + +namespace rmcs_core::controller::chassis { + +class WheelLegChassisController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + WheelLegChassisController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , following_velocity_controller_(7.0, 0.0, 0.0) { + following_velocity_controller_.output_max = angular_velocity_max; + following_velocity_controller_.output_min = -angular_velocity_max; + + 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("/remote/rotary_knob", rotary_knob_); + + register_input("/gimbal/yaw/angle", gimbal_yaw_angle_, false); + register_input("/gimbal/yaw/control_angle_error", gimbal_yaw_angle_error_, false); + + register_input("/chassis/yaw/angle", chassis_yaw_angle_imu_); + + register_output("/chassis/angle", chassis_angle_, nan); + register_output("/chassis/control_angle", chassis_control_angle_, nan); + + register_output("/chassis/control_mode", mode_); + + register_output("/chassis/status/balanceless", is_balanceless_, false); + register_output("/chassis/status/rescue_tip_over", is_rescue_tip_over_, false); + + register_output("/chassis/status/leg_extended", is_leg_extended_, false); + register_output("/chassis/status/jump", is_jump_active_, false); + register_output("/chassis/status/climb", is_climb_active_, false); + + register_output("/chassis/control_velocity", chassis_control_velocity_); + } + + void before_updating() override { + if (!gimbal_yaw_angle_.ready()) { + gimbal_yaw_angle_.make_and_bind_directly(0.0); + RCLCPP_WARN(get_logger(), "Failed to fetch \"/gimbal/yaw/angle\". Set to 0.0."); + } + if (!gimbal_yaw_angle_error_.ready()) { + gimbal_yaw_angle_error_.make_and_bind_directly(0.0); + RCLCPP_WARN( + get_logger(), "Failed to fetch \"/gimbal/yaw/control_angle_error\". Set to 0.0."); + } + } + + void update() override { + using namespace rmcs_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; + } + + auto mode = *mode_; + if (switch_left != Switch::DOWN) { + if (last_switch_right_ == Switch::DOWN && switch_right == Switch::MIDDLE) { + if (mode == rmcs_msgs::WheelLegMode::BALANCELESS) { + mode = rmcs_msgs::WheelLegMode::FOLLOW; + } + } + + if ((last_switch_right_ == Switch::MIDDLE && switch_right == Switch::DOWN) + || (!last_keyboard_.c && keyboard.c)) { + if (mode == rmcs_msgs::WheelLegMode::SPIN) { + mode = rmcs_msgs::WheelLegMode::FOLLOW; + } else { + mode = rmcs_msgs::WheelLegMode::SPIN; + spinning_forward_ = !spinning_forward_; + } + } else if (!last_keyboard_.x && keyboard.x) { + mode = mode == rmcs_msgs::WheelLegMode::LAUNCH_RAMP + ? rmcs_msgs::WheelLegMode::FOLLOW + : rmcs_msgs::WheelLegMode::LAUNCH_RAMP; + } else if (!last_keyboard_.b && keyboard.b) { + mode = mode == rmcs_msgs::WheelLegMode::BALANCELESS + ? rmcs_msgs::WheelLegMode::FOLLOW + : rmcs_msgs::WheelLegMode::BALANCELESS; + } + + // Change leg length + if (!last_keyboard_.z && keyboard.z) { + leg_extended_ = !leg_extended_; + } + + // Jump + jump_active_ = !last_keyboard_.e && keyboard.e; + + // Climb + climb_active_ = !last_keyboard_.q && keyboard.q; + + // Rescue tip-over + rescue_tip_over_ = !last_keyboard_.g && keyboard.g; + + *is_balanceless_ = mode == rmcs_msgs::WheelLegMode::BALANCELESS; + *is_leg_extended_ = leg_extended_; + *is_jump_active_ = jump_active_; + *is_climb_active_ = climb_active_; + *is_rescue_tip_over_ = rescue_tip_over_; + + *mode_ = mode; + } + + update_velocity_control(); + update_chassis_angle_control(); + } while (false); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + + void reset_all_controls() { + *mode_ = rmcs_msgs::WheelLegMode::BALANCELESS; + + *chassis_control_velocity_ = {nan, nan, nan}; + } + + void update_velocity_control() { + auto translational_velocity = update_translational_velocity_control(); + auto angular_velocity = update_angular_velocity_control(); + + chassis_control_velocity_->vector << translational_velocity, angular_velocity; + } + + Eigen::Vector2d update_translational_velocity_control() { + auto keyboard = *keyboard_; + Eigen::Vector2d keyboard_move{keyboard.w - keyboard.s, keyboard.a - keyboard.d}; + + Eigen::Vector2d translational_velocity = + Eigen::Rotation2Dd{*gimbal_yaw_angle_} * (*joystick_right_ + keyboard_move); + + if (translational_velocity.norm() > 1.0) + translational_velocity.normalize(); + + translational_velocity *= translational_velocity_max; + + return translational_velocity; + } + + void update_chassis_angle_control() { + double chassis_control_angle = nan; + + *chassis_control_angle_ = chassis_control_angle; + } + + double update_angular_velocity_control() { + double angular_velocity = 0.0; + double chassis_control_angle = nan; + + switch (*mode_) { + case rmcs_msgs::WheelLegMode::BALANCELESS: + case rmcs_msgs::WheelLegMode::SPIN: { + angular_velocity = + 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + chassis_control_angle = + *chassis_yaw_angle_imu_; // When spinning, yaw angle's output is set to zero. + } break; + case rmcs_msgs::WheelLegMode::FOLLOW: + case rmcs_msgs::WheelLegMode::LAUNCH_RAMP: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + // err: [0, 2pi) -> signed + // Only one direction can be used for alignment. + // TODO: Dynamically determine the split angle based on chassis velocity. + constexpr double alignment = 2 * std::numbers::pi; + if (err > alignment / 2) + err -= alignment; + + angular_velocity = following_velocity_controller_.update(err); + + chassis_control_angle = 0.0; + } break; + } + *chassis_angle_ = 2 * std::numbers::pi - *gimbal_yaw_angle_; + *chassis_control_angle_ = chassis_control_angle; + + return angular_velocity; + } + + double calculate_unsigned_chassis_angle_error(double& chassis_control_angle) { + chassis_control_angle = *gimbal_yaw_angle_error_; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + // chassis_control_angle: [0, 2pi). + + // err = setpoint - measurement + // ^ ^ + // |gimbal_yaw_angle_error |chassis_angle + // ^ + // |(2pi - gimbal_yaw_angle) + double err = chassis_control_angle + *gimbal_yaw_angle_; + if (err >= 2 * std::numbers::pi) + err -= 2 * std::numbers::pi; + // err: [0, 2pi). + + return err; + } + +private: + static constexpr double inf = std::numeric_limits::infinity(); + static constexpr double nan = std::numeric_limits::quiet_NaN(); + + // Maximum control velocities + static constexpr double translational_velocity_max = 2.0; + static constexpr double angular_velocity_max = 2.0; + + InputInterface joystick_right_; + InputInterface joystick_left_; + InputInterface switch_right_; + InputInterface switch_left_; + InputInterface mouse_velocity_; + InputInterface mouse_; + InputInterface keyboard_; + InputInterface rotary_knob_; + + rmcs_msgs::Switch last_switch_right_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Switch last_switch_left_ = rmcs_msgs::Switch::UNKNOWN; + rmcs_msgs::Keyboard last_keyboard_ = rmcs_msgs::Keyboard::zero(); + + InputInterface gimbal_yaw_angle_, gimbal_yaw_angle_error_; + InputInterface chassis_yaw_angle_imu_; + + OutputInterface chassis_angle_, chassis_control_angle_; + + OutputInterface mode_; + + OutputInterface is_balanceless_; + OutputInterface is_rescue_tip_over_; + + OutputInterface is_leg_extended_; + OutputInterface is_jump_active_; + OutputInterface is_climb_active_; + + bool spinning_forward_ = true; + bool leg_extended_ = false, jump_active_ = false, climb_active_ = false, + rescue_tip_over_ = false, launch_ramp_active_ = false; + + pid::PidCalculator following_velocity_controller_; + + OutputInterface chassis_yaw_control_angle_; + OutputInterface chassis_control_velocity_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::WheelLegChassisController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_controller.cpp new file mode 100644 index 00000000..725328e4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_controller.cpp @@ -0,0 +1,773 @@ +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "controller/lqr/lqr_calculator.hpp" +#include "controller/pid/pid_calculator.hpp" +#include "desire_state_solver.hpp" +#include "filter/kalman_filter.hpp" +#include "filter/low_pass_filter.hpp" +#include "rmcs_executor/component.hpp" + +#include "vmc_solver.hpp" + +namespace rmcs_core::controller::chassis { +class WheelLegController + : public rmcs_executor::Component + , public rclcpp::Node { +public: + WheelLegController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , body_mess_(13.3) + , leg_mess_(0.629) + , wheel_mess_(0.459) + , wheel_radius_(0.06) + , wheel_distance_(0.2192) + , centroid_position_coefficient_(0.2945) + , left_leg_vmc_solver_(0.21, 0.25, 0.0) + , right_leg_vmc_solver_(0.21, 0.25, 0.0) + , velocity_kalman_filter_(A_, H_, Q_, R_) + , chassis_gyro_velocity_filter_(5.0, 1000.0) + , desire_state_solver_(0.15) + , roll_angle_pid_calculator_(1.0, 0.0, 1.0) + , leg_length_pid_calculator_(90.0, 0.0, 0.0) + , balanceless_left_wheel_pid_calculator_(3.0, 0.0, 0.0) + , balanceless_right_wheel_pid_calculator_(3.0, 0.0, 0.0) + , rescue_velocity_pid_calculator_(3.0, 0.0, 0.0) { + + register_input("/chassis/left_front_hip/max_torque", hip_motor_max_control_torque_); + register_input("/chassis/left_wheel/max_torque", wheel_motor_max_control_torque_); + + register_input("/chassis/control_velocity", chassis_control_velocity_); + register_input("/chassis/control_mode", mode_); + + register_input("/chassis/control_angle", chassis_control_angle_); + + register_input("/chassis/status/balanceless", is_balanceless_); + register_input("/chassis/status/rescue_tip_over", is_rescue_tip_over_); + + register_input("/chassis/status/leg_extended", is_leg_extended_); + register_input("/chassis/status/jump", is_jump_active_); + register_input("/chassis/status/climb", is_climb_active_); + + register_input("/chassis/left_front_hip/angle", left_front_hip_angle_); + register_input("/chassis/left_back_hip/angle", left_back_hip_angle_); + register_input("/chassis/right_front_hip/angle", right_front_hip_angle_); + register_input("/chassis/right_back_hip/angle", right_back_hip_angle_); + + register_input("/chassis/left_front_hip/velocity", left_front_hip_velocity_); + register_input("/chassis/left_back_hip/velocity", left_back_hip_velocity_); + register_input("/chassis/right_front_hip/velocity", right_front_hip_velocity_); + register_input("/chassis/right_back_hip/velocity", right_back_hip_velocity_); + + register_input("/chassis/left_front_hip/torque", left_front_hip_torque_); + register_input("/chassis/left_back_hip/torque", left_back_hip_torque_); + register_input("/chassis/right_front_hip/torque", right_front_hip_torque_); + register_input("/chassis/right_back_hip/torque", right_back_hip_torque_); + + register_input("/chassis/left_wheel/velocity", left_wheel_velocity_); + register_input("/chassis/right_wheel/velocity", right_wheel_velocity_); + + register_input("/chassis/x_axis/acceleration", chassis_x_axis_acceleration_imu_); + register_input("/chassis/z_axis/acceleration", chassis_z_axis_acceleration_imu_); + + register_input("/chassis/yaw/velocity", chassis_yaw_velocity_imu_); + register_input("/chassis/pitch/velocity", chassis_pitch_velocity_imu_); + register_input("/chassis/roll/velocity", chassis_roll_velocity_imu_); + + register_input("/chassis/yaw/angle", chassis_yaw_angle_imu_); + register_input("/chassis/pitch/angle", chassis_pitch_angle_imu_); + register_input("/chassis/roll/angle", chassis_roll_angle_imu_); + + register_output("/chassis/left_front_hip/control_torque", left_front_hip_control_torque_); + register_output("/chassis/left_back_hip/control_torque", left_back_hip_control_torque_); + register_output("/chassis/right_front_hip/control_torque", right_front_hip_control_torque_); + register_output("/chassis/right_back_hip/control_torque", right_back_hip_control_torque_); + + register_output("/chassis/left_front_hip/control_angle", left_front_hip_control_angle_); + register_output("/chassis/left_back_hip/control_angle", left_back_hip_control_angle_); + register_output("/chassis/right_front_hip/control_angle", right_front_hip_control_angle_); + register_output("/chassis/right_back_hip/control_angle", right_back_hip_control_angle_); + + register_output("/chassis/left_wheel/control_torque", left_wheel_control_torque_); + register_output("/chassis/right_wheel/control_torque", right_wheel_control_torque_); + + velocity_kalman_filter_.set_process_noise_transition(W_); + } + + ~WheelLegController() override { reset_all_controls(); } + + void before_updating() override { + RCLCPP_INFO( + get_logger(), "Max control torque of hip motor: %.f, wheel motor: %.f", + *hip_motor_max_control_torque_, *wheel_motor_max_control_torque_); + } + + void update() override { + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } + + // Observer + auto hip_angles = calculate_hips_angles(); + auto wheel_velocities = calculate_wheel_velocities(); + + auto leg_posture = calculate_leg_posture(hip_angles); + auto distance = calculate_translational_distance(leg_posture, wheel_velocities); + + // State: [s ds phi d_phi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b] + auto measure_state = calculate_measure_state(distance, leg_posture); + + // Controller + auto chassis_control_velocity = calculate_chassis_control_velocity(); + auto leg_forces = calculate_leg_force(leg_posture, measure_state); + + auto desire_state = calculate_desire_state(chassis_control_velocity, measure_state); + auto control_torques = calculate_control_torques(desire_state, measure_state, leg_posture); + + // if (*is_balanceless_) { + // update_balanceless_control_torques(chassis_control_velocity, wheel_velocities); + // return; + // } + + // if (!stand_active_) { + // update_hip_stand_control_torques(leg_posture); + // return; + // } + + // if (*is_rescue_tip_over_) { + // update_hip_rescue_tip_over_control_torques(); + // } + + update_hip_and_wheel_torques(leg_forces, control_torques); + + // *left_front_hip_control_torque_ = 0.0; + // *left_back_hip_control_torque_ = 0.0; + *right_front_hip_control_torque_ = 0.0; + *right_back_hip_control_torque_ = 0.0; + + *left_wheel_control_torque_ = 0.0; + *right_wheel_control_torque_ = 0.0; + } + +private: + struct LegPosture { + Eigen::Vector2d leg_length; + Eigen::Vector2d tilt_angle; + + Eigen::Vector2d diff_leg_length; + Eigen::Vector2d diff_tilt_angle; + + Eigen::Vector2d second_order_diff_leg_length; + Eigen::Vector2d second_order_diff_tilt_angle; + }; + + void reset_all_controls() { + left_leg_vmc_solver_.reset(); + right_leg_vmc_solver_.reset(); + + desire_state_solver_.reset(); + velocity_kalman_filter_.reset(); + chassis_gyro_velocity_filter_.reset(); + + *left_front_hip_control_torque_ = nan_; + *left_back_hip_control_torque_ = nan_; + *right_front_hip_control_torque_ = nan_; + *right_back_hip_control_torque_ = nan_; + + *left_wheel_control_torque_ = 0.0; + *right_wheel_control_torque_ = 0.0; + + *left_front_hip_control_angle_ = nan_; + *left_back_hip_control_angle_ = nan_; + *right_front_hip_control_angle_ = nan_; + *right_back_hip_control_angle_ = nan_; + } + + Eigen::Vector2d calculate_wheel_velocities() { + return { + *left_wheel_velocity_ * wheel_radius_, // + *right_wheel_velocity_ * wheel_radius_}; + } + + Eigen::Vector4d calculate_hips_angles() { + return { + *left_front_hip_angle_, // + *left_back_hip_angle_, // + *right_back_hip_angle_, // + *right_front_hip_angle_, // + }; + } + + LegPosture calculate_leg_posture(const Eigen::Vector4d& hip_angles) { + const auto& [lf, lb, rb, rf] = hip_angles; + LegPosture result; + + const auto& [left_leg_length, left_tilt_angle] = left_leg_vmc_solver_.update(lf, lb); + const auto& [right_leg_length, right_tilt_angle] = right_leg_vmc_solver_.update(rf, rb); + + result.leg_length = Eigen::Vector2d{left_leg_length, right_leg_length}; + result.tilt_angle = Eigen::Vector2d{left_tilt_angle, right_tilt_angle}; + + result.tilt_angle.array() += *chassis_pitch_angle_imu_; + + result.diff_leg_length = (result.leg_length - last_leg_length_) / dt_; + last_leg_length_ = result.leg_length; + result.diff_tilt_angle = (result.tilt_angle - last_tilt_angle_) / dt_; + last_tilt_angle_ = result.tilt_angle; + + result.second_order_diff_leg_length = (result.diff_leg_length - last_dot_leg_length_) / dt_; + last_dot_leg_length_ = result.diff_leg_length; + result.second_order_diff_tilt_angle = (result.diff_tilt_angle - last_dot_tilt_angle_) / dt_; + last_dot_tilt_angle_ = result.diff_tilt_angle; + + return result; + } + + Eigen::Vector2d calculate_support_force(LegPosture leg_posture) { + Eigen::Vector2d result; + auto& [left_support_force, right_support_force] = result; + + const auto& [left_leg_force, left_leg_torque] = left_leg_vmc_solver_.update_virtual_torque( + *left_front_hip_torque_, *left_back_hip_torque_); + const auto& [right_leg_force, right_leg_torque] = + right_leg_vmc_solver_.update_virtual_torque( + *right_front_hip_torque_, *right_back_hip_torque_); + + auto left_leg_to_wheel_force = + left_leg_force * std::cos(leg_posture.tilt_angle.x()) + + left_leg_torque * std::sin(leg_posture.tilt_angle.x()) / leg_posture.leg_length.x(); + auto right_leg_to_wheel_force = + right_leg_force * std::cos(leg_posture.tilt_angle.y()) + + right_leg_torque * std::sin(leg_posture.tilt_angle.y()) / leg_posture.leg_length.y(); + + // todo: test + auto left_wheel_vertical_accel = + *chassis_z_axis_acceleration_imu_ + - leg_posture.second_order_diff_leg_length.x() * std::cos(leg_posture.tilt_angle.x()) + + 2 * leg_posture.diff_leg_length.x() * leg_posture.diff_tilt_angle.x() + * std::sin(leg_posture.tilt_angle.x()) + + leg_posture.leg_length.x() * leg_posture.second_order_diff_tilt_angle.x() + * std::sin(leg_posture.tilt_angle.x()) + + leg_posture.leg_length.x() * leg_posture.diff_tilt_angle.x() + * leg_posture.diff_tilt_angle.x() * std::cos(leg_posture.tilt_angle.x()); + + auto right_wheel_vertical_accel = + *chassis_z_axis_acceleration_imu_ + - leg_posture.second_order_diff_leg_length.y() * std::cos(leg_posture.tilt_angle.y()) + + 2 * leg_posture.diff_leg_length.y() * leg_posture.diff_tilt_angle.y() + * std::sin(leg_posture.tilt_angle.y()) + + leg_posture.leg_length.y() * leg_posture.second_order_diff_tilt_angle.y() + * std::sin(leg_posture.tilt_angle.y()) + + leg_posture.leg_length.y() * leg_posture.diff_tilt_angle.y() + * leg_posture.diff_tilt_angle.y() * std::cos(leg_posture.tilt_angle.y()); + + left_support_force = + left_leg_to_wheel_force + wheel_mess_ * (g_ + left_wheel_vertical_accel); + right_support_force = + right_leg_to_wheel_force + wheel_mess_ * (g_ + right_wheel_vertical_accel); + + return result; + } + + Eigen::Vector2d + calculate_translational_distance(LegPosture leg_posture, Eigen::Vector2d wheel_velocities) { + Eigen::Vector2d result; + auto& [distance, velocity] = result; + + auto wheel_velocity = (wheel_velocities.x() + wheel_velocities.y()) / 2.0; + + auto left_leg_velocity = + leg_posture.leg_length.x() * std::cos(leg_posture.tilt_angle.x()) + * leg_posture.diff_tilt_angle.x() + + leg_posture.diff_leg_length.x() * std::sin(leg_posture.tilt_angle.x()); + + auto right_leg_velocity = + leg_posture.leg_length.y() * std::cos(leg_posture.tilt_angle.y()) + * leg_posture.diff_tilt_angle.y() + + leg_posture.diff_leg_length.y() * std::sin(leg_posture.tilt_angle.y()); + + // auto calculate_velocity = wheel_velocity + (left_leg_velocity + right_leg_velocity) + // / 2.0; + auto calculate_velocity = wheel_velocity; + + // Velocity ​​estimation is referenced from the article: + // https://zhuanlan.zhihu.com/p/689921165 + // auto estimate_velocity = velocity_kalman_filter_.update( + // Eigen::Vector2d{calculate_velocity, *chassis_x_axis_acceleration_imu_}); + + velocity = calculate_velocity; + + auto is_parked = std::abs(velocity) < 1e-2; + // When the vehicle stops the position control is activated. + if (!last_is_parked_ && is_parked) { + distance = last_distance_ + velocity * dt_; + } else { + distance = 0.0; + } + last_is_parked_ = is_parked; + + distance = last_distance_ + velocity * dt_; + last_distance_ = distance; + + return Eigen::Vector2d{distance, velocity}; + } + + rmcs_msgs::WheelLegState + calculate_measure_state(Eigen::Vector2d distance, LegPosture leg_posture) { + rmcs_msgs::WheelLegState measure_state; + + measure_state.distance = distance.x(); + measure_state.velocity = distance.y(); + + measure_state.yaw_angle = *chassis_yaw_angle_imu_; + measure_state.yaw_velocity = *chassis_yaw_velocity_imu_; + + measure_state.left_tilt_angle = leg_posture.tilt_angle.x(); + measure_state.left_tilt_velocity = leg_posture.diff_tilt_angle.x(); + + measure_state.right_tilt_angle = leg_posture.tilt_angle.y(); + measure_state.right_tilt_velocity = leg_posture.diff_tilt_angle.y(); + + measure_state.body_pitch_angle = *chassis_pitch_angle_imu_; + measure_state.body_pitch_velocity = *chassis_pitch_velocity_imu_; + + measure_state.distance = 0.0; + measure_state.velocity = 0.0; + + // measure_state.yaw_angle = 0.0; + // measure_state.yaw_velocity = 0.0; + + // measure_state.left_tilt_angle = 0.0; + // measure_state.left_tilt_velocity = 0.0; + + // measure_state.right_tilt_angle = 0.0; + // measure_state.right_tilt_velocity = 0.0; + + // measure_state.body_pitch_angle = 0.0; + // measure_state.body_pitch_velocity = 0.0; + + return measure_state; + } + + rmcs_msgs::WheelLegState detect_chassis_levitate( + Eigen::Vector2d support_forces, rmcs_msgs::WheelLegState measure_state) { + auto& [left_support_force, right_support_force] = support_forces; + + double levitate_force = 20.0; + double normal_force = 50.0; + + auto average_support_force = (left_support_force + right_support_force) / 2.0; + + auto mode = *mode_; + auto mode_active = mode != rmcs_msgs::WheelLegMode::BALANCELESS; + + if (mode_active) { + if (average_support_force < levitate_force) { + levitate_active_ = true; + } else if (average_support_force > normal_force) { + levitate_active_ = false; + } + + if (left_support_force < levitate_force) { + left_levitate_active_ = true; + } else if (left_support_force > normal_force) { + left_levitate_active_ = false; + } + + if (right_support_force < levitate_force) { + right_levitate_active_ = true; + } else if (right_support_force > normal_force) { + right_levitate_active_ = false; + } + } else { + levitate_active_ = false; + left_levitate_active_ = false; + right_levitate_active_ = false; + } + + if (levitate_active_) { + RCLCPP_INFO( + get_logger(), "Chassis is levitating! Average support force: %f", + average_support_force); + measure_state.distance = 0.0; + } + return measure_state; + } + + // When the vehicle is about to fall, reset the leg length. + void detect_chassis_fall() { + // Detect pitch and roll angle + if (fabs(*chassis_roll_angle_imu_ - desire_roll_angle_) > about_to_fall_roll_angle_) { + about_to_fall_ = true; + } + + if (levitate_active_) { + about_to_fall_ = false; + } + + // It will inactive when rotating, climbing, or launching ramp. + if (*is_climb_active_ || *mode_ == rmcs_msgs::WheelLegMode::LAUNCH_RAMP + || *mode_ == rmcs_msgs::WheelLegMode::SPIN) { + about_to_fall_ = false; + } + } + + Eigen::Vector3d calculate_chassis_control_velocity() { + Eigen::Vector3d chassis_control_velocity; + chassis_control_velocity = chassis_control_velocity_->vector; + return chassis_control_velocity; + } + + rmcs_msgs::WheelLegState calculate_desire_state( + Eigen::Vector3d control_velocity, rmcs_msgs::WheelLegState measure_state) { + rmcs_msgs::WheelLegState desire_state{}; + + desire_state_solver_.update_measure_state(measure_state); + desire_state = desire_state_solver_.update(std::move(control_velocity)); + + desire_leg_length_ = + desire_state_solver_.update_desire_leg_length(*is_leg_extended_, 0.13, 0.36); + desire_roll_angle_ = desire_state_solver_.update_desire_roll_angle(); + + return desire_state; + } + + Eigen::Vector2d + calculate_leg_force(LegPosture leg_posture, rmcs_msgs::WheelLegState measure_state) { + Eigen::Vector2d result; + + auto leg_length = (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0; + + auto roll_control_force = + roll_angle_pid_calculator_.update(desire_roll_angle_ - *chassis_roll_angle_imu_); + auto leg_length_control_force = + leg_length_pid_calculator_.update(desire_leg_length_ - leg_length); + + auto calculate_compensation_feedforward_force = [this](double coefficient) { + return (body_mess_ / 2.0 + centroid_position_coefficient_ * leg_mess_) * coefficient; + }; + + auto gravity_feedforward_control_force = calculate_compensation_feedforward_force(g_); + auto inertial_feedforward_control_force = calculate_compensation_feedforward_force( + leg_length / (2 * wheel_distance_) * measure_state.yaw_velocity + * measure_state.velocity); + + // F_ψ + // F_bl_l = 1 1 1 -1 * F_l + // F_bl_r -1 1 1 1 F_g + // F_i + result = Eigen::Vector2d{ + roll_control_force + leg_length_control_force + gravity_feedforward_control_force + - inertial_feedforward_control_force, // F_bl_l + -roll_control_force + leg_length_control_force + gravity_feedforward_control_force + + inertial_feedforward_control_force // F_bl_r + }; + + return result; + } + + Eigen::Vector4d calculate_control_torques( + rmcs_msgs::WheelLegState desire_state, rmcs_msgs::WheelLegState measure_state, + LegPosture leg_posture) { + Eigen::Vector4d result{}; + + Eigen::Vector error_state = desire_state.vector() - measure_state.vector(); + auto gain = lqr_calculator_.update(leg_posture.leg_length.x(), leg_posture.leg_length.y()); + + if (levitate_active_) { + // When levitating, only keep legs vertical,and only remain legs' outputs. + // set zero + error_state(0) = 0.0; // distance + error_state(1) = 0.0; // velocity + error_state(2) = 0.0; // yaw angle + error_state(3) = 0.0; // yaw velocity + + error_state(8) = 0.0; // body pitch angle + error_state(9) = 0.0; // body pitch velocity + } + + result = -1.0 * gain * error_state; + + auto output_0 = -gain(0, 0) * error_state(0); + auto output_1 = -gain(0, 1) * error_state(1); + auto output_2 = -gain(0, 2) * error_state(2); + auto output_3 = -gain(0, 3) * error_state(3); + auto output_4 = -gain(0, 4) * error_state(4); + auto output_5 = -gain(0, 5) * error_state(5); + auto output_6 = -gain(0, 6) * error_state(6); + auto output_7 = -gain(0, 7) * error_state(7); + auto output_8 = -gain(0, 8) * error_state(8); + auto output_9 = -gain(0, 9) * error_state(9); + + RCLCPP_INFO( + get_logger(), "output: [%f, %f, %f, %f, %f, %f, %f, %f, %f, %f], %f, %f", output_0, + output_1, output_2, output_3, output_4, output_5, output_6, output_7, output_8, + output_9, result.x(), result.y()); + + // RCLCPP_INFO( + // get_logger(), "measure: [%f, %f, %f, %f, %f, %f, %f, %f, %f, %f]", + // measure_state.distance, measure_state.velocity, measure_state.yaw_angle, + // measure_state.yaw_velocity, measure_state.left_tilt_angle, + // measure_state.left_tilt_velocity, measure_state.right_tilt_angle, + // measure_state.right_tilt_velocity, measure_state.body_pitch_angle, + // measure_state.body_pitch_velocity); + + return result; + } + + void update_balanceless_control_torques( + Eigen::Vector3d chassis_control_velocity, Eigen::Vector2d wheel_velocities) { + auto& [chassis_control_velocity_x, chassis_control_velocity_y, chassis_control_velocity_z] = + chassis_control_velocity; + auto& [left_wheel_velocity, right_wheel_velocity] = wheel_velocities; + + double left_wheel_control_torque = balanceless_left_wheel_pid_calculator_.update( + (chassis_control_velocity_x - chassis_control_velocity_y) - left_wheel_velocity); + double right_wheel_control_torque = balanceless_right_wheel_pid_calculator_.update( + (chassis_control_velocity_x + chassis_control_velocity_y) - right_wheel_velocity); + + *left_wheel_control_torque_ = clamp_wheel_control_torque(left_wheel_control_torque); + *right_wheel_control_torque_ = clamp_wheel_control_torque(right_wheel_control_torque); + } + + void update_hip_zero_point_control_angles() { + *left_front_hip_control_angle_ = 0.0; + *left_back_hip_control_angle_ = 0.0; + *right_front_hip_control_angle_ = 0.0; + *right_back_hip_control_angle_ = 0.0; + + reset_control_torques(); + } + + void update_hip_rescue_tip_over_control_torques() { + double control_velocity = pi_; + + // stand_active_ = false; + auto left_front_hip_control_torque = + rescue_velocity_pid_calculator_.update(control_velocity - *left_front_hip_velocity_); + auto left_back_hip_control_torque = + rescue_velocity_pid_calculator_.update(control_velocity - *left_back_hip_velocity_); + auto right_front_hip_control_torque = + rescue_velocity_pid_calculator_.update(control_velocity - *right_front_hip_velocity_); + auto right_back_hip_control_torque = + rescue_velocity_pid_calculator_.update(control_velocity - *right_back_hip_velocity_); + + *left_front_hip_control_torque_ = left_front_hip_control_torque; + *left_back_hip_control_torque_ = left_back_hip_control_torque; + *right_front_hip_control_torque_ = right_front_hip_control_torque; + *right_back_hip_control_torque_ = right_back_hip_control_torque; + + reset_control_angles(); + } + + void update_hip_stand_control_torques(LegPosture leg_posture) { + auto leg_length = (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0; + + auto leg_length_control_force = + leg_length_pid_calculator_.update(min_leg_length_ - leg_length); + + if (leg_length < min_leg_length_ + 0.01) { + stand_active_ = true; + } + + RCLCPP_INFO( + get_logger(), "control force: %f, leg length: %f,stand active: %d", + leg_length_control_force, leg_length, stand_active_); + + *left_front_hip_control_torque_ = -leg_length_control_force; + *left_back_hip_control_torque_ = leg_length_control_force; + *right_front_hip_control_torque_ = -leg_length_control_force; + *right_back_hip_control_torque_ = leg_length_control_force; + } + + void update_hip_and_wheel_torques(Eigen::Vector2d leg_forces, Eigen::Vector4d control_torques) { + auto& [left_leg_force, right_leg_force] = leg_forces; + auto& [left_wheel_control_torque, right_wheel_control_torque, left_leg_control_torque, right_leg_control_torque] = + control_torques; + + auto left_hip_control_torque = + left_leg_vmc_solver_.update_joint_torque(left_leg_force, left_leg_control_torque); + auto right_hip_control_torque = + right_leg_vmc_solver_.update_joint_torque(right_leg_force, right_leg_control_torque); + + // RCLCPP_INFO( + // get_logger(), "lf: %f, lb: %f, rf: %f, rb: %f,control_torque: %f", + // left_hip_control_torque.x(), left_hip_control_torque.y(), + // right_hip_control_torque.x(), right_hip_control_torque.y(), torque); + + if (levitate_active_) { + *left_wheel_control_torque_ = 0.0; + *right_wheel_control_torque_ = 0.0; + } + + *left_wheel_control_torque_ = clamp_wheel_control_torque(left_wheel_control_torque); + *right_wheel_control_torque_ = clamp_wheel_control_torque(right_wheel_control_torque); + + *left_front_hip_control_torque_ = clamp_hip_control_torque(-left_hip_control_torque.x()); + *left_back_hip_control_torque_ = clamp_hip_control_torque(left_hip_control_torque.y()); + *right_front_hip_control_torque_ = clamp_hip_control_torque(-right_hip_control_torque.x()); + *right_back_hip_control_torque_ = clamp_hip_control_torque(right_hip_control_torque.y()); + + // RCLCPP_INFO( + // get_logger(), "lf: %f, lb: %f, rb: %f, rf: %f", *left_front_hip_control_torque_, + // *left_back_hip_control_torque_, *right_back_hip_control_torque_, + // *right_front_hip_control_torque_); + + reset_control_angles(); + } + + static double clamp_wheel_control_torque(const double& torque) { + return std::clamp(torque, -2.4, 2.4); + } + + static double clamp_hip_control_torque(const double& torque) { + return std::clamp(torque, -10., 10.); + } + + void reset_control_torques() { + *left_front_hip_control_torque_ = nan_; + *left_back_hip_control_torque_ = nan_; + *right_front_hip_control_torque_ = nan_; + *right_back_hip_control_torque_ = nan_; + } + + void reset_control_angles() { + *left_front_hip_control_angle_ = nan_; + *left_back_hip_control_angle_ = nan_; + *right_front_hip_control_angle_ = nan_; + *right_back_hip_control_angle_ = nan_; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double inf_ = std::numeric_limits::infinity(); + + static constexpr double pi_ = std::numbers::pi; + + static constexpr double dt_ = 1e-3; + static constexpr double g_ = 9.80665; + + static constexpr double max_leg_length_ = 0.36; + static constexpr double min_leg_length_ = 0.13; + + static constexpr double about_to_fall_roll_angle_ = 0.6; + + static constexpr double sigma_q_ = 1.0; + static constexpr double sigma_v_ = 0.75; + static constexpr double sigma_a_ = 1.0; + + const Eigen::Matrix2d A_ = (Eigen::Matrix2d() << 1, dt_, 1, 1).finished(); + const Eigen::Matrix2d H_ = Eigen::Vector2d::Identity().asDiagonal(); + const Eigen::Matrix2d W_ = Eigen::Vector2d{0.5 * dt_ * dt_, dt_}.asDiagonal(); + const Eigen::Matrix2d Q_ = + Eigen::Vector2d{sigma_q_ * sigma_q_, sigma_q_* sigma_q_}.asDiagonal(); + const Eigen::Matrix2d R_ = + Eigen::Vector2d{sigma_v_ * sigma_v_, sigma_a_* sigma_a_}.asDiagonal(); + + const double body_mess_; + const double leg_mess_; + const double wheel_mess_; + const double wheel_radius_; + const double wheel_distance_; + const double centroid_position_coefficient_; + + InputInterface hip_motor_max_control_torque_; + InputInterface wheel_motor_max_control_torque_; + + InputInterface left_front_hip_angle_; + InputInterface left_back_hip_angle_; + InputInterface right_front_hip_angle_; + InputInterface right_back_hip_angle_; + + InputInterface left_front_hip_velocity_; + InputInterface left_back_hip_velocity_; + InputInterface right_front_hip_velocity_; + InputInterface right_back_hip_velocity_; + + InputInterface left_front_hip_torque_; + InputInterface left_back_hip_torque_; + InputInterface right_front_hip_torque_; + InputInterface right_back_hip_torque_; + + InputInterface left_wheel_velocity_; + InputInterface right_wheel_velocity_; + + InputInterface chassis_x_axis_acceleration_imu_; + InputInterface chassis_z_axis_acceleration_imu_; + + InputInterface chassis_yaw_velocity_imu_; + InputInterface chassis_pitch_velocity_imu_; + InputInterface chassis_roll_velocity_imu_; + + InputInterface chassis_yaw_angle_imu_; + InputInterface chassis_pitch_angle_imu_; + InputInterface chassis_roll_angle_imu_; + + InputInterface chassis_control_velocity_; + InputInterface mode_; + + InputInterface chassis_control_angle_; + + InputInterface is_balanceless_; + InputInterface is_rescue_tip_over_; + + InputInterface is_leg_extended_; + InputInterface is_jump_active_; + InputInterface is_climb_active_; + + OutputInterface left_front_hip_control_torque_; + OutputInterface left_back_hip_control_torque_; + OutputInterface right_front_hip_control_torque_; + OutputInterface right_back_hip_control_torque_; + + OutputInterface left_front_hip_control_angle_; + OutputInterface left_back_hip_control_angle_; + OutputInterface right_front_hip_control_angle_; + OutputInterface right_back_hip_control_angle_; + + OutputInterface left_wheel_control_torque_; + OutputInterface right_wheel_control_torque_; + + double desire_leg_length_, desire_roll_angle_; + bool levitate_active_{false}, left_levitate_active_{false}, right_levitate_active_{false}; + + bool about_to_fall_{false}, rescue_tip_over_{false}; + + bool stand_active_{false}; + Eigen::Vector2d last_leg_length_, last_tilt_angle_; + Eigen::Vector2d last_dot_leg_length_, last_dot_tilt_angle_; + double last_distance_; + bool last_is_parked_{false}; + + Eigen::Vector3d filtered_gyro_velocity_; + + VmcSolver left_leg_vmc_solver_, right_leg_vmc_solver_; + filter::KalmanFilter<2, 2> velocity_kalman_filter_; + filter::LowPassFilter<3> chassis_gyro_velocity_filter_; + + DesireStateSolver desire_state_solver_; + + pid::PidCalculator roll_angle_pid_calculator_, leg_length_pid_calculator_; + pid::PidCalculator balanceless_left_wheel_pid_calculator_, + balanceless_right_wheel_pid_calculator_; + pid::PidCalculator rescue_velocity_pid_calculator_; + lqr::LqrCalculator lqr_calculator_; +}; +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::WheelLegController, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp new file mode 100644 index 00000000..4df23ad4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp @@ -0,0 +1,526 @@ +#include +#include +#include + +// Due to the nonlinear nature of the wheeled leg's kinematic model, the Jacobian matrix derived +// from its linearized kinematic equations becomes highly complex and verbose. Furthermore, since +// leg length varies, the Jacobian matrix also changes dynamically, making conventional LQR +// operations impractical. Therefore, calculations are first performed in MATLAB to obtain all gain +// matrices across the entire leg length range. Subsequently, a bivariate function of the left +// and right leg lengths is fitted to derive the gain matrix K. + +namespace rmcs_core::controller::lqr { + +class LqrCalculator { +public: + LqrCalculator() = default; + + auto update(double l_l, double l_r) -> Eigen::Matrix { + calculate_fit_gain(l_l, l_r); + return gain_; + } + +private: + void calculate_fit_gain(double l_l, double l_r) { + double K_result[40]; + + double K_result_tmp; + double b_K_result_tmp; + double c_K_result_tmp; + double d_K_result_tmp; + double e_K_result_tmp; + double f_K_result_tmp; + double t2; + double t3; + double t4; + double t5; + double t6; + double t7; + + // Q = diag([50 1 50 5 200 1 200 1 90000 1]); + // R = diag([5 5 1 1]); + /* This function was generated by the Symbolic Math Toolbox version 24.2. + */ + /* 2026-03-03 22:47:45 */ + t2 = l_l * l_l; + t3 = std::pow(l_l, 3.0); + t5 = l_r * l_r; + t6 = std::pow(l_r, 3.0); + t4 = t2 * t2; + t7 = t5 * t5; + K_result_tmp = l_l * l_r; + b_K_result_tmp = l_l * t5; + c_K_result_tmp = l_l * t6; + d_K_result_tmp = l_r * t2; + e_K_result_tmp = l_r * t3; + f_K_result_tmp = t2 * t5; + K_result[0] = + (((((l_l * 58.228736048753483 + l_r * 33.103667133299957) - t2 * 247.06523961628389) + + t3 * 444.41379470087611) + - t4 * 324.04872498056068) + + ((((t5 * -27.627137177622231 - t6 * 137.8807600352639) + t7 * 198.21345056752861) + - K_result_tmp * 308.73984691592369) + + b_K_result_tmp * 267.96175273009129)) + + ((((c_K_result_tmp * 9.08196319858063 + d_K_result_tmp * 770.58679400356664) + - e_K_result_tmp * 538.73756232870119) + - f_K_result_tmp * 421.81357833750042) + - 2.8104538078645449); + K_result[1] = + (((((l_l * 62.624920877281482 + l_r * 16.717501327576919) - t2 * 382.36985482885791) + + t3 * 987.79798175503208) + - t4 * 968.14111559165838) + + ((((t5 * -18.413365869447269 - t6 * 128.041787493163) + t7 * 238.80695093998889) + - K_result_tmp * 103.7843555941548) + + b_K_result_tmp * 236.733047170841)) + + ((((c_K_result_tmp * -144.55644166273819 + d_K_result_tmp * 33.427333164975558) + + e_K_result_tmp * 244.5316281076085) + - f_K_result_tmp * 239.45277981777639) + - 2.1791443227497149); + K_result[2] = + (((((l_l * 45.199668912874657 - l_r * 133.37039925261709) - t2 * 313.65146568983653) + + t3 * 595.028744608601) + - t4 * 347.01549018702309) + + (((((t5 * 740.77330333419991 - t6 * 1364.3839191756549) + t7 * 915.878738434753) + + K_result_tmp * 390.32339261336381) + - b_K_result_tmp * 1563.3084684772871) + + c_K_result_tmp * 924.3439106607243)) + + (((d_K_result_tmp * -176.90117159488489 - e_K_result_tmp * 373.82111774148268) + + f_K_result_tmp * 1390.897918209871) + + 5.2190528601972819); + K_result[3] = + ((((((l_l * -95.845777377017981 - l_r * 30.72147189942379) + t2 * 528.88006720250166) + - t3 * 1015.512695899405) + + t4 * 637.68738136157413) + - t5 * 66.986405633576425) + + ((((t6 * 315.79585515596318 - t7 * 317.57679216164792) + + K_result_tmp * 594.55592177587926) + - b_K_result_tmp * 570.44629557727546) + + c_K_result_tmp * 225.46417690934081)) + + (((d_K_result_tmp * -1856.4738304888349 + e_K_result_tmp * 1858.823995736886) + + f_K_result_tmp * 669.80668709854081) + + 7.3650163595102827); + K_result[4] = + (((((l_l * 36.995257450124839 + l_r * 12.55026750009255) - t2 * 166.18810963330739) + + t3 * 289.4751384119557) + - t4 * 201.85967866740819) + + ((((t5 * 38.3880830401618 - t6 * 232.21550667564219) + t7 * 248.1973618059354) + - K_result_tmp * 215.51446488840591) + + b_K_result_tmp * 114.0878835202176)) + + ((((c_K_result_tmp * 92.578360381467448 + d_K_result_tmp * 605.85647925738556) + - e_K_result_tmp * 441.93914615759343) + - f_K_result_tmp * 295.32901320048148) + - 0.41931959930014751); + K_result[5] = + (((((l_l * 37.961363816258071 + l_r * 1.4119265703988519) - t2 * 277.03608860455779) + + t3 * 775.84381877914984) + - t4 * 798.52454084931458) + + ((((t5 * 29.366151229234511 - t6 * 197.903846308387) + t7 * 278.9149347533837) + - K_result_tmp * 28.053992751778651) + + b_K_result_tmp * 114.2151588570368)) + + ((((c_K_result_tmp * -83.965953117067727 - d_K_result_tmp * 100.92588214903731) + + e_K_result_tmp * 308.004300372261) + - f_K_result_tmp * 121.23522603912021) + + 0.10994337762760779); + K_result[6] = + ((((((l_l * 37.049884812542693 - l_r * 127.9672518694564) - t2 * 280.79636927197743) + + t3 * 524.379350516641) + - t4 * 278.28627740861731) + + t5 * 676.53633625429188) + + ((((t6 * -1196.8605588657031 + t7 * 776.98031728853891) + + K_result_tmp * 350.95942296883231) + - b_K_result_tmp * 1386.2516759925029) + + c_K_result_tmp * 748.64913428876548)) + + (((d_K_result_tmp * -89.856077579745786 - e_K_result_tmp * 467.34126122417149) + + f_K_result_tmp * 1275.5281346346969) + + 5.99664580396614); + K_result[7] = + (((((l_l * -89.670509769918851 - l_r * 35.999895350734519) + t2 * 451.2549383184982) + - t3 * 790.71019195217275) + + t4 * 437.51663627405679) + + ((((t5 * -32.575128085752183 + t6 * 220.565490077677) - t7 * 215.19516337547549) + + K_result_tmp * 545.396199463892) + - b_K_result_tmp * 501.3084186236652)) + + ((((c_K_result_tmp * 144.38244592692541 - d_K_result_tmp * 1641.35022798624) + + e_K_result_tmp * 1600.5632133735221) + + f_K_result_tmp * 606.79421501970444) + + 7.9317062154928024); + K_result[8] = + ((((((l_l * 51.577407150102523 - l_r * 39.3979818534695) - t2 * 315.24055484465413) + + t3 * 776.840507806621) + - t4 * 758.16765518070565) + + t5 * 314.13344756914091) + + ((((t6 * -594.63991424554888 + t7 * 270.276717903546) + - K_result_tmp * 21.814210226192682) + - b_K_result_tmp * 342.088733888716) + + c_K_result_tmp * 735.57044414981783)) + + (((d_K_result_tmp * 159.90362783204969 + e_K_result_tmp * 206.5364740808115) + - f_K_result_tmp * 413.15127752314078) + - 2.4068995511497668); + K_result[9] = + (((((l_l * 20.53656644858567 - l_r * 42.864693352418627) - t2 * 290.52665870270249) + + t3 * 920.26748845973259) + - t4 * 958.10370351267773) + + ((((t5 * 162.0390556234299 - t6 * 308.9714037111238) + t7 * 134.48258846522381) + + K_result_tmp * 242.7556195917528) + - b_K_result_tmp * 217.46356464069859)) + + ((((c_K_result_tmp * 459.94067595358928 - d_K_result_tmp * 707.64660802859112) + + e_K_result_tmp * 941.92423932619306) + - f_K_result_tmp * 259.02918572370652) + + 2.78567303171074); + K_result[10] = + (((((l_l * 6.5775853010830216 - l_r * 51.541606325315783) - t2 * 113.0407397213974) + + t3 * 212.55103804137309) + - t4 * 132.60947514989519) + + ((((t5 * -100.70935280893291 + t6 * 965.39604273081454) - t7 * 1132.1522757852181) + + K_result_tmp * 433.72380392092509) + - b_K_result_tmp * 941.840457189477)) + + ((((c_K_result_tmp * 342.61790204450239 - d_K_result_tmp * 746.7076972112917) + + e_K_result_tmp * 599.50732396263356) + + f_K_result_tmp * 775.65595411408913) + - 1.04591027895466); + K_result[11] = + (((((l_l * -1.699054416577543 + l_r * 8.7738985827291334) + t2 * 264.96477569735208) + - t3 * 1412.226886309782) + + t4 * 1912.3056541742969) + + ((((t5 * -159.91699240473679 + t6 * 357.074092707535) - t7 * 135.4729368979817) + - K_result_tmp * 132.51931828305641) + + b_K_result_tmp * 527.773327732422)) + + ((((c_K_result_tmp * -995.85853612360154 + d_K_result_tmp * 655.16017084696375) + - e_K_result_tmp * 1162.7801501513411) + + f_K_result_tmp * 174.073223265881) + + 3.4807002393381361); + K_result[12] = + (((((l_l * 19.2912855880038 - l_r * 15.59006591047687) - t2 * 126.4063075263532) + + t3 * 316.0008493924114) + - t4 * 305.8852073042824) + + ((((t5 * 122.5024065897455 - t6 * 221.77960111450659) + t7 * 97.494722786072813) + + K_result_tmp * 1.538105880685477) + - b_K_result_tmp * 168.11633504451751)) + + ((((c_K_result_tmp * 289.7307432078648 + d_K_result_tmp * 62.856368980169449) + + e_K_result_tmp * 56.819665366943717) + - f_K_result_tmp * 116.4321822117082) + - 0.89211028040034157); + K_result[13] = + (((((l_l * 7.7338960323159478 - l_r * 15.671016038549251) - t2 * 113.5531545626657) + + t3 * 369.716857917176) + - t4 * 402.223599839497) + + ((((t5 * 60.452358518621921 - t6 * 116.11855770874919) + t7 * 40.857521200214073) + + K_result_tmp * 93.424925224177514) + - b_K_result_tmp * 75.656746031420781)) + + ((((c_K_result_tmp * 215.49146103404269 - d_K_result_tmp * 297.20004298969769) + + e_K_result_tmp * 444.1622228587579) + - f_K_result_tmp * 170.3452803115739) + + 1.05196229835372); + K_result[14] = + (((((l_l * 0.72266573134172729 - l_r * 20.918303791295308) - t2 * 55.480524588133029) + + t3 * 138.072245511404) + - t4 * 101.86482279644611) + + ((((t5 * -48.596671037258027 + t6 * 379.32706837522761) - t7 * 395.47803470363169) + + K_result_tmp * 193.27642427620231) + - b_K_result_tmp * 355.66080710707479)) + + ((((c_K_result_tmp * 0.62759137639297491 - d_K_result_tmp * 354.79293483631159) + + e_K_result_tmp * 213.58411514631521) + + f_K_result_tmp * 443.9628188463189) + - 0.248606035620003); + K_result[15] = + (((((l_l * -2.331523503866241 + l_r * 6.6872525523815787) + t2 * 121.02435807687669) + - t3 * 569.36243639593363) + + t4 * 722.62972804903427) + + ((((t5 * -70.181960935253926 + t6 * 130.431896393743) - t7 * 39.907907631632021) + - K_result_tmp * 57.960278978475458) + + b_K_result_tmp * 260.55692671847851)) + + ((((c_K_result_tmp * -377.53453789617828 + d_K_result_tmp * 202.8183718739219) + - e_K_result_tmp * 317.72759627626817) + - f_K_result_tmp * 33.729455672491078) + + 1.25525610407707); + K_result[16] = + ((((((l_l * 148.34175476383379 + l_r * 207.966709996097) - t2 * 698.76655224496278) + + t3 * 1695.5704254763209) + - t4 * 1411.84685332747) + - t5 * 975.40666122237667) + + (((((t6 * 1043.498995320381 + t7 * 175.079867115138) + - K_result_tmp * 1240.1043098701821) + + b_K_result_tmp * 3444.33705956047) + - c_K_result_tmp * 3571.28772073013) + + d_K_result_tmp * 2394.0408240941761)) + + ((e_K_result_tmp * -2629.5266782767371 - f_K_result_tmp * 1248.888310453209) + - 19.644085951107851); + K_result[17] = + ((((((l_l * 135.90705046483151 + l_r * 273.90781157732431) - t2 * 405.97709893626148) + + t3 * 149.74006691267979) + + t4 * 154.9397619975629) + - t5 * 1378.4705609299481) + + (((((t6 * 3751.0276638740052 - t7 * 3323.697684130791) + - K_result_tmp * 1284.6253574316361) + - b_K_result_tmp * 220.18713945411631) + + c_K_result_tmp * 286.74364972226329) + + d_K_result_tmp * 5193.6228202681459)) + + ((e_K_result_tmp * -4761.1931848640106 - f_K_result_tmp * 1216.918862322118) + - 18.896681717238739); + K_result[18] = + ((((((l_l * 273.23831876652758 - l_r * 518.917376160053) - t2 * 1572.962249519197) + + t3 * 3980.2168396595121) + - t4 * 3004.6122541564418) + + t5 * 4965.73427092819) + + (((((t6 * -14741.573790620891 + t7 * 14133.648444324641) + - K_result_tmp * 502.98807241679049) + - b_K_result_tmp * 441.91513506961172) + - c_K_result_tmp * 55.455202164980392) + + d_K_result_tmp * 1019.426228953821)) + + ((e_K_result_tmp * -3485.6317899744381 + f_K_result_tmp * 3758.6001897489359) + + 14.75198405409205); + K_result[19] = + ((((((l_l * -128.19104048905959 - l_r * 262.02437944578412) + t2 * 616.59418773734342) + - t3 * 1274.5843627789941) + + t4 * 566.2486480392057) + + t5 * 974.03462520892379) + + (((((t6 * -640.48292692163488 - t7 * 900.26435000822664) + + K_result_tmp * 2099.585097699633) + - b_K_result_tmp * 5817.7929448079094) + + c_K_result_tmp * 5942.5041818988557) + - d_K_result_tmp * 4892.6178941754479)) + + ((e_K_result_tmp * 5937.489393094 + f_K_result_tmp * 2433.1132836876968) + + 9.11731354085421); + K_result[20] = + (((((l_l * -19.90536935447378 - l_r * 40.532760516244842) + t2 * 81.604031621603269) + - t3 * 151.37439331904639) + + t4 * 144.32877449311289) + + ((((t5 * 241.61830850037961 - t6 * 665.29081165091316) + t7 * 633.18882127968516) + + K_result_tmp * 127.41876701174731) + - b_K_result_tmp * 185.24680601851949)) + + ((((c_K_result_tmp * 44.095805713565021 - d_K_result_tmp * 346.7031922528239) + + e_K_result_tmp * 160.19740081318071) + + f_K_result_tmp * 388.309686918287) + + 2.4932655339066052); + K_result[21] = + (((((l_l * -32.214181421295287 - l_r * 23.308786682921031) + t2 * 185.0763194887142) + - t3 * 505.95319317668748) + + t4 * 533.47570202008626) + + (((((t5 * 104.3246217330357 - t6 * 218.4421672747099) + t7 * 233.98704660058269) + + K_result_tmp * 132.54057020867751) + - b_K_result_tmp * 360.13213865101221) + + c_K_result_tmp * 33.58601331476914)) + + (((d_K_result_tmp * -168.6869130806526 - e_K_result_tmp * 139.39264624224671) + + f_K_result_tmp * 607.404956085748) + + 2.528939097759423); + K_result[22] = + ((((((l_l * -73.468680020839116 - l_r * 139.34719164846661) + t2 * 281.736456883201) + - t3 * 478.52406016941569) + + t4 * 298.45508145958718) + + t5 * 692.83625731199515) + + ((((t6 * -1431.112907187374 + t7 * 1009.673299590109) + + K_result_tmp * 367.46261021059269) + - b_K_result_tmp * 638.0149212210415) + + c_K_result_tmp * 621.89419781089612)) + + (((d_K_result_tmp * -799.32112713913023 + e_K_result_tmp * 690.14700424549346) + + f_K_result_tmp * 327.28002956982237) + + 11.67481120676139); + K_result[23] = + ((((((l_l * -144.16533642481309 - l_r * 58.879774411056488) + t2 * 759.27555784601373) + - t3 * 1816.21341478931) + + t4 * 1564.076299085546) + + t5 * 166.95024865353) + + ((((t6 * -107.70643089018169 - t7 * 51.36552097461248) + + K_result_tmp * 336.12313235616818) + - b_K_result_tmp * 868.30625683240578) + + c_K_result_tmp * 604.29562610538073)) + + (((d_K_result_tmp * -344.23089952144568 + e_K_result_tmp * 144.84086144542289) + + f_K_result_tmp * 477.227902261538) + + 10.56958501924821); + K_result[24] = + ((((((l_l * 265.92356991359048 + l_r * 136.09665232173481) - t2 * 1676.729242505729) + + t3 * 4177.3922622201426) + - t4 * 3563.777583564342) + - t5 * 383.3162869772047) + + (((((t6 * 167.0093233847455 + t7 * 704.18911191583243) + - K_result_tmp * 1030.4982078401561) + + b_K_result_tmp * 1625.876432697112) + - c_K_result_tmp * 2654.1201517656409) + + d_K_result_tmp * 3139.4410998713938)) + + ((e_K_result_tmp * -3989.2775805431788 + f_K_result_tmp * 265.8032322914143) + - 17.923123361129861); + K_result[25] = + ((((((l_l * 165.87822406236671 + l_r * 188.8827987290156) - t2 * 1038.4030316983651) + + t3 * 2565.381122099755) + - t4 * 3031.0179590773441) + - t5 * 957.79178550515007) + + (((((t6 * 2849.6080286297711 - t7 * 2982.9710578479048) + - K_result_tmp * 773.42273491360527) + - b_K_result_tmp * 630.88959816365946) + + c_K_result_tmp * 2387.1248505329791) + + d_K_result_tmp * 3048.1413011036661)) + + ((e_K_result_tmp * -278.9963681705795 - f_K_result_tmp * 3748.5146705300481) + - 20.506023237161351); + K_result[26] = + ((((((l_l * 282.62473563390739 - l_r * 617.48046578412777) - t2 * 2386.5855580423458) + + t3 * 6709.3111107593859) + - t4 * 5236.2906273195094) + + t5 * 4708.7644185571926) + + (((((t6 * -13898.332538807679 + t7 * 14889.466801418081) + + K_result_tmp * 721.99825562849458) + - b_K_result_tmp * 1316.4094776487709) + - c_K_result_tmp * 5262.4974339808687) + - d_K_result_tmp * 1651.064775755201)) + + ((e_K_result_tmp * -4624.4550983701884 + f_K_result_tmp * 11223.540750787581) + + 8.1706330027919183); + K_result[27] = + ((((((l_l * -220.650798687078 - l_r * 175.38156831070481) + t2 * 1360.5068272525559) + - t3 * 2632.26488635414) + + t4 * 720.0924719059991) + + t5 * 536.88397023607752) + + ((((t6 * -423.49261769393689 - t7 * 728.0263366591447) + + K_result_tmp * 2195.7595651091551) + - b_K_result_tmp * 3435.299526884075) + + c_K_result_tmp * 5248.2926984051473)) + + (((d_K_result_tmp * -7535.5864386627954 + e_K_result_tmp * 11599.91573885726) + - f_K_result_tmp * 1292.485937994757) + + 21.337946464281561); + K_result[28] = + (((((l_l * -19.63601091357544 - l_r * 38.197419113076229) + t2 * 61.25944368117969) + - t3 * 94.764418769428488) + + t4 * 106.68241200576929) + + ((((t5 * 216.6566111166218 - t6 * 583.75878541510008) + t7 * 603.82942328913771) + + K_result_tmp * 158.4677397753108) + - b_K_result_tmp * 269.41809070013483)) + + ((((c_K_result_tmp * -49.145228754568173 - d_K_result_tmp * 343.099189196269) + + e_K_result_tmp * 41.455352393830381) + + f_K_result_tmp * 601.65635603759) + + 2.6580022116202762); + K_result[29] = + (((((l_l * -34.899831297204813 - l_r * 22.833521448530639) + t2 * 197.58111728289111) + - t3 * 507.53181338454459) + + t4 * 471.40930444158022) + + (((((t5 * 105.5585473773459 - t6 * 224.5158205089468) + t7 * 231.3280436597895) + + K_result_tmp * 129.60745144512859) + - b_K_result_tmp * 347.18618487867428) + + c_K_result_tmp * 114.9359670095277)) + + (((d_K_result_tmp * -220.982423727273 + e_K_result_tmp * 73.556995823672551) + + f_K_result_tmp * 433.04704515253837) + + 2.3509210386938451); + K_result[30] = + ((((((l_l * -70.004947841126011 - l_r * 137.8045609465353) + t2 * 234.10435704022831) + - t3 * 298.32598635163492) + + t4 * 129.93927394490959) + + t5 * 721.32140071126059) + + ((((t6 * -1708.441474902465 + t7 * 1484.12177067571) + + K_result_tmp * 332.97797958092139) + - b_K_result_tmp * 372.81925826393581) + + c_K_result_tmp * 5.9600249777896481)) + + (((d_K_result_tmp * -815.75133504851215 + e_K_result_tmp * 501.17087290251811) + + f_K_result_tmp * 665.12823990358788) + + 10.863104904686979); + K_result[31] = + ((((((l_l * -150.24802711608129 - l_r * 59.002410970125922) + t2 * 740.89284784982817) + - t3 * 1526.468364312283) + + t4 * 1062.146810634383) + + t5 * 178.89130062553789) + + ((((t6 * -187.92005550723621 - t7 * 4.7484201834860871) + + K_result_tmp * 410.61859771781872) + - b_K_result_tmp * 856.9640995451922) + + c_K_result_tmp * 864.98230598495286)) + + (((d_K_result_tmp * -774.34580016866073 + e_K_result_tmp * 926.84249789950582) + + f_K_result_tmp * 123.5347637461572) + + 11.40440724817717); + K_result[32] = + ((((((l_l * 736.876257179423 + l_r * 24.57871704269186) - t2 * 1712.368318318677) + - t3 * 2016.708865159032) + + t4 * 5366.706755588727) + - t5 * 556.96187658861686) + + (((((t6 * 4953.7655203150989 - t7 * 4158.0534193163157) + - K_result_tmp * 6350.6691451684956) + - b_K_result_tmp * 1296.840720394365) + - c_K_result_tmp * 505.08151469443311) + + d_K_result_tmp * 24976.367954435271)) + + ((e_K_result_tmp * -24697.680856733328 - f_K_result_tmp * 1537.158407461053) + - 38.221087328987451); + K_result[33] = + ((((((l_l * -111.7465678538375 + l_r * 763.4600655332946) - t2 * 2344.1945611221909) + + t3 * 15367.220037727569) + - t4 * 23985.566072400321) + - t5 * 334.55505448680282) + + (((((t6 * -2388.80358474881 - t7 * 2071.0627607914389) + - K_result_tmp * 4574.5811282504856) + + b_K_result_tmp * 14976.275505038881) + + c_K_result_tmp * 13135.09010058618) + - d_K_result_tmp * 6735.1311526055433)) + + ((e_K_result_tmp * 33325.191621329432 - f_K_result_tmp * 39302.43020772918) + - 36.182644324436573); + K_result[34] = + ((((((l_l * 2420.503318815789 + l_r * 6143.8944379083869) - t2 * 9280.24016315598) + + t3 * 14298.01216915547) + - t4 * 5802.1046953095693) + - t5 * 35386.999984935523) + + (((((t6 * 76545.214712403555 - t7 * 51108.20121957302) + - K_result_tmp * 13291.407167858129) + + b_K_result_tmp * 16546.96365316361) + - c_K_result_tmp * 40641.614259948306) + + d_K_result_tmp * 36146.3207118627)) + + ((e_K_result_tmp * -49036.879471330627 + f_K_result_tmp * 22072.777873299241) + - 251.3196049645417); + K_result[35] = + ((((((l_l * 7454.5610907465971 + l_r * 395.83294707917258) - t2 * 51192.718548341429) + + t3 * 141603.7524464239) + - t4 * 138922.9760037543) + + t5 * 4489.5515450181392) + + (((((t6 * -23705.802238864639 + t7 * 23523.85335666623) + - K_result_tmp * 5230.4462406190878) + + b_K_result_tmp * 19054.785103179471) + + c_K_result_tmp * 2709.6044186886079) + - d_K_result_tmp * 13172.699200378371)) + + ((e_K_result_tmp * 37692.776031145462 - f_K_result_tmp * 27616.302386032919) + - 222.48855189921889); + K_result[36] = + ((((((l_l * 131.41777137182311 + l_r * 196.94458882445139) - t2 * 516.06737949077467) + + t3 * 853.110221233211) + - t4 * 661.54984473162676) + - t5 * 1073.025086126939) + + ((((t6 * 2729.0504665161111 - t7 * 2457.65720047848) + - K_result_tmp * 888.28592169532919) + + b_K_result_tmp * 1091.394886173809) + - c_K_result_tmp * 477.3105776775206)) + + (((d_K_result_tmp * 2526.5521720645388 - e_K_result_tmp * 1765.359149347313) + - f_K_result_tmp * 1816.875247340846) + - 14.61403178775773); + K_result[37] = + ((((((l_l * 180.51178326205991 + l_r * 132.1858833559553) - t2 * 1083.3885780721059) + + t3 * 2997.962279988516) + - t4 * 3188.066777707395) + - t5 * 507.97379577084808) + + (((((t6 * 1017.850835039008 - t7 * 1089.332095938563) + - K_result_tmp * 748.44306920015936) + + b_K_result_tmp * 1605.1230885767461) + + c_K_result_tmp * 148.16348316990781) + + d_K_result_tmp * 1116.17529547479)) + + ((e_K_result_tmp * 624.031731806414 - f_K_result_tmp * 3151.285188018489) + - 14.01007950341004); + K_result[38] = + ((((((l_l * 411.43812374517017 + l_r * 588.46790950689967) - t2 * 1693.6615335326451) + + t3 * 2973.9705447492761) + - t4 * 1775.724544927338) + - t5 * 2711.51493725362) + + (((((t6 * 5470.16624803537 - t7 * 3612.7035410009039) + - K_result_tmp * 1771.438581005081) + + b_K_result_tmp * 2401.6127061675152) + - c_K_result_tmp * 3173.0482147419671) + + d_K_result_tmp * 4388.3635019216472)) + + ((e_K_result_tmp * -4752.7051042806 + f_K_result_tmp * 17.11838297076914) + - 50.250386522225263); + K_result[39] = + ((((((l_l * 731.44526909753881 + l_r * 191.83506761012259) - t2 * 4150.7971047625942) + + t3 * 10424.458049645669) + - t4 * 9459.4121383535385) + - t5 * 402.47966125676709) + + ((((t6 * -256.33154253826632 + t7 * 705.78299551337511) + - K_result_tmp * 1064.344092768267) + + b_K_result_tmp * 3146.3426354255312) + - c_K_result_tmp * 1733.633436063812)) + + (((d_K_result_tmp * 254.611995259114 + e_K_result_tmp * 1224.48570331033) + - f_K_result_tmp * 2305.3502994921878) + - 46.54601482144038); + + gain_ = Eigen::Map>(K_result); + } + + Eigen::Matrix gain_; +}; +} // namespace rmcs_core::controller::lqr diff --git a/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_controller.hpp b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_controller.hpp new file mode 100644 index 00000000..15f9ea88 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_controller.hpp @@ -0,0 +1,183 @@ + +#include +#include +#include +// #include +#include +#include + +// https://github.com/giacomo-b/CppRobotics/blob/e2b5cdad0ea3f982be2c310cf4d9023d347f97a4/include/robotics/linear_control/lqr.hpp + +namespace rmcs_core::controller::lqr { + +template +using ColumnVector = Eigen::Matrix; + +template +using SquareMatrix = Eigen::Matrix; + +template +using Matrix = Eigen::Matrix; + +double deg2rad(double deg) { return deg * M_PI / 180.0; } + +class NormalDistributionRandomGenerator { +public: + template + ColumnVector GetColumnVector() { + ColumnVector rand_vector; + + for (auto i = 0; i < rand_vector.size(); i++) + rand_vector(i) = distribution(generator); + + return rand_vector; + } + +private: + std::default_random_engine generator; + std::normal_distribution distribution{0.0, 1.0}; +}; + +template +class LqrController { + static_assert(StateSize > 0); + static_assert(InputSize > 0); + + using VectorNx1 = ColumnVector; + using VectorMx1 = ColumnVector; + + using MatrixNxN = SquareMatrix; + using MatrixMxM = SquareMatrix; + using InputMatrix = Matrix; + using GainMatrix = Matrix; + + using State = VectorNx1; + +public: + /** + * @brief Creates a new Linear-Quadratic Regulator + * @param A state matrix + * @param B control matrix + * @param Q state weights matrix + * @param R control weights matrix + * @todo pass a linear system as input + */ + LqrController(MatrixNxN A, InputMatrix B, MatrixNxN Q, MatrixMxM R) + : A(A) + , B(B) + , Q(Q) + , R(R) + , K(ComputeGain()) {} + + /** + * @brief Solves the LQR problem + * @param initial the initial state + * @param target the target state + * @return a vector containing the state along the whole path + * @todo this behavior should be in a user-defined loop, refactor so that only one step is + * carried out at a time + */ + std::vector Solve(State initial, State target, double dt) { + std::vector path{initial}; + path.reserve( + (unsigned int)std::round(max_time / dt) + 1); // TODO: currently assuming the worst case + + State x = initial - target; + VectorMx1 u; + + bool path_found = false; + double time = 0; + double goal_dist_squared = std::pow(goal_dist, 2); + + while (time < max_time) { + time += dt; + + u = Input(x); + x = A * x + B * u; + + path.push_back(x + target); + + if (x.squaredNorm() <= goal_dist_squared) { + path_found = true; + break; + } + } + + if (!path_found) { + std::cerr << "Couldn't find a path\n"; + return {}; + } + + return path; + } + + /** + * @brief Sets the maximum simulation time + * @param limit time limit + * @todo remove + */ + void SetTimeLimit(double limit) { max_time = limit; } + + /** + * @brief Sets the tolerance w.r.t. the target + * @param tol tolerance + * @todo remove, the user should decide when to exit + */ + void SetFinalPositionTolerance(double tol) { goal_dist = tol; } + + /** + * @brief Sets the maximum number of iterations + * @param limit maximum number of iterations + * @todo remove, the user should decide when to exit + */ + void SetIterationsLimit(int limit) { max_iter = limit; } + + /** + * @brief Sets the tolerance for the resolution of the Discrete Algebraic Riccati Equation + * @param tol tolerance + */ + void SetDARETolerance(double tol) { eps = tol; } + +private: + VectorMx1 Input(State x) const { return -K * x; } + + /** + * @brief Computes the LQR problem gain matrix + * @return + */ + GainMatrix ComputeGain() { + MatrixNxN X = SolveDARE(); + return (B.transpose() * X * B + R).inverse() * (B.transpose() * X * A); + } + + /** + * @brief Solves a discrete-time algebraic Riccati equation + * @return + */ + MatrixNxN SolveDARE() const { + MatrixNxN X = Q, Xn = Q; + for (auto _ = 0; _ < max_iter; _++) { + Xn = A.transpose() * X * A + - A.transpose() * X * B * (R + B.transpose() * X * B).inverse() * B.transpose() * X + * A + + Q; + if ((Xn - X).template lpNorm() < eps) + break; + X = Xn; + } + return Xn; + } + + MatrixNxN A; + InputMatrix B; + MatrixNxN Q; + MatrixMxM R; + GainMatrix K; + + double max_time{100.0}; + double goal_dist{0.1}; + int max_iter{10}; + double eps{0.01}; +}; + +} // namespace rmcs_core::controller::lqr \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp new file mode 100644 index 00000000..148069ea --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp @@ -0,0 +1,98 @@ +#pragma once + +#include + +#include + +namespace rmcs_core::filter { + +template +using Matrix = Eigen::Matrix; + +template +class KalmanFilter { + using State = Eigen::Vector; + using Measure = Eigen::Vector; + using Control = Eigen::Vector; + +public: + explicit KalmanFilter( + const Matrix& A, const Matrix& H, const Matrix& Q, + const Matrix& R) + : measurement_noise_covariance_(R) + , state_transition_(A) + , process_noise_covariance_(Q) + , measurement_transition_(H) { + reset(); + + set_input_transition(); + set_error_covariance(); + set_process_noise_transition(); + set_measurement_noise_transition(); + } + + void reset() { + prior_estimate_ = State::Zero(); + posterior_estimate_ = State::Zero(); + + kalman_gain_ = Matrix::Zero(); + } + + void set_initial_state(const State& initial_state) { posterior_estimate_ = initial_state; } + + void set_input_transition(const Matrix& B = Matrix::Zero()) { + input_transition_ = B; + } + + void set_error_covariance(const Matrix& P = 1000. * Matrix::Identity()) { + posterior_error_covariance_ = P; + } + + void set_process_noise_transition(const Matrix& W = Matrix::Identity()) { + process_noise_transition_ = W; + } + + void set_measurement_noise_transition(const Matrix& V = Matrix::Identity()) { + measurement_noise_transition_ = V; + } + + State update(const Measure& measurement, const Control& control_vector = Control::Zero()) { + // Prediction + prior_estimate_ = + state_transition_ * posterior_estimate_ + input_transition_ * control_vector; + prior_error_covariance_ = + state_transition_ * posterior_error_covariance_ * state_transition_.transpose() + + process_noise_transition_ * process_noise_covariance_ + * process_noise_transition_.transpose(); + + // Correction + kalman_gain_ = prior_error_covariance_ * measurement_transition_.transpose() + * (measurement_transition_ * prior_error_covariance_ + * measurement_transition_.transpose() + + measurement_noise_transition_ * measurement_noise_covariance_ + * measurement_noise_transition_.transpose()) + .inverse(); + prior_estimate_ += kalman_gain_ * (measurement - measurement_transition_ * prior_estimate_); + + // Update + posterior_error_covariance_ = + (Matrix::Identity() - kalman_gain_ * measurement_transition_) + * prior_error_covariance_; + posterior_estimate_ = prior_estimate_; + + return posterior_estimate_; + } + +private: + State prior_estimate_, posterior_estimate_; + + Matrix measurement_noise_transition_, measurement_noise_covariance_; + Matrix state_transition_, process_noise_transition_, process_noise_covariance_, + prior_error_covariance_, posterior_error_covariance_; + + Matrix kalman_gain_; + Matrix input_transition_; + + Matrix measurement_transition_; +}; +} // namespace rmcs_core::filter \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp new file mode 100644 index 00000000..df151b66 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp @@ -0,0 +1,114 @@ +#include +#include + +#include "librmcs/device/dm_motor.hpp" + +namespace rmcs_core::hardware::device { + +class DmMotor : public librmcs::device::DmMotor { +public: + DmMotor( + rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, + const std::string& name_prefix) { + status_component.register_output(name_prefix + "/angle", angle_, 0.0); + status_component.register_output(name_prefix + "/velocity", velocity_, 0.0); + status_component.register_output(name_prefix + "/torque", torque_, 0.0); + status_component.register_output(name_prefix + "/max_torque", max_torque_, 0.0); + + command_component.register_input(name_prefix + "/control_angle", control_angle_, false); + command_component.register_input(name_prefix + "/control_torque", control_torque_, false); + command_component.register_input( + name_prefix + "/control_velocity", control_velocity_, false); + } + + DmMotor( + rmcs_executor::Component& status_component, rmcs_executor::Component& command_component, + const std::string& name_prefix, const Config& config) + : DmMotor(status_component, command_component, name_prefix) { + configure(config); + } + + void configure(const Config& config) { + librmcs::device::DmMotor::configure(config); + + *max_torque_ = max_torque(); + } + + void update_status() { + librmcs::device::DmMotor::update_status(); + *angle_ = angle(); + *velocity_ = velocity(); + *torque_ = torque(); + + motor_state_ = error_state(); + } + + double control_torque() { + if (control_torque_.ready()) [[likely]] + return *control_torque_; + else + return std::numeric_limits::quiet_NaN(); + } + + double control_angle() { + if (control_angle_.ready()) [[likely]] + return *control_angle_; + else + return std::numeric_limits::quiet_NaN(); + } + + constexpr static uint64_t generate_enable_command() { + return librmcs::device::DmMotor::generate_enable_command(); + } + + constexpr static uint64_t generate_clear_error_command() { + return librmcs::device::DmMotor::generate_clear_error_command(); + } + + uint64_t generate_disable_command() { + return librmcs::device::DmMotor::generate_disable_command(); + } + + uint64_t generate_torque_command(double control_torque) { + return librmcs::device::DmMotor::generate_torque_command(control_torque); + } + + uint64_t generate_angle_command() { + if (motor_state_ == 0) { + return generate_enable_command(); + } else if (motor_state_ == 1) { + return librmcs::device::DmMotor::generate_angle_command(control_angle()); + } else { + return generate_clear_error_command(); + } + } + + uint64_t generate_command() { + if (motor_state_ == 0) { + return generate_enable_command(); + } else if (motor_state_ == 1) { + // if (!std::isnan(control_angle())) { + // return generate_angle_command(); + // } else if (!std::isnan(control_torque())) { + // return generate_torque_command(control_torque()); + // } + + return generate_torque_command(control_torque()); + } else { + return generate_clear_error_command(); + } + } + +private: + rmcs_executor::Component::OutputInterface angle_; + rmcs_executor::Component::OutputInterface velocity_; + rmcs_executor::Component::OutputInterface torque_; + rmcs_executor::Component::OutputInterface max_torque_; + + rmcs_executor::Component::InputInterface control_angle_; + rmcs_executor::Component::InputInterface control_velocity_; + rmcs_executor::Component::InputInterface control_torque_; + + uint8_t motor_state_; +}; +} // namespace rmcs_core::hardware::device \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp new file mode 100644 index 00000000..86eafda2 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -0,0 +1,462 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include "hardware/device/bmi088.hpp" +#include "hardware/device/dji_motor.hpp" +#include "hardware/device/dm_motor.hpp" +#include "hardware/device/dr16.hpp" +#include "hardware/device/lk_motor.hpp" + +namespace rmcs_core::hardware { +class WheelLegInfantry + : public rmcs_executor::Component + , public rclcpp::Node { +public: + WheelLegInfantry() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , logger_(get_logger()) + , infantry_command_( + create_partner_component( + get_component_name() + "_command", *this)) { + + // top_board_ = std::make_unique( + // *this, *infantry_command_, + // static_cast(get_parameter("usb_pid_top_board").as_int())); + bottom_board_ = std::make_unique( + *this, *infantry_command_, + static_cast(get_parameter("usb_pid_bottom_board").as_int())); + + gimbal_calibrate_subscription_ = create_subscription( + "/gimbal/calibrate", rclcpp::QoS{0}, [this](std_msgs::msg::Int32::UniquePtr&& msg) { + gimbal_calibrate_subscription_callback(std::move(msg)); + }); + chassis_hips_calibrate_subscription_ = create_subscription( + "/chassis/hip/calibrate", rclcpp::QoS{0}, + [this](std_msgs::msg::Int32::UniquePtr&& msg) { + chassis_hip_calibrate_subscription_callback(std::move(msg)); + }); + } + + void update() override { bottom_board_->update(); } + + void command_update() { bottom_board_->command_update(); } + +private: + void gimbal_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + // RCLCPP_INFO( + // logger_, "[gimbal calibration] New yaw offset: %ld", + // bottom_board_->gimbal_yaw_motor_.calibrate_zero_point()); + // RCLCPP_INFO( + // logger_, "[gimbal calibration] New pitch offset: %ld", + // top_board_->gimbal_pitch_motor_.calibrate_zero_point()); + } + + void chassis_hip_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) { + RCLCPP_INFO( + logger_, "[chassis calibration] New left front hip offset: %ld", + bottom_board_->chassis_hip_motors[0].calibrate_zero_point()); + RCLCPP_INFO( + logger_, "[chassis calibration] New left back hip offset: %ld", + bottom_board_->chassis_hip_motors[1].calibrate_zero_point()); + RCLCPP_INFO( + logger_, "[chassis calibration] New right back hip offset: %ld", + bottom_board_->chassis_hip_motors[2].calibrate_zero_point()); + RCLCPP_INFO( + logger_, "[chassis calibration] New right front hip offset: %ld", + bottom_board_->chassis_hip_motors[3].calibrate_zero_point()); + } + class WheelLegInfantryCommand : public rmcs_executor::Component { + public: + explicit WheelLegInfantryCommand(WheelLegInfantry& infantry) + : infantry_(infantry) {} + + void update() override { infantry_.command_update(); } + + WheelLegInfantry& infantry_; + }; + + class TopBoard final : private librmcs::client::CBoard { + public: + friend class WheelLegInfantry; + explicit TopBoard( + WheelLegInfantry& infantry, WheelLegInfantryCommand& infantry_command, int usb_pid = -1) + : librmcs::client::CBoard(usb_pid) + , imu_(1000, 0.2, 0.0) + , tf_(infantry.tf_) + , gimbal_pitch_motor_( + infantry, infantry_command, "/gimbal/pitch", + device::LkMotor::Config{device::LkMotor::Type::MG5010E_I10} + .set_encoder_zero_point( + static_cast( + infantry.get_parameter("pitch_motor_zero_point").as_int()))) + , friction_wheel_motors_( + {infantry, infantry_command, "/gimbal/left_friction", + device::DjiMotor::Config{device::DjiMotor::Type::M3508}.set_reduction_ratio( + 1.0)}, + {infantry, infantry_command, "/gimbal/right_friction", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reduction_ratio(1.0) + .set_reversed()}) + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Get the mapping with the following code. + // The rotation angle must be an exact multiple of 90 degrees, otherwise use a + // matrix. + + // Eigen::AngleAxisd pitch_link_to_imu_link{ + // std::numbers::pi, Eigen::Vector3d::UnitZ()}; + // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; + // std::cout << mapping << std::endl; + + return std::make_tuple(x, y, z); + }); + + infantry.register_output("/gimbal/yaw/velocity", gimbal_yaw_velocity_imu_); + infantry.register_output("/gimbal/pitch/velocity", gimbal_pitch_velocity_imu_); + } + + ~TopBoard() final { + stop_handling_events(); + event_thread_.join(); + } + + void update() { + imu_.update_status(); + Eigen::Quaterniond gimbal_imu_pose{imu_.q0(), imu_.q1(), imu_.q2(), imu_.q3()}; + + tf_->set_transform( + gimbal_imu_pose.conjugate()); + + gimbal_pitch_motor_.update_status(); + for (auto& motor : friction_wheel_motors_) + motor.update_status(); + + tf_->set_state( + gimbal_pitch_motor_.angle()); + + *gimbal_yaw_velocity_imu_ = imu_.gz(); + *gimbal_pitch_velocity_imu_ = imu_.gy(); + } + + void command_update() { + uint16_t control_commands[4]{}; + control_commands[0] = friction_wheel_motors_[0].generate_command(); + control_commands[1] = friction_wheel_motors_[1].generate_command(); + + transmit_buffer_.add_can1_transmission( + 0x200, std::bit_cast(control_commands)); + + transmit_buffer_.add_can2_transmission( + 0x141, gimbal_pitch_motor_.generate_velocity_command( + gimbal_pitch_motor_.control_velocity())); + + transmit_buffer_.trigger_transmission(); + } + + private: + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x201) { + friction_wheel_motors_[0].store_status(can_data); + } else if (can_id == 0x202) { + friction_wheel_motors_[1].store_status(can_data); + } + } + + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x141) + gimbal_pitch_motor_.store_status(can_data); + } + + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_accelerometer_status(x, y, z); + } + + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_gyroscope_status(x, y, z); + } + + device::Bmi088 imu_; + + OutputInterface& tf_; + + OutputInterface gimbal_yaw_velocity_imu_; + OutputInterface gimbal_pitch_velocity_imu_; + + device::LkMotor gimbal_pitch_motor_; + + device::DjiMotor friction_wheel_motors_[2]; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + }; + class BottomBoard final : private librmcs::client::CBoard { + public: + friend class WheelLegInfantry; + explicit BottomBoard( + WheelLegInfantry& infantry, WheelLegInfantryCommand& infantry_command, int usb_pid = -1) + : librmcs::client::CBoard(usb_pid) + , dr16_(infantry) + , imu_(1000, 0.2, 0.0) + , tf_(infantry.tf_) + , chassis_wheel_motors_( + {infantry, infantry_command, "/chassis/left_wheel", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reduction_ratio(268.0 / 17.0) + .enable_multi_turn_angle()}, + {infantry, infantry_command, "/chassis/right_wheel", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reduction_ratio(268.0 / 17.0) + .enable_multi_turn_angle() + .set_reversed()}) + , chassis_hip_motors( + {infantry, infantry_command, "/chassis/left_front_hip", + device::DmMotor::Config{device::DmMotor::Type::DM8009} + .set_encoder_zero_point( + static_cast( + infantry.get_parameter("left_front_hip_motor_zero_point").as_int())) + .set_reversed()}, + {infantry, infantry_command, "/chassis/left_back_hip", + device::DmMotor::Config{device::DmMotor::Type::DM8009} + .set_encoder_zero_point( + static_cast( + infantry.get_parameter("left_back_hip_motor_zero_point").as_int())) + .set_reversed()}, + {infantry, infantry_command, "/chassis/right_back_hip", + device::DmMotor::Config{device::DmMotor::Type::DM8009}.set_encoder_zero_point( + static_cast( + infantry.get_parameter("right_back_hip_motor_zero_point").as_int()))}, + {infantry, infantry_command, "/chassis/right_front_hip", + device::DmMotor::Config{device::DmMotor::Type::DM8009}.set_encoder_zero_point( + static_cast( + infantry.get_parameter("right_front_hip_motor_zero_point").as_int()))}) + , gimbal_yaw_motor_( + infantry, infantry_command, "/gimbal/yaw", + device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10} + .set_encoder_zero_point( + static_cast( + infantry.get_parameter("yaw_motor_zero_point").as_int()))) + , bullet_feeder_motor_( + infantry, infantry_command, "/gimbal/bullet_feeder", + device::DjiMotor::Config{device::DjiMotor::Type::M2006} + .enable_multi_turn_angle() + .set_reversed() + .set_reduction_ratio(19 * 2)) + , transmit_buffer_(*this, 32) + , event_thread_([this]() { handle_events(); }) { + + imu_.set_coordinate_mapping([](double x, double y, double z) { + // Get the mapping with the following code. + // The rotation angle must be an exact multiple of 90 degrees, otherwise use a + // matrix. + + // Eigen::AngleAxisd pitch_link_to_imu_link{ + // std::numbers::pi, Eigen::Vector3d::UnitZ()}; + // Eigen::Vector3d mapping = pitch_link_to_imu_link * Eigen::Vector3d{1, 2, 3}; + // std::cout << mapping << std::endl; + + return std::make_tuple(x, y, z); + }); + + infantry.register_output("/referee/serial", referee_serial_); + + referee_serial_->read = [this](std::byte* buffer, size_t size) { + return referee_ring_buffer_receive_.pop_front_multi( + [&buffer](std::byte byte) { *buffer++ = byte; }, size); + }; + referee_serial_->write = [this](const std::byte* buffer, size_t size) { + transmit_buffer_.add_uart1_transmission(buffer, size); + return size; + }; + + infantry.register_output( + "/chassis/x_axis/acceleration", chassis_x_axis_acceleration_imu_); + infantry.register_output( + "/chassis/z_axis/acceleration", chassis_z_axis_acceleration_imu_); + + infantry.register_output("/chassis/yaw/velocity", chassis_yaw_velocity_imu_); + infantry.register_output("/chassis/pitch/velocity", chassis_pitch_velocity_imu_); + infantry.register_output("/chassis/roll/velocity", chassis_roll_velocity_imu_); + + infantry.register_output("/chassis/yaw/angle", chassis_yaw_angle_imu_); + infantry.register_output("/chassis/pitch/angle", chassis_pitch_angle_imu_); + infantry.register_output("/chassis/roll/angle", chassis_roll_angle_imu_); + } + + ~BottomBoard() { + stop_handling_events(); + event_thread_.join(); + } + + void update() { + dr16_.update_status(); + + update_imu(); + + for (auto& motor : chassis_wheel_motors_) + motor.update_status(); + + for (auto& motor : chassis_hip_motors) + motor.update_status(); + + gimbal_yaw_motor_.update_status(); + tf_->set_state( + gimbal_yaw_motor_.angle()); + + bullet_feeder_motor_.update_status(); + } + + void command_update() { + uint16_t control_commands[4]{}; + + control_commands[0] = chassis_wheel_motors_[0].generate_command(); + transmit_buffer_.add_can1_transmission( + 0x200, std::bit_cast(control_commands)); + + transmit_buffer_.add_can1_transmission(0x01, chassis_hip_motors[0].generate_command()); + transmit_buffer_.add_can1_transmission(0x02, chassis_hip_motors[1].generate_command()); + + control_commands[0] = 0; + control_commands[1] = chassis_wheel_motors_[1].generate_command(); + transmit_buffer_.add_can2_transmission( + 0x200, std::bit_cast(control_commands)); + + transmit_buffer_.add_can2_transmission(0x03, chassis_hip_motors[2].generate_command()); + transmit_buffer_.add_can2_transmission(0x04, chassis_hip_motors[3].generate_command()); + + transmit_buffer_.trigger_transmission(); + } + + void update_imu() { + imu_.update_status(); + + *chassis_yaw_angle_imu_ = std::atan2( + 2.0 * (imu_.q0() * imu_.q3() + imu_.q1() * imu_.q2()), + 2.0 * (imu_.q0() * imu_.q0() + imu_.q1() * imu_.q1()) - 1.0); + *chassis_pitch_angle_imu_ = + std::asin(-2.0 * (imu_.q1() * imu_.q3() - imu_.q0() * imu_.q2())); + *chassis_roll_angle_imu_ = std::atan2( + 2.0 * (imu_.q0() * imu_.q1() + imu_.q2() * imu_.q3()), + 2.0 * (imu_.q0() * imu_.q0() + imu_.q3() * imu_.q3()) - 1.0); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_velocity_imu_ = imu_.gy(); + *chassis_roll_velocity_imu_ = imu_.gx(); + + *chassis_x_axis_acceleration_imu_ = imu_.ax(); + *chassis_z_axis_acceleration_imu_ = imu_.az(); + } + + private: + void can1_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(can_data); + } else if (can_id == 0x01) { + chassis_hip_motors[0].store_status(can_data); + } else if (can_id == 0x02) { + chassis_hip_motors[1].store_status(can_data); + } + } + + void can2_receive_callback( + uint32_t can_id, uint64_t can_data, bool is_extended_can_id, + bool is_remote_transmission, uint8_t can_data_length) override { + if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] + return; + + if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(can_data); + } else if (can_id == 0x03) { + chassis_hip_motors[2].store_status(can_data); + } else if (can_id == 0x04) { + chassis_hip_motors[3].store_status(can_data); + } + } + + void uart1_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + referee_ring_buffer_receive_.emplace_back_multi( + [&uart_data](std::byte* storage) { *storage = *uart_data++; }, uart_data_length); + } + + void dbus_receive_callback(const std::byte* uart_data, uint8_t uart_data_length) override { + dr16_.store_status(uart_data, uart_data_length); + } + + void accelerometer_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_accelerometer_status(x, y, z); + } + + void gyroscope_receive_callback(int16_t x, int16_t y, int16_t z) override { + imu_.store_gyroscope_status(x, y, z); + } + + device::Dr16 dr16_; + + device::Bmi088 imu_; + + OutputInterface chassis_x_axis_acceleration_imu_; + OutputInterface chassis_z_axis_acceleration_imu_; + + OutputInterface chassis_yaw_velocity_imu_; + OutputInterface chassis_pitch_velocity_imu_; + OutputInterface chassis_roll_velocity_imu_; + + OutputInterface chassis_yaw_angle_imu_; + OutputInterface chassis_pitch_angle_imu_; + OutputInterface chassis_roll_angle_imu_; + + OutputInterface& tf_; + + device::DjiMotor chassis_wheel_motors_[2]; + + device::DmMotor chassis_hip_motors[4]; + + device::LkMotor gimbal_yaw_motor_; + device::DjiMotor bullet_feeder_motor_; + + librmcs::utility::RingBuffer referee_ring_buffer_receive_{256}; + OutputInterface referee_serial_; + + librmcs::client::CBoard::TransmitBuffer transmit_buffer_; + std::thread event_thread_; + }; + + rclcpp::Logger logger_; + + OutputInterface tf_; + + std::shared_ptr infantry_command_; + + // std::unique_ptr top_board_; + std::unique_ptr bottom_board_; + + rclcpp::Subscription::SharedPtr gimbal_calibrate_subscription_; + rclcpp::Subscription::SharedPtr chassis_hips_calibrate_subscription_; +}; +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::WheelLegInfantry, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_executor/src/main.cpp b/rmcs_ws/src/rmcs_executor/src/main.cpp index 0de17487..99311f24 100644 --- a/rmcs_ws/src/rmcs_executor/src/main.cpp +++ b/rmcs_ws/src/rmcs_executor/src/main.cpp @@ -44,7 +44,7 @@ int main(int argc, char** argv) { std::vector component_descriptions; if (!executor->get_parameter("components", component_descriptions)) - throw std::runtime_error("para"); + throw std::runtime_error("Missing parameter 'components' or config is not found"); std::regex regex(R"(\s*(\S+)\s*->\s*(\S+)\s*)"); for (const auto& component_description : component_descriptions) { diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_mode.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_mode.hpp new file mode 100644 index 00000000..c5b9cfdb --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_mode.hpp @@ -0,0 +1,12 @@ +#pragma once + +#include + +namespace rmcs_msgs { +enum class WheelLegMode : uint8_t { + BALANCELESS = 0, + SPIN = 1, + FOLLOW = 2, + LAUNCH_RAMP = 3, +}; +} diff --git a/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_state.hpp b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_state.hpp new file mode 100644 index 00000000..9bf48f89 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_state.hpp @@ -0,0 +1,24 @@ +#pragma once + +#include + +namespace rmcs_msgs { + +struct WheelLegState { + double distance; + double velocity; + double yaw_angle; + double yaw_velocity; + double left_tilt_angle; + double left_tilt_velocity; + double right_tilt_angle; + double right_tilt_velocity; + double body_pitch_angle; + double body_pitch_velocity; + + Eigen::Map> vector() const { + return Eigen::Map>(&distance); + } +}; + +} // namespace rmcs_msgs \ No newline at end of file