From 87254f76557dcbeb529e8a19f1147640d9ec6616 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 1 Aug 2025 13:20:50 +0800 Subject: [PATCH 01/35] Optimize exception message of components analysis --- rmcs_ws/src/rmcs_executor/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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) { From c75f55d4c23ba8acef18ef5a2b60383d9ae4cf6e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Wed, 13 Aug 2025 15:50:30 +0800 Subject: [PATCH 02/35] Update docker compose yaml and add .env --- .env | 3 +++ docker-compose.yml | 11 +++++++---- 2 files changed, 10 insertions(+), 4 deletions(-) create mode 100644 .env 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/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} From de060989833180c64e059b765061ff6e9fe387e9 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 12 Sep 2025 18:27:08 +0800 Subject: [PATCH 03/35] Modify submodule url and update scripts --- .gitmodules | 2 +- .script/build-rmcs | 2 +- .script/clean-rmcs | 3 ++- .script/sync-remote | 2 +- .script/template/env_setup.bash | 5 +++-- .script/template/env_setup.zsh | 5 +++-- Dockerfile | 2 +- 7 files changed, 12 insertions(+), 9 deletions(-) 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..800a68fc 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 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/sync-remote b/.script/sync-remote index 60a2b816..f9c915fc 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 = "${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..2906efbf 100644 --- a/.script/template/env_setup.bash +++ b/.script/template/env_setup.bash @@ -1,13 +1,14 @@ #!/bin/bash export ROS_LOCALHOST_ONLY=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..9de2f1e1 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -1,13 +1,14 @@ #!/bin/bash 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)" diff --git a/Dockerfile b/Dockerfile index 37bf1097..c4a67af1 100644 --- a/Dockerfile +++ b/Dockerfile @@ -123,7 +123,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 From 1e700303c885cc0cf30c7472178c00bb11d40a9c Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Oct 2025 20:52:56 +0800 Subject: [PATCH 04/35] Add workflow for updating docker images --- .github/workflows/update-image.yml | 42 ++++++++++++++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100644 .github/workflows/update-image.yml diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml new file mode 100644 index 00000000..6d4e0dfc --- /dev/null +++ b/.github/workflows/update-image.yml @@ -0,0 +1,42 @@ +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: 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 From a7d319dfef02982f2a37c46a997f2dbb75bd56b8 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Oct 2025 21:10:02 +0800 Subject: [PATCH 05/35] Add id_rsa setup for image building --- .github/workflows/update-image.yml | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index 6d4e0dfc..a9e77b45 100644 --- a/.github/workflows/update-image.yml +++ b/.github/workflows/update-image.yml @@ -25,6 +25,14 @@ jobs: username: ${{ secrets.DOCKERHUB_USERNAME }} password: ${{ secrets.DOCKERHUB_TOKEN }} + - name: Set up SSH keys + run: | + mkdir -p ~/.ssh + 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: From 26dfdf2e439beb66dfa0cef93278f2d85aa0d342 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 24 Oct 2025 21:22:45 +0800 Subject: [PATCH 06/35] Update workflows: change ssh secrets path --- .github/workflows/update-image.yml | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/.github/workflows/update-image.yml b/.github/workflows/update-image.yml index a9e77b45..71c5538f 100644 --- a/.github/workflows/update-image.yml +++ b/.github/workflows/update-image.yml @@ -27,11 +27,10 @@ jobs: - name: Set up SSH keys run: | - mkdir -p ~/.ssh - 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 + 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 From f0393d96ebf12ca8e7333807792188196da035d6 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Fri, 31 Oct 2025 04:05:47 +0800 Subject: [PATCH 07/35] Update develop scripts and add env param --- .script/build-rmcs | 2 +- .script/template/env_setup.bash | 1 + .script/template/env_setup.zsh | 1 + 3 files changed, 3 insertions(+), 1 deletion(-) diff --git a/.script/build-rmcs b/.script/build-rmcs index 800a68fc..ef5d85ff 100755 --- a/.script/build-rmcs +++ b/.script/build-rmcs @@ -4,4 +4,4 @@ source /opt/ros/jazzy/setup.bash cd ${RMCS_PATH}/rmcs_ws -colcon build --symlink-install --merge-install +colcon build --symlink-install --merge-install $@ diff --git a/.script/template/env_setup.bash b/.script/template/env_setup.bash index 2906efbf..f8ab1dd2 100644 --- a/.script/template/env_setup.bash +++ b/.script/template/env_setup.bash @@ -1,6 +1,7 @@ #!/bin/bash export ROS_LOCALHOST_ONLY=1 +export RCUTILS_COLORIZED_OUTPUT=1 export RMCS_PATH="/workspaces/RMCS" source /opt/ros/jazzy/setup.bash diff --git a/.script/template/env_setup.zsh b/.script/template/env_setup.zsh index 9de2f1e1..335250c9 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -1,5 +1,6 @@ #!/bin/bash +export RCUTILS_COLORIZED_OUTPUT=1 export ROS_LOCALHOST_ONLY=1 export RMCS_PATH="/workspaces/RMCS" From 307530712d3a0a77ba6da035c6a4c4b908683cd6 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Nov 2025 13:54:34 +0800 Subject: [PATCH 08/35] Add autoaim visualization script --- .script/play-autoaim | 38 ++++++++++++++++++++++++++++++++++++++ 1 file changed, 38 insertions(+) create mode 100755 .script/play-autoaim diff --git a/.script/play-autoaim b/.script/play-autoaim new file mode 100755 index 00000000..e90521dc --- /dev/null +++ b/.script/play-autoaim @@ -0,0 +1,38 @@ +#!/bin/sh + +if [ "$#" -ne 2 ]; then + echo "用法: $0 <用户名> <主机>" + echo "示例: $0 user localhost" + exit 1 +fi + +REMOTE_PLAYER="vlc" + +REMOTE_USER="$1" +REMOTE_HOST="$2" + +REMOTE_PATH="/tmp/auto_aim.sdp" +LOCAL_PATH="/tmp/auto_aim.sdp" + +scp remote:tmp/auto_aim.sdp /tmp/auto_aim.sdp +if [ $? -ne 0 ]; then + echo "⚠️ 从 remote 拷贝失败。是否继续？(y/n)" + read -r answer + case "$answer" in + [Yy]*) echo "继续执行后续操作…" ;; + *) + echo "已取消。" + exit 1 + ;; + esac +fi + +scp "${REMOTE_PATH}" "${REMOTE_USER}@${REMOTE_HOST}:${LOCAL_PATH}" +if [ $? -ne 0 ]; then + echo "❌ scp 拷贝失败" + exit 1 +fi + +echo "✅ 文件已拷贝到宿主机：${LOCAL_PATH}" + +ssh -f "${REMOTE_USER}@${REMOTE_HOST}" "DISPLAY=:0 ${REMOTE_PLAYER} '${LOCAL_PATH}'" From d0fabc6d9d4c8307cb481074ee1b43162f387a66 Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Nov 2025 15:29:53 +0800 Subject: [PATCH 09/35] Update develop script --- .script/play-autoaim | 8 ++++---- .script/sync-remote | 2 +- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/.script/play-autoaim b/.script/play-autoaim index e90521dc..4a506a2b 100755 --- a/.script/play-autoaim +++ b/.script/play-autoaim @@ -1,15 +1,15 @@ #!/bin/sh -if [ "$#" -ne 2 ]; then - echo "用法: $0 <用户名> <主机>" - echo "示例: $0 user localhost" +if [ "$#" -ne 1 ]; then + echo "用法: $0 <用户名>" + echo "示例: $0 user" exit 1 fi REMOTE_PLAYER="vlc" REMOTE_USER="$1" -REMOTE_HOST="$2" +REMOTE_HOST="localhost" REMOTE_PATH="/tmp/auto_aim.sdp" LOCAL_PATH="/tmp/auto_aim.sdp" diff --git a/.script/sync-remote b/.script/sync-remote index f9c915fc..7d62782b 100755 --- a/.script/sync-remote +++ b/.script/sync-remote @@ -8,7 +8,7 @@ import subprocess from colorama import Fore, Style -SRC_DIR = "${RMCS_PATH}/rmcs_ws/install" +SRC_DIR = os.getenv("RMCS_PATH") + "/rmcs_ws/install" DST_DIR = "ssh://remote//rmcs_install" SOCKET_PATH = "/tmp/sync-remote" From a6ca16a478f49db5019101b30829025b1541b62e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Nov 2025 15:46:41 +0800 Subject: [PATCH 10/35] Update develop script --- .script/play-autoaim | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/.script/play-autoaim b/.script/play-autoaim index 4a506a2b..67e86159 100755 --- a/.script/play-autoaim +++ b/.script/play-autoaim @@ -14,7 +14,7 @@ REMOTE_HOST="localhost" REMOTE_PATH="/tmp/auto_aim.sdp" LOCAL_PATH="/tmp/auto_aim.sdp" -scp remote:tmp/auto_aim.sdp /tmp/auto_aim.sdp +scp remote:${REMOTE_PATH} ${REMOTE_PATH} if [ $? -ne 0 ]; then echo "⚠️ 从 remote 拷贝失败。是否继续？(y/n)" read -r answer From b4c8ed63e20726db935448c3efc95fd3c0651a0a Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Sat, 1 Nov 2025 15:50:37 +0800 Subject: [PATCH 11/35] Add gstreamer dependency to dockerfile --- Dockerfile | 1 + 1 file changed, 1 insertion(+) diff --git a/Dockerfile b/Dockerfile index c4a67af1..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 \ From a760bd8a66417283d557742ea2cb259f9d511e2e Mon Sep 17 00:00:00 2001 From: creeper5820 Date: Tue, 4 Nov 2025 23:07:44 +0800 Subject: [PATCH 12/35] Add scripts autocomplete support for zsh --- .script/complete/_play-autoaim | 6 +++ .script/complete/_set-remote | 22 ++++++++++ .script/play-autoaim | 77 ++++++++++++++++++++++++---------- .script/template/env_setup.zsh | 4 ++ 4 files changed, 86 insertions(+), 23 deletions(-) create mode 100644 .script/complete/_play-autoaim create mode 100644 .script/complete/_set-remote 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 index 67e86159..8f367049 100755 --- a/.script/play-autoaim +++ b/.script/play-autoaim @@ -1,38 +1,69 @@ #!/bin/sh -if [ "$#" -ne 1 ]; then - echo "用法: $0 <用户名>" - echo "示例: $0 user" - exit 1 -fi +set -e -REMOTE_PLAYER="vlc" +MONITOR_USER="" +MONITOR_PLAYER="vlc" +MONITOR_HOST="localhost" -REMOTE_USER="$1" -REMOTE_HOST="localhost" +SDP_PATH="/tmp/auto_aim.sdp" -REMOTE_PATH="/tmp/auto_aim.sdp" -LOCAL_PATH="/tmp/auto_aim.sdp" +USE_REMOTE=false +SKIP_COPY=false -scp remote:${REMOTE_PATH} ${REMOTE_PATH} -if [ $? -ne 0 ]; then - echo "⚠️ 从 remote 拷贝失败。是否继续？(y/n)" - read -r answer - case "$answer" in - [Yy]*) echo "继续执行后续操作…" ;; +# 参数解析 +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 "已取消。" + echo "未知参数: $1" + echo "用法: $0 --user <用户名> [--remote] [--no-copy]" exit 1 ;; esac -fi + shift +done -scp "${REMOTE_PATH}" "${REMOTE_USER}@${REMOTE_HOST}:${LOCAL_PATH}" -if [ $? -ne 0 ]; then - echo "❌ scp 拷贝失败" +if [ -z "$MONITOR_USER" ]; then + echo "❌ 缺少 --user 参数" + echo "用法: play-autoaim --user <用户名> [--remote] [--no-copy]" exit 1 fi -echo "✅ 文件已拷贝到宿主机：${LOCAL_PATH}" +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 "${REMOTE_USER}@${REMOTE_HOST}" "DISPLAY=:0 ${REMOTE_PLAYER} '${LOCAL_PATH}'" +ssh -f "${MONITOR_USER}@${MONITOR_HOST}" "DISPLAY=:0 ${MONITOR_PLAYER} '${SDP_PATH}' --network-caching=50" diff --git a/.script/template/env_setup.zsh b/.script/template/env_setup.zsh index 335250c9..2f7a0ee6 100644 --- a/.script/template/env_setup.zsh +++ b/.script/template/env_setup.zsh @@ -16,3 +16,7 @@ eval "$(register-python-argcomplete ros2)" eval "$(register-python-argcomplete colcon)" export RMCS_ROBOT_TYPE="" + +fpath=(${RMCS_PATH}/.script/complete $fpath) +autoload -Uz compinit +compinit From 757b66bff21316cda7acd4e9888337ef56e3b8a2 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sat, 8 Nov 2025 15:43:38 +0800 Subject: [PATCH 13/35] First commit --- rmcs_ws/src/rmcs_core/plugins.xml | 3 +++ .../controller/chassis/wheel_leg/README.md | 0 .../controller/chassis/wheel_leg/observer.hpp | 9 +++++++ .../chassis/wheel_leg_controller.cpp | 25 +++++++++++++++++++ .../src/hardware/wheelleg_infantry.cpp | 16 ++++++++++++ 5 files changed, 53 insertions(+) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/README.md create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index ac7d1368..0ab75304 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -29,6 +29,9 @@ Steering wheel controller. + + Test plugin. + Test plugin. 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..e69de29b diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.hpp new file mode 100644 index 00000000..8ec0bdce --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.hpp @@ -0,0 +1,9 @@ + +namespace rmcs_core::controller::chassis { +class Observer { +public: + Observer() {} + +private: +}; +} // namespace rmcs_core::controller::chassis \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp new file mode 100644 index 00000000..d2a7f7ac --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp @@ -0,0 +1,25 @@ + +#include + +#include "rmcs_executor/component.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)) {} + + void update() { update_status(); } + +private: + void update_status() {} +}; +} // 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/hardware/wheelleg_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp new file mode 100644 index 00000000..1be1a38e --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp @@ -0,0 +1,16 @@ +#include + +#include "librmcs/client/cboard.hpp" +#include "rmcs_executor/component.hpp" + +namespace rmcs_core::hardware { +class WheelLegInfantry + : public rmcs_executor::Component + , public rclcpp::Node + , private librmcs::client::CBoard { +public: + WheelLegInfantry() + : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} + , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} {} +}; +} // namespace rmcs_core::hardware \ No newline at end of file From dd496307414d904c68cf45acf4524df92ddf87d7 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sun, 23 Nov 2025 09:02:11 +0800 Subject: [PATCH 14/35] Update wheelleg infantry hardware --- .../config/wheelleg_infantry.yaml | 141 +++++++ .../src/hardware/wheelleg_infantry.cpp | 365 +++++++++++++++++- .../include/rmcs_msgs/wheel_leg_state.hpp | 18 + 3 files changed, 519 insertions(+), 5 deletions(-) create mode 100644 rmcs_ws/src/rmcs_bringup/config/wheelleg_infantry.yaml create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_state.hpp 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..243e722f --- /dev/null +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg_infantry.yaml @@ -0,0 +1,141 @@ +rmcs_executor: + ros__parameters: + update_rate: 1000.0 + components: + - rmcs_core::hardware::WheelLegInfantry -> infantry_hardware + + - 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::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + # - rmcs_core::controller::chassis::WheellegController -> wheel_leg_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: 0x488d + yaw_motor_zero_point: 32285 + pitch_motor_zero_point: 6321 + +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/src/hardware/wheelleg_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp index 1be1a38e..73dc08f8 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp @@ -1,16 +1,371 @@ +#include #include +#include +#include +#include +#include -#include "librmcs/client/cboard.hpp" -#include "rmcs_executor/component.hpp" +#include "hardware/device/bmi088.hpp" +#include "hardware/device/dji_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 - , private librmcs::client::CBoard { + , public rclcpp::Node { public: WheelLegInfantry() : Node{get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)} - , librmcs::client::CBoard{static_cast(get_parameter("usb_pid").as_int())} {} + , 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 {} + + void 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) {} + 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[2] = friction_wheel_motors_[0].generate_command(); + control_commands[3] = 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 == 0x203) { + friction_wheel_motors_[0].store_status(can_data); + } else if (can_id == 0x204) { + 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(13.0) + .enable_multi_turn_angle() + .set_reversed()}, + {infantry, infantry_command, "/chassis/right_wheel", + device::DjiMotor::Config{device::DjiMotor::Type::M3508} + .set_reduction_ratio(13.0) + .enable_multi_turn_angle() + .set_reversed()}) + , 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(); }) { + + 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/acceleration", chassis_x_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(); + + 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]; + 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::atan2( + 2.0 * (imu_.q0() * imu_.q1() + imu_.q2() * imu_.q3()), + 2.0 * (imu_.q0() * imu_.q0() + imu_.q3() * imu_.q3()) - 1.0); + *chassis_roll_angle_imu_ = + std::asin(-2.0 * (imu_.q1() * imu_.q3() - imu_.q0() * imu_.q2())); + + *chassis_yaw_velocity_imu_ = imu_.gz(); + *chassis_pitch_velocity_imu_ = imu_.gy(); + *chassis_roll_velocity_imu_ = imu_.gx(); + + *chassis_x_acceleration_imu_ = imu_.ax(); + } + + 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) {} + } + + 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) {} + } + + 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_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 \ No newline at end of file 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..7be33582 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_state.hpp @@ -0,0 +1,18 @@ +#pragma once + +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; +}; + +} // namespace rmcs_msgs \ No newline at end of file From 9093860f28a2ef44f79d3d632b269bcb5b3de58e Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Mon, 24 Nov 2025 11:42:12 +0800 Subject: [PATCH 15/35] Delete `observer.hpp`, integrate the content to the `wheel_leg_controller.cpp`. --- .../controller/chassis/wheel_leg/observer.cpp | 224 ++++++++++++ .../controller/chassis/wheel_leg/observer.hpp | 9 - .../chassis/wheel_leg/vmc_solver.hpp | 102 ++++++ .../chassis/wheel_leg_controller.cpp | 323 +++++++++++++++++- .../rmcs_core/src/filter/kalman_filter.hpp | 82 +++++ .../include/rmcs_msgs/wheel_leg_state.hpp | 6 + 6 files changed, 733 insertions(+), 13 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/vmc_solver.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp new file mode 100644 index 00000000..cc1f2044 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp @@ -0,0 +1,224 @@ +#include +#include +#include +#include + +#include "filter/kalman_filter.hpp" +#include "vmc_solver.hpp" + +namespace rmcs_core::controller::chassis { +class Observer + : public rmcs_executor::Component + , public rclcpp::Node { +public: + Observer() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , body_mess_(15.0) + , leg_mess_(0.86) + , wheel_radius_(0.06) + , wheel_distance_(0.2135) + , centroid_position_coefficient_() + , 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_, W_) { + + 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_wheel/velocity", left_wheel_velocity_); + register_input("/chassis/right_wheel/velocity", right_wheel_velocity_); + + register_input("/chassis/x/acceleration", chassis_x_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_leg_length", left_leg_length_); + register_output("chassis/right_leg_length", right_leg_length_); + register_output("/chassis/measure_state", measure_state_); + } + + void update() override { + // state vector: s ds phi dphi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b + auto wheel_velocities = calculate_wheel_velocities(); + auto hip_angles = calculate_hips_angles(); + + auto leg_posture = calculate_leg_posture(hip_angles); + auto distance = calculate_translational_distance(leg_posture, wheel_velocities); + + auto support_force = calculate_support_force(leg_posture); + + *measure_state_ = calculate_measure_state(distance, leg_posture); + } + +private: + struct LegPosture { + Eigen::Vector2d leg_length; + Eigen::Vector2d tilt_angle; + + Eigen::Vector2d dot_leg_length; + Eigen::Vector2d dot_tilt_angle; + + Eigen::Vector2d second_order_diff_leg_length; + Eigen::Vector2d second_order_diff_tilt_angle; + }; + + 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 &lf = hip_angles.x(), &lb = hip_angles.y(), &rb = hip_angles.z(), + &rf = hip_angles.w(); + + 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}; + + *left_leg_length_ = left_leg_length; + *right_leg_length_ = right_leg_length; + + result.dot_leg_length = (result.leg_length - last_leg_length_) / dt_; + last_leg_length_ = result.leg_length; + result.dot_tilt_angle = (result.tilt_angle - last_tilt_angle_) / dt_; + last_tilt_angle_ = result.tilt_angle; + + result.second_order_diff_leg_length = (result.dot_leg_length - last_dot_leg_length_) / dt_; + last_dot_leg_length_ = result.dot_leg_length; + result.second_order_diff_tilt_angle = (result.dot_tilt_angle - last_dot_tilt_angle_) / dt_; + last_dot_tilt_angle_ = result.dot_tilt_angle; + + return result; + } + + Eigen::Vector2d + calculate_translational_distance(LegPosture leg_posture, Eigen::Vector2d wheel_velocities) { + std::tuple result{}; + auto& [distance, velocity] = result; + + auto wheel_velocity = (wheel_velocities.x() + wheel_velocities.y()) / 2.0; + leg_posture.tilt_angle.array() += *chassis_pitch_angle_imu_; + + auto left_leg_velocity = + leg_posture.leg_length.x() * std::cos(leg_posture.tilt_angle.x()) + * leg_posture.dot_tilt_angle.x() + + leg_posture.dot_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.dot_tilt_angle.y() + + leg_posture.dot_leg_length.y() + + std::sin(leg_posture.tilt_angle.y()); + + auto calculate_velocity = wheel_velocity + (left_leg_velocity + right_leg_velocity) / 2.0; + + auto estimate_velocity = velocity_kalman_filter_.update( + Eigen::Vector2d{calculate_velocity, *chassis_x_acceleration_imu_}); + + velocity = estimate_velocity.x(); + distance += velocity * dt_; + + return Eigen::Vector2d{}; + } + + Eigen::Vector2d calculate_support_force(LegPosture leg_posture) {} + + 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.dot_tilt_angle.x(); + + measure_state.right_tilt_angle = leg_posture.tilt_angle.y(); + measure_state.right_tilt_velocity = leg_posture.dot_tilt_angle.y(); + + measure_state.body_pitch_angle = *chassis_pitch_angle_imu_; + measure_state.body_pitch_velocity = *chassis_pitch_velocity_imu_; + + return measure_state; + } + + static constexpr double dt_ = 1e-3; + static constexpr double g_ = 9.80665; + + 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::Matrix2d() << 1.0, 0.0, 0.0, 1.0).finished(); + const Eigen::Matrix2d W_ = (Eigen::Matrix2d() << 0.5 * dt_ * dt_, 0.0, 0.0, dt_).finished(); + const Eigen::Matrix2d Q_ = + (Eigen::Matrix2d() << sigma_q_ * sigma_q_, 0.0, 0.0, sigma_q_* sigma_q_).finished(); + const Eigen::Matrix2d R_ = + (Eigen::Matrix2d() << sigma_v_ * sigma_v_, 0.0, 0.0, sigma_a_* sigma_a_).finished(); + + const double body_mess_; + const double leg_mess_; + const double wheel_radius_; + const double wheel_distance_; + const double centroid_position_coefficient_; + + InputInterface left_front_hip_angle_; + InputInterface left_back_hip_angle_; + InputInterface right_front_hip_angle_; + InputInterface right_back_hip_angle_; + + InputInterface left_wheel_velocity_; + InputInterface right_wheel_velocity_; + + InputInterface chassis_x_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_; + + OutputInterface left_leg_length_; + OutputInterface right_leg_length_; + + OutputInterface measure_state_; + + Eigen::Vector2d last_leg_length_, last_tilt_angle_, last_dot_leg_length_, last_dot_tilt_angle_; + double last_left_leg_length_, last_right_leg_length_; + + VmcSolver left_leg_vmc_solver_, right_leg_vmc_solver_; + filter::KalmanFilter<2, 2> velocity_kalman_filter_; +}; +} // namespace rmcs_core::controller::chassis +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::Observer, rmcs_executor::Component) \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.hpp deleted file mode 100644 index 8ec0bdce..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.hpp +++ /dev/null @@ -1,9 +0,0 @@ - -namespace rmcs_core::controller::chassis { -class Observer { -public: - Observer() {} - -private: -}; -} // namespace rmcs_core::controller::chassis \ 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..5cc0fb91 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/vmc_solver.hpp @@ -0,0 +1,102 @@ +#include +#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) {} + + auto update(double phi1, double phi4) -> std::tuple { + if (std::isnan(phi1) || std::isnan(phi4)) { + reset(); + return {0.0, 0.0}; + } + + calculate_five_link_solution(phi1, phi4); + + return get_leg_posture(); + } + + Eigen::Vector2d get_joint_torque(double F, double Tp) { + return joint_torque_matrix_ * Eigen::Vector2d{F, Tp}; + } + + Eigen::Vector2d get_virtual_torque(double T1, double T2) { + return joint_torque_matrix_.inverse() * Eigen::Vector2d{T1, T2}; + } + +private: + void reset() { + tilt_angle_ = nan_; + leg_length_ = nan_; + } + + 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((xb - xd) * (xb - xd) + (yd - yb) * (yd - yb)); + + auto a = 2 * l2_ * (xd - xb), b = 2 * l2_ * (yb - yd), + 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 - phi4), + j21 = l1_ * std::cos(phi0 - phi2) * 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; + } + + auto get_leg_posture() const -> std::tuple { + return {leg_length_, tilt_angle_}; + } + + 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_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp index d2a7f7ac..75952f92 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp @@ -1,7 +1,11 @@ - #include +#include +#include +#include "controller/pid/pid_calculator.hpp" +#include "filter/kalman_filter.hpp" #include "rmcs_executor/component.hpp" +#include "wheel_leg/vmc_solver.hpp" namespace rmcs_core::controller::chassis { class WheelLegController @@ -11,12 +15,323 @@ class WheelLegController WheelLegController() : Node( get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) {} + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) + , body_mess_(15.0) + , leg_mess_(0.86) + , wheel_radius_(0.06) + , wheel_distance_(0.2135) + , centroid_position_coefficient_() + , 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_, W_) + , roll_angle_pid_calculator_(0.0, 0.0, 0.0) + , leg_length_pid_calculator_(0.0, 0.0, 0.0) { + + register_input("/chassis/control_velocity", chassis_control_velocity_); + + 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_wheel/velocity", left_wheel_velocity_); + register_input("/chassis/right_wheel/velocity", right_wheel_velocity_); + + register_input("/chassis/x/acceleration", chassis_x_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_wheel/control_torque", left_wheel_control_torque_); + register_output("/chassis/right_wheel/control_torque", right_wheel_control_torque_); + } + + void update() override { + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } - void update() { update_status(); } + // state vector: s ds phi dphi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b + auto wheel_velocities = calculate_wheel_velocities(); + auto hip_angles = calculate_hips_angles(); + + auto leg_posture = calculate_leg_posture(hip_angles); + auto distance = calculate_translational_distance(leg_posture, wheel_velocities); + + auto support_force = calculate_support_force(leg_posture); + + auto measure_state = calculate_measure_state(distance, leg_posture); + + auto chassis_control_velocity = calculate_chassis_control_velocity(); + auto desire_state = calculate_desire_state(chassis_control_velocity); + + auto leg_forces = calculate_leg_force(leg_posture, measure_state); + auto control_torques = calculate_control_torques(desire_state, measure_state); + + update_hip_and_wheel_torques(leg_forces, control_torques); + } private: - void update_status() {} + struct LegPosture { + Eigen::Vector2d leg_length; + Eigen::Vector2d tilt_angle; + + Eigen::Vector2d dot_leg_length; + Eigen::Vector2d dot_tilt_angle; + + Eigen::Vector2d second_order_diff_leg_length; + Eigen::Vector2d second_order_diff_tilt_angle; + }; + + 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 &lf = hip_angles.x(), &lb = hip_angles.y(), &rb = hip_angles.z(), + &rf = hip_angles.w(); + + 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.dot_leg_length = (result.leg_length - last_leg_length_) / dt_; + last_leg_length_ = result.leg_length; + result.dot_tilt_angle = (result.tilt_angle - last_tilt_angle_) / dt_; + last_tilt_angle_ = result.tilt_angle; + + result.second_order_diff_leg_length = (result.dot_leg_length - last_dot_leg_length_) / dt_; + last_dot_leg_length_ = result.dot_leg_length; + result.second_order_diff_tilt_angle = (result.dot_tilt_angle - last_dot_tilt_angle_) / dt_; + last_dot_tilt_angle_ = result.dot_tilt_angle; + + return result; + } + + Eigen::Vector2d + calculate_translational_distance(LegPosture leg_posture, Eigen::Vector2d wheel_velocities) { + std::tuple result{}; + auto& [distance, velocity] = result; + + auto wheel_velocity = (wheel_velocities.x() + wheel_velocities.y()) / 2.0; + leg_posture.tilt_angle.array() += *chassis_pitch_angle_imu_; + + auto left_leg_velocity = + leg_posture.leg_length.x() * std::cos(leg_posture.tilt_angle.x()) + * leg_posture.dot_tilt_angle.x() + + leg_posture.dot_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.dot_tilt_angle.y() + + leg_posture.dot_leg_length.y() + + std::sin(leg_posture.tilt_angle.y()); + + auto calculate_velocity = wheel_velocity + (left_leg_velocity + right_leg_velocity) / 2.0; + + auto estimate_velocity = velocity_kalman_filter_.update( + Eigen::Vector2d{calculate_velocity, *chassis_x_acceleration_imu_}); + + velocity = estimate_velocity.x(); + distance += velocity * dt_; + + return Eigen::Vector2d{}; + } + + Eigen::Vector2d calculate_support_force(LegPosture leg_posture) {} + + 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.dot_tilt_angle.x(); + + measure_state.right_tilt_angle = leg_posture.tilt_angle.y(); + measure_state.right_tilt_velocity = leg_posture.dot_tilt_angle.y(); + + measure_state.body_pitch_angle = *chassis_pitch_angle_imu_; + measure_state.body_pitch_velocity = *chassis_pitch_velocity_imu_; + + return measure_state; + } + + Eigen::Vector3d calculate_chassis_control_velocity() { + Eigen::Vector3d chassis_control_velocity; + chassis_control_velocity.x() = chassis_control_velocity_->vector.x(); + chassis_control_velocity.y() = 0.0; + chassis_control_velocity.z() = chassis_control_velocity_->vector.z(); + return chassis_control_velocity; + } + + rmcs_msgs::WheelLegState calculate_desire_state(Eigen::Vector3d control_velocity) { + rmcs_msgs::WheelLegState desire_state; + + return desire_state; + } + + Eigen::Vector2d + calculate_leg_force(LegPosture leg_posture, rmcs_msgs::WheelLegState measure_state) { + Eigen::Vector2d result; + + auto roll_feedforward = roll_angle_pid_calculator_.update(0 - *chassis_roll_angle_imu_); + auto leg_length_feedforward = leg_length_pid_calculator_.update( + leg_length_desire_ - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); + + auto calculate_compensation_feedforward = [this](double coefficient) { + return (body_mess_ / 2.0 + centroid_position_coefficient_ * leg_mess_) * coefficient; + }; + + auto gravity_feedforward = calculate_compensation_feedforward(g_); + auto inertial_feedforward = calculate_compensation_feedforward( + (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0 / (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_feedforward + leg_length_feedforward + gravity_feedforward - inertial_feedforward, + -roll_feedforward + leg_length_feedforward + gravity_feedforward + + inertial_feedforward}; + + return result; + } + + Eigen::Vector4d calculate_control_torques( + rmcs_msgs::WheelLegState desire_state, rmcs_msgs::WheelLegState measure_state) { + Eigen::Vector4d result; + + auto error_state = desire_state.vector() - measure_state.vector(); + + return result; + } + + void update_hip_and_wheel_torques(Eigen::Vector2d leg_forces, Eigen::Vector4d control_torques) { + auto left_leg_force = leg_forces.x(), right_leg_force = leg_forces.y(); + auto left_wheel_control_torque = control_torques.x(), + right_wheel_control_torque = control_torques.y(), + left_leg_control_torque = control_torques.z(), + right_leg_control_torque = control_torques.w(); + + *left_wheel_control_torque_ = left_wheel_control_torque; + *right_wheel_control_torque_ = right_wheel_control_torque; + + auto left_hip_control_torque = + left_leg_vmc_solver_.get_joint_torque(left_leg_force, left_leg_control_torque); + auto right_hip_control_torque = + right_leg_vmc_solver_.get_joint_torque(right_leg_force, right_leg_control_torque); + + *left_front_hip_control_torque_ = left_hip_control_torque.x(); + *left_back_hip_control_torque_ = left_hip_control_torque.y(); + *right_front_hip_control_torque_ = right_hip_control_torque.x(); + *right_back_hip_control_torque_ = right_hip_control_torque.y(); + } + + void reset_all_controls() { + *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; + } + + static constexpr double nan_ = std::numeric_limits::quiet_NaN(); + static constexpr double inf_ = std::numeric_limits::infinity(); + + static constexpr double dt_ = 1e-3; + static constexpr double g_ = 9.80665; + + 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::Matrix2d() << 1.0, 0.0, 0.0, 1.0).finished(); + const Eigen::Matrix2d W_ = (Eigen::Matrix2d() << 0.5 * dt_ * dt_, 0.0, 0.0, dt_).finished(); + const Eigen::Matrix2d Q_ = + (Eigen::Matrix2d() << sigma_q_ * sigma_q_, 0.0, 0.0, sigma_q_* sigma_q_).finished(); + const Eigen::Matrix2d R_ = + (Eigen::Matrix2d() << sigma_v_ * sigma_v_, 0.0, 0.0, sigma_a_* sigma_a_).finished(); + + const double body_mess_; + const double leg_mess_; + const double wheel_radius_; + const double wheel_distance_; + const double centroid_position_coefficient_; + + InputInterface left_front_hip_angle_; + InputInterface left_back_hip_angle_; + InputInterface right_front_hip_angle_; + InputInterface right_back_hip_angle_; + + InputInterface left_wheel_velocity_; + InputInterface right_wheel_velocity_; + + InputInterface chassis_x_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 power_limit_; + + 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_wheel_control_torque_; + OutputInterface right_wheel_control_torque_; + + // rmcs_msgs::WheelLegState measure_state_, desire_state_; + + double leg_length_desire_; + + Eigen::Vector2d last_leg_length_, last_tilt_angle_, last_dot_leg_length_, last_dot_tilt_angle_; + + VmcSolver left_leg_vmc_solver_, right_leg_vmc_solver_; + filter::KalmanFilter<2, 2> velocity_kalman_filter_; + + pid::PidCalculator roll_angle_pid_calculator_, leg_length_pid_calculator_; }; } // namespace rmcs_core::controller::chassis 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..a5f6e3c4 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp @@ -0,0 +1,82 @@ +#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: + KalmanFilter() {} + + KalmanFilter( + const Matrix& A, const Matrix& H, const Matrix& Q, + const Matrix& R, const Matrix& W = Matrix::Identity(), + const Matrix& V = Matrix::Identity(), + const Matrix& B = Matrix::Zero()) + : measurement_noise_transition_(V) + , measurement_noise_covariance_(R) + , state_transition_(A) + , process_noise_transition_(W) + , process_noise_covariance_(Q) + , control_input_(B) + , measurement_transition_(H) { + reset(); + } + + void reset() { + posterior_estimate_ = State::Zero(); + prior_estimate_ = State::Zero(); + + posterior_error_covariance_ = 1000.0 * Matrix::Identity(); + prior_error_covariance_ = posterior_error_covariance_; + + kalman_gain_ = Matrix::Zero(); + } + + State update(const Measure& measurement, const Control& control_vector = Control::Zero()) { + // prediction + prior_estimate_ = state_transition_ * posterior_estimate_ + control_input_ * 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(); + posterior_estimate_ += + kalman_gain_ * (measurement - measurement_transition_ * prior_estimate_); + + // update + posterior_error_covariance_ = + (Matrix::Identity() - kalman_gain_ * measurement_transition_) + * prior_error_covariance_; + + 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 control_input_; + + Matrix measurement_transition_; +}; +} // namespace rmcs_core::filter \ No newline at end of file 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 index 7be33582..9bf48f89 100644 --- 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 @@ -1,5 +1,7 @@ #pragma once +#include + namespace rmcs_msgs { struct WheelLegState { @@ -13,6 +15,10 @@ struct WheelLegState { 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 From 525323e1e3949d63a0620a8c6d6a2aeec6c034a2 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Wed, 26 Nov 2025 22:13:01 +0800 Subject: [PATCH 16/35] Complete control struction. --- .../chassis/wheel_leg/lqr_controller.hpp | 9 ++ .../controller/chassis/wheel_leg/observer.cpp | 1 + .../chassis/wheel_leg/vmc_solver.hpp | 10 +- .../{ => wheel_leg}/wheel_leg_controller.cpp | 122 +++++++++++++----- .../rmcs_core/src/filter/kalman_filter.hpp | 8 +- .../src/hardware/wheelleg_infantry.cpp | 11 +- 6 files changed, 116 insertions(+), 45 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/lqr_controller.hpp rename rmcs_ws/src/rmcs_core/src/controller/chassis/{ => wheel_leg}/wheel_leg_controller.cpp (76%) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/lqr_controller.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/lqr_controller.hpp new file mode 100644 index 00000000..5f1dcc74 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/lqr_controller.hpp @@ -0,0 +1,9 @@ +namespace rmcs_core::controller::chassis { +class LqrController { +public: + LqrController() {} + void update() {} + +private: +}; +} // namespace rmcs_core::controller::chassis \ No newline at end of file diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp index cc1f2044..532e1f11 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp @@ -2,6 +2,7 @@ #include #include #include +#include #include "filter/kalman_filter.hpp" #include "vmc_solver.hpp" 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 index 5cc0fb91..4416d1ce 100644 --- 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 @@ -1,7 +1,8 @@ +#pragma once + #include #include #include -#include namespace rmcs_core::controller::chassis { class VmcSolver { @@ -13,7 +14,7 @@ class VmcSolver { , l4_(l1) , l5_(l5) {} - auto update(double phi1, double phi4) -> std::tuple { + Eigen::Vector2d update(double phi1, double phi4) { if (std::isnan(phi1) || std::isnan(phi4)) { reset(); return {0.0, 0.0}; @@ -73,7 +74,6 @@ class VmcSolver { {j11, j12}, {j21, j22} }; - auto rotation_matrix = Eigen::Rotation2Dd(phi0 - pi_ / 2.0); auto transform_matrix = Eigen::Matrix2d{ {0, -1 / leg_length_}, @@ -83,9 +83,7 @@ class VmcSolver { joint_torque_matrix_ = jacobian_matrix.transpose() * rotation_matrix * transform_matrix; } - auto get_leg_posture() const -> std::tuple { - return {leg_length_, tilt_angle_}; - } + Eigen::Vector2d get_leg_posture() const { return Eigen::Vector2d{leg_length_, tilt_angle_}; } static constexpr double inf_ = std::numeric_limits::infinity(); static constexpr double nan_ = std::numeric_limits::quiet_NaN(); diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_controller.cpp similarity index 76% rename from rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp rename to rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_controller.cpp index 75952f92..b3cb8313 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_controller.cpp @@ -1,11 +1,14 @@ +#include +#include #include #include #include +#include #include "controller/pid/pid_calculator.hpp" #include "filter/kalman_filter.hpp" #include "rmcs_executor/component.hpp" -#include "wheel_leg/vmc_solver.hpp" +#include "vmc_solver.hpp" namespace rmcs_core::controller::chassis { class WheelLegController @@ -18,6 +21,7 @@ class WheelLegController rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , body_mess_(15.0) , leg_mess_(0.86) + , wheel_mess_() , wheel_radius_(0.06) , wheel_distance_(0.2135) , centroid_position_coefficient_() @@ -34,10 +38,16 @@ class WheelLegController 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/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/acceleration", chassis_x_acceleration_imu_); + 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_); @@ -61,8 +71,7 @@ class WheelLegController reset_all_controls(); return; } - - // state vector: s ds phi dphi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b + // Observer auto wheel_velocities = calculate_wheel_velocities(); auto hip_angles = calculate_hips_angles(); @@ -71,8 +80,10 @@ class WheelLegController auto support_force = calculate_support_force(leg_posture); + // State: [s ds phi dphi 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 desire_state = calculate_desire_state(chassis_control_velocity); @@ -94,6 +105,16 @@ class WheelLegController Eigen::Vector2d second_order_diff_tilt_angle; }; + void reset_all_controls() { + *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; + } + Eigen::Vector2d calculate_wheel_velocities() { return { *left_wheel_velocity_ * wheel_radius_, // @@ -104,17 +125,15 @@ class WheelLegController return { *left_front_hip_angle_, // *left_back_hip_angle_, // + *right_front_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; + const auto& [lf, lb, rf, rb] = hip_angles; LegPosture result; - const auto &lf = hip_angles.x(), &lb = hip_angles.y(), &rb = hip_angles.z(), - &rf = hip_angles.w(); - 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); @@ -134,9 +153,54 @@ class WheelLegController 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_.get_virtual_torque( + *left_front_hip_torque_, *left_back_hip_torque_); + const auto& [right_leg_force, right_leg_torque] = right_leg_vmc_solver_.get_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.dot_leg_length.x() * leg_posture.dot_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.dot_tilt_angle.x() + * leg_posture.dot_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.dot_leg_length.y() * leg_posture.dot_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.dot_tilt_angle.y() + * leg_posture.dot_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) { - std::tuple result{}; + Eigen::Vector2d result; auto& [distance, velocity] = result; auto wheel_velocity = (wheel_velocities.x() + wheel_velocities.y()) / 2.0; @@ -154,17 +218,16 @@ class WheelLegController auto calculate_velocity = wheel_velocity + (left_leg_velocity + right_leg_velocity) / 2.0; + // https://zhuanlan.zhihu.com/p/689921165 auto estimate_velocity = velocity_kalman_filter_.update( - Eigen::Vector2d{calculate_velocity, *chassis_x_acceleration_imu_}); + Eigen::Vector2d{calculate_velocity, *chassis_x_axis_acceleration_imu_}); velocity = estimate_velocity.x(); distance += velocity * dt_; - return Eigen::Vector2d{}; + return result; } - Eigen::Vector2d calculate_support_force(LegPosture leg_posture) {} - rmcs_msgs::WheelLegState calculate_measure_state(Eigen::Vector2d distance, LegPosture leg_posture) { rmcs_msgs::WheelLegState measure_state; @@ -196,7 +259,7 @@ class WheelLegController rmcs_msgs::WheelLegState calculate_desire_state(Eigen::Vector3d control_velocity) { rmcs_msgs::WheelLegState desire_state; - + auto& [vx, vy, wz] = control_velocity; return desire_state; } @@ -239,11 +302,9 @@ class WheelLegController } void update_hip_and_wheel_torques(Eigen::Vector2d leg_forces, Eigen::Vector4d control_torques) { - auto left_leg_force = leg_forces.x(), right_leg_force = leg_forces.y(); - auto left_wheel_control_torque = control_torques.x(), - right_wheel_control_torque = control_torques.y(), - left_leg_control_torque = control_torques.z(), - right_leg_control_torque = control_torques.w(); + 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; *left_wheel_control_torque_ = left_wheel_control_torque; *right_wheel_control_torque_ = right_wheel_control_torque; @@ -259,16 +320,6 @@ class WheelLegController *right_back_hip_control_torque_ = right_hip_control_torque.y(); } - void reset_all_controls() { - *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; - } - static constexpr double nan_ = std::numeric_limits::quiet_NaN(); static constexpr double inf_ = std::numeric_limits::infinity(); @@ -289,6 +340,7 @@ class WheelLegController 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_; @@ -298,10 +350,16 @@ class WheelLegController InputInterface right_front_hip_angle_; InputInterface right_back_hip_angle_; + 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_acceleration_imu_; + InputInterface chassis_x_axis_acceleration_imu_; + InputInterface chassis_z_axis_acceleration_imu_; InputInterface chassis_yaw_velocity_imu_; InputInterface chassis_pitch_velocity_imu_; @@ -322,8 +380,6 @@ class WheelLegController OutputInterface left_wheel_control_torque_; OutputInterface right_wheel_control_torque_; - // rmcs_msgs::WheelLegState measure_state_, desire_state_; - double leg_length_desire_; Eigen::Vector2d last_leg_length_, last_tilt_angle_, last_dot_leg_length_, last_dot_tilt_angle_; diff --git a/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp index a5f6e3c4..fc08912e 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include @@ -42,14 +44,14 @@ class KalmanFilter { } State update(const Measure& measurement, const Control& control_vector = Control::Zero()) { - // prediction + // Prediction prior_estimate_ = state_transition_ * posterior_estimate_ + control_input_ * control_vector; prior_error_covariance_ = state_transition_ * posterior_error_covariance_ * state_transition_.transpose() + process_noise_transition_ * process_noise_covariance_ * process_noise_transition_.transpose(); - // correction + // Correction kalman_gain_ = prior_error_covariance_ * measurement_transition_.transpose() * (measurement_transition_ * prior_error_covariance_ * measurement_transition_.transpose() @@ -59,7 +61,7 @@ class KalmanFilter { posterior_estimate_ += kalman_gain_ * (measurement - measurement_transition_ * prior_estimate_); - // update + // Update posterior_error_covariance_ = (Matrix::Identity() - kalman_gain_ * measurement_transition_) * prior_error_covariance_; diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp index 73dc08f8..97c9ce92 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp @@ -237,7 +237,10 @@ class WheelLegInfantry return size; }; - infantry.register_output("/chassis/x/acceleration", chassis_x_acceleration_imu_); + 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_); @@ -289,7 +292,8 @@ class WheelLegInfantry *chassis_pitch_velocity_imu_ = imu_.gy(); *chassis_roll_velocity_imu_ = imu_.gx(); - *chassis_x_acceleration_imu_ = imu_.ax(); + *chassis_x_axis_acceleration_imu_ = imu_.ax(); + *chassis_z_axis_acceleration_imu_ = imu_.az(); } private: @@ -331,7 +335,8 @@ class WheelLegInfantry device::Bmi088 imu_; - OutputInterface chassis_x_acceleration_imu_; + OutputInterface chassis_x_axis_acceleration_imu_; + OutputInterface chassis_z_axis_acceleration_imu_; OutputInterface chassis_yaw_velocity_imu_; OutputInterface chassis_pitch_velocity_imu_; From 26dc59141a794136addd581ceceebd39c9648881 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sun, 30 Nov 2025 18:51:34 +0800 Subject: [PATCH 17/35] Add hardware support of `dm_motor.hpp` and update `wheelleg_infantry.cpp`. --- .../src/hardware/device/dm_motor.hpp | 73 ++++++++++++ .../src/hardware/wheelleg_infantry.cpp | 106 +++++++++++++++--- 2 files changed, 166 insertions(+), 13 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp 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..a646b2dd --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp @@ -0,0 +1,73 @@ +#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_torque", control_torque_, 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(); + } + + double control_torque() { + if (control_torque_.ready()) [[likely]] + return *control_torque_; + 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_torque_command(double control_torque) { + return librmcs::device::DmMotor::generate_torque_command(control_torque); + } + + uint64_t generate_disable_command() { + return librmcs::device::DmMotor::generate_disable_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::OutputInterface rotor_temperature_; + + rmcs_executor::Component::InputInterface control_torque_; +}; +} // 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 index 97c9ce92..60ee7aa9 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp @@ -1,3 +1,5 @@ +#include +#include #include #include #include @@ -7,6 +9,7 @@ #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" @@ -132,8 +135,8 @@ class WheelLegInfantry void command_update() { uint16_t control_commands[4]{}; - control_commands[2] = friction_wheel_motors_[0].generate_command(); - control_commands[3] = friction_wheel_motors_[1].generate_command(); + 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)); @@ -152,9 +155,9 @@ class WheelLegInfantry if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] return; - if (can_id == 0x203) { + if (can_id == 0x201) { friction_wheel_motors_[0].store_status(can_data); - } else if (can_id == 0x204) { + } else if (can_id == 0x202) { friction_wheel_motors_[1].store_status(can_data); } } @@ -209,8 +212,16 @@ class WheelLegInfantry {infantry, infantry_command, "/chassis/right_wheel", device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reduction_ratio(13.0) - .enable_multi_turn_angle() - .set_reversed()}) + .enable_multi_turn_angle()}) + , chassis_hip_motors( + {infantry, infantry_command, "/chassis/left_front_hip", + device::DmMotor::Config{device::DmMotor::Type::DM8009}}, + {infantry, infantry_command, "/chassis/left_back_hip", + device::DmMotor::Config{device::DmMotor::Type::DM8009}}, + {infantry, infantry_command, "/chassis/right_front_hip", + device::DmMotor::Config{device::DmMotor::Type::DM8009}}, + {infantry, infantry_command, "/chassis/right_back_hip", + device::DmMotor::Config{device::DmMotor::Type::DM8009}}) , gimbal_yaw_motor_( infantry, infantry_command, "/gimbal/yaw", device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10} @@ -226,6 +237,19 @@ class WheelLegInfantry , 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) { @@ -272,7 +296,44 @@ class WheelLegInfantry } void command_update() { - uint16_t control_commands[4]; + uint16_t control_commands[4]{}; + + control_commands[0] = chassis_wheel_motors_[0].generate_command(); + control_commands[1] = chassis_wheel_motors_[1].generate_command(); + control_commands[3] = bullet_feeder_motor_.generate_command(); + + transmit_buffer_.add_can1_transmission( + 0x200, std::bit_cast(control_commands)); + + transmit_buffer_.add_can1_transmission( + 0x141, + gimbal_yaw_motor_.generate_torque_command(gimbal_yaw_motor_.control_torque())); + + if (!is_hips_enable_) { + transmit_buffer_.add_can2_transmission( + 0x01, chassis_hip_motors[0].generate_enable_command()); + transmit_buffer_.add_can2_transmission( + 0x02, chassis_hip_motors[1].generate_enable_command()); + transmit_buffer_.add_can2_transmission( + 0x03, chassis_hip_motors[2].generate_enable_command()); + transmit_buffer_.add_can2_transmission( + 0x04, chassis_hip_motors[3].generate_enable_command()); + is_hips_enable_ = !is_hips_enable_; + } else { + transmit_buffer_.add_can2_transmission( + 0x01, chassis_hip_motors[0].generate_torque_command( + chassis_hip_motors[0].control_torque())); + transmit_buffer_.add_can2_transmission( + 0x02, chassis_hip_motors[1].generate_torque_command( + chassis_hip_motors[1].control_torque())); + transmit_buffer_.add_can2_transmission( + 0x03, chassis_hip_motors[2].generate_torque_command( + chassis_hip_motors[2].control_torque())); + transmit_buffer_.add_can2_transmission( + 0x04, chassis_hip_motors[3].generate_torque_command( + chassis_hip_motors[3].control_torque())); + } + transmit_buffer_.trigger_transmission(); } @@ -282,11 +343,11 @@ class WheelLegInfantry *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::atan2( + *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_roll_angle_imu_ = - std::asin(-2.0 * (imu_.q1() * imu_.q3() - imu_.q0() * imu_.q2())); *chassis_yaw_velocity_imu_ = imu_.gz(); *chassis_pitch_velocity_imu_ = imu_.gy(); @@ -303,7 +364,15 @@ class WheelLegInfantry if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] return; - if (can_id == 0x201) {} + if (can_id == 0x201) { + chassis_wheel_motors_[0].store_status(can_data); + } else if (can_id == 0x202) { + chassis_wheel_motors_[1].store_status(can_data); + } else if (can_id == 0x203) { + bullet_feeder_motor_.store_status(can_data); + } else if (can_id == 0x141) { + gimbal_yaw_motor_.store_status(can_data); + } } void can2_receive_callback( @@ -312,7 +381,15 @@ class WheelLegInfantry if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] return; - if (can_id == 0x141) {} + 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); + } 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 { @@ -331,6 +408,7 @@ class WheelLegInfantry 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_; @@ -349,7 +427,9 @@ class WheelLegInfantry OutputInterface& tf_; device::DjiMotor chassis_wheel_motors_[2]; - // device::DmMotor chassis_hip_motors[4]; + + bool is_hips_enable_{false}; + device::DmMotor chassis_hip_motors[4]; device::LkMotor gimbal_yaw_motor_; device::DjiMotor bullet_feeder_motor_; From c09ace2fe8b742bf1be6eaee8d7d43cbaf7d526a Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Mon, 1 Dec 2025 05:29:03 +0800 Subject: [PATCH 18/35] Add lqr controller --- .../chassis/wheel_leg/lqr_controller.hpp | 9 - .../src/controller/lqr/lqr_calculator.hpp | 507 ++++++++++++++++++ .../src/controller/lqr/lqr_controller.hpp | 157 ++++++ 3 files changed, 664 insertions(+), 9 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/lqr_controller.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_controller.hpp diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/lqr_controller.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/lqr_controller.hpp deleted file mode 100644 index 5f1dcc74..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/lqr_controller.hpp +++ /dev/null @@ -1,9 +0,0 @@ -namespace rmcs_core::controller::chassis { -class LqrController { -public: - LqrController() {} - void update() {} - -private: -}; -} // namespace rmcs_core::controller::chassis \ 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..99898dc3 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp @@ -0,0 +1,507 @@ +#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; + + // This function was generated by the Symbolic Math Toolbox version 24.2. + // 2025-11-13 22:44:20 + 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 * 65.742000232621649 - l_r * 164.16682583486471) - t2 * 52.381670299384247) + - t3 * 386.2464616614821) + + t4 * 542.26001657504969) + + t5 * 392.11423826325591) + + ((((t6 * 97.875583809753323 - t7 * 691.72153331253764) + - K_result_tmp * 20.138079062233381) + - b_K_result_tmp * 1657.812224997456) + + c_K_result_tmp * 1985.3342273262449)) + + (((d_K_result_tmp * 1415.261073756446 - e_K_result_tmp * 1100.267333961333) + - f_K_result_tmp * 373.0130513504314) + + 0.62665696127003523); + K_result[1] = + ((((((l_l * -137.6782528259844 + l_r * 38.291917257296006) + t2 * 601.06706713336121) + - t3 * 967.765443429392) + + t4 * 463.649191705752) + + t5 * 180.95650637985389) + + ((((t6 * -578.14999435552363 + t7 * 391.957501942056) + - K_result_tmp * 465.45483092991282) + + b_K_result_tmp * 976.92640151459261) + - c_K_result_tmp * 67.562213729824677)) + + (((d_K_result_tmp * 61.659385499165687 + e_K_result_tmp * 797.36074479286879) + - f_K_result_tmp * 1256.302427350575) + + 0.80890190594414357); + K_result[2] = + ((((((l_l * -58.936551645814767 + l_r * 100.43070443026269) + t2 * 134.7057987502493) + - t3 * 171.715152262072) + + t4 * 260.01877309025127) + - t5 * 608.01436024643863) + + (((((t6 * 1073.386058341245 - t7 * 334.41160739101912) + + K_result_tmp * 113.639816009988) + + b_K_result_tmp * 421.62514920632708) + - c_K_result_tmp * 1786.6435368411981) + - d_K_result_tmp * 435.55944825783172)) + + ((e_K_result_tmp * -550.66675594008791 + f_K_result_tmp * 1643.2489674730241) + + 10.79434735925515); + K_result[3] = + ((((((l_l * 17.470935993895338 + l_r * 50.831427775719263) - t2 * 687.4474700837809) + + t3 * 2152.3857311851771) + - t4 * 2023.7205507551851) + - t5 * 625.1804360193164) + + ((((t6 * 1253.250708378072 - t7 * 859.1228344233549) + + K_result_tmp * 805.83458525202445) + - b_K_result_tmp * 246.38589855911019) + - c_K_result_tmp * 335.89539317621069)) + + (((d_K_result_tmp * -1942.114665231356 + e_K_result_tmp * 1309.7478537128129) + + f_K_result_tmp * 884.74139025727879) + + 9.13562076257349); + K_result[4] = + (((((l_l * 51.618765926331527 - l_r * 101.13738170798339) - t2 * 8.95708958199305) + - t3 * 314.67475435008328) + + t4 * 387.6334722914317) + + ((((t5 * 261.89330760067128 + t6 * 29.258960918484082) - t7 * 430.22788200317518) + - K_result_tmp * 127.3840659524775) + - b_K_result_tmp * 915.65481615560964)) + + ((((c_K_result_tmp * 1255.9332693258041 + d_K_result_tmp * 1037.1094349037769) + - e_K_result_tmp * 698.84642810472917) + - f_K_result_tmp * 430.54673417571621) + - 2.1515748989123931); + K_result[5] = + (((((l_l * -119.3609663914195 + l_r * 77.261236019827066) + t2 * 395.04190370737882) + - t3 * 537.72615245148461) + + t4 * 182.66443016706981) + + ((((t5 * -198.7156378678909 + t6 * 418.64243536679629) - t7 * 480.36171209178849) + - K_result_tmp * 118.1955126585697) + + b_K_result_tmp * 303.50802825084207)) + + ((((c_K_result_tmp * 275.56047258933432 - d_K_result_tmp * 226.41242349661471) + + e_K_result_tmp * 720.23624044046949) + - f_K_result_tmp * 723.90438215562028) + - 2.5296472311547742); + K_result[6] = + (((((l_l * -58.56170849808084 + l_r * 39.916737454873079) + t2 * 68.947989451313433) + - t3 * 36.528448326658513) + + t4 * 117.4406998564984) + + (((((t5 * -325.777173508277 + t6 * 621.513686909637) - t7 * 159.79216027074841) + + K_result_tmp * 240.12099506453529) + - b_K_result_tmp * 82.581978326705723) + - c_K_result_tmp * 1007.408302279198)) + + (((d_K_result_tmp * -407.37199710550118 - e_K_result_tmp * 400.58426030231459) + + f_K_result_tmp * 1356.806219869289) + + 11.944576963344529); + K_result[7] = + ((((((l_l * 40.361578166435663 - l_r * 41.942511122782122) - t2 * 585.48729241581793) + + t3 * 1623.9865989528621) + - t4 * 1435.968108924161) + - t5 * 135.4895503768636) + + ((((t6 * 339.70973345186269 - t7 * 244.15181963706041) + + K_result_tmp * 609.062295677131) + - b_K_result_tmp * 506.592857295121) + + c_K_result_tmp * 124.65353222908691)) + + (((d_K_result_tmp * -1145.8936460038419 + e_K_result_tmp * 667.51865206944569) + + f_K_result_tmp * 623.81296716252916) + + 10.91385918044422); + K_result[8] = + (((((l_l * -10.14292967245026 + l_r * 19.510955097222649) + t2 * 63.688427354792381) + - t3 * 126.7030263345557) + + t4 * 90.019434614642108) + + ((((t5 * -12.62910633624522 - t6 * 68.095117792740581) + t7 * 92.6100356329447) + - K_result_tmp * 64.0139294363693) + + b_K_result_tmp * 179.84689623277129)) + + ((((c_K_result_tmp * -131.83732123625609 + d_K_result_tmp * 34.954651254795969) + + e_K_result_tmp * 10.54375820451925) + - f_K_result_tmp * 79.4730085041239) + - 2.782183870224511); + K_result[9] = + (((((l_l * -21.676879342448569 + l_r * 12.321226124105211) + t2 * 45.053038755393537) + - t3 * 40.736498825878989) + + t4 * 14.182293926652751) + + ((((t5 * -54.233045925723118 + t6 * 96.923717117908481) - t7 * 41.0847809943677) + + K_result_tmp * 20.86964231983946) + - b_K_result_tmp * 21.805525628883171)) + + ((((c_K_result_tmp * -73.703002934173924 - d_K_result_tmp * 48.205145186361243) + + e_K_result_tmp * 0.37164453769723782) + + f_K_result_tmp * 110.0699205718546) + + 2.7929967821710409); + K_result[10] = + (((((l_l * -6.8780348541810667 - l_r * 16.099961296316138) + t2 * 34.829152893578431) + - t3 * 110.1446416680607) + + t4 * 116.4556040715007) + + ((((t5 * 66.4965947634005 - t6 * 67.1180404126167) + t7 * 0.22275801877981269) + - K_result_tmp * 28.765159166789282) + - b_K_result_tmp * 102.0190546135326)) + + ((((c_K_result_tmp * 130.8600727825069 + d_K_result_tmp * 160.2637982055565) + - e_K_result_tmp * 155.37865024067131) + - f_K_result_tmp * 8.093955982347282) + - 3.2046817899359779); + K_result[11] = + (((((l_l * 22.023525789365639 + l_r * 2.3226842069266689) - t2 * 109.7742823693729) + + t3 * 223.706876603369) + - t4 * 182.1064429077538) + + ((((t5 * -16.298544568580731 + t6 * 64.146476977234713) - t7 * 93.856384238343054) + + K_result_tmp * 46.885252027497913) + - b_K_result_tmp * 119.386123646578)) + + ((((c_K_result_tmp * 179.52484051833181 - d_K_result_tmp * 37.095894231595892) + + e_K_result_tmp * 70.533519774817037) + - f_K_result_tmp * 65.1655497798367) + + 3.11156333830338); + K_result[12] = + (((((l_l * -6.0347124087155848 + l_r * 10.256338714606731) + t2 * 36.034487936211761) + - t3 * 63.9608770978389) + + t4 * 39.509679028783658) + + ((((t5 * -5.0414487721761976 - t6 * 43.969340282364918) + t7 * 57.711832988519753) + - K_result_tmp * 34.056119963357752) + + b_K_result_tmp * 109.2951827140096)) + + ((((c_K_result_tmp * -78.837922176786833 + d_K_result_tmp * 1.3934012016259609) + + e_K_result_tmp * 27.765227145899889) + - f_K_result_tmp * 49.3863021069217) + - 1.238124950078388); + K_result[13] = + (((((l_l * -10.714551320965811 + l_r * 6.8511821798768047) + t2 * 12.01790656654911) + + t3 * 2.2127122543525761) + - t4 * 8.0181093327580673) + + ((((t5 * -40.302270522501743 + t6 * 88.415260482909858) - t7 * 60.325646830687759) + + K_result_tmp * 28.709721645369189) + - b_K_result_tmp * 51.015910354347888)) + + ((((c_K_result_tmp * -5.0937792766477807 - d_K_result_tmp * 34.576148111107557) + - e_K_result_tmp * 3.785457234395825) + + f_K_result_tmp * 72.4475588587211) + + 1.222238411094988); + K_result[14] = + (((((l_l * -0.98229349452137571 - l_r * 11.72348070058845) + t2 * 5.9245692433909536) + - t3 * 35.034102192476048) + + t4 * 42.954840028608707) + + ((((t5 * 50.073120095831833 - t6 * 70.907851635712859) + t7 * 27.953446951940919) + - K_result_tmp * 19.2867397771467) + - b_K_result_tmp * 41.6406668283246)) + + ((((c_K_result_tmp * 69.243025367636363 + d_K_result_tmp * 90.719777052373544) + - e_K_result_tmp * 77.862567260434417) + - f_K_result_tmp * 23.859736386417872) + - 1.2622286836864891); + K_result[15] = + (((((l_l * 12.49202706521565 + l_r * 0.48242804319495131) - t2 * 50.637397892085311) + + t3 * 85.099425101873862) + - t4 * 59.2977455404037) + + ((((t5 * 3.133871025887625 + t6 * 8.719689032169196) - t7 * 28.545856331239222) + + K_result_tmp * 9.704499714564653) + - b_K_result_tmp * 67.055766513467)) + + ((((c_K_result_tmp * 102.2402656009333 + d_K_result_tmp * 31.282480931863091) + - e_K_result_tmp * 7.4954001635994079) + - f_K_result_tmp * 44.9149716129839) + + 1.24331399972177); + K_result[16] = + (((((l_l * 11.80021731015565 - l_r * 72.921640114139976) - t2 * 44.566015557682263) + - t3 * 78.570378889585214) + + t4 * 182.99527774593719) + + ((((t5 * -60.682819229444988 + t6 * 704.14696546445816) - t7 * 828.47684708198051) + + K_result_tmp * 197.50068793180131) + - b_K_result_tmp * 1177.897504958082)) + + ((((c_K_result_tmp * 938.34507403309215 + d_K_result_tmp * 426.965973056169) + - e_K_result_tmp * 560.60692284051106) + + f_K_result_tmp * 347.9969568508659) + + 1.803625058148816); + K_result[17] = + (((((l_l * -17.888453298326709 - l_r * 32.853034524878623) + t2 * 122.7775490392724) + - t3 * 198.74621637453919) + + t4 * 27.595960348631191) + + (((((t5 * 278.12055552001408 - t6 * 515.30272840351313) + t7 * 242.2610324760027) + - K_result_tmp * 227.472493036576) + + b_K_result_tmp * 266.87686015907678) + + c_K_result_tmp * 390.26919078295742)) + + (((d_K_result_tmp * 168.777011867226 + e_K_result_tmp * 396.2767322366152) + - f_K_result_tmp * 874.39629365858411) + + 1.1879488399098279); + K_result[18] = + (((((l_l * -28.445674224257161 + l_r * 133.1992896905158) + t2 * 55.1250721006242) + - t3 * 9.127477406915542) + + t4 * 30.864302634941609) + + (((((t5 * -450.093388257261 + t6 * 615.42324342693928) - t7 * 69.2633438874434) + + K_result_tmp * 80.407025457619014) + + b_K_result_tmp * 312.69109226779511) + - c_K_result_tmp * 1102.736261460343)) + + (((d_K_result_tmp * -374.10368958919719 - e_K_result_tmp * 121.7149564247612) + + f_K_result_tmp * 863.39469234281091) + - 1.96009566573998); + K_result[19] = + (((((l_l * 18.78880668336452 - l_r * 2.571667298988491) - t2 * 258.34664122577118) + + t3 * 864.76966519815642) + - t4 * 869.57534622032949) + + ((((t5 * -258.44331430146428 + t6 * 377.55191135262658) - t7 * 191.7135178023396) + + K_result_tmp * 242.98508560433339) + + b_K_result_tmp * 508.268432159587)) + + ((((c_K_result_tmp * -560.4897619157739 - d_K_result_tmp * 1061.8106963679541) + + e_K_result_tmp * 909.91716648345994) + + f_K_result_tmp * 131.5914813453644) + + 1.6391768009073691); + K_result[20] = + (((((l_l * 1.07983786439577 - l_r * 10.85618036361061) + t2 * 4.7358339031914536) + - t3 * 34.514852987299847) + + t4 * 40.764983995143439) + + ((((t5 * 26.12298910332014 - t6 * 13.57931670210346) - t7 * 11.54685053771852) + - K_result_tmp * 1.6269455566548079) + - b_K_result_tmp * 102.7027833408567)) + + ((((c_K_result_tmp * 91.770348703220975 + d_K_result_tmp * 89.207170899830118) + - e_K_result_tmp * 79.931486153081309) + + f_K_result_tmp * 6.7521077655079376) + + 0.0025908774155117271); + K_result[21] = + (((((l_l * -2.6496231970894191 + l_r * 3.6017440706653381) + t2 * 7.8617729919378752) + + t3 * 5.7342237019208291) + - t4 * 28.61456510643778) + + ((((t5 * 4.0149967654720964 - t6 * 16.666913064436219) + t7 * 10.130713027105751) + - K_result_tmp * 32.792294304764937) + + b_K_result_tmp * 69.573733050802574)) + + ((((c_K_result_tmp * -1.066120573218639 - d_K_result_tmp * 5.2876177445718344) + + e_K_result_tmp * 75.295254409004713) + - f_K_result_tmp * 102.6000526130947) + - 0.1041166910682552); + K_result[22] = + (((((l_l * -1.902702025435336 + l_r * 9.0335048779233453) - t2 * 3.375142801075008) + + t3 * 13.065553621697561) + + t4 * 0.96593307394933925) + + ((((t5 * -45.982059498839661 + t6 * 105.1023659282083) - t7 * 53.227488625637832) + + K_result_tmp * 9.6441441714189047) + + b_K_result_tmp * 4.4540686649446579)) + + ((((c_K_result_tmp * -121.27470177917959 - d_K_result_tmp * 21.4496511806005) + - e_K_result_tmp * 56.922388789445847) + + f_K_result_tmp * 140.14209389526241) + + 0.74742646741770979); + K_result[23] = + (((((l_l * 6.7117464679656473 - l_r * 5.5193433696722964) - t2 * 57.702151660228573) + + t3 * 155.494968036898) + - t4 * 141.534067840227) + + ((((t5 * -15.09975616056647 + t6 * 27.877725762457459) - t7 * 29.852453414222211) + + K_result_tmp * 71.348557470119147) + - b_K_result_tmp * 35.895772934463267)) + + ((((c_K_result_tmp * 30.005199399353721 - d_K_result_tmp * 162.25347557148271) + + e_K_result_tmp * 126.39625164423541) + + f_K_result_tmp * 37.887711347213482) + + 0.097399914303206075); + K_result[24] = + (((((l_l * 7.70073388030568 - l_r * 63.100221178070427) - t2 * 42.877327064547) + - t3 * 38.4557022086672) + + t4 * 148.87403977509661) + + (((((t5 * -41.18911635088039 + t6 * 324.13808597207509) - t7 * 378.27955922157179) + + K_result_tmp * 289.06912083291542) + - b_K_result_tmp * 703.52215034804158) + + c_K_result_tmp * 447.767897697821)) + + (((d_K_result_tmp * 57.978454542837071 - e_K_result_tmp * 367.21431953613057) + + f_K_result_tmp * 403.45336928732809) + + 1.374175360254855); + K_result[25] = + (((((l_l * -34.271583878074168 - l_r * 29.913461143979561) + t2 * 224.19402546780739) + - t3 * 577.62094264252391) + + t4 * 502.25124692438709) + + ((((t5 * 155.78495563762749 - t6 * 258.48412888852408) + t7 * 160.0800391231009) + - K_result_tmp * 276.58644176442073) + - b_K_result_tmp * 85.252780026231719)) + + ((((c_K_result_tmp * 203.49175400523029 + d_K_result_tmp * 783.76146947832626) + - e_K_result_tmp * 547.34695858930672) + - f_K_result_tmp * 241.29651092478079) + + 2.07433587994801); + K_result[26] = + (((((l_l * -32.979566290220269 + l_r * 41.04746633958591) + t2 * 121.2972867871816) + - t3 * 165.1175272875835) + + t4 * 120.3412976409259) + + ((((t5 * -247.11022974725549 + t6 * 193.8453578504097) + t7 * 66.108292240088318) + - K_result_tmp * 105.10267134404209) + + b_K_result_tmp * 767.29896191758928)) + + ((((c_K_result_tmp * -823.89125859187141 - d_K_result_tmp * 199.29290253184371) + + e_K_result_tmp * 60.863480529475737) + + f_K_result_tmp * 63.7339163475523) + + 2.1718492070851041); + K_result[27] = + ((((((l_l * 38.456927557669012 + l_r * 81.7260725318992) - t2 * 309.28313282821739) + + t3 * 915.01988327380718) + - t4 * 903.37988553500691) + - t5 * 58.265221403698348) + + ((((t6 * 50.832569529983488 + t7 * 36.462534269965722) + - K_result_tmp * 33.186333302981957) + + b_K_result_tmp * 103.59863126837951) + - c_K_result_tmp * 233.80049506267329)) + + (((d_K_result_tmp * -327.7715140418897 + e_K_result_tmp * 411.940426952421) + + f_K_result_tmp * 132.4570946986926) + - 2.8976996017412531); + K_result[28] = + (((((l_l * 3.4947139055562189 - l_r * 2.8762091187738559) - t2 * 6.9559169859923893) + - t3 * 14.72910312548712) + + t4 * 32.293937674470307) + + ((((t5 * -30.532879193449141 + t6 * 110.6611830556341) - t7 * 107.3996861918399) + + K_result_tmp * 19.1563631472269) + - b_K_result_tmp * 118.4010600812998)) + + ((((c_K_result_tmp * 74.124175872787333 + d_K_result_tmp * 68.61937763112806) + - e_K_result_tmp * 93.730726689404065) + + f_K_result_tmp * 54.701802340110021) + - 0.095369362848991793); + K_result[29] = + (((((l_l * -8.9520651627639527 - l_r * 0.17863115794247009) + t2 * 36.794761124293309) + - t3 * 67.982046941977146) + + t4 * 38.361112332731089) + + ((((t5 * 5.7870457692441759 + t6 * 19.440243364683951) - t7 * 42.880578263950952) + - K_result_tmp * 17.2861068745997) + - b_K_result_tmp * 43.929952294540293)) + + ((((c_K_result_tmp * 95.026585666370252 + d_K_result_tmp * 40.371332021670732) + + e_K_result_tmp * 23.70081960655245) + - f_K_result_tmp * 74.300378821324685) + - 0.030843619913440531); + K_result[30] = + (((((l_l * -8.3708331033036352 + l_r * 7.5156256422416634) + t2 * 23.093414882983371) + - t3 * 27.864687284415229) + + t4 * 22.877963575065589) + + ((((t5 * -12.965862943889871 - t6 * 24.791983789362181) + t7 * 70.6777074129471) + - K_result_tmp * 0.71903852590290274) + + b_K_result_tmp * 63.651976625924107)) + + ((((c_K_result_tmp * -139.5291109794579 - d_K_result_tmp * 49.29458563771324) + - e_K_result_tmp * 12.05622901625731) + + f_K_result_tmp * 98.436392990512758) + + 0.22966749250505389); + K_result[31] = + (((((l_l * 3.176755727085248 + l_r * 5.1076007224127471) - t2 * 44.906252850069578) + + t3 * 144.3968751091314) + - t4 * 147.06084439492491) + + ((((t5 * -26.48236655128424 + t6 * 19.628061747036789) - t7 * 0.56906371336675921) + + K_result_tmp * 25.01184753100198) + + b_K_result_tmp * 71.93443352924966)) + + ((((c_K_result_tmp * -46.2074855104563 - d_K_result_tmp * 119.4531974180299) + + e_K_result_tmp * 133.14416085501631) + - f_K_result_tmp * 41.812868518699233) + + 0.68308102771958357); + K_result[32] = + (((((l_l * 50.879352779123387 + l_r * 28.194123394531889) - t2 * 179.17147687860131) + + t3 * 335.8254768437061) + - t4 * 255.96761923728531) + + ((((t5 * -13.940709611916819 - t6 * 89.8770543394501) + t7 * 94.728603839377982) + + K_result_tmp * 19.054275951961959) + - b_K_result_tmp * 34.3976853249082)) + + ((((c_K_result_tmp * 148.75291971412329 - d_K_result_tmp * 11.180862497058589) + + e_K_result_tmp * 57.265316055825437) + - f_K_result_tmp * 135.6413818102948) + - 20.172356813422681); + K_result[33] = + (((((l_l * 41.725434886435508 + l_r * 30.947724523191251) + t2 * 65.469048699201679) + - t3 * 445.59273353881468) + + t4 * 536.86429878402407) + + ((((t5 * 21.104942768016031 - t6 * 69.667024191408359) + t7 * 87.138961079062327) + - K_result_tmp * 225.29814265883419) + + b_K_result_tmp * 89.575132174286665)) + + ((((c_K_result_tmp * -158.81970858425419 + d_K_result_tmp * 547.9641799270081) + - e_K_result_tmp * 564.18274613860012) + + f_K_result_tmp * 67.1685069442193) + - 19.79082845756766); + K_result[34] = + ((((((l_l * 91.513990141990732 - l_r * 268.96214357902733) - t2 * 371.596573631106) + + t3 * 677.82105811918188) + - t4 * 465.47011003702318) + + t5 * 953.40595570907612) + + ((((t6 * -1764.92023062041 + t7 * 1356.2015535350381) + + K_result_tmp * 84.348385930053709) + - b_K_result_tmp * 76.4317576112754) + - c_K_result_tmp * 143.2514639033121)) + + (((d_K_result_tmp * -74.915655701766312 - e_K_result_tmp * 79.63131178704613) + + f_K_result_tmp * 244.2021163096536) + - 21.102831555636691); + K_result[35] = + ((((((l_l * -296.87998316188322 + l_r * 126.58401117224869) + t2 * 1115.194104751552) + - t3 * 1983.1793019098129) + + t4 * 1331.6403747020131) + - t5 * 488.45285285093939) + + ((((t6 * 818.203627871081 - t7 * 596.73174649167709) + - K_result_tmp * 4.5910207918667192) + + b_K_result_tmp * 340.31150206254563) + - c_K_result_tmp * 44.069124590005067)) + + (((d_K_result_tmp * -305.7515288221918 + e_K_result_tmp * 594.9891156093762) + - f_K_result_tmp * 466.193593561064) + - 21.47495663926485); + K_result[36] = + (((((l_l * 11.392511706212719 - l_r * 0.019919442643399139) - t2 * 33.036000568074464) + + t3 * 45.229888787307459) + - t4 * 25.83122067217786) + + ((((t5 * 16.0588020701805 - t6 * 34.77543439033996) + t7 * 16.989606928881919) + - K_result_tmp * 11.56065095113656) + - b_K_result_tmp * 18.48752355406614)) + + ((((c_K_result_tmp * 53.984020765081333 + d_K_result_tmp * 48.610017472052647) + - e_K_result_tmp * 26.847077236222429) + - f_K_result_tmp * 43.117210202465273) + - 2.80096795627845); + K_result[37] = + (((((l_l * 1.01868813668573 + l_r * 9.7678472261414786) + t2 * 26.76988568651274) + - t3 * 82.6997890765413) + + t4 * 78.323925407915524) + + ((((t5 * -18.764544497852341 + t6 * 23.7069928773335) - t7 * 10.29607919916687) + - K_result_tmp * 33.450195169068948) + + b_K_result_tmp * 46.312109913360793)) + + ((((c_K_result_tmp * -42.248576904462567 + d_K_result_tmp * 46.837783829110229) + - e_K_result_tmp * 35.978866730761993) + - f_K_result_tmp * 10.2048920756438) + - 2.7648775417528082); + K_result[38] = + (((((l_l * 5.3394416391660284 - l_r * 30.305337343897559) - t2 * 30.916161492834991) + + t3 * 66.423979189730787) + - t4 * 47.586725416788049) + + ((((t5 * 89.827930646733819 - t6 * 149.86163683193411) + t7 * 114.8244343538348) + + K_result_tmp * 35.053067473828122) + - b_K_result_tmp * 39.908343172898817)) + + ((((c_K_result_tmp * -19.72862085458355 - d_K_result_tmp * 52.955773659738348) + + e_K_result_tmp * 4.9336162031679249) + + f_K_result_tmp * 78.971143997122454) + - 1.784280298872104); + K_result[39] = + (((((l_l * -33.840444163836992 + l_r * 10.12146590899663) + t2 * 106.1588170712585) + - t3 * 165.62368999517739) + + t4 * 101.0068447724255) + + ((((t5 * -47.854596393644528 + t6 * 93.337432454580949) - t7 * 73.1422620631561) + + K_result_tmp * 28.202553361804579) + - b_K_result_tmp * 19.579641636014081)) + + ((((c_K_result_tmp * 20.28076139526188 - d_K_result_tmp * 66.326812404169672) + + e_K_result_tmp * 64.8711496264674) + + f_K_result_tmp * 2.9656492393058351) + - 1.853469249301628); + + gain_ = Eigen::Map>(K_result); + } + + Eigen::Matrix gain_; +}; +} // namespace rmcs_core::controller::lqr \ No newline at end of file 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..ab5aed7f --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_controller.hpp @@ -0,0 +1,157 @@ + +// #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 +// 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::chassis \ No newline at end of file From 6947cbf5e93fbbfc1a230d1ee4117279f658715e Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Mon, 8 Dec 2025 03:35:32 +0800 Subject: [PATCH 19/35] Update hardware of wheeleg-infantry, rename the files' name to `wheelleg-infantry`. --- ...g_infantry.yaml => wheelleg-infantry.yaml} | 28 +-- rmcs_ws/src/rmcs_core/plugins.xml | 5 +- .../controller/chassis/wheel_leg/observer.cpp | 225 ------------------ ...leg_infantry.cpp => wheelleg-infantry.cpp} | 26 +- 4 files changed, 33 insertions(+), 251 deletions(-) rename rmcs_ws/src/rmcs_bringup/config/{wheelleg_infantry.yaml => wheelleg-infantry.yaml} (75%) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp rename rmcs_ws/src/rmcs_core/src/hardware/{wheelleg_infantry.cpp => wheelleg-infantry.cpp} (96%) diff --git a/rmcs_ws/src/rmcs_bringup/config/wheelleg_infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml similarity index 75% rename from rmcs_ws/src/rmcs_bringup/config/wheelleg_infantry.yaml rename to rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml index 243e722f..316ace41 100644 --- a/rmcs_ws/src/rmcs_bringup/config/wheelleg_infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml @@ -4,24 +4,24 @@ rmcs_executor: components: - rmcs_core::hardware::WheelLegInfantry -> infantry_hardware - - rmcs_core::referee::Status -> referee_status - - rmcs_core::referee::Command -> referee_command + # - 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::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::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::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::ChassisController -> chassis_controller - - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - # - rmcs_core::controller::chassis::WheellegController -> wheel_leg_controller + # - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller + - rmcs_core::controller::chassis::WheelLegController -> wheel_leg_controller # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller @@ -29,7 +29,7 @@ rmcs_executor: infantry_hardware: ros__parameters: usb_pid_top_board: 0x93ac - usb_pid_bottom_board: 0x488d + usb_pid_bottom_board: 0xf30c yaw_motor_zero_point: 32285 pitch_motor_zero_point: 6321 diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 0ab75304..6f60f99f 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -17,6 +17,9 @@ Test plugin. + + Test plugin. + Test plugin. @@ -30,7 +33,7 @@ Steering wheel controller. - Test plugin. + Wheel leg controller. Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp deleted file mode 100644 index 532e1f11..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/observer.cpp +++ /dev/null @@ -1,225 +0,0 @@ -#include -#include -#include -#include -#include - -#include "filter/kalman_filter.hpp" -#include "vmc_solver.hpp" - -namespace rmcs_core::controller::chassis { -class Observer - : public rmcs_executor::Component - , public rclcpp::Node { -public: - Observer() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , body_mess_(15.0) - , leg_mess_(0.86) - , wheel_radius_(0.06) - , wheel_distance_(0.2135) - , centroid_position_coefficient_() - , 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_, W_) { - - 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_wheel/velocity", left_wheel_velocity_); - register_input("/chassis/right_wheel/velocity", right_wheel_velocity_); - - register_input("/chassis/x/acceleration", chassis_x_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_leg_length", left_leg_length_); - register_output("chassis/right_leg_length", right_leg_length_); - register_output("/chassis/measure_state", measure_state_); - } - - void update() override { - // state vector: s ds phi dphi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b - auto wheel_velocities = calculate_wheel_velocities(); - auto hip_angles = calculate_hips_angles(); - - auto leg_posture = calculate_leg_posture(hip_angles); - auto distance = calculate_translational_distance(leg_posture, wheel_velocities); - - auto support_force = calculate_support_force(leg_posture); - - *measure_state_ = calculate_measure_state(distance, leg_posture); - } - -private: - struct LegPosture { - Eigen::Vector2d leg_length; - Eigen::Vector2d tilt_angle; - - Eigen::Vector2d dot_leg_length; - Eigen::Vector2d dot_tilt_angle; - - Eigen::Vector2d second_order_diff_leg_length; - Eigen::Vector2d second_order_diff_tilt_angle; - }; - - 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 &lf = hip_angles.x(), &lb = hip_angles.y(), &rb = hip_angles.z(), - &rf = hip_angles.w(); - - 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}; - - *left_leg_length_ = left_leg_length; - *right_leg_length_ = right_leg_length; - - result.dot_leg_length = (result.leg_length - last_leg_length_) / dt_; - last_leg_length_ = result.leg_length; - result.dot_tilt_angle = (result.tilt_angle - last_tilt_angle_) / dt_; - last_tilt_angle_ = result.tilt_angle; - - result.second_order_diff_leg_length = (result.dot_leg_length - last_dot_leg_length_) / dt_; - last_dot_leg_length_ = result.dot_leg_length; - result.second_order_diff_tilt_angle = (result.dot_tilt_angle - last_dot_tilt_angle_) / dt_; - last_dot_tilt_angle_ = result.dot_tilt_angle; - - return result; - } - - Eigen::Vector2d - calculate_translational_distance(LegPosture leg_posture, Eigen::Vector2d wheel_velocities) { - std::tuple result{}; - auto& [distance, velocity] = result; - - auto wheel_velocity = (wheel_velocities.x() + wheel_velocities.y()) / 2.0; - leg_posture.tilt_angle.array() += *chassis_pitch_angle_imu_; - - auto left_leg_velocity = - leg_posture.leg_length.x() * std::cos(leg_posture.tilt_angle.x()) - * leg_posture.dot_tilt_angle.x() - + leg_posture.dot_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.dot_tilt_angle.y() - + leg_posture.dot_leg_length.y() - + std::sin(leg_posture.tilt_angle.y()); - - auto calculate_velocity = wheel_velocity + (left_leg_velocity + right_leg_velocity) / 2.0; - - auto estimate_velocity = velocity_kalman_filter_.update( - Eigen::Vector2d{calculate_velocity, *chassis_x_acceleration_imu_}); - - velocity = estimate_velocity.x(); - distance += velocity * dt_; - - return Eigen::Vector2d{}; - } - - Eigen::Vector2d calculate_support_force(LegPosture leg_posture) {} - - 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.dot_tilt_angle.x(); - - measure_state.right_tilt_angle = leg_posture.tilt_angle.y(); - measure_state.right_tilt_velocity = leg_posture.dot_tilt_angle.y(); - - measure_state.body_pitch_angle = *chassis_pitch_angle_imu_; - measure_state.body_pitch_velocity = *chassis_pitch_velocity_imu_; - - return measure_state; - } - - static constexpr double dt_ = 1e-3; - static constexpr double g_ = 9.80665; - - 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::Matrix2d() << 1.0, 0.0, 0.0, 1.0).finished(); - const Eigen::Matrix2d W_ = (Eigen::Matrix2d() << 0.5 * dt_ * dt_, 0.0, 0.0, dt_).finished(); - const Eigen::Matrix2d Q_ = - (Eigen::Matrix2d() << sigma_q_ * sigma_q_, 0.0, 0.0, sigma_q_* sigma_q_).finished(); - const Eigen::Matrix2d R_ = - (Eigen::Matrix2d() << sigma_v_ * sigma_v_, 0.0, 0.0, sigma_a_* sigma_a_).finished(); - - const double body_mess_; - const double leg_mess_; - const double wheel_radius_; - const double wheel_distance_; - const double centroid_position_coefficient_; - - InputInterface left_front_hip_angle_; - InputInterface left_back_hip_angle_; - InputInterface right_front_hip_angle_; - InputInterface right_back_hip_angle_; - - InputInterface left_wheel_velocity_; - InputInterface right_wheel_velocity_; - - InputInterface chassis_x_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_; - - OutputInterface left_leg_length_; - OutputInterface right_leg_length_; - - OutputInterface measure_state_; - - Eigen::Vector2d last_leg_length_, last_tilt_angle_, last_dot_leg_length_, last_dot_tilt_angle_; - double last_left_leg_length_, last_right_leg_length_; - - VmcSolver left_leg_vmc_solver_, right_leg_vmc_solver_; - filter::KalmanFilter<2, 2> velocity_kalman_filter_; -}; -} // namespace rmcs_core::controller::chassis -#include - -PLUGINLIB_EXPORT_CLASS(rmcs_core::controller::chassis::Observer, rmcs_executor::Component) \ 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 similarity index 96% rename from rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp rename to rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index 60ee7aa9..6c9a124f 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg_infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -25,9 +25,9 @@ class WheelLegInfantry 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())); + // 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())); @@ -43,18 +43,18 @@ class WheelLegInfantry }); } - void update() override {} + void update() override { bottom_board_->update(); } - void command_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()); + // 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) {} @@ -247,7 +247,7 @@ class WheelLegInfantry // 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); + return std::make_tuple(x, y, z); }); infantry.register_output("/referee/serial", referee_serial_); @@ -447,10 +447,14 @@ class WheelLegInfantry std::shared_ptr infantry_command_; - std::unique_ptr top_board_; + // 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 \ No newline at end of file +} // namespace rmcs_core::hardware + +#include + +PLUGINLIB_EXPORT_CLASS(rmcs_core::hardware::WheelLegInfantry, rmcs_executor::Component) \ No newline at end of file From f8af8c45ee02f941c651892d8e98778c07140a33 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Thu, 11 Dec 2025 05:47:19 +0800 Subject: [PATCH 20/35] Update hardware support of `DmMotor`. --- .../rmcs_core/src/hardware/device/dm_motor.hpp | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) 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 index a646b2dd..834424d6 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp @@ -36,6 +36,8 @@ class DmMotor : public librmcs::device::DmMotor { *angle_ = angle(); *velocity_ = velocity(); *torque_ = torque(); + + motor_state_ = error_state(); } double control_torque() { @@ -61,13 +63,24 @@ class DmMotor : public librmcs::device::DmMotor { return librmcs::device::DmMotor::generate_disable_command(); } + uint64_t generate_command() { + if (motor_state_ == 0) { + return generate_enable_command(); + } else if (motor_state_ == 1) { + 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::OutputInterface rotor_temperature_; rmcs_executor::Component::InputInterface control_torque_; + + uint8_t motor_state_; }; } // namespace rmcs_core::hardware::device \ No newline at end of file From 5c8ffd3fb3fef40248e5412ec15f1350d939be75 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Thu, 11 Dec 2025 05:48:48 +0800 Subject: [PATCH 21/35] The VMC solution works well. --- .../config/wheelleg-infantry.yaml | 2 + .../chassis/wheel_leg/desire_state_solver.hpp | 50 +++ .../chassis/wheel_leg/vmc_solver.hpp | 19 +- .../wheel_leg/wheel_leg_controller.cpp | 88 +++-- .../src/controller/lqr/lqr_controller.hpp | 332 ++++++++++-------- .../src/hardware/wheelleg-infantry.cpp | 57 ++- 6 files changed, 325 insertions(+), 223 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_solver.hpp diff --git a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml index 316ace41..daad82eb 100644 --- a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml @@ -32,6 +32,8 @@ infantry_hardware: usb_pid_bottom_board: 0xf30c yaw_motor_zero_point: 32285 pitch_motor_zero_point: 6321 + left_front_hip_motor_zero_point: 14944 + left_back_hip_motor_zero_point: 28913 yaw_angle_pid_controller: ros__parameters: 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..9c9a19b1 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_solver.hpp @@ -0,0 +1,50 @@ + +#include "rmcs_msgs/wheel_leg_state.hpp" + +namespace rmcs_core::controller::chassis { + +class DesireStateSolver { +public: + explicit DesireStateSolver(double desire_leg_length) + : desire_leg_length_(desire_leg_length) {} + + rmcs_msgs::WheelLegState update(const double& vx, const double& vz, const double& wz) { + desire_leg_length_ += vz * leg_length_sensitivity_; + + // distance :always 0 during velocity control + desire_state_.distance = 0; + desire_state_.velocity = vx; + + // yaw angle: + desire_state_.yaw_angle = 0; + desire_state_.yaw_velocity = wz; + + return desire_state_; + } + + // void update() + + void update_current_distance() {} + + void update_current_yaw_angle() {} + + double update_desire_leg_length() const { return desire_leg_length_; } + + double update_desire_roll_angle() const { return desire_roll_angle_; } + +private: + void reset_all_controls() { + desire_leg_length_ = 0.15; + desire_roll_angle_ = 0.0; + } + + constexpr static double leg_length_sensitivity_ = 0.0003; + + constexpr static double dt_ = 0.001; + + double desire_leg_length_; + double desire_roll_angle_; + + rmcs_msgs::WheelLegState desire_state_; +}; +} // namespace rmcs_core::controller::chassis \ 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 index 4416d1ce..f5e3861f 100644 --- 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 @@ -2,7 +2,6 @@ #include #include -#include namespace rmcs_core::controller::chassis { class VmcSolver { @@ -45,12 +44,12 @@ class VmcSolver { 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((xb - xd) * (xb - xd) + (yd - yb) * (yd - yb)); + auto lbd = std::sqrt((xd - xb) * (xd - xb) + (yd - yb) * (yd - yb)); - auto a = 2 * l2_ * (xd - xb), b = 2 * l2_ * (yb - yd), + 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)), + 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), @@ -64,8 +63,8 @@ class VmcSolver { 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 - phi4), - j21 = l1_ * std::cos(phi0 - phi2) * std::sin(phi1 - 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)); @@ -75,12 +74,10 @@ class VmcSolver { {j21, j22} }; auto rotation_matrix = Eigen::Rotation2Dd(phi0 - pi_ / 2.0); - auto transform_matrix = Eigen::Matrix2d{ - {0, -1 / leg_length_}, - {1, 0} - }; + Eigen::Matrix2d transform_matrix = Eigen::Vector2d{-1 / leg_length_, 1}.asDiagonal(); - joint_torque_matrix_ = jacobian_matrix.transpose() * rotation_matrix * transform_matrix; + joint_torque_matrix_ = + jacobian_matrix.transpose() * rotation_matrix * transform_matrix.transpose(); } Eigen::Vector2d get_leg_posture() const { return Eigen::Vector2d{leg_length_, tilt_angle_}; } 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 index b3cb8313..fb217168 100644 --- 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 @@ -1,11 +1,15 @@ #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 "rmcs_executor/component.hpp" #include "vmc_solver.hpp" @@ -20,18 +24,21 @@ class WheelLegController get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) , body_mess_(15.0) - , leg_mess_(0.86) - , wheel_mess_() + , leg_mess_(0.629) + , wheel_mess_(0.459) , wheel_radius_(0.06) , wheel_distance_(0.2135) , centroid_position_coefficient_() , 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_, W_) + , desire_state_solver_(0.15) , roll_angle_pid_calculator_(0.0, 0.0, 0.0) , leg_length_pid_calculator_(0.0, 0.0, 0.0) { register_input("/chassis/control_velocity", chassis_control_velocity_); + register_input("/chassis/control_mode", mode_); + // register_input("/chassis/control_power_limit", power_limit_); register_input("/chassis/left_front_hip/angle", left_front_hip_angle_); register_input("/chassis/left_back_hip/angle", left_back_hip_angle_); @@ -67,10 +74,11 @@ class WheelLegController } void update() override { - if (std::isnan(chassis_control_velocity_->vector[0])) { - reset_all_controls(); - return; - } + // if (std::isnan(chassis_control_velocity_->vector[0])) { + // reset_all_controls(); + // return; + // } + // Observer auto wheel_velocities = calculate_wheel_velocities(); auto hip_angles = calculate_hips_angles(); @@ -78,7 +86,7 @@ class WheelLegController auto leg_posture = calculate_leg_posture(hip_angles); auto distance = calculate_translational_distance(leg_posture, wheel_velocities); - auto support_force = calculate_support_force(leg_posture); + // auto support_force = calculate_support_force(leg_posture); // State: [s ds phi dphi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b] auto measure_state = calculate_measure_state(distance, leg_posture); @@ -88,9 +96,11 @@ class WheelLegController auto desire_state = calculate_desire_state(chassis_control_velocity); auto leg_forces = calculate_leg_force(leg_posture, measure_state); - auto control_torques = calculate_control_torques(desire_state, measure_state); + auto control_torques = calculate_control_torques(desire_state, measure_state, leg_posture); update_hip_and_wheel_torques(leg_forces, control_torques); + + reset_all_controls(); } private: @@ -122,16 +132,18 @@ class WheelLegController } Eigen::Vector4d calculate_hips_angles() { + auto norm_angle = [](double angle) { return 2 * pi_ - angle; }; + return { - *left_front_hip_angle_, // - *left_back_hip_angle_, // - *right_front_hip_angle_, // - *right_back_hip_angle_, // + norm_angle(*left_front_hip_angle_), // + norm_angle(*left_back_hip_angle_), // + norm_angle(*right_back_hip_angle_), // + norm_angle(*right_front_hip_angle_), // }; } LegPosture calculate_leg_posture(const Eigen::Vector4d& hip_angles) { - const auto& [lf, lb, rf, rb] = 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); @@ -198,6 +210,8 @@ class WheelLegController return result; } + void detect_chassis_leave_the_ground() {} + Eigen::Vector2d calculate_translational_distance(LegPosture leg_posture, Eigen::Vector2d wheel_velocities) { Eigen::Vector2d result; @@ -251,15 +265,18 @@ class WheelLegController Eigen::Vector3d calculate_chassis_control_velocity() { Eigen::Vector3d chassis_control_velocity; - chassis_control_velocity.x() = chassis_control_velocity_->vector.x(); - chassis_control_velocity.y() = 0.0; - chassis_control_velocity.z() = chassis_control_velocity_->vector.z(); + + chassis_control_velocity = chassis_control_velocity_->vector; + return chassis_control_velocity; } rmcs_msgs::WheelLegState calculate_desire_state(Eigen::Vector3d control_velocity) { rmcs_msgs::WheelLegState desire_state; - auto& [vx, vy, wz] = control_velocity; + + // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity + auto& [vx, vz, wz] = control_velocity; + return desire_state; } @@ -267,16 +284,16 @@ class WheelLegController calculate_leg_force(LegPosture leg_posture, rmcs_msgs::WheelLegState measure_state) { Eigen::Vector2d result; - auto roll_feedforward = roll_angle_pid_calculator_.update(0 - *chassis_roll_angle_imu_); - auto leg_length_feedforward = leg_length_pid_calculator_.update( + auto roll_control_force = roll_angle_pid_calculator_.update(0 - *chassis_roll_angle_imu_); + auto leg_length_control_force = leg_length_pid_calculator_.update( leg_length_desire_ - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); - auto calculate_compensation_feedforward = [this](double coefficient) { + auto calculate_compensation_feedforward_force = [this](double coefficient) { return (body_mess_ / 2.0 + centroid_position_coefficient_ * leg_mess_) * coefficient; }; - auto gravity_feedforward = calculate_compensation_feedforward(g_); - auto inertial_feedforward = calculate_compensation_feedforward( + auto gravity_feedforward_control_force = calculate_compensation_feedforward_force(g_); + auto inertial_feedforward_control_force = calculate_compensation_feedforward_force( (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0 / (2 * wheel_distance_) * measure_state.yaw_velocity * measure_state.velocity); @@ -285,18 +302,23 @@ class WheelLegController // F_bl_r -1 1 1 1 F_g // F_i result = Eigen::Vector2d{ - roll_feedforward + leg_length_feedforward + gravity_feedforward - inertial_feedforward, - -roll_feedforward + leg_length_feedforward + gravity_feedforward - + inertial_feedforward}; + 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) { + rmcs_msgs::WheelLegState desire_state, rmcs_msgs::WheelLegState measure_state, + LegPosture leg_posture) { Eigen::Vector4d result; auto error_state = desire_state.vector() - measure_state.vector(); + auto gain = lqr_calculator_.update(leg_posture.leg_length.x(), leg_posture.leg_length.y()); + auto test_result = gain * error_state; return result; } @@ -323,6 +345,8 @@ class WheelLegController 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; @@ -331,12 +355,12 @@ class WheelLegController static constexpr double sigma_a_ = 1.0; const Eigen::Matrix2d A_ = (Eigen::Matrix2d() << 1, dt_, 1, 1).finished(); - const Eigen::Matrix2d H_ = (Eigen::Matrix2d() << 1.0, 0.0, 0.0, 1.0).finished(); - const Eigen::Matrix2d W_ = (Eigen::Matrix2d() << 0.5 * dt_ * dt_, 0.0, 0.0, dt_).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::Matrix2d() << sigma_q_ * sigma_q_, 0.0, 0.0, sigma_q_* sigma_q_).finished(); + Eigen::Vector2d{sigma_q_ * sigma_q_, sigma_q_* sigma_q_}.asDiagonal(); const Eigen::Matrix2d R_ = - (Eigen::Matrix2d() << sigma_v_ * sigma_v_, 0.0, 0.0, sigma_a_* sigma_a_).finished(); + Eigen::Vector2d{sigma_v_ * sigma_v_, sigma_a_* sigma_a_}.asDiagonal(); const double body_mess_; const double leg_mess_; @@ -371,6 +395,7 @@ class WheelLegController InputInterface chassis_control_velocity_; InputInterface power_limit_; + InputInterface mode_; OutputInterface left_front_hip_control_torque_; OutputInterface left_back_hip_control_torque_; @@ -387,7 +412,10 @@ class WheelLegController VmcSolver left_leg_vmc_solver_, right_leg_vmc_solver_; filter::KalmanFilter<2, 2> velocity_kalman_filter_; + DesireStateSolver desire_state_solver_; + pid::PidCalculator roll_angle_pid_calculator_, leg_length_pid_calculator_; + lqr::LqrCalculator lqr_calculator_; }; } // namespace rmcs_core::controller::chassis 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 index ab5aed7f..15f9ea88 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_controller.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_controller.hpp @@ -1,157 +1,183 @@ -// #include -// #include -// #include -// // #include -// #include +#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 -// 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::chassis \ No newline at end of file +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/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index 6c9a124f..f941b3e6 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -57,7 +57,20 @@ class WheelLegInfantry // top_board_->gimbal_pitch_motor_.calibrate_zero_point()); } - void chassis_hip_calibrate_subscription_callback(std_msgs::msg::Int32::UniquePtr) {} + 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) @@ -207,17 +220,20 @@ class WheelLegInfantry {infantry, infantry_command, "/chassis/left_wheel", device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reduction_ratio(13.0) - .enable_multi_turn_angle() - .set_reversed()}, + .enable_multi_turn_angle()}, {infantry, infantry_command, "/chassis/right_wheel", device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reduction_ratio(13.0) .enable_multi_turn_angle()}) , chassis_hip_motors( {infantry, infantry_command, "/chassis/left_front_hip", - device::DmMotor::Config{device::DmMotor::Type::DM8009}}, + device::DmMotor::Config{device::DmMotor::Type::DM8009}.set_encoder_zero_point( + static_cast( + infantry.get_parameter("left_front_hip_motor_zero_point").as_int()))}, {infantry, infantry_command, "/chassis/left_back_hip", - device::DmMotor::Config{device::DmMotor::Type::DM8009}}, + device::DmMotor::Config{device::DmMotor::Type::DM8009}.set_encoder_zero_point( + static_cast( + infantry.get_parameter("left_back_hip_motor_zero_point").as_int()))}, {infantry, infantry_command, "/chassis/right_front_hip", device::DmMotor::Config{device::DmMotor::Type::DM8009}}, {infantry, infantry_command, "/chassis/right_back_hip", @@ -288,6 +304,9 @@ class WheelLegInfantry 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()); @@ -309,30 +328,10 @@ class WheelLegInfantry 0x141, gimbal_yaw_motor_.generate_torque_command(gimbal_yaw_motor_.control_torque())); - if (!is_hips_enable_) { - transmit_buffer_.add_can2_transmission( - 0x01, chassis_hip_motors[0].generate_enable_command()); - transmit_buffer_.add_can2_transmission( - 0x02, chassis_hip_motors[1].generate_enable_command()); - transmit_buffer_.add_can2_transmission( - 0x03, chassis_hip_motors[2].generate_enable_command()); - transmit_buffer_.add_can2_transmission( - 0x04, chassis_hip_motors[3].generate_enable_command()); - is_hips_enable_ = !is_hips_enable_; - } else { - transmit_buffer_.add_can2_transmission( - 0x01, chassis_hip_motors[0].generate_torque_command( - chassis_hip_motors[0].control_torque())); - transmit_buffer_.add_can2_transmission( - 0x02, chassis_hip_motors[1].generate_torque_command( - chassis_hip_motors[1].control_torque())); - transmit_buffer_.add_can2_transmission( - 0x03, chassis_hip_motors[2].generate_torque_command( - chassis_hip_motors[2].control_torque())); - transmit_buffer_.add_can2_transmission( - 0x04, chassis_hip_motors[3].generate_torque_command( - chassis_hip_motors[3].control_torque())); - } + transmit_buffer_.add_can2_transmission(0x01, chassis_hip_motors[0].generate_command()); + transmit_buffer_.add_can2_transmission(0x02, chassis_hip_motors[1].generate_command()); + 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(); } From 87715ea27672e3b9b0b3158650bf88e290495842 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Fri, 12 Dec 2025 08:01:18 +0800 Subject: [PATCH 22/35] Update chassis hips calibration|Update `wheelleg-infantry` hardware|Adjust can bus allocation --- .../config/wheelleg-infantry.yaml | 2 + .../src/hardware/device/dm_motor.hpp | 8 ++-- .../src/hardware/wheelleg-infantry.cpp | 47 ++++++++++++------- 3 files changed, 35 insertions(+), 22 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml index daad82eb..1e45a1ed 100644 --- a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml @@ -34,6 +34,8 @@ infantry_hardware: pitch_motor_zero_point: 6321 left_front_hip_motor_zero_point: 14944 left_back_hip_motor_zero_point: 28913 + right_back_hip_motor_zero_point: 12639 + right_front_hip_motor_zero_point: 8753 yaw_angle_pid_controller: ros__parameters: 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 index 834424d6..73073279 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp @@ -55,12 +55,12 @@ class DmMotor : public librmcs::device::DmMotor { return librmcs::device::DmMotor::generate_clear_error_command(); } - uint64_t generate_torque_command(double control_torque) { - return librmcs::device::DmMotor::generate_torque_command(control_torque); + constexpr static uint64_t generate_disable_command() { + return librmcs::device::DmMotor::generate_disable_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_command() { diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index f941b3e6..41d6870a 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -234,10 +234,18 @@ class WheelLegInfantry device::DmMotor::Config{device::DmMotor::Type::DM8009}.set_encoder_zero_point( static_cast( infantry.get_parameter("left_back_hip_motor_zero_point").as_int()))}, - {infantry, infantry_command, "/chassis/right_front_hip", - device::DmMotor::Config{device::DmMotor::Type::DM8009}}, {infantry, infantry_command, "/chassis/right_back_hip", - device::DmMotor::Config{device::DmMotor::Type::DM8009}}) + device::DmMotor::Config{device::DmMotor::Type::DM8009} + .set_encoder_zero_point( + static_cast( + infantry.get_parameter("right_back_hip_motor_zero_point").as_int())) + .set_reversed()}, + {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())) + .set_reversed()}) , gimbal_yaw_motor_( infantry, infantry_command, "/gimbal/yaw", device::LkMotor::Config{device::LkMotor::Type::MG4010E_I10} @@ -328,10 +336,11 @@ class WheelLegInfantry 0x141, gimbal_yaw_motor_.generate_torque_command(gimbal_yaw_motor_.control_torque())); + transmit_buffer_.add_can1_transmission(0x03, chassis_hip_motors[2].generate_command()); + transmit_buffer_.add_can1_transmission(0x04, chassis_hip_motors[3].generate_command()); + transmit_buffer_.add_can2_transmission(0x01, chassis_hip_motors[0].generate_command()); transmit_buffer_.add_can2_transmission(0x02, chassis_hip_motors[1].generate_command()); - 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(); } @@ -363,14 +372,21 @@ class WheelLegInfantry 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 == 0x202) { - chassis_wheel_motors_[1].store_status(can_data); - } else if (can_id == 0x203) { - bullet_feeder_motor_.store_status(can_data); - } else if (can_id == 0x141) { - gimbal_yaw_motor_.store_status(can_data); + ; + + // if (can_id == 0x201) { + // chassis_wheel_motors_[0].store_status(can_data); + // } else if (can_id == 0x202) { + // chassis_wheel_motors_[1].store_status(can_data); + // } else if (can_id == 0x203) { + // bullet_feeder_motor_.store_status(can_data); + // } else if (can_id == 0x141) { + // gimbal_yaw_motor_.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); } } @@ -384,10 +400,6 @@ class WheelLegInfantry chassis_hip_motors[0].store_status(can_data); } else if (can_id == 0x02) { chassis_hip_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); } } @@ -427,7 +439,6 @@ class WheelLegInfantry device::DjiMotor chassis_wheel_motors_[2]; - bool is_hips_enable_{false}; device::DmMotor chassis_hip_motors[4]; device::LkMotor gimbal_yaw_motor_; From 4693f55fc9432dd03bf486e13809b6548e1250a4 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sun, 21 Dec 2025 14:35:57 +0800 Subject: [PATCH 23/35] Revise vmc solution. --- .../chassis/wheel_leg/vmc_solver.hpp | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) 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 index f5e3861f..193b9aff 100644 --- 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 @@ -2,6 +2,7 @@ #include #include +#include namespace rmcs_core::controller::chassis { class VmcSolver { @@ -13,6 +14,11 @@ class VmcSolver { , l4_(l1) , l5_(l5) {} + void reset() { + tilt_angle_ = nan_; + leg_length_ = nan_; + } + Eigen::Vector2d update(double phi1, double phi4) { if (std::isnan(phi1) || std::isnan(phi4)) { reset(); @@ -24,20 +30,15 @@ class VmcSolver { return get_leg_posture(); } - Eigen::Vector2d get_joint_torque(double F, double Tp) { + Eigen::Vector2d update_joint_torque(double F, double Tp) { return joint_torque_matrix_ * Eigen::Vector2d{F, Tp}; } - Eigen::Vector2d get_virtual_torque(double T1, double T2) { + Eigen::Vector2d update_virtual_torque(double T1, double T2) { return joint_torque_matrix_.inverse() * Eigen::Vector2d{T1, T2}; } private: - void reset() { - tilt_angle_ = nan_; - leg_length_ = nan_; - } - 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 @@ -74,10 +75,12 @@ class VmcSolver { {j21, j22} }; auto rotation_matrix = Eigen::Rotation2Dd(phi0 - pi_ / 2.0); - Eigen::Matrix2d transform_matrix = Eigen::Vector2d{-1 / leg_length_, 1}.asDiagonal(); + auto transform_matrix = Eigen::Matrix2d{ + {0, -1 / leg_length_}, + {1, 0} + }; - joint_torque_matrix_ = - jacobian_matrix.transpose() * rotation_matrix * transform_matrix.transpose(); + joint_torque_matrix_ = jacobian_matrix.transpose() * rotation_matrix * transform_matrix; } Eigen::Vector2d get_leg_posture() const { return Eigen::Vector2d{leg_length_, tilt_angle_}; } From a4b5160645731c187a410da94ff591c8f35a1a0e Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sun, 21 Dec 2025 14:36:39 +0800 Subject: [PATCH 24/35] Revise kalman filter calculation. --- .../rmcs_core/src/filter/kalman_filter.hpp | 52 ++++++++++++------- 1 file changed, 33 insertions(+), 19 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp index fc08912e..4256d020 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp @@ -16,36 +16,50 @@ class KalmanFilter { using Control = Eigen::Vector; public: - KalmanFilter() {} - - KalmanFilter( + explicit KalmanFilter( const Matrix& A, const Matrix& H, const Matrix& Q, - const Matrix& R, const Matrix& W = Matrix::Identity(), - const Matrix& V = Matrix::Identity(), - const Matrix& B = Matrix::Zero()) - : measurement_noise_transition_(V) - , measurement_noise_covariance_(R) + const Matrix& R) + : measurement_noise_covariance_(R) , state_transition_(A) - , process_noise_transition_(W) , process_noise_covariance_(Q) - , control_input_(B) , measurement_transition_(H) { reset(); + + set_input_transition(); + set_error_covariance(); + set_process_noise_transition(); + set_measurement_noise_transition(); } void reset() { - posterior_estimate_ = State::Zero(); prior_estimate_ = State::Zero(); - - posterior_error_covariance_ = 1000.0 * Matrix::Identity(); - prior_error_covariance_ = posterior_error_covariance_; + 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_ + control_input_ * control_vector; + 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_ @@ -58,13 +72,13 @@ class KalmanFilter { + measurement_noise_transition_ * measurement_noise_covariance_ * measurement_noise_transition_.transpose()) .inverse(); - posterior_estimate_ += - kalman_gain_ * (measurement - measurement_transition_ * prior_estimate_); + prior_estimate_ += kalman_gain_ * (measurement - measurement_transition_ * prior_estimate_); - // Update + // // Update posterior_error_covariance_ = (Matrix::Identity() - kalman_gain_ * measurement_transition_) * prior_error_covariance_; + posterior_estimate_ = prior_estimate_; return posterior_estimate_; } @@ -77,7 +91,7 @@ class KalmanFilter { prior_error_covariance_, posterior_error_covariance_; Matrix kalman_gain_; - Matrix control_input_; + Matrix input_transition_; Matrix measurement_transition_; }; From de1d5faff3a6ccd946efc32839c842fb781b2664 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sun, 21 Dec 2025 14:40:08 +0800 Subject: [PATCH 25/35] Update wheelleg-infantry hardware. --- .../src/hardware/wheelleg-infantry.cpp | 58 ++++++++----------- 1 file changed, 25 insertions(+), 33 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index 41d6870a..107ab2fb 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -49,9 +49,9 @@ class WheelLegInfantry 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 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()); @@ -220,7 +220,8 @@ class WheelLegInfantry {infantry, infantry_command, "/chassis/left_wheel", device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reduction_ratio(13.0) - .enable_multi_turn_angle()}, + .enable_multi_turn_angle() + .set_reversed()}, {infantry, infantry_command, "/chassis/right_wheel", device::DjiMotor::Config{device::DjiMotor::Type::M3508} .set_reduction_ratio(13.0) @@ -326,21 +327,19 @@ class WheelLegInfantry uint16_t control_commands[4]{}; control_commands[0] = chassis_wheel_motors_[0].generate_command(); - control_commands[1] = chassis_wheel_motors_[1].generate_command(); - control_commands[3] = bullet_feeder_motor_.generate_command(); - transmit_buffer_.add_can1_transmission( 0x200, std::bit_cast(control_commands)); - transmit_buffer_.add_can1_transmission( - 0x141, - gimbal_yaw_motor_.generate_torque_command(gimbal_yaw_motor_.control_torque())); + transmit_buffer_.add_can1_transmission(0x01, chassis_hip_motors[0].generate_command()); + transmit_buffer_.add_can1_transmission(0x02, chassis_hip_motors[1].generate_command()); - transmit_buffer_.add_can1_transmission(0x03, chassis_hip_motors[2].generate_command()); - transmit_buffer_.add_can1_transmission(0x04, chassis_hip_motors[3].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(0x01, chassis_hip_motors[0].generate_command()); - transmit_buffer_.add_can2_transmission(0x02, chassis_hip_motors[1].generate_command()); + 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(); } @@ -372,21 +371,12 @@ class WheelLegInfantry 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 == 0x202) { - // chassis_wheel_motors_[1].store_status(can_data); - // } else if (can_id == 0x203) { - // bullet_feeder_motor_.store_status(can_data); - // } else if (can_id == 0x141) { - // gimbal_yaw_motor_.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); + 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); } } @@ -396,10 +386,12 @@ class WheelLegInfantry if (is_extended_can_id || is_remote_transmission || can_data_length < 8) [[unlikely]] return; - 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); + 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); } } From 8d0f2cea82fea660021f08f4c591d0de948a076a Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Tue, 20 Jan 2026 03:55:09 +0800 Subject: [PATCH 26/35] Update angle control of DmMotor drive. --- .../src/hardware/device/dm_motor.hpp | 24 ++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) 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 index 73073279..804dff32 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp @@ -15,7 +15,10 @@ class DmMotor : public librmcs::device::DmMotor { 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( @@ -47,6 +50,13 @@ class DmMotor : public librmcs::device::DmMotor { 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(); } @@ -55,7 +65,7 @@ class DmMotor : public librmcs::device::DmMotor { return librmcs::device::DmMotor::generate_clear_error_command(); } - constexpr static uint64_t generate_disable_command() { + uint64_t generate_disable_command() { return librmcs::device::DmMotor::generate_disable_command(); } @@ -63,6 +73,16 @@ class DmMotor : public librmcs::device::DmMotor { 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(); @@ -79,6 +99,8 @@ class DmMotor : public librmcs::device::DmMotor { 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_; From 88eceb65ebe0872401f34427130f2fa7ee80aeda Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Tue, 20 Jan 2026 03:57:02 +0800 Subject: [PATCH 27/35] Update wheelleg-infantry hardware. --- .../src/hardware/wheelleg-infantry.cpp | 48 ++++++++++--------- 1 file changed, 26 insertions(+), 22 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index 107ab2fb..32bb6f59 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -219,34 +219,34 @@ class WheelLegInfantry , chassis_wheel_motors_( {infantry, infantry_command, "/chassis/left_wheel", device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(13.0) - .enable_multi_turn_angle() - .set_reversed()}, + .set_reduction_ratio(268.0 / 17) + .enable_multi_turn_angle()}, {infantry, infantry_command, "/chassis/right_wheel", device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(13.0) - .enable_multi_turn_angle()}) + .set_reduction_ratio(268.0 / 17) + .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()))}, - {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()))}, - {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.get_parameter("left_front_hip_motor_zero_point").as_int())) .set_reversed()}, - {infantry, infantry_command, "/chassis/right_front_hip", + {infantry, infantry_command, "/chassis/left_back_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())) - .set_reversed()}) + 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} @@ -326,20 +326,24 @@ class WheelLegInfantry void command_update() { uint16_t control_commands[4]{}; + transmit_buffer_.add_can1_transmission( + 0x01, chassis_hip_motors[0].generate_angle_command()); + transmit_buffer_.add_can1_transmission( + 0x02, chassis_hip_motors[1].generate_angle_command()); + 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_.add_can2_transmission( + 0x03, chassis_hip_motors[2].generate_angle_command()); + transmit_buffer_.add_can2_transmission( + 0x04, chassis_hip_motors[3].generate_angle_command()); transmit_buffer_.trigger_transmission(); } From 20a7f63060ab128f51b9515a52b85dbfa5b490d3 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Tue, 20 Jan 2026 03:58:49 +0800 Subject: [PATCH 28/35] The lqr parameter polarity is correct, but the bench model is not yet balanced. --- .../config/wheelleg-infantry.yaml | 23 +- .../controller/chassis/chassis_controller.cpp | 26 +- .../chassis/wheel_leg/desire_state_solver.hpp | 39 +- .../wheel_leg/wheel_leg_controller.cpp | 249 ++++-- .../src/controller/lqr/lqr_calculator.hpp | 834 +++++++++--------- .../rmcs_core/src/filter/kalman_filter.hpp | 2 +- 6 files changed, 668 insertions(+), 505 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml index 1e45a1ed..f9763639 100644 --- a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml @@ -4,6 +4,8 @@ rmcs_executor: 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 @@ -23,6 +25,7 @@ rmcs_executor: # - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::WheelLegController -> wheel_leg_controller + # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer # - rmcs_auto_aim::AutoAimController -> auto_aim_controller @@ -32,10 +35,22 @@ infantry_hardware: usb_pid_bottom_board: 0xf30c yaw_motor_zero_point: 32285 pitch_motor_zero_point: 6321 - left_front_hip_motor_zero_point: 14944 - left_back_hip_motor_zero_point: 28913 - right_back_hip_motor_zero_point: 12639 - right_front_hip_motor_zero_point: 8753 + 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/velocity + - /chassis/left_wheel/control_torque + - /chassis/right_wheel/control_torque + yaw_angle_pid_controller: ros__parameters: 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/desire_state_solver.hpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_solver.hpp index 9c9a19b1..ab3bd0fa 100644 --- 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 @@ -1,47 +1,50 @@ #include "rmcs_msgs/wheel_leg_state.hpp" +#include namespace rmcs_core::controller::chassis { class DesireStateSolver { public: explicit DesireStateSolver(double desire_leg_length) - : desire_leg_length_(desire_leg_length) {} + : initial_leg_length_(desire_leg_length) { + reset(); + } + + void reset() { + desire_leg_length_ = initial_leg_length_; + desire_roll_angle_ = 0.0; + } rmcs_msgs::WheelLegState update(const double& vx, const double& vz, const double& wz) { - desire_leg_length_ += vz * leg_length_sensitivity_; + // desire_leg_length_ += vz * leg_length_sensitivity_; // distance :always 0 during velocity control - desire_state_.distance = 0; - desire_state_.velocity = vx; + desire_state_.distance = 0.0; + desire_state_.velocity = 0.0; // yaw angle: - desire_state_.yaw_angle = 0; - desire_state_.yaw_velocity = wz; + desire_state_.yaw_angle = 0.0; + desire_state_.yaw_velocity = 0.0; return desire_state_; } - // void update() - - void update_current_distance() {} - - void update_current_yaw_angle() {} + double update_desire_leg_length(const double min, const double max) { + desire_leg_length_ = std::clamp(desire_leg_length_, min, max); - double update_desire_leg_length() const { return desire_leg_length_; } + return desire_leg_length_; + } double update_desire_roll_angle() const { return desire_roll_angle_; } private: - void reset_all_controls() { - desire_leg_length_ = 0.15; - desire_roll_angle_ = 0.0; - } - - constexpr static double leg_length_sensitivity_ = 0.0003; + constexpr static double leg_length_sensitivity_ = 0.00001; constexpr static double dt_ = 0.001; + const double initial_leg_length_; + double desire_leg_length_; double desire_roll_angle_; 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 index fb217168..f86c667f 100644 --- 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 @@ -1,6 +1,9 @@ +#include #include -#include #include + +#include +#include #include #include #include @@ -11,7 +14,9 @@ #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 { @@ -23,18 +28,22 @@ class WheelLegController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , body_mess_(15.0) + , body_mess_(14.19) , leg_mess_(0.629) , wheel_mess_(0.459) , wheel_radius_(0.06) , wheel_distance_(0.2135) - , centroid_position_coefficient_() + , 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_, W_) + , velocity_kalman_filter_(A_, H_, Q_, R_) + , chassis_angle_filter_(5.0, 1000.0) , desire_state_solver_(0.15) - , roll_angle_pid_calculator_(0.0, 0.0, 0.0) - , leg_length_pid_calculator_(0.0, 0.0, 0.0) { + , roll_angle_pid_calculator_(1.0, 0.0, 1.0) + , leg_length_pid_calculator_(350.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_); @@ -69,27 +78,46 @@ class WheelLegController 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_); + + register_output("/chassis/velocity", velocity_); + + 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; - // } + if (std::isnan(chassis_control_velocity_->vector[0])) { + reset_all_controls(); + return; + } // Observer - auto wheel_velocities = calculate_wheel_velocities(); 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); - // auto support_force = calculate_support_force(leg_posture); + auto filtered_chassis_angle = chassis_angle_filter_.update( + Eigen::Vector3d{ + *chassis_roll_angle_imu_, *chassis_pitch_angle_imu_, *chassis_yaw_angle_imu_}); - // State: [s ds phi dphi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b] - auto measure_state = calculate_measure_state(distance, leg_posture); + // 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, filtered_chassis_angle); // Controller auto chassis_control_velocity = calculate_chassis_control_velocity(); @@ -100,7 +128,28 @@ class WheelLegController update_hip_and_wheel_torques(leg_forces, control_torques); - reset_all_controls(); + *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; + + // RCLCPP_INFO( + // get_logger(), "lf: %f, lb: %f, lw: %f, rf: %f, rb: %f, rw:%f", + // *left_front_hip_control_torque_, *left_back_hip_control_torque_, + // *left_wheel_control_torque_, *right_front_hip_control_torque_, + // *right_back_hip_control_torque_, *right_wheel_control_torque_); + + *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; + + // RCLCPP_INFO( + // get_logger(), "pitch: %f, lw torque: %f, rw torque: %f", *chassis_pitch_angle_imu_, + // *left_wheel_control_torque_, *right_wheel_control_torque_); + + // *left_wheel_control_torque_ = 0.0; + // *right_wheel_control_torque_ = 0.0; } private: @@ -116,13 +165,25 @@ class WheelLegController }; void reset_all_controls() { - *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_leg_vmc_solver_.reset(); + right_leg_vmc_solver_.reset(); + + desire_state_solver_.reset(); + velocity_kalman_filter_.reset(); + chassis_angle_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() { @@ -132,13 +193,11 @@ class WheelLegController } Eigen::Vector4d calculate_hips_angles() { - auto norm_angle = [](double angle) { return 2 * pi_ - angle; }; - return { - norm_angle(*left_front_hip_angle_), // - norm_angle(*left_back_hip_angle_), // - norm_angle(*right_back_hip_angle_), // - norm_angle(*right_front_hip_angle_), // + *left_front_hip_angle_, // + *left_back_hip_angle_, // + *right_back_hip_angle_, // + *right_front_hip_angle_, // }; } @@ -169,10 +228,11 @@ class WheelLegController Eigen::Vector2d result; auto& [left_support_force, right_support_force] = result; - const auto& [left_leg_force, left_leg_torque] = left_leg_vmc_solver_.get_virtual_torque( + 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_.get_virtual_torque( - *right_front_hip_torque_, *right_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()) @@ -225,25 +285,27 @@ class WheelLegController * leg_posture.dot_tilt_angle.x() + leg_posture.dot_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.dot_tilt_angle.y() - + leg_posture.dot_leg_length.y() - + std::sin(leg_posture.tilt_angle.y()); + auto right_leg_velocity = + leg_posture.leg_length.y() * std::cos(leg_posture.tilt_angle.y()) + * leg_posture.dot_tilt_angle.y() + + leg_posture.dot_leg_length.y() * std::sin(leg_posture.tilt_angle.y()); auto calculate_velocity = wheel_velocity + (left_leg_velocity + right_leg_velocity) / 2.0; + // 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 = estimate_velocity.x(); - distance += velocity * dt_; + velocity = calculate_velocity; + distance = last_distance_ + velocity * dt_; + last_distance_ = distance; - return result; + return Eigen::Vector2d{distance, velocity}; } - rmcs_msgs::WheelLegState - calculate_measure_state(Eigen::Vector2d distance, LegPosture leg_posture) { + rmcs_msgs::WheelLegState calculate_measure_state( + Eigen::Vector2d distance, LegPosture leg_posture, Eigen::Vector3d filtered_chassis_angle) { rmcs_msgs::WheelLegState measure_state; measure_state.distance = distance.x(); measure_state.velocity = distance.y(); @@ -260,23 +322,34 @@ class WheelLegController measure_state.body_pitch_angle = *chassis_pitch_angle_imu_; measure_state.body_pitch_velocity = *chassis_pitch_velocity_imu_; + 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; + return measure_state; } 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 desire_state; + rmcs_msgs::WheelLegState desire_state{}; // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity auto& [vx, vz, wz] = control_velocity; + desire_state = desire_state_solver_.update(vx, vz, wz); + desire_roll_angle_ = desire_state_solver_.update_desire_roll_angle(); + desire_leg_length_ = desire_state_solver_.update_desire_leg_length(0.13, 0.36); + return desire_state; } @@ -284,9 +357,10 @@ class WheelLegController calculate_leg_force(LegPosture leg_posture, rmcs_msgs::WheelLegState measure_state) { Eigen::Vector2d result; - auto roll_control_force = roll_angle_pid_calculator_.update(0 - *chassis_roll_angle_imu_); + 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( - leg_length_desire_ - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); + desire_leg_length_ - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); auto calculate_compensation_feedforward_force = [this](double coefficient) { return (body_mess_ / 2.0 + centroid_position_coefficient_ * leg_mess_) * coefficient; @@ -314,11 +388,48 @@ class WheelLegController Eigen::Vector4d calculate_control_torques( rmcs_msgs::WheelLegState desire_state, rmcs_msgs::WheelLegState measure_state, LegPosture leg_posture) { - Eigen::Vector4d result; + Eigen::Vector4d result{}; auto error_state = desire_state.vector() - measure_state.vector(); + // RCLCPP_INFO( + // get_logger(), + // "desire_state: " + // "[%f, %f, %f, %f, %f, %f, %f, %f, %f, %f]", + // desire_state.vector()(0), desire_state.vector()(1), desire_state.vector()(2), + // desire_state.vector()(3), desire_state.vector()(4), desire_state.vector()(5), + // desire_state.vector()(6), desire_state.vector()(7), desire_state.vector()(8), + // desire_state.vector()(9)); + + // RCLCPP_INFO( + // get_logger(), + // "measure_state: " + // "[%f, %f, %f, %f, %f, %f, %f, %f, %f, %f]", + // measure_state.vector()(0), measure_state.vector()(1), measure_state.vector()(2), + // measure_state.vector()(3), measure_state.vector()(4), measure_state.vector()(5), + // measure_state.vector()(6), measure_state.vector()(7), measure_state.vector()(8), + // measure_state.vector()(9)); + + // RCLCPP_INFO( + // get_logger(), + // "error_state: " + // "[%f, %f, %f, %f, %f, %f, %f, %f, %f, %f]", + // error_state(0), error_state(1), error_state(2), error_state(3), error_state(4), + // error_state(5), error_state(6), error_state(7), error_state(8), error_state(9)); + auto gain = lqr_calculator_.update(leg_posture.leg_length.x(), leg_posture.leg_length.y()); - auto test_result = gain * error_state; + + auto err_0 = error_state(0); + auto err_1 = error_state(1); + auto err_8 = error_state(8); + + RCLCPP_INFO(get_logger(), "gain debug: %f, %f, %f", err_0, err_1, err_8); + + result = -gain * error_state; + + std::ostringstream oss; + oss << "\nLQR gain matrix (4x10):\n" << gain; + + // RCLCPP_INFO(this->get_logger(), "%s", oss.str().c_str()); return result; } @@ -328,18 +439,26 @@ class WheelLegController auto& [left_wheel_control_torque, right_wheel_control_torque, left_leg_control_torque, right_leg_control_torque] = control_torques; - *left_wheel_control_torque_ = left_wheel_control_torque; - *right_wheel_control_torque_ = right_wheel_control_torque; - auto left_hip_control_torque = - left_leg_vmc_solver_.get_joint_torque(left_leg_force, left_leg_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_.get_joint_torque(right_leg_force, right_leg_control_torque); + right_leg_vmc_solver_.update_joint_torque(right_leg_force, right_leg_control_torque); + + *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()); + } + + static double clamp_wheel_control_torque(const double& torque) { + return std::clamp(torque, -5., 5.); + } - *left_front_hip_control_torque_ = left_hip_control_torque.x(); - *left_back_hip_control_torque_ = left_hip_control_torque.y(); - *right_front_hip_control_torque_ = right_hip_control_torque.x(); - *right_back_hip_control_torque_ = right_hip_control_torque.y(); + static double clamp_hip_control_torque(const double& torque) { + return std::clamp(torque, -5., 5.); } static constexpr double nan_ = std::numeric_limits::quiet_NaN(); @@ -350,6 +469,12 @@ class WheelLegController static constexpr double dt_ = 1e-3; static constexpr double g_ = 9.80665; + static constexpr double front_upper_limit_angle_ = 7 * pi_ / 6; + static constexpr double front_lower_limit_angle_ = 2 * pi_ / 3; + + static constexpr double back_upper_limit_angle_ = -pi_ / 6; + static constexpr double back_lower_limit_angle_ = pi_ / 3; + static constexpr double sigma_q_ = 1.0; static constexpr double sigma_v_ = 0.75; static constexpr double sigma_a_ = 1.0; @@ -369,6 +494,9 @@ class WheelLegController 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_; @@ -402,15 +530,26 @@ class WheelLegController 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 leg_length_desire_; + OutputInterface distance_; + OutputInterface velocity_; + + double desire_leg_length_, desire_roll_angle_; - Eigen::Vector2d last_leg_length_, last_tilt_angle_, last_dot_leg_length_, last_dot_tilt_angle_; + Eigen::Vector2d last_leg_length_, last_tilt_angle_; + Eigen::Vector2d last_dot_leg_length_, last_dot_tilt_angle_; + double last_distance_; VmcSolver left_leg_vmc_solver_, right_leg_vmc_solver_; filter::KalmanFilter<2, 2> velocity_kalman_filter_; + filter::LowPassFilter<3> chassis_angle_filter_; DesireStateSolver desire_state_solver_; 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 index 99898dc3..c63ccf33 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp @@ -5,8 +5,8 @@ // 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. +// 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 { @@ -35,9 +35,9 @@ class LqrCalculator { double t5; double t6; double t7; - - // This function was generated by the Symbolic Math Toolbox version 24.2. - // 2025-11-13 22:44:20 + /* This function was generated by the Symbolic Math Toolbox version 24.2. + */ + /* 2026-01-19 23:59:32 */ t2 = l_l * l_l; t3 = std::pow(l_l, 3.0); t5 = l_r * l_r; @@ -51,457 +51,463 @@ class LqrCalculator { e_K_result_tmp = l_r * t3; f_K_result_tmp = t2 * t5; K_result[0] = - ((((((l_l * 65.742000232621649 - l_r * 164.16682583486471) - t2 * 52.381670299384247) - - t3 * 386.2464616614821) - + t4 * 542.26001657504969) - + t5 * 392.11423826325591) - + ((((t6 * 97.875583809753323 - t7 * 691.72153331253764) - - K_result_tmp * 20.138079062233381) - - b_K_result_tmp * 1657.812224997456) - + c_K_result_tmp * 1985.3342273262449)) - + (((d_K_result_tmp * 1415.261073756446 - e_K_result_tmp * 1100.267333961333) - - f_K_result_tmp * 373.0130513504314) - + 0.62665696127003523); + ((((((l_l * 49.519066094841527 - l_r * 264.64687470180331) + t2 * 99.452877456856243) + - t3 * 1107.179794403276) + + t4 * 1237.6537352291259) + + t5 * 1023.277655259573) + + ((((t6 * -983.266843751636 - t7 * 252.25747351809451) + - K_result_tmp * 295.76210498572442) + - b_K_result_tmp * 2355.6271941424029) + + c_K_result_tmp * 3746.923048388433)) + + (((d_K_result_tmp * 2865.1105821277388 - e_K_result_tmp * 1716.2352595057371) + - f_K_result_tmp * 1806.8754403979481) + + 15.900988036830091); K_result[1] = - ((((((l_l * -137.6782528259844 + l_r * 38.291917257296006) + t2 * 601.06706713336121) - - t3 * 967.765443429392) - + t4 * 463.649191705752) - + t5 * 180.95650637985389) - + ((((t6 * -578.14999435552363 + t7 * 391.957501942056) - - K_result_tmp * 465.45483092991282) - + b_K_result_tmp * 976.92640151459261) - - c_K_result_tmp * 67.562213729824677)) - + (((d_K_result_tmp * 61.659385499165687 + e_K_result_tmp * 797.36074479286879) - - f_K_result_tmp * 1256.302427350575) - + 0.80890190594414357); + (((((l_l * -143.60853008303431 - l_r * 76.577529041723878) + t2 * 656.85046062467666) + - t3 * 842.09297000827712) + - t4 * 32.76964572047001) + + ((((t5 * 834.25533479864612 - t6 * 2368.1241198739731) + t7 * 1915.778721879132) + - K_result_tmp * 644.29035571669613) + + b_K_result_tmp * 1884.455621992498)) + + ((((c_K_result_tmp * -38.317955165453853 - d_K_result_tmp * 288.90240271970362) + + e_K_result_tmp * 2207.1694551630708) + - f_K_result_tmp * 2823.3307327260482) + + 16.371189276639019); K_result[2] = - ((((((l_l * -58.936551645814767 + l_r * 100.43070443026269) + t2 * 134.7057987502493) - - t3 * 171.715152262072) - + t4 * 260.01877309025127) - - t5 * 608.01436024643863) - + (((((t6 * 1073.386058341245 - t7 * 334.41160739101912) - + K_result_tmp * 113.639816009988) - + b_K_result_tmp * 421.62514920632708) - - c_K_result_tmp * 1786.6435368411981) - - d_K_result_tmp * 435.55944825783172)) - + ((e_K_result_tmp * -550.66675594008791 + f_K_result_tmp * 1643.2489674730241) - + 10.79434735925515); + (((((l_l * -97.844014952553238 + l_r * 378.89581426555532) - t2 * 3.2709323635840342) + - t3 * 617.54531952903278) + + t4 * 1587.6914459852469) + + (((((t5 * -3192.383141148433 + t6 * 8643.5016180517468) - t7 * 6858.5379856925456) + + K_result_tmp * 1215.8171078840321) + - b_K_result_tmp * 5116.7565361133811) + + c_K_result_tmp * 468.02132002607561)) + + (((d_K_result_tmp * 1822.110648684868 - e_K_result_tmp * 5531.0110808088057) + + f_K_result_tmp * 6330.6342246015874) + + 23.918447993158551); K_result[3] = - ((((((l_l * 17.470935993895338 + l_r * 50.831427775719263) - t2 * 687.4474700837809) - + t3 * 2152.3857311851771) - - t4 * 2023.7205507551851) - - t5 * 625.1804360193164) - + ((((t6 * 1253.250708378072 - t7 * 859.1228344233549) - + K_result_tmp * 805.83458525202445) - - b_K_result_tmp * 246.38589855911019) - - c_K_result_tmp * 335.89539317621069)) - + (((d_K_result_tmp * -1942.114665231356 + e_K_result_tmp * 1309.7478537128129) - + f_K_result_tmp * 884.74139025727879) - + 9.13562076257349); + ((((((l_l * 346.73534170236547 - l_r * 61.162969453871128) - t2 * 2349.440418123213) + + t3 * 6376.9835107582576) + - t4 * 5700.0575049241206) + + t5 * 236.79479788310971) + + ((((t6 * -1624.9251344998779 + t7 * 2201.8983295157) + + K_result_tmp * 109.1319860299308) + + b_K_result_tmp * 3459.4266148232141) + - c_K_result_tmp * 4126.827233021615)) + + (((d_K_result_tmp * -3454.3777854229438 + e_K_result_tmp * 2670.930990837684) + + f_K_result_tmp * 963.163050643568) + + 23.856340045205862); K_result[4] = - (((((l_l * 51.618765926331527 - l_r * 101.13738170798339) - t2 * 8.95708958199305) - - t3 * 314.67475435008328) - + t4 * 387.6334722914317) - + ((((t5 * 261.89330760067128 + t6 * 29.258960918484082) - t7 * 430.22788200317518) - - K_result_tmp * 127.3840659524775) - - b_K_result_tmp * 915.65481615560964)) - + ((((c_K_result_tmp * 1255.9332693258041 + d_K_result_tmp * 1037.1094349037769) - - e_K_result_tmp * 698.84642810472917) - - f_K_result_tmp * 430.54673417571621) - - 2.1515748989123931); + ((((((l_l * 10.358543016163781 - l_r * 203.5005190393093) + t2 * 77.322396566930934) + - t3 * 534.43329728971435) + + t4 * 551.78736549994528) + + t5 * 809.48036773883371) + + (((((t6 * -1096.6801521517809 + t7 * 333.607780639757) + - K_result_tmp * 103.1587125717415) + - b_K_result_tmp * 1238.7424777305059) + + c_K_result_tmp * 1985.963425665808) + + d_K_result_tmp * 1302.772593072503)) + + ((e_K_result_tmp * -694.89934496015758 - f_K_result_tmp * 941.56357617693993) + + 13.69625723986436); K_result[5] = - (((((l_l * -119.3609663914195 + l_r * 77.261236019827066) + t2 * 395.04190370737882) - - t3 * 537.72615245148461) - + t4 * 182.66443016706981) - + ((((t5 * -198.7156378678909 + t6 * 418.64243536679629) - t7 * 480.36171209178849) - - K_result_tmp * 118.1955126585697) - + b_K_result_tmp * 303.50802825084207)) - + ((((c_K_result_tmp * 275.56047258933432 - d_K_result_tmp * 226.41242349661471) - + e_K_result_tmp * 720.23624044046949) - - f_K_result_tmp * 723.90438215562028) - - 2.5296472311547742); + (((((l_l * -66.640041023853186 - l_r * 141.47011617717121) + t2 * 150.59964643868221) + - t3 * 68.51003645798) + - t4 * 227.6211902207628) + + (((((t5 * 921.76947545449275 - t6 * 2275.2452174080072) + t7 * 1852.854117749418) + - K_result_tmp * 207.02288529783721) + + b_K_result_tmp * 547.88315957204441) + + c_K_result_tmp * 313.02286196382079)) + + (((d_K_result_tmp * 38.009318060403963 + e_K_result_tmp * 786.65440847261709) + - f_K_result_tmp * 1329.615475193938) + + 14.6845396412346); K_result[6] = - (((((l_l * -58.56170849808084 + l_r * 39.916737454873079) + t2 * 68.947989451313433) - - t3 * 36.528448326658513) - + t4 * 117.4406998564984) - + (((((t5 * -325.777173508277 + t6 * 621.513686909637) - t7 * 159.79216027074841) - + K_result_tmp * 240.12099506453529) - - b_K_result_tmp * 82.581978326705723) - - c_K_result_tmp * 1007.408302279198)) - + (((d_K_result_tmp * -407.37199710550118 - e_K_result_tmp * 400.58426030231459) - + f_K_result_tmp * 1356.806219869289) - + 11.944576963344529); + ((((((l_l * -17.382379082981291 + l_r * 191.83433091723029) - t2 * 215.38248243992419) + + t3 * 177.04185746057911) + + t4 * 452.87712648183128) + - t5 * 1979.00131627843) + + (((((t6 * 5670.0460946420853 - t7 * 4746.17358236347) + + K_result_tmp * 798.05813369076827) + - b_K_result_tmp * 3247.4314296435468) + + c_K_result_tmp * 601.27494988667547) + + d_K_result_tmp * 857.500856843406)) + + ((e_K_result_tmp * -3074.645335299082 + f_K_result_tmp * 3794.5698038129) + + 24.769647992705309); K_result[7] = - ((((((l_l * 40.361578166435663 - l_r * 41.942511122782122) - t2 * 585.48729241581793) - + t3 * 1623.9865989528621) - - t4 * 1435.968108924161) - - t5 * 135.4895503768636) - + ((((t6 * 339.70973345186269 - t7 * 244.15181963706041) - + K_result_tmp * 609.062295677131) - - b_K_result_tmp * 506.592857295121) - + c_K_result_tmp * 124.65353222908691)) - + (((d_K_result_tmp * -1145.8936460038419 + e_K_result_tmp * 667.51865206944569) - + f_K_result_tmp * 623.81296716252916) - + 10.91385918044422); + ((((((l_l * 576.15348930080586 - l_r * 487.76939670822492) - t2 * 1893.6926319332761) + + t3 * 3681.5449393308159) + - t4 * 2816.6765021749479) + + t5 * 3090.264111200559) + + (((((t6 * -8407.85151129691 + t7 * 8002.5217822476279) + - K_result_tmp * 2108.2855913523831) + + b_K_result_tmp * 5840.5504568379029) + - c_K_result_tmp * 5036.24707424001) + + d_K_result_tmp * 1193.6117313315069)) + + ((e_K_result_tmp * -217.37287189235431 - f_K_result_tmp * 1939.0760868151449) + + 30.237336006835161); K_result[8] = - (((((l_l * -10.14292967245026 + l_r * 19.510955097222649) + t2 * 63.688427354792381) - - t3 * 126.7030263345557) - + t4 * 90.019434614642108) - + ((((t5 * -12.62910633624522 - t6 * 68.095117792740581) + t7 * 92.6100356329447) - - K_result_tmp * 64.0139294363693) - + b_K_result_tmp * 179.84689623277129)) - + ((((c_K_result_tmp * -131.83732123625609 + d_K_result_tmp * 34.954651254795969) - + e_K_result_tmp * 10.54375820451925) - - f_K_result_tmp * 79.4730085041239) - - 2.782183870224511); + (((((l_l * -4.9310648643240711 + l_r * 7.2002264945658059) + t2 * 56.541139543005222) + - t3 * 102.5320921260893) + + t4 * 26.27002527512958) + + ((((t5 * 50.068639129019552 - t6 * 200.98443862514611) + t7 * 166.1267154593283) + - K_result_tmp * 114.2926111207355) + + b_K_result_tmp * 277.88339705788911)) + + ((((c_K_result_tmp * -28.049826104012091 + d_K_result_tmp * 47.182490609015133) + + e_K_result_tmp * 194.55326847625531) + - f_K_result_tmp * 380.25992676832033) + - 1.030633894714478); K_result[9] = - (((((l_l * -21.676879342448569 + l_r * 12.321226124105211) + t2 * 45.053038755393537) - - t3 * 40.736498825878989) - + t4 * 14.182293926652751) - + ((((t5 * -54.233045925723118 + t6 * 96.923717117908481) - t7 * 41.0847809943677) - + K_result_tmp * 20.86964231983946) - - b_K_result_tmp * 21.805525628883171)) - + ((((c_K_result_tmp * -73.703002934173924 - d_K_result_tmp * 48.205145186361243) - + e_K_result_tmp * 0.37164453769723782) - + f_K_result_tmp * 110.0699205718546) - + 2.7929967821710409); + (((((l_l * -5.0710009122425461 + l_r * 2.6087176730532828) - t2 * 26.10498657983155) + + t3 * 145.76721162582751) + - t4 * 152.43837950316191) + + ((((t5 * -8.0709031232571888 - t6 * 61.067919012609217) + t7 * 119.5131965706356) + + K_result_tmp * 41.566927163698153) + + b_K_result_tmp * 142.75388407411839)) + + ((((c_K_result_tmp * -246.33121589791989 - d_K_result_tmp * 246.5650211952194) + + e_K_result_tmp * 152.39762579184031) + + f_K_result_tmp * 145.09575109945391) + + 1.0715692326760511); K_result[10] = - (((((l_l * -6.8780348541810667 - l_r * 16.099961296316138) + t2 * 34.829152893578431) - - t3 * 110.1446416680607) - + t4 * 116.4556040715007) - + ((((t5 * 66.4965947634005 - t6 * 67.1180404126167) + t7 * 0.22275801877981269) - - K_result_tmp * 28.765159166789282) - - b_K_result_tmp * 102.0190546135326)) - + ((((c_K_result_tmp * 130.8600727825069 + d_K_result_tmp * 160.2637982055565) - - e_K_result_tmp * 155.37865024067131) - - f_K_result_tmp * 8.093955982347282) - - 3.2046817899359779); + (((((l_l * 36.282243998885193 - l_r * 42.736359961147407) - t2 * 134.57305471180271) + + t3 * 135.76272734464609) + - t4 * 63.080334582434567) + + ((((t5 * 153.9990008140889 + t6 * 5.2197575556166971) - t7 * 310.97844675008218) + - K_result_tmp * 37.957816081050083) + - b_K_result_tmp * 645.94605036131293)) + + ((((c_K_result_tmp * 1026.4867885180111 + d_K_result_tmp * 579.75897072585519) + - e_K_result_tmp * 300.01225094583879) + - f_K_result_tmp * 419.3268494407335) + - 1.343725561213682); K_result[11] = - (((((l_l * 22.023525789365639 + l_r * 2.3226842069266689) - t2 * 109.7742823693729) - + t3 * 223.706876603369) - - t4 * 182.1064429077538) - + ((((t5 * -16.298544568580731 + t6 * 64.146476977234713) - t7 * 93.856384238343054) - + K_result_tmp * 46.885252027497913) - - b_K_result_tmp * 119.386123646578)) - + ((((c_K_result_tmp * 179.52484051833181 - d_K_result_tmp * 37.095894231595892) - + e_K_result_tmp * 70.533519774817037) - - f_K_result_tmp * 65.1655497798367) - + 3.11156333830338); + ((((((l_l * 50.308991746733767 - l_r * 42.088168791831322) - t2 * 167.9521291036682) + + t3 * 147.86587729762459) + + t4 * 49.125374064513913) + + t5 * 171.97186944647) + + ((((t6 * -294.74943355780908 + t7 * 211.6763639699939) + + K_result_tmp * 6.8727777660254219) + - b_K_result_tmp * 234.02410438551649) + + c_K_result_tmp * 142.19120834845069)) + + (((d_K_result_tmp * 317.23125017640132 - e_K_result_tmp * 467.73969646015257) + + f_K_result_tmp * 128.8736972561571) + + 1.221908704316141); K_result[12] = - (((((l_l * -6.0347124087155848 + l_r * 10.256338714606731) + t2 * 36.034487936211761) - - t3 * 63.9608770978389) - + t4 * 39.509679028783658) - + ((((t5 * -5.0414487721761976 - t6 * 43.969340282364918) + t7 * 57.711832988519753) - - K_result_tmp * 34.056119963357752) - + b_K_result_tmp * 109.2951827140096)) - + ((((c_K_result_tmp * -78.837922176786833 + d_K_result_tmp * 1.3934012016259609) - + e_K_result_tmp * 27.765227145899889) - - f_K_result_tmp * 49.3863021069217) - - 1.238124950078388); + (((((l_l * -0.43202559927386192 + l_r * 0.93803408263641763) + t2 * 23.607116009271479) + - t3 * 38.824180630565913) + - t4 * 0.32985508226492821) + + ((((t5 * 52.800897882197383 - t6 * 180.39639471996321) + t7 * 155.0680099600124) + - K_result_tmp * 77.970231172436769) + + b_K_result_tmp * 195.34719940933039)) + + ((((c_K_result_tmp * -43.711863873056 + d_K_result_tmp * 30.090161482807051) + + e_K_result_tmp * 110.6512271136605) + - f_K_result_tmp * 228.69006124297439) + - 0.436882415295074); K_result[13] = - (((((l_l * -10.714551320965811 + l_r * 6.8511821798768047) + t2 * 12.01790656654911) - + t3 * 2.2127122543525761) - - t4 * 8.0181093327580673) - + ((((t5 * -40.302270522501743 + t6 * 88.415260482909858) - t7 * 60.325646830687759) - + K_result_tmp * 28.709721645369189) - - b_K_result_tmp * 51.015910354347888)) - + ((((c_K_result_tmp * -5.0937792766477807 - d_K_result_tmp * 34.576148111107557) - - e_K_result_tmp * 3.785457234395825) - + f_K_result_tmp * 72.4475588587211) - + 1.222238411094988); + (((((l_l * -0.39877026474984828 - l_r * 2.102000934947367) - t2 * 29.817933341206331) + + t3 * 94.000592870016021) + - t4 * 80.942110508246131) + + ((((t5 * 17.1715754475731 - t6 * 86.7487985872154) + t7 * 112.80495582302861) + + K_result_tmp * 24.7703413840776) + + b_K_result_tmp * 57.078733547244923)) + + ((((c_K_result_tmp * -114.8656757646461 - d_K_result_tmp * 94.834802564094659) + + e_K_result_tmp * 40.30789843738642) + + f_K_result_tmp * 69.846040449244541) + + 0.57678686077621544); K_result[14] = - (((((l_l * -0.98229349452137571 - l_r * 11.72348070058845) + t2 * 5.9245692433909536) - - t3 * 35.034102192476048) - + t4 * 42.954840028608707) - + ((((t5 * 50.073120095831833 - t6 * 70.907851635712859) + t7 * 27.953446951940919) - - K_result_tmp * 19.2867397771467) - - b_K_result_tmp * 41.6406668283246)) - + ((((c_K_result_tmp * 69.243025367636363 + d_K_result_tmp * 90.719777052373544) - - e_K_result_tmp * 77.862567260434417) - - f_K_result_tmp * 23.859736386417872) - - 1.2622286836864891); + ((((((l_l * 41.01714688375553 - l_r * 41.740439088847211) - t2 * 134.0165738647942) + + t3 * 152.15407358304611) + - t4 * 77.631416308602311) + + t5 * 169.50036791429241) + + ((((t6 * -131.02195559427651 - t7 * 117.6264132202827) + - K_result_tmp * 59.34665458780011) + - b_K_result_tmp * 407.1482346494368) + + c_K_result_tmp * 722.92324002860767)) + + (((d_K_result_tmp * 452.26498436810181 - e_K_result_tmp * 230.8871877635589) + - f_K_result_tmp * 352.24463818778008) + - 0.73253334243048862); K_result[15] = - (((((l_l * 12.49202706521565 + l_r * 0.48242804319495131) - t2 * 50.637397892085311) - + t3 * 85.099425101873862) - - t4 * 59.2977455404037) - + ((((t5 * 3.133871025887625 + t6 * 8.719689032169196) - t7 * 28.545856331239222) - + K_result_tmp * 9.704499714564653) - - b_K_result_tmp * 67.055766513467)) - + ((((c_K_result_tmp * 102.2402656009333 + d_K_result_tmp * 31.282480931863091) - - e_K_result_tmp * 7.4954001635994079) - - f_K_result_tmp * 44.9149716129839) - + 1.24331399972177); + (((((l_l * 53.20780754797925 - l_r * 59.363970356947632) - t2 * 56.741579275110738) + - t3 * 59.125893770778063) + + t4 * 140.4423314862585) + + (((((t5 * 399.685870960481 - t6 * 1055.4816716720329) + t7 * 993.77240053326977) + - K_result_tmp * 280.11459619488261) + + b_K_result_tmp * 608.686408048281) + - c_K_result_tmp * 534.19642558508349)) + + (((d_K_result_tmp * 346.9310960540945 - e_K_result_tmp * 240.2883599970055) + - f_K_result_tmp * 226.46361185602419) + + 1.168728749491823); K_result[16] = - (((((l_l * 11.80021731015565 - l_r * 72.921640114139976) - t2 * 44.566015557682263) - - t3 * 78.570378889585214) - + t4 * 182.99527774593719) - + ((((t5 * -60.682819229444988 + t6 * 704.14696546445816) - t7 * 828.47684708198051) - + K_result_tmp * 197.50068793180131) - - b_K_result_tmp * 1177.897504958082)) - + ((((c_K_result_tmp * 938.34507403309215 + d_K_result_tmp * 426.965973056169) - - e_K_result_tmp * 560.60692284051106) - + f_K_result_tmp * 347.9969568508659) - + 1.803625058148816); + (((((l_l * 11.71972683002242 - l_r * 126.87142463921) - t2 * 73.646496070906039) + - t3 * 5.1831939756143024) + + t4 * 125.97561603422059) + + ((((t5 * 200.532465328053 + t6 * 299.4401806851692) - t7 * 618.256090284853) + + K_result_tmp * 189.38416508910791) + - b_K_result_tmp * 1214.622066777335)) + + ((((c_K_result_tmp * 1018.5690855414171 + d_K_result_tmp * 444.1479800549875) + - e_K_result_tmp * 577.54019833923269) + + f_K_result_tmp * 354.74738960072727) + + 2.880316436994236); K_result[17] = - (((((l_l * -17.888453298326709 - l_r * 32.853034524878623) + t2 * 122.7775490392724) - - t3 * 198.74621637453919) - + t4 * 27.595960348631191) - + (((((t5 * 278.12055552001408 - t6 * 515.30272840351313) + t7 * 242.2610324760027) - - K_result_tmp * 227.472493036576) - + b_K_result_tmp * 266.87686015907678) - + c_K_result_tmp * 390.26919078295742)) - + (((d_K_result_tmp * 168.777011867226 + e_K_result_tmp * 396.2767322366152) - - f_K_result_tmp * 874.39629365858411) - + 1.1879488399098279); + (((((l_l * -16.01687326095627 - l_r * 31.17209782525941) + t2 * 68.353368980049353) + - t3 * 79.657700428563857) + - t4 * 42.980493838949137) + + ((((t5 * 237.80812859490371 - t6 * 498.43661415478158) + t7 * 294.5933925236248) + - K_result_tmp * 177.22445950336839) + + b_K_result_tmp * 408.33392276148072)) + + ((((c_K_result_tmp * 83.376925162558152 + d_K_result_tmp * 13.27478528759312) + + e_K_result_tmp * 414.98285182467231) + - f_K_result_tmp * 686.57475645659724) + + 3.4401540343306758); K_result[18] = - (((((l_l * -28.445674224257161 + l_r * 133.1992896905158) + t2 * 55.1250721006242) - - t3 * 9.127477406915542) - + t4 * 30.864302634941609) - + (((((t5 * -450.093388257261 + t6 * 615.42324342693928) - t7 * 69.2633438874434) - + K_result_tmp * 80.407025457619014) - + b_K_result_tmp * 312.69109226779511) - - c_K_result_tmp * 1102.736261460343)) - + (((d_K_result_tmp * -374.10368958919719 - e_K_result_tmp * 121.7149564247612) - + f_K_result_tmp * 863.39469234281091) - - 1.96009566573998); + (((((l_l * -43.709720515444488 + l_r * 393.82610533556812) - t2 * 266.68184468524419) + + t3 * 838.24841844184073) + - t4 * 429.42581454015033) + + (((((t5 * -3067.44921170092 + t6 * 7650.62377132923) - t7 * 5856.5509642053639) + + K_result_tmp * 1149.4480724340131) + - b_K_result_tmp * 2034.8252778743331) + - c_K_result_tmp * 1385.288317518226)) + + (((d_K_result_tmp * -1471.326502745718 - e_K_result_tmp * 1010.378770914998) + + f_K_result_tmp * 4588.4212273993817) + + 32.289415910184253); K_result[19] = - (((((l_l * 18.78880668336452 - l_r * 2.571667298988491) - t2 * 258.34664122577118) - + t3 * 864.76966519815642) - - t4 * 869.57534622032949) - + ((((t5 * -258.44331430146428 + t6 * 377.55191135262658) - t7 * 191.7135178023396) - + K_result_tmp * 242.98508560433339) - + b_K_result_tmp * 508.268432159587)) - + ((((c_K_result_tmp * -560.4897619157739 - d_K_result_tmp * 1061.8106963679541) - + e_K_result_tmp * 909.91716648345994) - + f_K_result_tmp * 131.5914813453644) - + 1.6391768009073691); + ((((((l_l * 246.94947250910911 - l_r * 172.80197916402381) - t2 * 1015.58091952831) + + t3 * 1671.5787677020851) + - t4 * 1168.1174111382211) + + t5 * 154.45950854878919) + + ((((t6 * 744.889689120951 - t7 * 1525.1144624647891) + + K_result_tmp * 228.11665881923381) + - b_K_result_tmp * 1449.14937107582) + + c_K_result_tmp * 2546.3764006341162)) + + (((d_K_result_tmp * 563.74347478178186 + e_K_result_tmp * 60.4695447475869) + - f_K_result_tmp * 1157.7491510298159) + - 2.545814116656036); K_result[20] = - (((((l_l * 1.07983786439577 - l_r * 10.85618036361061) + t2 * 4.7358339031914536) - - t3 * 34.514852987299847) - + t4 * 40.764983995143439) - + ((((t5 * 26.12298910332014 - t6 * 13.57931670210346) - t7 * 11.54685053771852) - - K_result_tmp * 1.6269455566548079) - - b_K_result_tmp * 102.7027833408567)) - + ((((c_K_result_tmp * 91.770348703220975 + d_K_result_tmp * 89.207170899830118) - - e_K_result_tmp * 79.931486153081309) - + f_K_result_tmp * 6.7521077655079376) - + 0.0025908774155117271); + (((((l_l * -0.023223509853613981 - l_r * 6.5635779314468019) + t2 * 1.128577109523099) + - t3 * 18.16450235434268) + + t4 * 23.32388944869983) + + ((((t5 * 5.6532418086495344 + t6 * 27.88689967980461) - t7 * 42.909497284496616) + + K_result_tmp * 5.7128580483588234) + - b_K_result_tmp * 87.214080251262772)) + + ((((c_K_result_tmp * 78.417535247498066 + d_K_result_tmp * 57.249764314084231) + - e_K_result_tmp * 53.37310163295372) + + f_K_result_tmp * 8.8684223183067559) + + 0.381593736642001); K_result[21] = - (((((l_l * -2.6496231970894191 + l_r * 3.6017440706653381) + t2 * 7.8617729919378752) - + t3 * 5.7342237019208291) - - t4 * 28.61456510643778) - + ((((t5 * 4.0149967654720964 - t6 * 16.666913064436219) + t7 * 10.130713027105751) - - K_result_tmp * 32.792294304764937) - + b_K_result_tmp * 69.573733050802574)) - + ((((c_K_result_tmp * -1.066120573218639 - d_K_result_tmp * 5.2876177445718344) - + e_K_result_tmp * 75.295254409004713) - - f_K_result_tmp * 102.6000526130947) - - 0.1041166910682552); + (((((l_l * -0.81101760566251568 - l_r * 3.8133722424750678) - t2 * 0.26401393871443041) + + t3 * 9.9088294525670779) + - t4 * 22.484918999011249) + + ((((t5 * 27.65949052736654 - t6 * 56.9605417835658) + t7 * 36.109541359064742) + - K_result_tmp * 16.9861695236361) + + b_K_result_tmp * 20.572638323789018)) + + ((((c_K_result_tmp * 33.923466245675108 + d_K_result_tmp * 10.60039299949762) + + e_K_result_tmp * 44.173594071078149) + - f_K_result_tmp * 81.474705141723163) + + 0.41851458577514777); K_result[22] = - (((((l_l * -1.902702025435336 + l_r * 9.0335048779233453) - t2 * 3.375142801075008) - + t3 * 13.065553621697561) - + t4 * 0.96593307394933925) - + ((((t5 * -45.982059498839661 + t6 * 105.1023659282083) - t7 * 53.227488625637832) - + K_result_tmp * 9.6441441714189047) - + b_K_result_tmp * 4.4540686649446579)) - + ((((c_K_result_tmp * -121.27470177917959 - d_K_result_tmp * 21.4496511806005) - - e_K_result_tmp * 56.922388789445847) - + f_K_result_tmp * 140.14209389526241) - + 0.74742646741770979); + (((((l_l * -0.859924635086052 + l_r * 23.3867212640602) - t2 * 19.954656301622769) + + t3 * 45.166391583956091) + - t4 * 4.7711413736387334) + + ((((t5 * -142.46824266969909 + t6 * 340.19874205057272) - t7 * 223.09499644210081) + + K_result_tmp * 57.848301015665143) + - b_K_result_tmp * 140.3500870510145)) + + ((((c_K_result_tmp * -133.45552213558221 - d_K_result_tmp * 27.398962553527149) + - e_K_result_tmp * 165.26862007713291) + + f_K_result_tmp * 356.39030582177151) + + 1.136938441957398); K_result[23] = - (((((l_l * 6.7117464679656473 - l_r * 5.5193433696722964) - t2 * 57.702151660228573) - + t3 * 155.494968036898) - - t4 * 141.534067840227) - + ((((t5 * -15.09975616056647 + t6 * 27.877725762457459) - t7 * 29.852453414222211) - + K_result_tmp * 71.348557470119147) - - b_K_result_tmp * 35.895772934463267)) - + ((((c_K_result_tmp * 30.005199399353721 - d_K_result_tmp * 162.25347557148271) - + e_K_result_tmp * 126.39625164423541) - + f_K_result_tmp * 37.887711347213482) - + 0.097399914303206075); + (((((l_l * 25.5467754669838 - l_r * 11.53039060762849) - t2 * 94.819296726406222) + + t3 * 182.22319590961729) + - t4 * 145.6811041000332) + + ((((t5 * 49.894308154300127 - t6 * 115.98612817156879) + t7 * 74.874780122016972) + - K_result_tmp * 27.631266508415891) + + b_K_result_tmp * 79.693725093069489)) + + ((((c_K_result_tmp * 25.01269019934249 - d_K_result_tmp * 2.8296726892568569) + + e_K_result_tmp * 71.2220134291065) + - f_K_result_tmp * 130.06363407940711) + + 0.056580538236626947); K_result[24] = - (((((l_l * 7.70073388030568 - l_r * 63.100221178070427) - t2 * 42.877327064547) - - t3 * 38.4557022086672) - + t4 * 148.87403977509661) - + (((((t5 * -41.18911635088039 + t6 * 324.13808597207509) - t7 * 378.27955922157179) - + K_result_tmp * 289.06912083291542) - - b_K_result_tmp * 703.52215034804158) - + c_K_result_tmp * 447.767897697821)) - + (((d_K_result_tmp * 57.978454542837071 - e_K_result_tmp * 367.21431953613057) - + f_K_result_tmp * 403.45336928732809) - + 1.374175360254855); + (((((l_l * -7.507923807477499 - l_r * 47.1166073834091) + t2 * 10.609183847451559) + - t3 * 266.8715875045138) + + t4 * 445.92045409782958) + + ((((t5 * -138.66856730072519 + t6 * 714.8166512354735) - t7 * 717.88430896844932) + + K_result_tmp * 305.36977833759732) + - b_K_result_tmp * 1290.1053940014231)) + + ((((c_K_result_tmp * 795.13732224673527 + d_K_result_tmp * 563.46305906966563) + - e_K_result_tmp * 1024.8457594211461) + + f_K_result_tmp * 672.41645240069522) + + 3.768869583068311); K_result[25] = - (((((l_l * -34.271583878074168 - l_r * 29.913461143979561) + t2 * 224.19402546780739) - - t3 * 577.62094264252391) - + t4 * 502.25124692438709) - + ((((t5 * 155.78495563762749 - t6 * 258.48412888852408) + t7 * 160.0800391231009) - - K_result_tmp * 276.58644176442073) - - b_K_result_tmp * 85.252780026231719)) - + ((((c_K_result_tmp * 203.49175400523029 + d_K_result_tmp * 783.76146947832626) - - e_K_result_tmp * 547.34695858930672) - - f_K_result_tmp * 241.29651092478079) - + 2.07433587994801); + (((((l_l * -96.5181386929551 - l_r * 13.29649407893276) + t2 * 296.91137344618278) + - t3 * 492.02077209854048) + + t4 * 258.47396521460638) + + ((((t5 * 64.41551158197602 + t6 * 116.7904093052677) - t7 * 299.53651415115871) + - K_result_tmp * 82.217207855106835) + - b_K_result_tmp * 571.88933368168887)) + + ((((c_K_result_tmp * 890.06726979519453 + d_K_result_tmp * 570.63485502912943) + - e_K_result_tmp * 170.323622829347) + - f_K_result_tmp * 468.472627641117) + + 2.672247540619805); K_result[26] = - (((((l_l * -32.979566290220269 + l_r * 41.04746633958591) + t2 * 121.2972867871816) - - t3 * 165.1175272875835) - + t4 * 120.3412976409259) - + ((((t5 * -247.11022974725549 + t6 * 193.8453578504097) + t7 * 66.108292240088318) - - K_result_tmp * 105.10267134404209) - + b_K_result_tmp * 767.29896191758928)) - + ((((c_K_result_tmp * -823.89125859187141 - d_K_result_tmp * 199.29290253184371) - + e_K_result_tmp * 60.863480529475737) - + f_K_result_tmp * 63.7339163475523) - + 2.1718492070851041); + ((((((l_l * -319.00139678526261 + l_r * 322.51636869379888) + t2 * 1281.5527044375519) + - t3 * 2311.5125789206832) + + t4 * 1993.574700206831) + - t5 * 1044.332181315531) + + ((((t6 * -1.7918399485947849 + t7 * 1992.929250143161) + - K_result_tmp * 490.87684038932031) + + b_K_result_tmp * 3668.6169654858149) + - c_K_result_tmp * 5930.3772134101646)) + + (((d_K_result_tmp * -637.32449153956736 - e_K_result_tmp * 1111.1476465127009) + + f_K_result_tmp * 2400.8298830774779) + + 1.784851464616279); K_result[27] = - ((((((l_l * 38.456927557669012 + l_r * 81.7260725318992) - t2 * 309.28313282821739) - + t3 * 915.01988327380718) - - t4 * 903.37988553500691) - - t5 * 58.265221403698348) - + ((((t6 * 50.832569529983488 + t7 * 36.462534269965722) - - K_result_tmp * 33.186333302981957) - + b_K_result_tmp * 103.59863126837951) - - c_K_result_tmp * 233.80049506267329)) - + (((d_K_result_tmp * -327.7715140418897 + e_K_result_tmp * 411.940426952421) - + f_K_result_tmp * 132.4570946986926) - - 2.8976996017412531); + ((((((l_l * 216.9577621182199 + l_r * 165.24949179860539) - t2 * 1864.3618153585389) + + t3 * 4655.7469807600628) + - t4 * 4112.06601451245) + - t5 * 505.81034264548231) + + (((((t6 * 1269.1369867703561 - t7 * 1326.40938672467) + - K_result_tmp * 5.4108660341742638) + - b_K_result_tmp * 1147.335547410969) + + c_K_result_tmp * 1890.0830855029651) + + d_K_result_tmp * 653.95338161024029)) + + ((e_K_result_tmp * 5.084398864909816 - f_K_result_tmp * 917.35945255922047) + + 30.628295146352809); K_result[28] = - (((((l_l * 3.4947139055562189 - l_r * 2.8762091187738559) - t2 * 6.9559169859923893) - - t3 * 14.72910312548712) - + t4 * 32.293937674470307) - + ((((t5 * -30.532879193449141 + t6 * 110.6611830556341) - t7 * 107.3996861918399) - + K_result_tmp * 19.1563631472269) - - b_K_result_tmp * 118.4010600812998)) - + ((((c_K_result_tmp * 74.124175872787333 + d_K_result_tmp * 68.61937763112806) - - e_K_result_tmp * 93.730726689404065) - + f_K_result_tmp * 54.701802340110021) - - 0.095369362848991793); + (((((l_l * -0.93660812157978057 - l_r * 3.9207061529273979) + t2 * 6.3893228756827174) + - t3 * 32.640909299313648) + + t4 * 38.518328893373322) + + ((((t5 * -1.3581206122441249 + t6 * 29.509888285995231) - t7 * 35.875273533069631) + + K_result_tmp * 6.89319242137903) + - b_K_result_tmp * 83.997723203541511)) + + ((((c_K_result_tmp * 67.489891732481766 + d_K_result_tmp * 67.7587501475345) + - e_K_result_tmp * 72.202271572623545) + + f_K_result_tmp * 14.94907237319339) + + 0.426380579431373); K_result[29] = - (((((l_l * -8.9520651627639527 - l_r * 0.17863115794247009) + t2 * 36.794761124293309) - - t3 * 67.982046941977146) - + t4 * 38.361112332731089) - + ((((t5 * 5.7870457692441759 + t6 * 19.440243364683951) - t7 * 42.880578263950952) - - K_result_tmp * 17.2861068745997) - - b_K_result_tmp * 43.929952294540293)) - + ((((c_K_result_tmp * 95.026585666370252 + d_K_result_tmp * 40.371332021670732) - + e_K_result_tmp * 23.70081960655245) - - f_K_result_tmp * 74.300378821324685) - - 0.030843619913440531); + (((((l_l * -3.4564175049273271 - l_r * 2.8998377043498329) + t2 * 3.9341691070930191) + + t3 * 6.8941482393910869) + - t4 * 22.95899407953609) + + ((((t5 * 20.53769497924948 - t6 * 36.025499539084379) + t7 * 18.47372141377998) + - K_result_tmp * 13.55529297648641) + - b_K_result_tmp * 12.09926055650894)) + + ((((c_K_result_tmp * 53.709465603232132 + d_K_result_tmp * 24.886483527707281) + + e_K_result_tmp * 27.31558305115016) + - f_K_result_tmp * 65.45860269074997) + + 0.37429283306853461); K_result[30] = - (((((l_l * -8.3708331033036352 + l_r * 7.5156256422416634) + t2 * 23.093414882983371) - - t3 * 27.864687284415229) - + t4 * 22.877963575065589) - + ((((t5 * -12.965862943889871 - t6 * 24.791983789362181) + t7 * 70.6777074129471) - - K_result_tmp * 0.71903852590290274) - + b_K_result_tmp * 63.651976625924107)) - + ((((c_K_result_tmp * -139.5291109794579 - d_K_result_tmp * 49.29458563771324) - - e_K_result_tmp * 12.05622901625731) - + f_K_result_tmp * 98.436392990512758) - + 0.22966749250505389); + (((((l_l * -9.6565031018839225 + l_r * 22.46623331639055) + t2 * 33.535209093237832) + - t3 * 70.054736204326119) + + t4 * 88.363856370153215) + + ((((t5 * -79.463062013880091 + t6 * 87.9501050435872) + t7 * 33.580415923316309) + - K_result_tmp * 21.403960521334191) + + b_K_result_tmp * 77.401455525504232)) + + ((((c_K_result_tmp * -297.50683047393392 + d_K_result_tmp * 41.411476255159577) + - e_K_result_tmp * 203.881334292463) + + f_K_result_tmp * 273.94901210779261) + + 0.13197911497832041); K_result[31] = - (((((l_l * 3.176755727085248 + l_r * 5.1076007224127471) - t2 * 44.906252850069578) - + t3 * 144.3968751091314) - - t4 * 147.06084439492491) - + ((((t5 * -26.48236655128424 + t6 * 19.628061747036789) - t7 * 0.56906371336675921) - + K_result_tmp * 25.01184753100198) - + b_K_result_tmp * 71.93443352924966)) - + ((((c_K_result_tmp * -46.2074855104563 - d_K_result_tmp * 119.4531974180299) - + e_K_result_tmp * 133.14416085501631) - - f_K_result_tmp * 41.812868518699233) - + 0.68308102771958357); + (((((l_l * 24.98521763004219 - l_r * 2.406387638957264) - t2 * 132.00968755691991) + + t3 * 308.50328620027989) + - t4 * 270.01982466300223) + + ((((t5 * 30.918646957157151 - t6 * 86.86839369021385) + t7 * 70.996646210221073) + - K_result_tmp * 3.0275374397098092) + + b_K_result_tmp * 40.753968446558659)) + + ((((c_K_result_tmp * 23.724421961450911 - d_K_result_tmp * 49.46111118869301) + + e_K_result_tmp * 107.0432400303656) + - f_K_result_tmp * 94.1927808093761) + + 1.1410323176360571); K_result[32] = - (((((l_l * 50.879352779123387 + l_r * 28.194123394531889) - t2 * 179.17147687860131) - + t3 * 335.8254768437061) - - t4 * 255.96761923728531) - + ((((t5 * -13.940709611916819 - t6 * 89.8770543394501) + t7 * 94.728603839377982) - + K_result_tmp * 19.054275951961959) - - b_K_result_tmp * 34.3976853249082)) - + ((((c_K_result_tmp * 148.75291971412329 - d_K_result_tmp * 11.180862497058589) - + e_K_result_tmp * 57.265316055825437) - - f_K_result_tmp * 135.6413818102948) - - 20.172356813422681); + ((((((l_l * 607.53713413393325 - l_r * 696.2848371822729) - t2 * 1481.909295298899) + + t3 * 771.28417173648961) + + t4 * 653.58511153113011) + + t5 * 4649.893859876287) + + (((((t6 * -10198.934594286249 + t7 * 7524.6262842060669) + - K_result_tmp * 1803.8962667109561) + - b_K_result_tmp * 371.36760215984492) + + c_K_result_tmp * 3389.8025393428829) + + d_K_result_tmp * 6256.5380716800692)) + + ((e_K_result_tmp * -4408.9658815096582 - f_K_result_tmp * 4173.7309837285793) + - 58.828410836047311); K_result[33] = - (((((l_l * 41.725434886435508 + l_r * 30.947724523191251) + t2 * 65.469048699201679) - - t3 * 445.59273353881468) - + t4 * 536.86429878402407) - + ((((t5 * 21.104942768016031 - t6 * 69.667024191408359) + t7 * 87.138961079062327) - - K_result_tmp * 225.29814265883419) - + b_K_result_tmp * 89.575132174286665)) - + ((((c_K_result_tmp * -158.81970858425419 + d_K_result_tmp * 547.9641799270081) - - e_K_result_tmp * 564.18274613860012) - + f_K_result_tmp * 67.1685069442193) - - 19.79082845756766); + ((((((l_l * -653.62358004500493 + l_r * 557.5978482450646) + t2 * 4807.3614757648638) + - t3 * 10480.149373651589) + + t4 * 7704.6958238523712) + - t5 * 606.40353516783262) + + (((((t6 * -1454.7355302778581 + t7 * 2636.839410467765) + - K_result_tmp * 2814.0824663909061) + + b_K_result_tmp * 8541.1366494593258) + - c_K_result_tmp * 6538.1961562525667) + - d_K_result_tmp * 157.1491500240777)) + + ((e_K_result_tmp * 3481.4038070772981 - f_K_result_tmp * 4334.5626868743384) + - 58.174164936061928); K_result[34] = - ((((((l_l * 91.513990141990732 - l_r * 268.96214357902733) - t2 * 371.596573631106) - + t3 * 677.82105811918188) - - t4 * 465.47011003702318) - + t5 * 953.40595570907612) - + ((((t6 * -1764.92023062041 + t7 * 1356.2015535350381) - + K_result_tmp * 84.348385930053709) - - b_K_result_tmp * 76.4317576112754) - - c_K_result_tmp * 143.2514639033121)) - + (((d_K_result_tmp * -74.915655701766312 - e_K_result_tmp * 79.63131178704613) - + f_K_result_tmp * 244.2021163096536) - - 21.102831555636691); + ((((((l_l * 1183.8346432291671 - l_r * 2654.290790420202) - t2 * 2635.9010415423509) + - t3 * 1287.9984721500371) + + t4 * 5735.6749771293626) + + t5 * 4459.0681221568229) + + (((((t6 * 3983.2381897822 - t7 * 10056.130412468839) + + K_result_tmp * 448.67823229709188) + - b_K_result_tmp * 12090.75441154277) + + c_K_result_tmp * 9151.2726429941231) + + d_K_result_tmp * 10248.6786824896)) + + ((e_K_result_tmp * -13373.787410082219 + f_K_result_tmp * 4986.8373581834312) + - 7.57255868874563); K_result[35] = - ((((((l_l * -296.87998316188322 + l_r * 126.58401117224869) + t2 * 1115.194104751552) - - t3 * 1983.1793019098129) - + t4 * 1331.6403747020131) - - t5 * 488.45285285093939) - + ((((t6 * 818.203627871081 - t7 * 596.73174649167709) - - K_result_tmp * 4.5910207918667192) - + b_K_result_tmp * 340.31150206254563) - - c_K_result_tmp * 44.069124590005067)) - + (((d_K_result_tmp * -305.7515288221918 + e_K_result_tmp * 594.9891156093762) - - f_K_result_tmp * 466.193593561064) - - 21.47495663926485); + ((((((l_l * -3351.3705772912549 + l_r * 1984.62856320945) + t2 * 9875.8191070780176) + - t3 * 11830.95528127786) + + t4 * 3489.0858320555872) + - t5 * 7068.4015245379878) + + (((((t6 * 7665.3833809610414 - t7 * 2949.4392326532789) + - K_result_tmp * 1201.8409809460941) + + b_K_result_tmp * 12974.60851081697) + - c_K_result_tmp * 6575.574460505457) + - d_K_result_tmp * 6245.6128033243458)) + + ((e_K_result_tmp * 11824.819651184051 - f_K_result_tmp * 10900.22429865702) + - 12.510139857385081); K_result[36] = - (((((l_l * 11.392511706212719 - l_r * 0.019919442643399139) - t2 * 33.036000568074464) - + t3 * 45.229888787307459) - - t4 * 25.83122067217786) - + ((((t5 * 16.0588020701805 - t6 * 34.77543439033996) + t7 * 16.989606928881919) - - K_result_tmp * 11.56065095113656) - - b_K_result_tmp * 18.48752355406614)) - + ((((c_K_result_tmp * 53.984020765081333 + d_K_result_tmp * 48.610017472052647) - - e_K_result_tmp * 26.847077236222429) - - f_K_result_tmp * 43.117210202465273) - - 2.80096795627845); + (((((l_l * 36.0877702072119 - l_r * 30.580318122323579) - t2 * 101.638356062782) + + t3 * 72.829249464794316) + + t4 * 22.70019064939078) + + ((((t5 * 203.89476900667071 - t6 * 404.672129286879) + t7 * 260.51444559800558) + - K_result_tmp * 90.157603500094766) + - b_K_result_tmp * 132.9697388620219)) + + ((((c_K_result_tmp * 296.06988343834621 + d_K_result_tmp * 403.10214342362121) + - e_K_result_tmp * 313.25367762463378) + - f_K_result_tmp * 200.96366946658051) + - 2.707053077709038); K_result[37] = - (((((l_l * 1.01868813668573 + l_r * 9.7678472261414786) + t2 * 26.76988568651274) - - t3 * 82.6997890765413) - + t4 * 78.323925407915524) - + ((((t5 * -18.764544497852341 + t6 * 23.7069928773335) - t7 * 10.29607919916687) - - K_result_tmp * 33.450195169068948) - + b_K_result_tmp * 46.312109913360793)) - + ((((c_K_result_tmp * -42.248576904462567 + d_K_result_tmp * 46.837783829110229) - - e_K_result_tmp * 35.978866730761993) - - f_K_result_tmp * 10.2048920756438) - - 2.7648775417528082); + (((((l_l * -17.310111615595719 + l_r * 21.923227715916841) + t2 * 157.3147881714151) + - t3 * 360.94549731649738) + + t4 * 262.35701082560342) + + ((((t5 * -8.69698923971202 - t6 * 107.9836556916003) + t7 * 139.57284078244061) + - K_result_tmp * 132.84274287187219) + + b_K_result_tmp * 332.53059461138758)) + + ((((c_K_result_tmp * -173.8348184266973 + d_K_result_tmp * 68.460363779449423) + + e_K_result_tmp * 125.6388283139173) + - f_K_result_tmp * 285.323977202314) + - 2.629495660749217); K_result[38] = - (((((l_l * 5.3394416391660284 - l_r * 30.305337343897559) - t2 * 30.916161492834991) - + t3 * 66.423979189730787) - - t4 * 47.586725416788049) - + ((((t5 * 89.827930646733819 - t6 * 149.86163683193411) + t7 * 114.8244343538348) - + K_result_tmp * 35.053067473828122) - - b_K_result_tmp * 39.908343172898817)) - + ((((c_K_result_tmp * -19.72862085458355 - d_K_result_tmp * 52.955773659738348) - + e_K_result_tmp * 4.9336162031679249) - + f_K_result_tmp * 78.971143997122454) - - 1.784280298872104); + (((((l_l * 13.636902704442891 - l_r * 91.497385538597968) - t2 * 71.889304191408968) + - t3 * 5.9134111513780248) + + t4 * 168.72072699920389) + + ((((t5 * 149.41072733500559 + t6 * 148.19191714789559) - t7 * 332.86042106760652) + + K_result_tmp * 209.91367021267271) + - b_K_result_tmp * 825.57826428644466)) + + ((((c_K_result_tmp * 504.8685922059783 + d_K_result_tmp * 163.82363148595121) + - e_K_result_tmp * 520.06893433140851) + + f_K_result_tmp * 554.1934582432765) + + 3.7444836441482678); K_result[39] = - (((((l_l * -33.840444163836992 + l_r * 10.12146590899663) + t2 * 106.1588170712585) - - t3 * 165.62368999517739) - + t4 * 101.0068447724255) - + ((((t5 * -47.854596393644528 + t6 * 93.337432454580949) - t7 * 73.1422620631561) - + K_result_tmp * 28.202553361804579) - - b_K_result_tmp * 19.579641636014081)) - + ((((c_K_result_tmp * 20.28076139526188 - d_K_result_tmp * 66.326812404169672) - + e_K_result_tmp * 64.8711496264674) - + f_K_result_tmp * 2.9656492393058351) - - 1.853469249301628); + (((((l_l * -79.482308974302569 + l_r * 0.88051189319374024) + t2 * 222.0089239157156) + - t3 * 184.5611579248029) + - t4 * 54.1993861076182) + + ((((t5 * 78.814542616513108 - t6 * 417.95942198293739) + t7 * 468.20554173223428) + - K_result_tmp * 13.01893698462518) + + b_K_result_tmp * 438.38586367721712)) + + ((((c_K_result_tmp * -307.11979965685009 - d_K_result_tmp * 350.89451154995533) + + e_K_result_tmp * 522.64203324908226) + - f_K_result_tmp * 262.31647394030739) + + 3.8682610522381911); gain_ = Eigen::Map>(K_result); } Eigen::Matrix gain_; }; -} // namespace rmcs_core::controller::lqr \ No newline at end of file +} // namespace rmcs_core::controller::lqr diff --git a/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp index 4256d020..148069ea 100644 --- a/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp +++ b/rmcs_ws/src/rmcs_core/src/filter/kalman_filter.hpp @@ -74,7 +74,7 @@ class KalmanFilter { .inverse(); prior_estimate_ += kalman_gain_ * (measurement - measurement_transition_ * prior_estimate_); - // // Update + // Update posterior_error_covariance_ = (Matrix::Identity() - kalman_gain_ * measurement_transition_) * prior_error_covariance_; From e4cb3bf3145621d55292b3f04ed282dd47a8101a Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Fri, 27 Feb 2026 22:16:53 +0800 Subject: [PATCH 29/35] Some debugging code. --- rmcs_ws/src/rmcs_core/librmcs | 2 +- .../chassis/wheel_leg/vmc_solver.hpp | 8 +- .../wheel_leg/wheel_leg_controller.cpp | 255 +++--- .../src/controller/lqr/lqr_calculator.hpp | 824 +++++++++--------- .../src/hardware/wheelleg-infantry.cpp | 12 +- .../include/rmcs_msgs/wheel_leg_mode.hpp | 15 + 6 files changed, 589 insertions(+), 527 deletions(-) create mode 100644 rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_mode.hpp diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs index b82f2eaf..6f79757c 160000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -1 +1 @@ -Subproject commit b82f2eafd21371a23d046e5f75884fe6c6e49124 +Subproject commit 6f79757c8c3b0a0668fde40f28db2e2ba7bb2e93 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 index 193b9aff..ab1bfed3 100644 --- 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 @@ -12,7 +12,9 @@ class VmcSolver { , l2_(l2) , l3_(l2) , l4_(l1) - , l5_(l5) {} + , l5_(l5) { + reset(); + } void reset() { tilt_angle_ = nan_; @@ -22,7 +24,7 @@ class VmcSolver { Eigen::Vector2d update(double phi1, double phi4) { if (std::isnan(phi1) || std::isnan(phi4)) { reset(); - return {0.0, 0.0}; + return Eigen::Vector2d{leg_length_, tilt_angle_}; } calculate_five_link_solution(phi1, phi4); @@ -83,7 +85,7 @@ class VmcSolver { joint_torque_matrix_ = jacobian_matrix.transpose() * rotation_matrix * transform_matrix; } - Eigen::Vector2d get_leg_posture() const { return Eigen::Vector2d{leg_length_, tilt_angle_}; } + 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(); 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 index f86c667f..e2c0f8e4 100644 --- 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 @@ -1,5 +1,6 @@ #include #include +#include #include #include @@ -28,16 +29,16 @@ class WheelLegController : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) - , body_mess_(14.19) + , body_mess_(13.3) , leg_mess_(0.629) , wheel_mess_(0.459) , wheel_radius_(0.06) - , wheel_distance_(0.2135) + , 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_angle_filter_(5.0, 1000.0) + , 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_(350.0, 0.0, 0.0) { @@ -88,6 +89,17 @@ class WheelLegController register_output("/chassis/velocity", velocity_); + register_output("/chassis/output_0", output_0_); + register_output("/chassis/output_1", output_1_); + register_output("/chassis/output_2", output_2_); + register_output("/chassis/output_3", output_3_); + register_output("/chassis/output_4", output_4_); + register_output("/chassis/output_5", output_5_); + register_output("/chassis/output_6", output_6_); + register_output("/chassis/output_7", output_7_); + register_output("/chassis/output_8", output_8_); + register_output("/chassis/output_9", output_9_); + velocity_kalman_filter_.set_process_noise_transition(W_); } @@ -110,14 +122,10 @@ class WheelLegController auto wheel_velocities = calculate_wheel_velocities(); auto leg_posture = calculate_leg_posture(hip_angles); - auto distance = calculate_translational_distance(leg_posture, wheel_velocities); - - auto filtered_chassis_angle = chassis_angle_filter_.update( - Eigen::Vector3d{ - *chassis_roll_angle_imu_, *chassis_pitch_angle_imu_, *chassis_yaw_angle_imu_}); + auto distance = calculate_translational_distance(leg_posture, wheel_velocities, hip_angles); // 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, filtered_chassis_angle); + auto measure_state = calculate_measure_state(distance, leg_posture); // Controller auto chassis_control_velocity = calculate_chassis_control_velocity(); @@ -126,27 +134,19 @@ class WheelLegController auto leg_forces = calculate_leg_force(leg_posture, measure_state); auto control_torques = calculate_control_torques(desire_state, measure_state, leg_posture); - update_hip_and_wheel_torques(leg_forces, control_torques); - - *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; + update_hip_control_angles(); - // RCLCPP_INFO( - // get_logger(), "lf: %f, lb: %f, lw: %f, rf: %f, rb: %f, rw:%f", - // *left_front_hip_control_torque_, *left_back_hip_control_torque_, - // *left_wheel_control_torque_, *right_front_hip_control_torque_, - // *right_back_hip_control_torque_, *right_wheel_control_torque_); + 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_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; // RCLCPP_INFO( - // get_logger(), "pitch: %f, lw torque: %f, rw torque: %f", *chassis_pitch_angle_imu_, - // *left_wheel_control_torque_, *right_wheel_control_torque_); + // get_logger(), "%f, %f, %f, %f", *left_front_hip_control_torque_, + // *left_back_hip_control_torque_, *right_back_hip_control_torque_, + // *right_front_hip_control_torque_); // *left_wheel_control_torque_ = 0.0; // *right_wheel_control_torque_ = 0.0; @@ -157,8 +157,8 @@ class WheelLegController Eigen::Vector2d leg_length; Eigen::Vector2d tilt_angle; - Eigen::Vector2d dot_leg_length; - Eigen::Vector2d dot_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; @@ -170,7 +170,7 @@ class WheelLegController desire_state_solver_.reset(); velocity_kalman_filter_.reset(); - chassis_angle_filter_.reset(); + chassis_gyro_velocity_filter_.reset(); *left_front_hip_control_torque_ = nan_; *left_back_hip_control_torque_ = nan_; @@ -201,6 +201,8 @@ class WheelLegController }; } + void calculate_chassis_euler_angles() {} + LegPosture calculate_leg_posture(const Eigen::Vector4d& hip_angles) { const auto& [lf, lb, rb, rf] = hip_angles; LegPosture result; @@ -211,15 +213,17 @@ class WheelLegController result.leg_length = Eigen::Vector2d{left_leg_length, right_leg_length}; result.tilt_angle = Eigen::Vector2d{left_tilt_angle, right_tilt_angle}; - result.dot_leg_length = (result.leg_length - last_leg_length_) / dt_; + // RCLCPP_INFO(get_logger(), "left angle: %f", left_tilt_angle); + + result.diff_leg_length = (result.leg_length - last_leg_length_) / dt_; last_leg_length_ = result.leg_length; - result.dot_tilt_angle = (result.tilt_angle - last_tilt_angle_) / dt_; + result.diff_tilt_angle = (result.tilt_angle - last_tilt_angle_) / dt_; last_tilt_angle_ = result.tilt_angle; - result.second_order_diff_leg_length = (result.dot_leg_length - last_dot_leg_length_) / dt_; - last_dot_leg_length_ = result.dot_leg_length; - result.second_order_diff_tilt_angle = (result.dot_tilt_angle - last_dot_tilt_angle_) / dt_; - last_dot_tilt_angle_ = result.dot_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; } @@ -245,22 +249,22 @@ class WheelLegController 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.dot_leg_length.x() * leg_posture.dot_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.dot_tilt_angle.x() - * leg_posture.dot_tilt_angle.x() * std::cos(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.dot_leg_length.y() * leg_posture.dot_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.dot_tilt_angle.y() - * leg_posture.dot_tilt_angle.y() * std::cos(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); @@ -272,8 +276,8 @@ class WheelLegController void detect_chassis_leave_the_ground() {} - Eigen::Vector2d - calculate_translational_distance(LegPosture leg_posture, Eigen::Vector2d wheel_velocities) { + Eigen::Vector2d calculate_translational_distance( + LegPosture leg_posture, Eigen::Vector2d wheel_velocities, Eigen::Vector4d hip_angles) { Eigen::Vector2d result; auto& [distance, velocity] = result; @@ -282,15 +286,17 @@ class WheelLegController auto left_leg_velocity = leg_posture.leg_length.x() * std::cos(leg_posture.tilt_angle.x()) - * leg_posture.dot_tilt_angle.x() - + leg_posture.dot_leg_length.x() * std::sin(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.dot_tilt_angle.y() - + leg_posture.dot_leg_length.y() * std::sin(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 + (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 @@ -298,38 +304,52 @@ class WheelLegController Eigen::Vector2d{calculate_velocity, *chassis_x_axis_acceleration_imu_}); velocity = calculate_velocity; + 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, Eigen::Vector3d filtered_chassis_angle) { + rmcs_msgs::WheelLegState + calculate_measure_state(Eigen::Vector2d distance, LegPosture leg_posture) { rmcs_msgs::WheelLegState measure_state; + + filtered_gyro_velocity_ = chassis_gyro_velocity_filter_.update( + Eigen::Vector3d{ + *chassis_roll_velocity_imu_, *chassis_pitch_velocity_imu_, + *chassis_yaw_velocity_imu_}); + leg_posture.tilt_angle.array() += *chassis_pitch_angle_imu_; + 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.yaw_velocity = filtered_gyro_velocity_.z(); measure_state.left_tilt_angle = leg_posture.tilt_angle.x(); - measure_state.left_tilt_velocity = leg_posture.dot_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.dot_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.body_pitch_velocity = filtered_gyro_velocity_.y(); + + // measure_state.distance = 0.0; + // measure_state.velocity = 0.0; + + // measure_state.yaw_angle = 0.0; + // measure_state.yaw_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.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.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; } @@ -346,7 +366,7 @@ class WheelLegController // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity auto& [vx, vz, wz] = control_velocity; - desire_state = desire_state_solver_.update(vx, vz, wz); + desire_state = desire_state_solver_.update(*mode_); desire_roll_angle_ = desire_state_solver_.update_desire_roll_angle(); desire_leg_length_ = desire_state_solver_.update_desire_leg_length(0.13, 0.36); @@ -391,49 +411,71 @@ class WheelLegController Eigen::Vector4d result{}; auto error_state = desire_state.vector() - measure_state.vector(); - // RCLCPP_INFO( - // get_logger(), - // "desire_state: " - // "[%f, %f, %f, %f, %f, %f, %f, %f, %f, %f]", - // desire_state.vector()(0), desire_state.vector()(1), desire_state.vector()(2), - // desire_state.vector()(3), desire_state.vector()(4), desire_state.vector()(5), - // desire_state.vector()(6), desire_state.vector()(7), desire_state.vector()(8), - // desire_state.vector()(9)); - - // RCLCPP_INFO( - // get_logger(), - // "measure_state: " - // "[%f, %f, %f, %f, %f, %f, %f, %f, %f, %f]", - // measure_state.vector()(0), measure_state.vector()(1), measure_state.vector()(2), - // measure_state.vector()(3), measure_state.vector()(4), measure_state.vector()(5), - // measure_state.vector()(6), measure_state.vector()(7), measure_state.vector()(8), - // measure_state.vector()(9)); - - // RCLCPP_INFO( - // get_logger(), - // "error_state: " - // "[%f, %f, %f, %f, %f, %f, %f, %f, %f, %f]", - // error_state(0), error_state(1), error_state(2), error_state(3), error_state(4), - // error_state(5), error_state(6), error_state(7), error_state(8), error_state(9)); auto gain = lqr_calculator_.update(leg_posture.leg_length.x(), leg_posture.leg_length.y()); + result = -gain * error_state; + auto err_0 = error_state(0); auto err_1 = error_state(1); + auto err_2 = error_state(2); + auto err_3 = error_state(3); + auto err_4 = error_state(4); + auto err_5 = error_state(5); + auto err_6 = error_state(6); + auto err_7 = error_state(7); auto err_8 = error_state(8); + auto err_9 = error_state(9); + + auto gain_00 = gain(0, 0); + auto gain_01 = gain(0, 1); + auto gain_02 = gain(0, 2); + auto gain_03 = gain(0, 3); + auto gain_04 = gain(0, 4); + auto gain_05 = gain(0, 5); + auto gain_06 = gain(0, 6); + auto gain_07 = gain(0, 7); + auto gain_08 = gain(0, 8); + auto gain_09 = gain(0, 9); + + auto output_0 = -gain_00 * err_0; + auto output_1 = -gain_01 * err_1; + auto output_2 = -gain_02 * err_2; + auto output_3 = -gain_03 * err_3; + auto output_4 = -gain_04 * err_4; + auto output_5 = -gain_05 * err_5; + auto output_6 = -gain_06 * err_6; + auto output_7 = -gain_07 * err_7; + auto output_8 = -gain_08 * err_8; + auto output_9 = -gain_09 * err_9; - RCLCPP_INFO(get_logger(), "gain debug: %f, %f, %f", err_0, err_1, err_8); - - result = -gain * error_state; + // 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()); - std::ostringstream oss; - oss << "\nLQR gain matrix (4x10):\n" << gain; + // 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); - // RCLCPP_INFO(this->get_logger(), "%s", oss.str().c_str()); + RCLCPP_INFO( + get_logger(), "pitch: %f, err: %f, output: %f, lw torque: %f, rw torque: %f", + measure_state.body_pitch_angle, error_state(8), output_8, result.x(), result.y()); return result; } + void update_hip_control_angles() { + *left_front_hip_control_angle_ = 0.2; + *left_back_hip_control_angle_ = -0.2; + *right_front_hip_control_angle_ = -0.2; + *right_back_hip_control_angle_ = 0.2; + } + 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] = @@ -449,16 +491,16 @@ class WheelLegController *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()); + *right_back_hip_control_torque_ = clamp_hip_control_torque(right_hip_control_torque.y()); + *right_front_hip_control_torque_ = clamp_hip_control_torque(-right_hip_control_torque.x()); } static double clamp_wheel_control_torque(const double& torque) { - return std::clamp(torque, -5., 5.); + return std::clamp(torque, -3.0, 3.0); } static double clamp_hip_control_torque(const double& torque) { - return std::clamp(torque, -5., 5.); + return std::clamp(torque, -10., 10.); } static constexpr double nan_ = std::numeric_limits::quiet_NaN(); @@ -469,11 +511,8 @@ class WheelLegController static constexpr double dt_ = 1e-3; static constexpr double g_ = 9.80665; - static constexpr double front_upper_limit_angle_ = 7 * pi_ / 6; - static constexpr double front_lower_limit_angle_ = 2 * pi_ / 3; - - static constexpr double back_upper_limit_angle_ = -pi_ / 6; - static constexpr double back_lower_limit_angle_ = pi_ / 3; + static constexpr double max_leg_length_ = 0.36; + static constexpr double min_leg_length_ = 0.13; static constexpr double sigma_q_ = 1.0; static constexpr double sigma_v_ = 0.75; @@ -538,18 +577,30 @@ class WheelLegController OutputInterface left_wheel_control_torque_; OutputInterface right_wheel_control_torque_; - OutputInterface distance_; OutputInterface velocity_; + OutputInterface output_0_; + OutputInterface output_1_; + OutputInterface output_2_; + OutputInterface output_3_; + OutputInterface output_4_; + OutputInterface output_5_; + OutputInterface output_6_; + OutputInterface output_7_; + OutputInterface output_8_; + OutputInterface output_9_; + double desire_leg_length_, desire_roll_angle_; Eigen::Vector2d last_leg_length_, last_tilt_angle_; Eigen::Vector2d last_dot_leg_length_, last_dot_tilt_angle_; double last_distance_; + 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_angle_filter_; + filter::LowPassFilter<3> chassis_gyro_velocity_filter_; DesireStateSolver desire_state_solver_; 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 index c63ccf33..07474261 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp @@ -1,4 +1,5 @@ #include +#include #include // Due to the nonlinear nature of the wheeled leg's kinematic model, the Jacobian matrix derived @@ -35,9 +36,12 @@ class LqrCalculator { double t5; double t6; double t7; + + // Q = diag([1 1 1 1 10 1 10 1 2500 1]); + // R = diag([10, 10, 1, 1]); /* This function was generated by the Symbolic Math Toolbox version 24.2. */ - /* 2026-01-19 23:59:32 */ + /* 2026-02-27 21:05:23 */ t2 = l_l * l_l; t3 = std::pow(l_l, 3.0); t5 = l_r * l_r; @@ -51,459 +55,449 @@ class LqrCalculator { e_K_result_tmp = l_r * t3; f_K_result_tmp = t2 * t5; K_result[0] = - ((((((l_l * 49.519066094841527 - l_r * 264.64687470180331) + t2 * 99.452877456856243) - - t3 * 1107.179794403276) - + t4 * 1237.6537352291259) - + t5 * 1023.277655259573) - + ((((t6 * -983.266843751636 - t7 * 252.25747351809451) - - K_result_tmp * 295.76210498572442) - - b_K_result_tmp * 2355.6271941424029) - + c_K_result_tmp * 3746.923048388433)) - + (((d_K_result_tmp * 2865.1105821277388 - e_K_result_tmp * 1716.2352595057371) - - f_K_result_tmp * 1806.8754403979481) - + 15.900988036830091); + (((((l_l * 2.4161625771455442 - l_r * 6.4503256101831674) - t2 * 4.7175983828711834) + - t3 * 9.4827610745871365) + + t4 * 17.255910198489548) + + ((((t5 * 13.0222623797468 + t6 * 11.641653511617831) - t7 * 31.0035373115271) + + K_result_tmp * 5.4254195965871324) + - b_K_result_tmp * 85.338177109904123)) + + ((((c_K_result_tmp * 85.440287758617359 + d_K_result_tmp * 59.820960326560787) + - e_K_result_tmp * 49.8679663677069) + - f_K_result_tmp * 5.6612267799112121) + - 0.16354769561007079); K_result[1] = - (((((l_l * -143.60853008303431 - l_r * 76.577529041723878) + t2 * 656.85046062467666) - - t3 * 842.09297000827712) - - t4 * 32.76964572047001) - + ((((t5 * 834.25533479864612 - t6 * 2368.1241198739731) + t7 * 1915.778721879132) - - K_result_tmp * 644.29035571669613) - + b_K_result_tmp * 1884.455621992498)) - + ((((c_K_result_tmp * -38.317955165453853 - d_K_result_tmp * 288.90240271970362) - + e_K_result_tmp * 2207.1694551630708) - - f_K_result_tmp * 2823.3307327260482) - + 16.371189276639019); + (((((l_l * -6.08302207335293 + l_r * 2.0142744068590139) + t2 * 22.943060350901909) + - t3 * 32.788866060224017) + + t4 * 12.0700541979319) + + ((((t5 * 5.1038452484121848 - t6 * 16.86627200113784) + t7 * 10.54500440168491) + - K_result_tmp * 14.42141170176407) + + b_K_result_tmp * 35.208495730890263)) + + ((((c_K_result_tmp * 1.3279664128930979 - d_K_result_tmp * 8.1046028301993065) + + e_K_result_tmp * 42.642098523750512) + - f_K_result_tmp * 51.552702341861533) + - 0.15698795750402059); K_result[2] = - (((((l_l * -97.844014952553238 + l_r * 378.89581426555532) - t2 * 3.2709323635840342) - - t3 * 617.54531952903278) - + t4 * 1587.6914459852469) - + (((((t5 * -3192.383141148433 + t6 * 8643.5016180517468) - t7 * 6858.5379856925456) - + K_result_tmp * 1215.8171078840321) - - b_K_result_tmp * 5116.7565361133811) - + c_K_result_tmp * 468.02132002607561)) - + (((d_K_result_tmp * 1822.110648684868 - e_K_result_tmp * 5531.0110808088057) - + f_K_result_tmp * 6330.6342246015874) - + 23.918447993158551); + (((((l_l * -7.0391718485499242 + l_r * 4.1396228642676061) + t2 * 12.33745678182799) + - t3 * 0.98606759263751176) + + t4 * 3.806439832412305) + + ((((t5 * -38.526555112601628 + t6 * 63.946154134287589) - t7 * 1.3092165744637649) + + K_result_tmp * 15.586793561656879) + + b_K_result_tmp * 64.7581727732454)) + + ((((c_K_result_tmp * -198.62878467860409 - d_K_result_tmp * 84.359584117828661) + - e_K_result_tmp * 14.215934150984531) + + f_K_result_tmp * 166.5259512146695) + + 1.7158251243263409); K_result[3] = - ((((((l_l * 346.73534170236547 - l_r * 61.162969453871128) - t2 * 2349.440418123213) - + t3 * 6376.9835107582576) - - t4 * 5700.0575049241206) - + t5 * 236.79479788310971) - + ((((t6 * -1624.9251344998779 + t7 * 2201.8983295157) - + K_result_tmp * 109.1319860299308) - + b_K_result_tmp * 3459.4266148232141) - - c_K_result_tmp * 4126.827233021615)) - + (((d_K_result_tmp * -3454.3777854229438 + e_K_result_tmp * 2670.930990837684) - + f_K_result_tmp * 963.163050643568) - + 23.856340045205862); + (((((l_l * 3.2505673234134531 - l_r * 3.500380774208117) - t2 * 50.611797162115543) + + t3 * 133.2326664631766) + - t4 * 115.87852399933379) + + ((((t5 * -25.07034906602615 + t6 * 77.646855700396756) - t7 * 82.720304705947541) + + K_result_tmp * 50.1415971452285) + - b_K_result_tmp * 75.962862672057426)) + + ((((c_K_result_tmp * 82.037299004697715 - d_K_result_tmp * 57.4476842579802) + + e_K_result_tmp * 37.039296399297562) + + f_K_result_tmp * 8.6305605362329239) + + 1.5594460786896229); K_result[4] = - ((((((l_l * 10.358543016163781 - l_r * 203.5005190393093) + t2 * 77.322396566930934) - - t3 * 534.43329728971435) - + t4 * 551.78736549994528) - + t5 * 809.48036773883371) - + (((((t6 * -1096.6801521517809 + t7 * 333.607780639757) - - K_result_tmp * 103.1587125717415) - - b_K_result_tmp * 1238.7424777305059) - + c_K_result_tmp * 1985.963425665808) - + d_K_result_tmp * 1302.772593072503)) - + ((e_K_result_tmp * -694.89934496015758 - f_K_result_tmp * 941.56357617693993) - + 13.69625723986436); + (((((l_l * 2.8285940429728611 - l_r * 5.1454314241415977) - t2 * 5.0429895063382792) + - t3 * 12.181920243732231) + + t4 * 20.20853074047443) + + ((((t5 * 1.832167410090793 + t6 * 49.666095833115328) - t7 * 71.585036684242255) + + K_result_tmp * 6.7614525434512522) + - b_K_result_tmp * 112.2320257202685)) + + ((((c_K_result_tmp * 119.3831306747833 + d_K_result_tmp * 76.132136747491444) + - e_K_result_tmp * 57.650962850068957) + - f_K_result_tmp * 15.931253109158931) + - 0.47857476782283581); K_result[5] = - (((((l_l * -66.640041023853186 - l_r * 141.47011617717121) + t2 * 150.59964643868221) - - t3 * 68.51003645798) - - t4 * 227.6211902207628) - + (((((t5 * 921.76947545449275 - t6 * 2275.2452174080072) + t7 * 1852.854117749418) - - K_result_tmp * 207.02288529783721) - + b_K_result_tmp * 547.88315957204441) - + c_K_result_tmp * 313.02286196382079)) - + (((d_K_result_tmp * 38.009318060403963 + e_K_result_tmp * 786.65440847261709) - - f_K_result_tmp * 1329.615475193938) - + 14.6845396412346); + (((((l_l * -9.9762094124309471 + l_r * 8.2824256552460209) + t2 * 26.72931192901931) + - t3 * 33.2666027507976) + + t4 * 8.2828108728362668) + + ((((t5 * -31.724688992305 + t6 * 81.101423007927608) - t7 * 84.684157838643614) + + K_result_tmp * 4.6527558336539547) + - b_K_result_tmp * 13.716265307127911)) + + ((((c_K_result_tmp * 58.061799134743993 - d_K_result_tmp * 22.912789739632519) + + e_K_result_tmp * 59.033367528110418) + - f_K_result_tmp * 54.943999830502257) + - 0.51141369450095631); K_result[6] = - ((((((l_l * -17.382379082981291 + l_r * 191.83433091723029) - t2 * 215.38248243992419) - + t3 * 177.04185746057911) - + t4 * 452.87712648183128) - - t5 * 1979.00131627843) - + (((((t6 * 5670.0460946420853 - t7 * 4746.17358236347) - + K_result_tmp * 798.05813369076827) - - b_K_result_tmp * 3247.4314296435468) - + c_K_result_tmp * 601.27494988667547) - + d_K_result_tmp * 857.500856843406)) - + ((e_K_result_tmp * -3074.645335299082 + f_K_result_tmp * 3794.5698038129) - + 24.769647992705309); + (((((l_l * -10.154870225751059 - l_r * 6.37776918322295) + t2 * 11.29176051531987) + + t3 * 8.8722661294377314) + - t4 * 1.8819662579794589) + + ((((t5 * 4.60605808880274 - t6 * 33.882556037844743) + t7 * 96.107182198047454) + + K_result_tmp * 34.992857966795448) + + b_K_result_tmp * 49.265448693483613)) + + ((((c_K_result_tmp * -238.1540715012849 - d_K_result_tmp * 115.2246820448856) + - e_K_result_tmp * 23.1633572479635) + + f_K_result_tmp * 235.54853927968949) + + 3.2205432084254011); K_result[7] = - ((((((l_l * 576.15348930080586 - l_r * 487.76939670822492) - t2 * 1893.6926319332761) - + t3 * 3681.5449393308159) - - t4 * 2816.6765021749479) - + t5 * 3090.264111200559) - + (((((t6 * -8407.85151129691 + t7 * 8002.5217822476279) - - K_result_tmp * 2108.2855913523831) - + b_K_result_tmp * 5840.5504568379029) - - c_K_result_tmp * 5036.24707424001) - + d_K_result_tmp * 1193.6117313315069)) - + ((e_K_result_tmp * -217.37287189235431 - f_K_result_tmp * 1939.0760868151449) - + 30.237336006835161); + (((((l_l * 8.7887078711611064 - l_r * 22.112513975325282) - t2 * 73.989854097659119) + + t3 * 176.850915622289) + - t4 * 145.318425792573) + + ((((t5 * 57.854515384424 - t6 * 101.671855514122) + t7 * 59.950083066232843) + + K_result_tmp * 48.290888622982067) + - b_K_result_tmp * 84.992610220595182)) + + ((((c_K_result_tmp * 107.7034607752615 - d_K_result_tmp * 36.994228932582857) + + e_K_result_tmp * 17.516753817495331) + - f_K_result_tmp * 7.7232399107197987) + + 3.04020285934469); K_result[8] = - (((((l_l * -4.9310648643240711 + l_r * 7.2002264945658059) + t2 * 56.541139543005222) - - t3 * 102.5320921260893) - + t4 * 26.27002527512958) - + ((((t5 * 50.068639129019552 - t6 * 200.98443862514611) + t7 * 166.1267154593283) - - K_result_tmp * 114.2926111207355) - + b_K_result_tmp * 277.88339705788911)) - + ((((c_K_result_tmp * -28.049826104012091 + d_K_result_tmp * 47.182490609015133) - + e_K_result_tmp * 194.55326847625531) - - f_K_result_tmp * 380.25992676832033) - - 1.030633894714478); + (((((l_l * -1.341768108308373 + l_r * 2.0675080402452779) + t2 * 5.9719771522544782) + - t3 * 13.735369286211069) + + t4 * 12.466035563884351) + + ((((t5 * -4.7099193109889059 + t6 * 2.9551341670866189) + t7 * 2.0567260095687718) + + K_result_tmp * 0.65410319297124675) + - b_K_result_tmp * 2.683887904192269)) + + ((((c_K_result_tmp * -3.40896810837682 + d_K_result_tmp * 5.390842430945586) + - e_K_result_tmp * 9.8911338552434529) + + f_K_result_tmp * 6.6989835210579134) + - 0.24391803832559791); K_result[9] = - (((((l_l * -5.0710009122425461 + l_r * 2.6087176730532828) - t2 * 26.10498657983155) - + t3 * 145.76721162582751) - - t4 * 152.43837950316191) - + ((((t5 * -8.0709031232571888 - t6 * 61.067919012609217) + t7 * 119.5131965706356) - + K_result_tmp * 41.566927163698153) - + b_K_result_tmp * 142.75388407411839)) - + ((((c_K_result_tmp * -246.33121589791989 - d_K_result_tmp * 246.5650211952194) - + e_K_result_tmp * 152.39762579184031) - + f_K_result_tmp * 145.09575109945391) - + 1.0715692326760511); + (((((l_l * -1.559554822738306 + l_r * 0.75321709360695388) + t2 * 3.743145643630144) + - t3 * 4.8734856284575) + + t4 * 1.2471528754593639) + + ((((t5 * -3.619584466709346 + t6 * 9.6796959498973223) - t7 * 8.6064081058276347) + - K_result_tmp * 1.5990467376817279) + - b_K_result_tmp * 6.1970291296308666)) + + ((((c_K_result_tmp * 8.2170799565409212 + d_K_result_tmp * 8.4705748348421874) + - e_K_result_tmp * 1.05563062850879) + - f_K_result_tmp * 6.9428579316985246) + + 0.2490163529167902); K_result[10] = - (((((l_l * 36.282243998885193 - l_r * 42.736359961147407) - t2 * 134.57305471180271) - + t3 * 135.76272734464609) - - t4 * 63.080334582434567) - + ((((t5 * 153.9990008140889 + t6 * 5.2197575556166971) - t7 * 310.97844675008218) - - K_result_tmp * 37.957816081050083) - - b_K_result_tmp * 645.94605036131293)) - + ((((c_K_result_tmp * 1026.4867885180111 + d_K_result_tmp * 579.75897072585519) - - e_K_result_tmp * 300.01225094583879) - - f_K_result_tmp * 419.3268494407335) - - 1.343725561213682); + (((((l_l * -0.82304214267667786 - l_r * 6.0675099681121312) + t2 * 5.38054700720794) + - t3 * 12.699803894120871) + + t4 * 11.24035769801084) + + ((((t5 * 35.395911327800107 - t6 * 82.305569188342062) + t7 * 69.198831566895777) + - K_result_tmp * 14.705594768955731) + + b_K_result_tmp * 31.02096841145816)) + + ((((c_K_result_tmp * -23.521075864380691 + d_K_result_tmp * 19.425620294282719) + - e_K_result_tmp * 12.73040430043762) + - f_K_result_tmp * 14.362960221684551) + - 0.01910547338046974); K_result[11] = - ((((((l_l * 50.308991746733767 - l_r * 42.088168791831322) - t2 * 167.9521291036682) - + t3 * 147.86587729762459) - + t4 * 49.125374064513913) - + t5 * 171.97186944647) - + ((((t6 * -294.74943355780908 + t7 * 211.6763639699939) - + K_result_tmp * 6.8727777660254219) - - b_K_result_tmp * 234.02410438551649) - + c_K_result_tmp * 142.19120834845069)) - + (((d_K_result_tmp * 317.23125017640132 - e_K_result_tmp * 467.73969646015257) - + f_K_result_tmp * 128.8736972561571) - + 1.221908704316141); + (((((l_l * 3.872202753627298 + l_r * 3.0977262629686) - t2 * 18.9380243053007) + + t3 * 45.893363568676868) + - t4 * 41.840086967144472) + + ((((t5 * -9.91916916591209 + t6 * 14.281122413414129) - t7 * 10.204817520388129) + + K_result_tmp * 2.1738726992458859) + + b_K_result_tmp * 8.0614755281300514)) + + ((((c_K_result_tmp * -0.22810761152897921 - d_K_result_tmp * 21.859084629550289) + + e_K_result_tmp * 27.69529001117715) + - f_K_result_tmp * 7.1287710182295783) + + 0.01637830255695779); K_result[12] = - (((((l_l * -0.43202559927386192 + l_r * 0.93803408263641763) + t2 * 23.607116009271479) - - t3 * 38.824180630565913) - - t4 * 0.32985508226492821) - + ((((t5 * 52.800897882197383 - t6 * 180.39639471996321) + t7 * 155.0680099600124) - - K_result_tmp * 77.970231172436769) - + b_K_result_tmp * 195.34719940933039)) - + ((((c_K_result_tmp * -43.711863873056 + d_K_result_tmp * 30.090161482807051) - + e_K_result_tmp * 110.6512271136605) - - f_K_result_tmp * 228.69006124297439) - - 0.436882415295074); + (((((l_l * -1.8478580245840139 + l_r * 2.959534805411745) + t2 * 7.7508367707514934) + - t3 * 17.371709188802431) + + t4 * 15.47320701537973) + + ((((t5 * -8.1954694077610313 + t6 * 9.7579436697661439) - t7 * 3.3176691510460379) + + K_result_tmp * 1.910833809201377) + - b_K_result_tmp * 6.4775401308520077)) + + ((((c_K_result_tmp * -0.53248387124696839 + d_K_result_tmp * 6.2176093680958706) + - e_K_result_tmp * 11.53634284427212) + + f_K_result_tmp * 7.9126970579635358) + - 0.30855666350815081); K_result[13] = - (((((l_l * -0.39877026474984828 - l_r * 2.102000934947367) - t2 * 29.817933341206331) - + t3 * 94.000592870016021) - - t4 * 80.942110508246131) - + ((((t5 * 17.1715754475731 - t6 * 86.7487985872154) + t7 * 112.80495582302861) - + K_result_tmp * 24.7703413840776) - + b_K_result_tmp * 57.078733547244923)) - + ((((c_K_result_tmp * -114.8656757646461 - d_K_result_tmp * 94.834802564094659) - + e_K_result_tmp * 40.30789843738642) - + f_K_result_tmp * 69.846040449244541) - + 0.57678686077621544); + (((((l_l * -2.1054029191801131 + l_r * 0.99969316946929176) + t2 * 4.2571840473558993) + - t3 * 5.2418907813031659) + + t4 * 1.0454909144757021) + + ((((t5 * -5.60706954435337 + t6 * 16.124893749903489) - t7 * 15.4486390745989) + - K_result_tmp * 0.17050548296766341) + - b_K_result_tmp * 13.77902340719332)) + + ((((c_K_result_tmp * 16.719054469203169 + d_K_result_tmp * 10.928540631199249) + - e_K_result_tmp * 1.8906549231684939) + - f_K_result_tmp * 8.5781770505704618) + + 0.30855257992253371); K_result[14] = - ((((((l_l * 41.01714688375553 - l_r * 41.740439088847211) - t2 * 134.0165738647942) - + t3 * 152.15407358304611) - - t4 * 77.631416308602311) - + t5 * 169.50036791429241) - + ((((t6 * -131.02195559427651 - t7 * 117.6264132202827) - - K_result_tmp * 59.34665458780011) - - b_K_result_tmp * 407.1482346494368) - + c_K_result_tmp * 722.92324002860767)) - + (((d_K_result_tmp * 452.26498436810181 - e_K_result_tmp * 230.8871877635589) - - f_K_result_tmp * 352.24463818778008) - - 0.73253334243048862); + (((((l_l * -0.925458609099666 - l_r * 8.3304605313851372) + t2 * 5.8705196123950776) + - t3 * 14.247848267239529) + + t4 * 13.103879385258219) + + ((((t5 * 49.875731821209641 - t6 * 119.7751589575233) + t7 * 103.715350391261) + - K_result_tmp * 19.49965846431434) + + b_K_result_tmp * 44.439955961888089)) + + ((((c_K_result_tmp * -36.818387129276928 + d_K_result_tmp * 24.63629215492475) + - e_K_result_tmp * 17.0968666836546) + - f_K_result_tmp * 17.25657234150486) + - 0.01312247351132222); K_result[15] = - (((((l_l * 53.20780754797925 - l_r * 59.363970356947632) - t2 * 56.741579275110738) - - t3 * 59.125893770778063) - + t4 * 140.4423314862585) - + (((((t5 * 399.685870960481 - t6 * 1055.4816716720329) + t7 * 993.77240053326977) - - K_result_tmp * 280.11459619488261) - + b_K_result_tmp * 608.686408048281) - - c_K_result_tmp * 534.19642558508349)) - + (((d_K_result_tmp * 346.9310960540945 - e_K_result_tmp * 240.2883599970055) - - f_K_result_tmp * 226.46361185602419) - + 1.168728749491823); + (((((l_l * 5.3340410979409159 + l_r * 3.7097142416343569) - t2 * 23.9767968934346) + + t3 * 56.723299093475063) + - t4 * 51.221461195396927) + + ((((t5 * -10.685977771085909 + t6 * 13.46359276363065) - t7 * 8.4510178889795391) + - K_result_tmp * 0.5725990958073367) + + b_K_result_tmp * 15.901070547299479)) + + ((((c_K_result_tmp * -5.1633341396214769 - d_K_result_tmp * 23.07671182323989) + + e_K_result_tmp * 32.04392799053177) + - f_K_result_tmp * 11.68760478880241) + + 0.02861473054099849); K_result[16] = - (((((l_l * 11.71972683002242 - l_r * 126.87142463921) - t2 * 73.646496070906039) - - t3 * 5.1831939756143024) - + t4 * 125.97561603422059) - + ((((t5 * 200.532465328053 + t6 * 299.4401806851692) - t7 * 618.256090284853) - + K_result_tmp * 189.38416508910791) - - b_K_result_tmp * 1214.622066777335)) - + ((((c_K_result_tmp * 1018.5690855414171 + d_K_result_tmp * 444.1479800549875) - - e_K_result_tmp * 577.54019833923269) - + f_K_result_tmp * 354.74738960072727) - + 2.880316436994236); + (((((l_l * 5.2972848332551621 - l_r * 27.429859360067429) - t2 * 27.127116215748519) + + t3 * 47.878168322524381) + - t4 * 36.153594345135581) + + (((((t5 * 17.443979497381079 + t6 * 110.0345464204547) - t7 * 149.3114795084017) + + K_result_tmp * 37.676996153487543) + - b_K_result_tmp * 181.78752464772509) + + c_K_result_tmp * 154.21686219229)) + + (((d_K_result_tmp * 8.13571881088043 - e_K_result_tmp * 2.945041346048114) + + f_K_result_tmp * 33.4683373212623) + + 0.70001959812415449); K_result[17] = - (((((l_l * -16.01687326095627 - l_r * 31.17209782525941) + t2 * 68.353368980049353) - - t3 * 79.657700428563857) - - t4 * 42.980493838949137) - + ((((t5 * 237.80812859490371 - t6 * 498.43661415478158) + t7 * 294.5933925236248) - - K_result_tmp * 177.22445950336839) - + b_K_result_tmp * 408.33392276148072)) - + ((((c_K_result_tmp * 83.376925162558152 + d_K_result_tmp * 13.27478528759312) - + e_K_result_tmp * 414.98285182467231) - - f_K_result_tmp * 686.57475645659724) - + 3.4401540343306758); + (((((l_l * -2.6513487306387442 - l_r * 18.037859626648331) + t2 * 14.412687045731159) + - t3 * 12.434191988956851) + - t4 * 7.6245878901630686) + + ((((t5 * 84.35383577919491 - t6 * 153.9335207875358) + t7 * 91.743799676562475) + + K_result_tmp * 5.9187789781112938) + + b_K_result_tmp * 50.713128893562377)) + + ((((c_K_result_tmp * 33.854175270461248 - d_K_result_tmp * 90.860755680034828) + + e_K_result_tmp * 122.73785652708671) + - f_K_result_tmp * 85.053267051066925) + + 0.48462980939584238); K_result[18] = - (((((l_l * -43.709720515444488 + l_r * 393.82610533556812) - t2 * 266.68184468524419) - + t3 * 838.24841844184073) - - t4 * 429.42581454015033) - + (((((t5 * -3067.44921170092 + t6 * 7650.62377132923) - t7 * 5856.5509642053639) - + K_result_tmp * 1149.4480724340131) - - b_K_result_tmp * 2034.8252778743331) - - c_K_result_tmp * 1385.288317518226)) - + (((d_K_result_tmp * -1471.326502745718 - e_K_result_tmp * 1010.378770914998) - + f_K_result_tmp * 4588.4212273993817) - + 32.289415910184253); + (((((l_l * -7.6662812062521306 + l_r * 86.64332891702847) - t2 * 5.0711482423355161) + + t3 * 62.102579908294658) + - t4 * 58.259796365305888) + + ((((t5 * -405.00361772778541 + t6 * 818.8772814087655) - t7 * 565.84129440775234) + + K_result_tmp * 122.0404229213346) + - b_K_result_tmp * 116.9914011133222)) + + ((((c_K_result_tmp * -146.00639918581891 - d_K_result_tmp * 252.15737405522279) + + e_K_result_tmp * 72.870754721170741) + + f_K_result_tmp * 332.55928318870139) + - 2.045707106826081); K_result[19] = - ((((((l_l * 246.94947250910911 - l_r * 172.80197916402381) - t2 * 1015.58091952831) - + t3 * 1671.5787677020851) - - t4 * 1168.1174111382211) - + t5 * 154.45950854878919) - + ((((t6 * 744.889689120951 - t7 * 1525.1144624647891) - + K_result_tmp * 228.11665881923381) - - b_K_result_tmp * 1449.14937107582) - + c_K_result_tmp * 2546.3764006341162)) - + (((d_K_result_tmp * 563.74347478178186 + e_K_result_tmp * 60.4695447475869) - - f_K_result_tmp * 1157.7491510298159) - - 2.545814116656036); + (((((l_l * -2.3255131457247682 - l_r * 21.225654779075239) - t2 * 4.8041773430892656) + - t3 * 2.431456755967782) + + t4 * 24.582564440978409) + + ((((t5 * -10.033306799041689 + t6 * 11.49419832788041) - t7 * 3.9373859134706959) + - K_result_tmp * 6.9870949680485657) + + b_K_result_tmp * 106.0025640537179)) + + ((((c_K_result_tmp * -78.868776410047161 + d_K_result_tmp * 54.345940883954533) + - e_K_result_tmp * 89.251445873726638) + - f_K_result_tmp * 38.089211539308117) + + 2.044158060186823); K_result[20] = - (((((l_l * -0.023223509853613981 - l_r * 6.5635779314468019) + t2 * 1.128577109523099) - - t3 * 18.16450235434268) - + t4 * 23.32388944869983) - + ((((t5 * 5.6532418086495344 + t6 * 27.88689967980461) - t7 * 42.909497284496616) - + K_result_tmp * 5.7128580483588234) - - b_K_result_tmp * 87.214080251262772)) - + ((((c_K_result_tmp * 78.417535247498066 + d_K_result_tmp * 57.249764314084231) - - e_K_result_tmp * 53.37310163295372) - + f_K_result_tmp * 8.8684223183067559) - + 0.381593736642001); + (((((l_l * 0.25813515449237068 - l_r * 1.991685745227957) - t2 * 0.72340081465349571) + - t3 * 0.1410253350059455) + + t4 * 0.38709288239498713) + + ((((t5 * 4.5238258868673782 - t6 * 2.4165966667437462) + t7 * 0.77889824111916373) + + K_result_tmp * 0.5669873455993214) + - b_K_result_tmp * 15.65918265225269)) + + ((((c_K_result_tmp * 10.34154274718372 + d_K_result_tmp * 8.2511713067634211) + - e_K_result_tmp * 5.12289167464622) + + f_K_result_tmp * 2.0333988771688061) + - 0.081300535630453488); K_result[21] = - (((((l_l * -0.81101760566251568 - l_r * 3.8133722424750678) - t2 * 0.26401393871443041) - + t3 * 9.9088294525670779) - - t4 * 22.484918999011249) - + ((((t5 * 27.65949052736654 - t6 * 56.9605417835658) + t7 * 36.109541359064742) - - K_result_tmp * 16.9861695236361) - + b_K_result_tmp * 20.572638323789018)) - + ((((c_K_result_tmp * 33.923466245675108 + d_K_result_tmp * 10.60039299949762) - + e_K_result_tmp * 44.173594071078149) - - f_K_result_tmp * 81.474705141723163) - + 0.41851458577514777); + (((((l_l * -0.6496084376171678 + l_r * 0.064060538620080987) + t2 * 0.15319711595788929) + + t3 * 6.1538887721810083) + - t4 * 9.72888858732645) + + ((((t5 * 0.83001962624743886 - t6 * 2.0059919875383) + t7 * 1.73040005752884) + + K_result_tmp * 0.50847684393085846) + + b_K_result_tmp * 14.0194728677426)) + + ((((c_K_result_tmp * -5.594666654232654 - d_K_result_tmp * 17.623741312671779) + + e_K_result_tmp * 23.66589919486816) + - f_K_result_tmp * 12.30395322310088) + + 0.002729575343444598); K_result[22] = - (((((l_l * -0.859924635086052 + l_r * 23.3867212640602) - t2 * 19.954656301622769) - + t3 * 45.166391583956091) - - t4 * 4.7711413736387334) - + ((((t5 * -142.46824266969909 + t6 * 340.19874205057272) - t7 * 223.09499644210081) - + K_result_tmp * 57.848301015665143) - - b_K_result_tmp * 140.3500870510145)) - + ((((c_K_result_tmp * -133.45552213558221 - d_K_result_tmp * 27.398962553527149) - - e_K_result_tmp * 165.26862007713291) - + f_K_result_tmp * 356.39030582177151) - + 1.136938441957398); + (((((l_l * -0.073161745714453288 - l_r * 0.938973252579294) - t2 * 5.2438383168354026) + + t3 * 15.708350806611611) + - t4 * 11.37576169115421) + + ((((t5 * -4.78000014867496 + t6 * 24.19477048650754) - t7 * 16.647789922927359) + + K_result_tmp * 9.8243406018208486) + - b_K_result_tmp * 11.0895530901302)) + + ((((c_K_result_tmp * -25.546895378673572 - d_K_result_tmp * 19.034984398896281) + - e_K_result_tmp * 5.405871978560981) + + f_K_result_tmp * 45.6896537802703) + + 0.83952349752701394); K_result[23] = - (((((l_l * 25.5467754669838 - l_r * 11.53039060762849) - t2 * 94.819296726406222) - + t3 * 182.22319590961729) - - t4 * 145.6811041000332) - + ((((t5 * 49.894308154300127 - t6 * 115.98612817156879) + t7 * 74.874780122016972) - - K_result_tmp * 27.631266508415891) - + b_K_result_tmp * 79.693725093069489)) - + ((((c_K_result_tmp * 25.01269019934249 - d_K_result_tmp * 2.8296726892568569) - + e_K_result_tmp * 71.2220134291065) - - f_K_result_tmp * 130.06363407940711) - + 0.056580538236626947); + (((((l_l * 3.7379527159754811 - l_r * 2.9064338875911861) - t2 * 17.72551739841867) + + t3 * 28.81301008142221) + - t4 * 16.49390613756561) + + ((((t5 * -1.433685008440267 + t6 * 5.8595958967904336) - t7 * 14.73704569383646) + + K_result_tmp * 8.8501264281054368) + - b_K_result_tmp * 21.786102608478359)) + + ((((c_K_result_tmp * 40.09534130182378 + d_K_result_tmp * 8.50776276108387) + - e_K_result_tmp * 11.44049203196354) + - f_K_result_tmp * 12.31666034251559) + - 0.01719178508400724); K_result[24] = - (((((l_l * -7.507923807477499 - l_r * 47.1166073834091) + t2 * 10.609183847451559) - - t3 * 266.8715875045138) - + t4 * 445.92045409782958) - + ((((t5 * -138.66856730072519 + t6 * 714.8166512354735) - t7 * 717.88430896844932) - + K_result_tmp * 305.36977833759732) - - b_K_result_tmp * 1290.1053940014231)) - + ((((c_K_result_tmp * 795.13732224673527 + d_K_result_tmp * 563.46305906966563) - - e_K_result_tmp * 1024.8457594211461) - + f_K_result_tmp * 672.41645240069522) - + 3.768869583068311); + (((((l_l * 0.39395398967460737 - l_r * 22.31504281772045) - t2 * 17.1137095559913) + + t3 * 41.012720713808832) + - t4 * 22.999044032147321) + + ((((t5 * 25.83816181171353 - t6 * 85.786788743370352) + t7 * 65.277973447427058) + + K_result_tmp * 103.8563188283494) + - b_K_result_tmp * 15.509350701766319)) + + ((((c_K_result_tmp * 6.874830876976616 - d_K_result_tmp * 166.0861494290217) + + e_K_result_tmp * 69.567862638225591) + + f_K_result_tmp * 54.2789691437973) + + 0.54290832206822459); K_result[25] = - (((((l_l * -96.5181386929551 - l_r * 13.29649407893276) + t2 * 296.91137344618278) - - t3 * 492.02077209854048) - + t4 * 258.47396521460638) - + ((((t5 * 64.41551158197602 + t6 * 116.7904093052677) - t7 * 299.53651415115871) - - K_result_tmp * 82.217207855106835) - - b_K_result_tmp * 571.88933368168887)) - + ((((c_K_result_tmp * 890.06726979519453 + d_K_result_tmp * 570.63485502912943) - - e_K_result_tmp * 170.323622829347) - - f_K_result_tmp * 468.472627641117) - + 2.672247540619805); + (((((l_l * -9.3121185838227 - l_r * 15.90891661324488) + t2 * 64.025011560449) + - t3 * 219.10982748723569) + + t4 * 256.696813709747) + + ((((t5 * 19.168159800935211 - t6 * 6.6781647121163639) + t7 * 26.660720978392511) + - K_result_tmp * 38.716894684700073) + - b_K_result_tmp * 138.5472259086842)) + + ((((c_K_result_tmp * -30.31039897392381 + d_K_result_tmp * 313.68037462772759) + - e_K_result_tmp * 439.9176421685662) + + f_K_result_tmp * 210.8260580402783) + + 0.89208135906197927); K_result[26] = - ((((((l_l * -319.00139678526261 + l_r * 322.51636869379888) + t2 * 1281.5527044375519) - - t3 * 2311.5125789206832) - + t4 * 1993.574700206831) - - t5 * 1044.332181315531) - + ((((t6 * -1.7918399485947849 + t7 * 1992.929250143161) - - K_result_tmp * 490.87684038932031) - + b_K_result_tmp * 3668.6169654858149) - - c_K_result_tmp * 5930.3772134101646)) - + (((d_K_result_tmp * -637.32449153956736 - e_K_result_tmp * 1111.1476465127009) - + f_K_result_tmp * 2400.8298830774779) - + 1.784851464616279); + (((((l_l * -17.531404935947339 - l_r * 5.5379421147849488) + t2 * 86.877975173402476) + - t3 * 162.8652804614745) + + t4 * 124.03639814451761) + + ((((t5 * -65.151898126156809 + t6 * 6.3907904320355344) + t7 * 58.1802335450001) + - K_result_tmp * 47.981448308496923) + + b_K_result_tmp * 421.45566825329041)) + + ((((c_K_result_tmp * -306.84781257881059 - d_K_result_tmp * 81.888196573182839) + + e_K_result_tmp * 91.9600372018616) + - f_K_result_tmp * 167.19745475043371) + + 2.043252259788491); K_result[27] = - ((((((l_l * 216.9577621182199 + l_r * 165.24949179860539) - t2 * 1864.3618153585389) - + t3 * 4655.7469807600628) - - t4 * 4112.06601451245) - - t5 * 505.81034264548231) - + (((((t6 * 1269.1369867703561 - t7 * 1326.40938672467) - - K_result_tmp * 5.4108660341742638) - - b_K_result_tmp * 1147.335547410969) - + c_K_result_tmp * 1890.0830855029651) - + d_K_result_tmp * 653.95338161024029)) - + ((e_K_result_tmp * 5.084398864909816 - f_K_result_tmp * 917.35945255922047) - + 30.628295146352809); + (((((l_l * 14.9553066935295 + l_r * 68.003429108183923) - t2 * 46.809788102527612) + + t3 * 64.579803561288415) + - t4 * 44.57757731944951) + + ((((t5 * 35.706257783390761 - t6 * 14.703161553398941) - t7 * 33.940970292680113) + - K_result_tmp * 302.03583695239479) + - b_K_result_tmp * 169.7211351741139)) + + ((((c_K_result_tmp * 188.32434877881221 + d_K_result_tmp * 696.98712982797178) + - e_K_result_tmp * 508.53543179759657) + - f_K_result_tmp * 26.364143072420081) + - 2.26029321870165); K_result[28] = - (((((l_l * -0.93660812157978057 - l_r * 3.9207061529273979) + t2 * 6.3893228756827174) - - t3 * 32.640909299313648) - + t4 * 38.518328893373322) - + ((((t5 * -1.3581206122441249 + t6 * 29.509888285995231) - t7 * 35.875273533069631) - + K_result_tmp * 6.89319242137903) - - b_K_result_tmp * 83.997723203541511)) - + ((((c_K_result_tmp * 67.489891732481766 + d_K_result_tmp * 67.7587501475345) - - e_K_result_tmp * 72.202271572623545) - + f_K_result_tmp * 14.94907237319339) - + 0.426380579431373); + (((((l_l * 0.48235990137702212 - l_r * 1.089085121366868) - t2 * 1.6216826044552231) + - t3 * 1.340524465328029) + + t4 * 4.9655661234827244) + + ((((t5 * -2.2966994150783089 + t6 * 10.483703915088411) - t7 * 10.20090928184332) + + K_result_tmp * 5.5681353016833661) + - b_K_result_tmp * 19.228317334687461)) + + ((((c_K_result_tmp * 8.6157599887378176 + d_K_result_tmp * 10.25151372048242) + - e_K_result_tmp * 17.685666776294831) + + f_K_result_tmp * 12.35620566699143) + + 0.0030673700589297151); K_result[29] = - (((((l_l * -3.4564175049273271 - l_r * 2.8998377043498329) + t2 * 3.9341691070930191) - + t3 * 6.8941482393910869) - - t4 * 22.95899407953609) - + ((((t5 * 20.53769497924948 - t6 * 36.025499539084379) + t7 * 18.47372141377998) - - K_result_tmp * 13.55529297648641) - - b_K_result_tmp * 12.09926055650894)) - + ((((c_K_result_tmp * 53.709465603232132 + d_K_result_tmp * 24.886483527707281) - + e_K_result_tmp * 27.31558305115016) - - f_K_result_tmp * 65.45860269074997) - + 0.37429283306853461); + (((((l_l * -1.911920897125398 + l_r * 0.24593426869670651) + t2 * 7.14798796912681) + - t3 * 13.34683038357106) + + t4 * 8.36966630134579) + + ((((t5 * -0.82327262510986665 + t6 * 5.6353471818981644) - t7 * 7.20030692817838) + - K_result_tmp * 2.3626434219982122) + - b_K_result_tmp * 10.84559032635412)) + + ((((c_K_result_tmp * 14.191595994996121 + d_K_result_tmp * 9.5820208368917452) + - e_K_result_tmp * 0.72146033628724082) + - f_K_result_tmp * 7.0720766273360356) + - 0.085054204897638916); K_result[30] = - (((((l_l * -9.6565031018839225 + l_r * 22.46623331639055) + t2 * 33.535209093237832) - - t3 * 70.054736204326119) - + t4 * 88.363856370153215) - + ((((t5 * -79.463062013880091 + t6 * 87.9501050435872) + t7 * 33.580415923316309) - - K_result_tmp * 21.403960521334191) - + b_K_result_tmp * 77.401455525504232)) - + ((((c_K_result_tmp * -297.50683047393392 + d_K_result_tmp * 41.411476255159577) - - e_K_result_tmp * 203.881334292463) - + f_K_result_tmp * 273.94901210779261) - + 0.13197911497832041); + (((((l_l * -4.1742566793689893 + l_r * 4.53872156768127) + t2 * 15.50734575111896) + - t3 * 21.734029451530979) + + t4 * 14.01623348892873) + + ((((t5 * -15.81279274016652 + t6 * 10.84830614627616) + t7 * 9.7763982601301826) + - K_result_tmp * 7.5835839567921228) + + b_K_result_tmp * 50.738989806783657)) + + ((((c_K_result_tmp * -66.155558012529809 - d_K_result_tmp * 23.528291640663021) + + e_K_result_tmp * 9.4980639030979326) + + f_K_result_tmp * 21.578793969913871) + + 0.01328455083350657); K_result[31] = - (((((l_l * 24.98521763004219 - l_r * 2.406387638957264) - t2 * 132.00968755691991) - + t3 * 308.50328620027989) - - t4 * 270.01982466300223) - + ((((t5 * 30.918646957157151 - t6 * 86.86839369021385) + t7 * 70.996646210221073) - - K_result_tmp * 3.0275374397098092) - + b_K_result_tmp * 40.753968446558659)) - + ((((c_K_result_tmp * 23.724421961450911 - d_K_result_tmp * 49.46111118869301) - + e_K_result_tmp * 107.0432400303656) - - f_K_result_tmp * 94.1927808093761) - + 1.1410323176360571); + (((((l_l * -1.1551293155815809 + l_r * 0.33674493816424428) - t2 * 8.7266330653949975) + + t3 * 35.817481600252279) + - t4 * 38.012160623059557) + + ((((t5 * -4.8098738057536732 + t6 * 11.551236579190411) - t7 * 10.063494830761011) + + K_result_tmp * 12.20219053016346) + - b_K_result_tmp * 5.6684943630282083)) + + ((((c_K_result_tmp * 4.9475479979805126 - d_K_result_tmp * 29.384960137862869) + + e_K_result_tmp * 28.1800964917464) + - f_K_result_tmp * 0.2356079701679093) + + 0.829686679336074); K_result[32] = - ((((((l_l * 607.53713413393325 - l_r * 696.2848371822729) - t2 * 1481.909295298899) - + t3 * 771.28417173648961) - + t4 * 653.58511153113011) - + t5 * 4649.893859876287) - + (((((t6 * -10198.934594286249 + t7 * 7524.6262842060669) - - K_result_tmp * 1803.8962667109561) - - b_K_result_tmp * 371.36760215984492) - + c_K_result_tmp * 3389.8025393428829) - + d_K_result_tmp * 6256.5380716800692)) - + ((e_K_result_tmp * -4408.9658815096582 - f_K_result_tmp * 4173.7309837285793) - - 58.828410836047311); + ((((((l_l * 41.585061490786657 + l_r * 6.0561184927155036) - t2 * 131.44125250470489) + + t3 * 276.60369310605978) + - t4 * 243.16860840798989) + + t5 * 138.8342320803927) + + ((((t6 * -536.99420235581056 + t7 * 506.83627363105433) + - K_result_tmp * 30.974532445559522) + + b_K_result_tmp * 284.90102053227321) + - c_K_result_tmp * 81.1773840485689)) + + (((d_K_result_tmp * -150.17204003042119 + e_K_result_tmp * 258.84938701550237) + - f_K_result_tmp * 270.52079892709418) + - 15.43664491638706); K_result[33] = - ((((((l_l * -653.62358004500493 + l_r * 557.5978482450646) + t2 * 4807.3614757648638) - - t3 * 10480.149373651589) - + t4 * 7704.6958238523712) - - t5 * 606.40353516783262) - + (((((t6 * -1454.7355302778581 + t7 * 2636.839410467765) - - K_result_tmp * 2814.0824663909061) - + b_K_result_tmp * 8541.1366494593258) - - c_K_result_tmp * 6538.1961562525667) - - d_K_result_tmp * 157.1491500240777)) - + ((e_K_result_tmp * 3481.4038070772981 - f_K_result_tmp * 4334.5626868743384) - - 58.174164936061928); + ((((((l_l * 7.7812536457489934 + l_r * 34.58984430113123) + t2 * 162.2342532314228) + - t3 * 580.49606213351228) + + t4 * 615.709068989336) + + t5 * 1.864881445630753) + + ((((t6 * -152.9485031252396 + t7 * 246.92556925550139) + - K_result_tmp * 157.85401654892149) + + b_K_result_tmp * 241.32772316785261) + - c_K_result_tmp * 414.22555809740021)) + + (((d_K_result_tmp * 300.17161904155921 - e_K_result_tmp * 403.90374035890858) + + f_K_result_tmp * 176.2543843424576) + - 15.14240934276431); K_result[34] = - ((((((l_l * 1183.8346432291671 - l_r * 2654.290790420202) - t2 * 2635.9010415423509) - - t3 * 1287.9984721500371) - + t4 * 5735.6749771293626) - + t5 * 4459.0681221568229) - + (((((t6 * 3983.2381897822 - t7 * 10056.130412468839) - + K_result_tmp * 448.67823229709188) - - b_K_result_tmp * 12090.75441154277) - + c_K_result_tmp * 9151.2726429941231) - + d_K_result_tmp * 10248.6786824896)) - + ((e_K_result_tmp * -13373.787410082219 + f_K_result_tmp * 4986.8373581834312) - - 7.57255868874563); + ((((((l_l * 155.13595827720121 - l_r * 418.37583361314842) - t2 * 543.373641613612) + + t3 * 691.76926597183729) + - t4 * 271.23706554060283) + + t5 * 1431.482565727471) + + ((((t6 * -2267.7472400909778 + t7 * 1429.60424102931) + + K_result_tmp * 27.746069443133429) + - b_K_result_tmp * 640.74522176670746) + + c_K_result_tmp * 541.46947755639383)) + + (((d_K_result_tmp * 656.83867263586546 - e_K_result_tmp * 736.97392885675254) + + f_K_result_tmp * 95.477527712626284) + - 15.81354296862791); K_result[35] = - ((((((l_l * -3351.3705772912549 + l_r * 1984.62856320945) + t2 * 9875.8191070780176) - - t3 * 11830.95528127786) - + t4 * 3489.0858320555872) - - t5 * 7068.4015245379878) - + (((((t6 * 7665.3833809610414 - t7 * 2949.4392326532789) - - K_result_tmp * 1201.8409809460941) - + b_K_result_tmp * 12974.60851081697) - - c_K_result_tmp * 6575.574460505457) - - d_K_result_tmp * 6245.6128033243458)) - + ((e_K_result_tmp * 11824.819651184051 - f_K_result_tmp * 10900.22429865702) - - 12.510139857385081); + ((((((l_l * -412.44731155317493 + l_r * 157.575918973223) + t2 * 1547.814259521376) + - t3 * 2744.5204060610122) + + t4 * 1820.219560753239) + - t5 * 572.5530328321322) + + ((((t6 * 974.3885700408365 - t7 * 772.40933205050339) + - K_result_tmp * 114.0501719428118) + + b_K_result_tmp * 404.09123492440278) + + c_K_result_tmp * 196.5762382296258)) + + (((d_K_result_tmp * -54.763517385076867 + e_K_result_tmp * 598.99679823348617) + - f_K_result_tmp * 907.63909918035654) + - 16.1955171039867); K_result[36] = - (((((l_l * 36.0877702072119 - l_r * 30.580318122323579) - t2 * 101.638356062782) - + t3 * 72.829249464794316) - + t4 * 22.70019064939078) - + ((((t5 * 203.89476900667071 - t6 * 404.672129286879) + t7 * 260.51444559800558) - - K_result_tmp * 90.157603500094766) - - b_K_result_tmp * 132.9697388620219)) - + ((((c_K_result_tmp * 296.06988343834621 + d_K_result_tmp * 403.10214342362121) - - e_K_result_tmp * 313.25367762463378) - - f_K_result_tmp * 200.96366946658051) - - 2.707053077709038); + (((((l_l * 4.8673282012038941 - l_r * 0.02810096066532166) - t2 * 15.310148167403151) + + t3 * 25.521677086020411) + - t4 * 17.4995686305953) + + ((((t5 * 9.638848303935486 - t6 * 30.50688725574598) + t7 * 25.60443722440948) + - K_result_tmp * 6.1293028770247213) + + b_K_result_tmp * 13.776513259656561)) + + ((((c_K_result_tmp * 1.203417106113112 + d_K_result_tmp * 7.4006684172429109) + - e_K_result_tmp * 0.078280734495663432) + - f_K_result_tmp * 18.943781238278682) + - 1.1179990644923421); K_result[37] = - (((((l_l * -17.310111615595719 + l_r * 21.923227715916841) + t2 * 157.3147881714151) - - t3 * 360.94549731649738) - + t4 * 262.35701082560342) - + ((((t5 * -8.69698923971202 - t6 * 107.9836556916003) + t7 * 139.57284078244061) - - K_result_tmp * 132.84274287187219) - + b_K_result_tmp * 332.53059461138758)) - + ((((c_K_result_tmp * -173.8348184266973 + d_K_result_tmp * 68.460363779449423) - + e_K_result_tmp * 125.6388283139173) - - f_K_result_tmp * 285.323977202314) - - 2.629495660749217); + (((((l_l * 0.9497695471639569 + l_r * 3.5989617911173322) + t2 * 10.71591703155174) + - t3 * 39.263582735580172) + + t4 * 41.177770681494088) + + ((((t5 * -5.4710586523999183 + t6 * 1.242243077227225) + t7 * 6.6991911879272843) + - K_result_tmp * 15.509720256253139) + + b_K_result_tmp * 22.679641928653769)) + + ((((c_K_result_tmp * -27.77118741940037 + d_K_result_tmp * 28.346828103653149) + - e_K_result_tmp * 30.186470237116961) + + f_K_result_tmp * 2.5521836768642578) + - 1.1001494805476479); K_result[38] = - (((((l_l * 13.636902704442891 - l_r * 91.497385538597968) - t2 * 71.889304191408968) - - t3 * 5.9134111513780248) - + t4 * 168.72072699920389) - + ((((t5 * 149.41072733500559 + t6 * 148.19191714789559) - t7 * 332.86042106760652) - + K_result_tmp * 209.91367021267271) - - b_K_result_tmp * 825.57826428644466)) - + ((((c_K_result_tmp * 504.8685922059783 + d_K_result_tmp * 163.82363148595121) - - e_K_result_tmp * 520.06893433140851) - + f_K_result_tmp * 554.1934582432765) - + 3.7444836441482678); + (((((l_l * 7.8471235256337026 - l_r * 16.3594913949482) - t2 * 26.445121281465969) + + t3 * 34.7361106687739) + - t4 * 13.76921938954532) + + ((((t5 * 46.241413401076052 - t6 * 61.397993220193591) + t7 * 34.434033845919679) + + K_result_tmp * 6.39408193708345) + - b_K_result_tmp * 30.03968685748271)) + + ((((c_K_result_tmp * 13.579957902895771 + d_K_result_tmp * 17.222149285946809) + - e_K_result_tmp * 31.03259575564466) + + f_K_result_tmp * 20.449160129221291) + - 1.0657499207339971); K_result[39] = - (((((l_l * -79.482308974302569 + l_r * 0.88051189319374024) + t2 * 222.0089239157156) - - t3 * 184.5611579248029) - - t4 * 54.1993861076182) - + ((((t5 * 78.814542616513108 - t6 * 417.95942198293739) + t7 * 468.20554173223428) - - K_result_tmp * 13.01893698462518) - + b_K_result_tmp * 438.38586367721712)) - + ((((c_K_result_tmp * -307.11979965685009 - d_K_result_tmp * 350.89451154995533) - + e_K_result_tmp * 522.64203324908226) - - f_K_result_tmp * 262.31647394030739) - + 3.8682610522381911); + (((((l_l * -19.99529511026709 + l_r * 12.06171890570675) + t2 * 72.537743990509469) + - t3 * 125.62751900310739) + + t4 * 81.444256368884183) + + ((((t5 * -37.515685359414221 + t6 * 63.248612731420778) - t7 * 49.34794327831667) + - K_result_tmp * 12.44633289988889) + + b_K_result_tmp * 18.729910747704128)) + + ((((c_K_result_tmp * 12.43815684624545 + d_K_result_tmp * 13.218379348440161) + + e_K_result_tmp * 16.536193254616439) + - f_K_result_tmp * 45.349373542642482) + - 1.095498659286591); gain_ = Eigen::Map>(K_result); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index 32bb6f59..ea637854 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -219,11 +219,11 @@ class WheelLegInfantry , chassis_wheel_motors_( {infantry, infantry_command, "/chassis/left_wheel", device::DjiMotor::Config{device::DjiMotor::Type::M3508} - .set_reduction_ratio(268.0 / 17) + .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) + .set_reduction_ratio(268.0 / 17.0) .enable_multi_turn_angle() .set_reversed()}) , chassis_hip_motors( @@ -326,15 +326,15 @@ class WheelLegInfantry 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_angle_command()); transmit_buffer_.add_can1_transmission( 0x02, chassis_hip_motors[1].generate_angle_command()); - control_commands[0] = chassis_wheel_motors_[0].generate_command(); - transmit_buffer_.add_can1_transmission( - 0x200, std::bit_cast(control_commands)); - control_commands[0] = 0; control_commands[1] = chassis_wheel_motors_[1].generate_command(); transmit_buffer_.add_can2_transmission( 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..2c75b8e6 --- /dev/null +++ b/rmcs_ws/src/rmcs_msgs/include/rmcs_msgs/wheel_leg_mode.hpp @@ -0,0 +1,15 @@ +#pragma once + +#include + +namespace rmcs_msgs { +enum class WheelLegMode : uint8_t { + STOP = 0, + SPIN = 1, + SPIN_2_FOLLOW = 2, + STEP_DOWN = 3, + LAUNCH_RAMP = 4, + BALANCELESS = 5, +}; + +} From 447ca2b0f2e08ba1712365031da5384279e5d64a Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sun, 1 Mar 2026 23:29:32 +0800 Subject: [PATCH 30/35] Rewrite desire_state_solver as a controller --- .../config/wheelleg-infantry.yaml | 2 +- rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../controller/chassis/wheel_leg/README.md | 1 + .../wheel_leg/desire_state_controller.cpp | 93 ++ .../chassis/wheel_leg/desire_state_solver.hpp | 65 +- .../wheel_leg_chassis_controller.cpp | 244 ++++++ .../wheel_leg/wheel_leg_controller.cpp | 61 +- .../src/controller/lqr/lqr_calculator.hpp | 824 +++++++++--------- .../src/hardware/wheelleg-infantry.cpp | 12 +- .../include/rmcs_msgs/wheel_leg_mode.hpp | 7 +- 10 files changed, 838 insertions(+), 474 deletions(-) create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp create mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_chassis_controller.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml index f9763639..95882a46 100644 --- a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml @@ -21,7 +21,7 @@ rmcs_executor: # - rmcs_core::controller::pid::PidController -> left_friction_velocity_pid_controller # - rmcs_core::controller::pid::PidController -> right_friction_velocity_pid_controller - - rmcs_core::controller::chassis::ChassisController -> chassis_controller + - rmcs_core::controller::chassis::WheelLegChassisController -> chassis_controller # - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - rmcs_core::controller::chassis::WheelLegController -> wheel_leg_controller diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 6f60f99f..2cb6b2bb 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -22,6 +22,9 @@ Test plugin. + + + Test plugin. Test plugin. 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 index e69de29b..51984762 100644 --- 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 @@ -0,0 +1 @@ +# desire state diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp new file mode 100644 index 00000000..489e6668 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp @@ -0,0 +1,93 @@ +#include +#include +#include +#include +#include + +namespace rmcs_core::controller::chassis { + +class DesireStateController + : public rmcs_executor::Component + , public rclcpp::Node { + +public: + DesireStateController() + : Node( + get_component_name(), + rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + register_input("/chassis/control_velocity", chassis_control_velocity_); + register_input("/chassis/leg/extended", is_leg_extended_); + register_input("/chassis/control_power_limit", power_limit_); + + reset(); + } + void reset() {} + + void update() override { + auto control_velocity = calculate_chassis_control_velocity(); + auto desire_state = update_desire_states(control_velocity); + + *desire_state_ = desire_state; + } + +private: + 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 update_desire_states(const Eigen::Vector3d& control_velocity) { + rmcs_msgs::WheelLegState desire_state; + + auto& [vx, vz, wz] = control_velocity; + + // distance :always 0 during velocity control + desire_state.distance = 0.0; // else measure.distance + 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.1; + desire_state.right_tilt_angle = -0.1; + + // pitch angle: + desire_state.body_pitch_angle = 0.0; + desire_state.body_pitch_velocity = 0.0; + + return desire_state; + } + + 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; + + InputInterface measure_state_; + InputInterface chassis_control_velocity_; + InputInterface power_limit_; + + InputInterface is_leg_extended_; + + OutputInterface desire_leg_length_; + OutputInterface desire_roll_angle_; + + OutputInterface desire_state_; + + bool park_mode_ = false; + + rmcs_msgs::WheelLegState chassis_mode_; +}; + +} // namespace rmcs_core::controller::chassis + +#include + +PLUGINLIB_EXPORT_CLASS( + rmcs_core::controller::chassis::DesireStateController, rmcs_executor::Component) \ No newline at end of file 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 index ab3bd0fa..cc642e35 100644 --- 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 @@ -1,8 +1,11 @@ +#pragma once +#include "rmcs_msgs/wheel_leg_mode.hpp" #include "rmcs_msgs/wheel_leg_state.hpp" -#include +#include +#include -namespace rmcs_core::controller::chassis { +namespace rmcs_core::controller { class DesireStateSolver { public: @@ -16,17 +19,49 @@ class DesireStateSolver { desire_roll_angle_ = 0.0; } - rmcs_msgs::WheelLegState update(const double& vx, const double& vz, const double& wz) { + rmcs_msgs::WheelLegState + update(rmcs_msgs::WheelLegMode mode, Eigen::Vector3d control_velocity) { + // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity + auto& [vx, vz, wz] = control_velocity; + // desire_leg_length_ += vz * leg_length_sensitivity_; + constexpr double power_kp_x = 0.26; + // power_limit_velocity_x = power_kp_x * std::sqrt(referee_->chassis_power_limit_); + + constexpr double power_kp_w = 0.40; + // power_limit_velocity_w = power_kp_w * std::sqrt(referee_->chassis_power_limit_); + + chassis_mode_ = mode; + + switch (chassis_mode_) { + case rmcs_msgs::WheelLegMode::STOP: break; + case rmcs_msgs::WheelLegMode::SPIN: break; + case rmcs_msgs::WheelLegMode::FOLLOW: break; + case rmcs_msgs::WheelLegMode::LAUNCH_RAMP: break; + case rmcs_msgs::WheelLegMode::BALANCELESS: break; + } + + 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_.distance = 0.0; // else measure.distance desire_state_.velocity = 0.0; // yaw angle: - desire_state_.yaw_angle = 0.0; + desire_state_.yaw_angle = 0.0; // else measure.yaw desire_state_.yaw_velocity = 0.0; + // leg tilt angle: + desire_state_.left_tilt_angle = -0.1; + desire_state_.right_tilt_angle = -0.1; + + desire_state_.body_pitch_angle = 0.0; + return desire_state_; } @@ -38,16 +73,34 @@ class DesireStateSolver { double update_desire_roll_angle() const { return desire_roll_angle_; } + void update_measure_state(rmcs_msgs::WheelLegState& measure_state) { + measure_state_ = measure_state; + } + + void update_power() {} + private: 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_; + double translate_velocity_; + double angular_velocity; + + bool park_mode_ = false; + double desire_leg_length_; double desire_roll_angle_; + rmcs_msgs::WheelLegMode chassis_mode_; + + rmcs_msgs::WheelLegState measure_state_; rmcs_msgs::WheelLegState desire_state_; }; -} // namespace rmcs_core::controller::chassis \ No newline at end of file +} // namespace rmcs_core::controller \ 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..bf3e4ec9 --- /dev/null +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/wheel_leg_chassis_controller.cpp @@ -0,0 +1,244 @@ +#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_output("/chassis/angle", chassis_angle_, nan); + register_output("/chassis/control_angle", chassis_control_angle_, nan); + + register_output("/chassis/control_mode", mode_); + register_output("/chassis/leg/extended", is_leg_extended_); + + 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::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_.z && keyboard.z) { + mode = mode == rmcs_msgs::WheelLegMode::BALANCELESS + ? rmcs_msgs::WheelLegMode::FOLLOW + : rmcs_msgs::WheelLegMode::BALANCELESS; + } + + // change leg length + if (!last_keyboard_.q && keyboard.q) { + leg_extended_ = !leg_extended_; + } + + *is_leg_extended_ = leg_extended_; + *mode_ = mode; + } + + update_velocity_control(); + } while (false); + + last_switch_right_ = switch_right; + last_switch_left_ = switch_left; + last_keyboard_ = keyboard; + } + + void reset_all_controls() { + *mode_ = rmcs_msgs::WheelLegMode::STOP; + + *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; + } + + double update_angular_velocity_control() { + double angular_velocity = 0.0; + double chassis_control_angle = nan; + + switch (*mode_) { + case rmcs_msgs::WheelLegMode::STOP: break; + case rmcs_msgs::WheelLegMode::SPIN: { + angular_velocity = + 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); + } break; + + case rmcs_msgs::WheelLegMode::FOLLOW: + case rmcs_msgs::WheelLegMode::BALANCELESS: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + // err: [0, 2pi) -> [0, alignment) -> signed. + // In step-down mode, two sides of the chassis can be used for alignment. + // TODO: Dynamically determine the split angle based on chassis velocity. + constexpr double alignment = std::numbers::pi; + while (err > alignment / 2) { + chassis_control_angle -= alignment; + if (chassis_control_angle < 0) + chassis_control_angle += 2 * std::numbers::pi; + err -= alignment; + } + + angular_velocity = following_velocity_controller_.update(err); + } break; + case rmcs_msgs::WheelLegMode::LAUNCH_RAMP: { + double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); + + // err: [0, 2pi) -> signed + // In launch ramp mode, 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); + } 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 = 16.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_; + OutputInterface chassis_angle_, chassis_control_angle_; + + OutputInterface mode_; + OutputInterface is_leg_extended_; + + bool leg_extended_ = false; + bool spinning_forward_ = true; + pid::PidCalculator following_velocity_controller_; + + 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 index e2c0f8e4..c2c98e32 100644 --- 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 @@ -8,6 +8,7 @@ #include #include #include +#include #include #include @@ -48,7 +49,6 @@ class WheelLegController register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_mode", mode_); - // register_input("/chassis/control_power_limit", power_limit_); register_input("/chassis/left_front_hip/angle", left_front_hip_angle_); register_input("/chassis/left_back_hip/angle", left_back_hip_angle_); @@ -87,19 +87,6 @@ class WheelLegController register_output("/chassis/left_wheel/control_torque", left_wheel_control_torque_); register_output("/chassis/right_wheel/control_torque", right_wheel_control_torque_); - register_output("/chassis/velocity", velocity_); - - register_output("/chassis/output_0", output_0_); - register_output("/chassis/output_1", output_1_); - register_output("/chassis/output_2", output_2_); - register_output("/chassis/output_3", output_3_); - register_output("/chassis/output_4", output_4_); - register_output("/chassis/output_5", output_5_); - register_output("/chassis/output_6", output_6_); - register_output("/chassis/output_7", output_7_); - register_output("/chassis/output_8", output_8_); - register_output("/chassis/output_9", output_9_); - velocity_kalman_filter_.set_process_noise_transition(W_); } @@ -129,7 +116,7 @@ class WheelLegController // Controller auto chassis_control_velocity = calculate_chassis_control_velocity(); - auto desire_state = calculate_desire_state(chassis_control_velocity); + auto desire_state = calculate_desire_state(chassis_control_velocity, measure_state); auto leg_forces = calculate_leg_force(leg_posture, measure_state); auto control_torques = calculate_control_torques(desire_state, measure_state, leg_posture); @@ -201,8 +188,6 @@ class WheelLegController }; } - void calculate_chassis_euler_angles() {} - LegPosture calculate_leg_posture(const Eigen::Vector4d& hip_angles) { const auto& [lf, lb, rb, rf] = hip_angles; LegPosture result; @@ -274,7 +259,7 @@ class WheelLegController return result; } - void detect_chassis_leave_the_ground() {} + void detect_chassis_levitate() {} Eigen::Vector2d calculate_translational_distance( LegPosture leg_posture, Eigen::Vector2d wheel_velocities, Eigen::Vector4d hip_angles) { @@ -360,16 +345,10 @@ class WheelLegController return chassis_control_velocity; } - rmcs_msgs::WheelLegState calculate_desire_state(Eigen::Vector3d control_velocity) { + rmcs_msgs::WheelLegState calculate_desire_state( + Eigen::Vector3d control_velocity, rmcs_msgs::WheelLegState measure_state) { rmcs_msgs::WheelLegState desire_state{}; - // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity - auto& [vx, vz, wz] = control_velocity; - - desire_state = desire_state_solver_.update(*mode_); - desire_roll_angle_ = desire_state_solver_.update_desire_roll_angle(); - desire_leg_length_ = desire_state_solver_.update_desire_leg_length(0.13, 0.36); - return desire_state; } @@ -378,9 +357,9 @@ class WheelLegController Eigen::Vector2d result; auto roll_control_force = - roll_angle_pid_calculator_.update(desire_roll_angle_ - *chassis_roll_angle_imu_); + roll_angle_pid_calculator_.update(d_roll_angle_ - *chassis_roll_angle_imu_); auto leg_length_control_force = leg_length_pid_calculator_.update( - desire_leg_length_ - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); + d_leg_length_ - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); auto calculate_compensation_feedforward_force = [this](double coefficient) { return (body_mess_ / 2.0 + centroid_position_coefficient_ * leg_mess_) * coefficient; @@ -463,8 +442,8 @@ class WheelLegController // measure_state.body_pitch_velocity); RCLCPP_INFO( - get_logger(), "pitch: %f, err: %f, output: %f, lw torque: %f, rw torque: %f", - measure_state.body_pitch_angle, error_state(8), output_8, result.x(), result.y()); + get_logger(), "x: %f, err: %f, output: %f, lw torque: %f, rw torque: %f", + measure_state.distance, error_state(0), output_0, result.x(), result.y()); return result; } @@ -496,11 +475,11 @@ class WheelLegController } static double clamp_wheel_control_torque(const double& torque) { - return std::clamp(torque, -3.0, 3.0); + return std::clamp(torque, -2.4, 2.4); } static double clamp_hip_control_torque(const double& torque) { - return std::clamp(torque, -10., 10.); + return std::clamp(torque, -15., 15.); } static constexpr double nan_ = std::numeric_limits::quiet_NaN(); @@ -561,8 +540,7 @@ class WheelLegController InputInterface chassis_roll_angle_imu_; InputInterface chassis_control_velocity_; - InputInterface power_limit_; - InputInterface mode_; + InputInterface mode_; OutputInterface left_front_hip_control_torque_; OutputInterface left_back_hip_control_torque_; @@ -577,20 +555,7 @@ class WheelLegController OutputInterface left_wheel_control_torque_; OutputInterface right_wheel_control_torque_; - OutputInterface velocity_; - - OutputInterface output_0_; - OutputInterface output_1_; - OutputInterface output_2_; - OutputInterface output_3_; - OutputInterface output_4_; - OutputInterface output_5_; - OutputInterface output_6_; - OutputInterface output_7_; - OutputInterface output_8_; - OutputInterface output_9_; - - double desire_leg_length_, desire_roll_angle_; + double d_leg_length_, d_roll_angle_; Eigen::Vector2d last_leg_length_, last_tilt_angle_; Eigen::Vector2d last_dot_leg_length_, last_dot_tilt_angle_; 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 index 07474261..f520f9c5 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp @@ -37,11 +37,9 @@ class LqrCalculator { double t6; double t7; - // Q = diag([1 1 1 1 10 1 10 1 2500 1]); - // R = diag([10, 10, 1, 1]); /* This function was generated by the Symbolic Math Toolbox version 24.2. */ - /* 2026-02-27 21:05:23 */ + /* 2026-03-01 16:47:18 */ t2 = l_l * l_l; t3 = std::pow(l_l, 3.0); t5 = l_r * l_r; @@ -55,449 +53,461 @@ class LqrCalculator { e_K_result_tmp = l_r * t3; f_K_result_tmp = t2 * t5; K_result[0] = - (((((l_l * 2.4161625771455442 - l_r * 6.4503256101831674) - t2 * 4.7175983828711834) - - t3 * 9.4827610745871365) - + t4 * 17.255910198489548) - + ((((t5 * 13.0222623797468 + t6 * 11.641653511617831) - t7 * 31.0035373115271) - + K_result_tmp * 5.4254195965871324) - - b_K_result_tmp * 85.338177109904123)) - + ((((c_K_result_tmp * 85.440287758617359 + d_K_result_tmp * 59.820960326560787) - - e_K_result_tmp * 49.8679663677069) - - f_K_result_tmp * 5.6612267799112121) - - 0.16354769561007079); + ((((((l_l * 92.355112446985217 - l_r * 273.86445506363663) - t2 * 224.254186023749) + + t3 * 88.871572657900288) + + t4 * 139.03867730571409) + + t5 * 1059.1334076957751) + + ((((t6 * -1713.582322380568 + t7 * 992.81463015698455) + - K_result_tmp * 49.716992277853123) + - b_K_result_tmp * 852.2018909512982) + + c_K_result_tmp * 1107.871721207649)) + + (((d_K_result_tmp * 877.76631886967994 - e_K_result_tmp * 757.77384415184133) + - f_K_result_tmp * 226.04938788522941) + - 0.71256354892448814); K_result[1] = - (((((l_l * -6.08302207335293 + l_r * 2.0142744068590139) + t2 * 22.943060350901909) - - t3 * 32.788866060224017) - + t4 * 12.0700541979319) - + ((((t5 * 5.1038452484121848 - t6 * 16.86627200113784) + t7 * 10.54500440168491) - - K_result_tmp * 14.42141170176407) - + b_K_result_tmp * 35.208495730890263)) - + ((((c_K_result_tmp * 1.3279664128930979 - d_K_result_tmp * 8.1046028301993065) - + e_K_result_tmp * 42.642098523750512) - - f_K_result_tmp * 51.552702341861533) - - 0.15698795750402059); + ((((((l_l * -273.93209042699431 + l_r * 87.1954445417931) + t2 * 1063.649758788918) + - t3 * 1822.080583531972) + + t4 * 1197.084415865671) + - t5 * 227.18179370482679) + + ((((t6 * 176.5072201142425 + t7 * 52.626985306441931) + - K_result_tmp * 19.096085264645971) + + b_K_result_tmp * 634.11059136111714) + - c_K_result_tmp * 671.58679586745416)) + + (((d_K_result_tmp * -668.10764063013244 + e_K_result_tmp * 728.95367898348775) + + f_K_result_tmp * 19.330597009900949) + - 0.43326490086341518); K_result[2] = - (((((l_l * -7.0391718485499242 + l_r * 4.1396228642676061) + t2 * 12.33745678182799) - - t3 * 0.98606759263751176) - + t4 * 3.806439832412305) - + ((((t5 * -38.526555112601628 + t6 * 63.946154134287589) - t7 * 1.3092165744637649) - + K_result_tmp * 15.586793561656879) - + b_K_result_tmp * 64.7581727732454)) - + ((((c_K_result_tmp * -198.62878467860409 - d_K_result_tmp * 84.359584117828661) - - e_K_result_tmp * 14.215934150984531) - + f_K_result_tmp * 166.5259512146695) - + 1.7158251243263409); + ((((((l_l * -169.01051203621179 + l_r * 157.94912265003771) + t2 * 409.75693102273408) + - t3 * 440.17392258420722) + + t4 * 203.41307893409541) + - t5 * 1410.070913028987) + + ((((t6 * 3668.204235391795 - t7 * 3090.2419270224232) + + K_result_tmp * 530.45507635166462) + - b_K_result_tmp * 766.6009383482052) + - c_K_result_tmp * 158.33857771268461)) + + (((d_K_result_tmp * -854.04868567405856 + e_K_result_tmp * 178.8600310246072) + + f_K_result_tmp * 1269.96976472993) + + 23.207128047454439); K_result[3] = - (((((l_l * 3.2505673234134531 - l_r * 3.500380774208117) - t2 * 50.611797162115543) - + t3 * 133.2326664631766) - - t4 * 115.87852399933379) - + ((((t5 * -25.07034906602615 + t6 * 77.646855700396756) - t7 * 82.720304705947541) - + K_result_tmp * 50.1415971452285) - - b_K_result_tmp * 75.962862672057426)) - + ((((c_K_result_tmp * 82.037299004697715 - d_K_result_tmp * 57.4476842579802) - + e_K_result_tmp * 37.039296399297562) - + f_K_result_tmp * 8.6305605362329239) - + 1.5594460786896229); + ((((((l_l * 149.78735965507289 - l_r * 147.8698841180518) - t2 * 1496.200184138826) + + t3 * 3924.5391044762191) + - t4 * 3446.6209396608679) + + t5 * 186.87271957958919) + + (((((t6 * 144.2208512426229 - t7 * 378.82716097452891) + + K_result_tmp * 760.93640228602408) + - b_K_result_tmp * 1279.8774466167081) + + c_K_result_tmp * 787.11174974028756) + - d_K_result_tmp * 987.68485697186634)) + + ((e_K_result_tmp * 365.67457696782378 + f_K_result_tmp * 908.53018463380181) + + 22.483942550576931); K_result[4] = - (((((l_l * 2.8285940429728611 - l_r * 5.1454314241415977) - t2 * 5.0429895063382792) - - t3 * 12.181920243732231) - + t4 * 20.20853074047443) - + ((((t5 * 1.832167410090793 + t6 * 49.666095833115328) - t7 * 71.585036684242255) - + K_result_tmp * 6.7614525434512522) - - b_K_result_tmp * 112.2320257202685)) - + ((((c_K_result_tmp * 119.3831306747833 + d_K_result_tmp * 76.132136747491444) - - e_K_result_tmp * 57.650962850068957) - - f_K_result_tmp * 15.931253109158931) - - 0.47857476782283581); + (((((l_l * 43.269453866615727 - l_r * 114.80831612833209) - t2 * 105.854559506629) + + t3 * 42.669181589675233) + + t4 * 65.787062747513673) + + (((((t5 * 451.24376532765962 - t6 * 763.6112387679841) + t7 * 462.32743900156117) + - K_result_tmp * 62.808969622301078) + - b_K_result_tmp * 304.7639016202404) + + c_K_result_tmp * 448.438009099465)) + + (((d_K_result_tmp * 443.936858957365 - e_K_result_tmp * 369.555533404123) + - f_K_result_tmp * 145.429502806394) + - 1.3855287343294009); K_result[5] = - (((((l_l * -9.9762094124309471 + l_r * 8.2824256552460209) + t2 * 26.72931192901931) - - t3 * 33.2666027507976) - + t4 * 8.2828108728362668) - + ((((t5 * -31.724688992305 + t6 * 81.101423007927608) - t7 * 84.684157838643614) - + K_result_tmp * 4.6527558336539547) - - b_K_result_tmp * 13.716265307127911)) - + ((((c_K_result_tmp * 58.061799134743993 - d_K_result_tmp * 22.912789739632519) - + e_K_result_tmp * 59.033367528110418) - - f_K_result_tmp * 54.943999830502257) - - 0.51141369450095631); + ((((((l_l * -116.0987786944432 + l_r * 42.400768303339511) + t2 * 447.5486993677203) + - t3 * 783.61417177682142) + + t4 * 523.30580265859476) + - t5 * 118.35673199708231) + + ((((t6 * 102.3525107169372 + t7 * 16.93592152337677) + - K_result_tmp * 32.999245323417853) + + b_K_result_tmp * 330.18195434976451) + - c_K_result_tmp * 348.58283653874241)) + + (((d_K_result_tmp * -265.22559675823692 + e_K_result_tmp * 303.82414683642389) + - f_K_result_tmp * 3.1891603029329092) + - 1.2748781040246611); K_result[6] = - (((((l_l * -10.154870225751059 - l_r * 6.37776918322295) + t2 * 11.29176051531987) - + t3 * 8.8722661294377314) - - t4 * 1.8819662579794589) - + ((((t5 * 4.60605808880274 - t6 * 33.882556037844743) + t7 * 96.107182198047454) - + K_result_tmp * 34.992857966795448) - + b_K_result_tmp * 49.265448693483613)) - + ((((c_K_result_tmp * -238.1540715012849 - d_K_result_tmp * 115.2246820448856) - - e_K_result_tmp * 23.1633572479635) - + f_K_result_tmp * 235.54853927968949) - + 3.2205432084254011); + (((((l_l * -86.242587295137454 + l_r * 71.764544206863746) + t2 * 193.7856638351403) + - t3 * 169.578789175179) + + t4 * 40.11978156373538) + + ((((t5 * -636.59304070297219 + t6 * 1687.490766398086) - t7 * 1440.71627320408) + + K_result_tmp * 293.41905516473793) + - b_K_result_tmp * 446.45269502879307)) + + ((((c_K_result_tmp * 1.287182725691272 - d_K_result_tmp * 505.80111961355118) + + e_K_result_tmp * 179.25054212280961) + + f_K_result_tmp * 644.25722998165952) + + 11.11928691495916); K_result[7] = - (((((l_l * 8.7887078711611064 - l_r * 22.112513975325282) - t2 * 73.989854097659119) - + t3 * 176.850915622289) - - t4 * 145.318425792573) - + ((((t5 * 57.854515384424 - t6 * 101.671855514122) + t7 * 59.950083066232843) - + K_result_tmp * 48.290888622982067) - - b_K_result_tmp * 84.992610220595182)) - + ((((c_K_result_tmp * 107.7034607752615 - d_K_result_tmp * 36.994228932582857) - + e_K_result_tmp * 17.516753817495331) - - f_K_result_tmp * 7.7232399107197987) - + 3.04020285934469); + ((((((l_l * 71.328342686041111 - l_r * 80.482930304154763) - t2 * 690.9068365075957) + + t3 * 1817.5540207706349) + - t4 * 1606.12373761799) + + t5 * 104.2230208950443) + + (((((t6 * 65.129162631819469 - t7 * 200.50502327691581) + + K_result_tmp * 404.8722608472865) + - b_K_result_tmp * 706.06425897129088) + + c_K_result_tmp * 474.38984547602229) + - d_K_result_tmp * 530.58430793217167)) + + ((e_K_result_tmp * 232.703390591384 + f_K_result_tmp * 453.90587169892359) + + 10.825512719740139); K_result[8] = - (((((l_l * -1.341768108308373 + l_r * 2.0675080402452779) + t2 * 5.9719771522544782) - - t3 * 13.735369286211069) - + t4 * 12.466035563884351) - + ((((t5 * -4.7099193109889059 + t6 * 2.9551341670866189) + t7 * 2.0567260095687718) - + K_result_tmp * 0.65410319297124675) - - b_K_result_tmp * 2.683887904192269)) - + ((((c_K_result_tmp * -3.40896810837682 + d_K_result_tmp * 5.390842430945586) - - e_K_result_tmp * 9.8911338552434529) - + f_K_result_tmp * 6.6989835210579134) - - 0.24391803832559791); + (((((l_l * -13.7107548402327 + l_r * 32.889390181255777) + t2 * 63.010136408437987) + - t3 * 120.2908917827902) + + t4 * 81.192186023692656) + + ((((t5 * -86.295654417094283 + t6 * 104.9401668042791) - t7 * 50.67862388884231) + - K_result_tmp * 41.24249620769973) + + b_K_result_tmp * 103.5981301062985)) + + ((((c_K_result_tmp * -59.890869038501847 + d_K_result_tmp * 33.80188037169389) + + e_K_result_tmp * 19.12753263341865) + - f_K_result_tmp * 80.97574572858079) + - 4.5523915416053544); K_result[9] = - (((((l_l * -1.559554822738306 + l_r * 0.75321709360695388) + t2 * 3.743145643630144) - - t3 * 4.8734856284575) - + t4 * 1.2471528754593639) - + ((((t5 * -3.619584466709346 + t6 * 9.6796959498973223) - t7 * 8.6064081058276347) - - K_result_tmp * 1.5990467376817279) - - b_K_result_tmp * 6.1970291296308666)) - + ((((c_K_result_tmp * 8.2170799565409212 + d_K_result_tmp * 8.4705748348421874) - - e_K_result_tmp * 1.05563062850879) - - f_K_result_tmp * 6.9428579316985246) - + 0.2490163529167902); + (((((l_l * -34.31841822255145 + l_r * 14.861992357354479) + t2 * 86.597549381777526) + - t3 * 100.17688511043821) + + t4 * 43.838807602816154) + + ((((t5 * -53.04373752679102 + t6 * 86.762137700278856) - t7 * 52.971609857210773) + + K_result_tmp * 32.578577722318776) + + b_K_result_tmp * 10.77693951169841)) + + ((((c_K_result_tmp * -37.291486037735318 - d_K_result_tmp * 123.30394009629551) + + e_K_result_tmp * 93.576355983719836) + + f_K_result_tmp * 47.409339150360353) + + 4.5696230962113047); K_result[10] = - (((((l_l * -0.82304214267667786 - l_r * 6.0675099681121312) + t2 * 5.38054700720794) - - t3 * 12.699803894120871) - + t4 * 11.24035769801084) - + ((((t5 * 35.395911327800107 - t6 * 82.305569188342062) + t7 * 69.198831566895777) - - K_result_tmp * 14.705594768955731) - + b_K_result_tmp * 31.02096841145816)) - + ((((c_K_result_tmp * -23.521075864380691 + d_K_result_tmp * 19.425620294282719) - - e_K_result_tmp * 12.73040430043762) - - f_K_result_tmp * 14.362960221684551) - - 0.01910547338046974); + ((((((l_l * -11.91422227139272 - l_r * 27.10235100220812) + t2 * 56.828975601220179) + - t3 * 146.68456970594369) + + t4 * 142.1147449488235) + + t5 * 154.3395319474929) + + ((((t6 * -303.00815664096717 + t7 * 218.64233138478039) + - K_result_tmp * 52.363722986539912) + - b_K_result_tmp * 32.932365924853627) + + c_K_result_tmp * 64.567068976981375)) + + (((d_K_result_tmp * 159.78300627157671 - e_K_result_tmp * 148.1391392880353) + - f_K_result_tmp * 22.805801758521429) + - 4.9638966135395766); K_result[11] = - (((((l_l * 3.872202753627298 + l_r * 3.0977262629686) - t2 * 18.9380243053007) - + t3 * 45.893363568676868) - - t4 * 41.840086967144472) - + ((((t5 * -9.91916916591209 + t6 * 14.281122413414129) - t7 * 10.204817520388129) - + K_result_tmp * 2.1738726992458859) - + b_K_result_tmp * 8.0614755281300514)) - + ((((c_K_result_tmp * -0.22810761152897921 - d_K_result_tmp * 21.859084629550289) - + e_K_result_tmp * 27.69529001117715) - - f_K_result_tmp * 7.1287710182295783) - + 0.01637830255695779); + (((((l_l * 31.461468399575121 + l_r * 7.0714134616560393) - t2 * 153.1939748607592) + + t3 * 286.66176831786368) + - t4 * 192.3952528553325) + + ((((t5 * -41.622794048448469 + t6 * 106.92112334509331) - t7 * 96.497579469107819) + + K_result_tmp * 38.724984322015622) + - b_K_result_tmp * 130.1571583349087)) + + ((((c_K_result_tmp * 92.751034357358009 + d_K_result_tmp * 53.504824436731951) + - e_K_result_tmp * 115.2693156111965) + + f_K_result_tmp * 61.291486557776317) + + 4.9914390609146686); K_result[12] = - (((((l_l * -1.8478580245840139 + l_r * 2.959534805411745) + t2 * 7.7508367707514934) - - t3 * 17.371709188802431) - + t4 * 15.47320701537973) - + ((((t5 * -8.1954694077610313 + t6 * 9.7579436697661439) - t7 * 3.3176691510460379) - + K_result_tmp * 1.910833809201377) - - b_K_result_tmp * 6.4775401308520077)) - + ((((c_K_result_tmp * -0.53248387124696839 + d_K_result_tmp * 6.2176093680958706) - - e_K_result_tmp * 11.53634284427212) - + f_K_result_tmp * 7.9126970579635358) - - 0.30855666350815081); + (((((l_l * -5.919673061242066 + l_r * 12.737241060505831) + t2 * 28.153436962463921) + - t3 * 55.044946554856367) + + t4 * 37.815885195552752) + + ((((t5 * -27.134619749177968 + t6 * 24.23448368243384) - t7 * 5.5552340928564732) + - K_result_tmp * 20.807810167510588) + + b_K_result_tmp * 48.792726242241237)) + + ((((c_K_result_tmp * -27.43725637899627 + d_K_result_tmp * 18.57434686475797) + + e_K_result_tmp * 7.059835621642593) + - f_K_result_tmp * 39.034123637183868) + - 1.6142866129663971); K_result[13] = - (((((l_l * -2.1054029191801131 + l_r * 0.99969316946929176) + t2 * 4.2571840473558993) - - t3 * 5.2418907813031659) - + t4 * 1.0454909144757021) - + ((((t5 * -5.60706954435337 + t6 * 16.124893749903489) - t7 * 15.4486390745989) - - K_result_tmp * 0.17050548296766341) - - b_K_result_tmp * 13.77902340719332)) - + ((((c_K_result_tmp * 16.719054469203169 + d_K_result_tmp * 10.928540631199249) - - e_K_result_tmp * 1.8906549231684939) - - f_K_result_tmp * 8.5781770505704618) - + 0.30855257992253371); + (((((l_l * -13.090877616107059 + l_r * 6.2229163275557253) + t2 * 26.982382900743119) + - t3 * 20.516693811938559) + - t4 * 0.066306227643642884) + + ((((t5 * -23.62070281444289 + t6 * 40.791472692179539) - t7 * 26.334497532889159) + + K_result_tmp * 16.707728884426221) + + b_K_result_tmp * 1.4385780808627)) + + ((((c_K_result_tmp * -14.24827386102737 - d_K_result_tmp * 58.955692957258833) + + e_K_result_tmp * 45.448830552771028) + + f_K_result_tmp * 22.9521033794778) + + 1.617778838865062); K_result[14] = - (((((l_l * -0.925458609099666 - l_r * 8.3304605313851372) + t2 * 5.8705196123950776) - - t3 * 14.247848267239529) - + t4 * 13.103879385258219) - + ((((t5 * 49.875731821209641 - t6 * 119.7751589575233) + t7 * 103.715350391261) - - K_result_tmp * 19.49965846431434) - + b_K_result_tmp * 44.439955961888089)) - + ((((c_K_result_tmp * -36.818387129276928 + d_K_result_tmp * 24.63629215492475) - - e_K_result_tmp * 17.0968666836546) - - f_K_result_tmp * 17.25657234150486) - - 0.01312247351132222); + (((((l_l * -3.04211669546084 - l_r * 12.385246956637429) + t2 * 12.52336612985486) + - t3 * 34.740141172306942) + + t4 * 36.038839915419331) + + ((((t5 * 65.036741890603878 - t6 * 122.9771371839622) + t7 * 85.231479654558) + - K_result_tmp * 22.765648957828152) + - b_K_result_tmp * 16.1753484701422)) + + ((((c_K_result_tmp * 32.4594886556945 + d_K_result_tmp * 65.664297399926809) + - e_K_result_tmp * 57.839965670120407) + - f_K_result_tmp * 11.24969820975552) + - 1.7172066697883861); K_result[15] = - (((((l_l * 5.3340410979409159 + l_r * 3.7097142416343569) - t2 * 23.9767968934346) - + t3 * 56.723299093475063) - - t4 * 51.221461195396927) - + ((((t5 * -10.685977771085909 + t6 * 13.46359276363065) - t7 * 8.4510178889795391) - - K_result_tmp * 0.5725990958073367) - + b_K_result_tmp * 15.901070547299479)) - + ((((c_K_result_tmp * -5.1633341396214769 - d_K_result_tmp * 23.07671182323989) - + e_K_result_tmp * 32.04392799053177) - - f_K_result_tmp * 11.68760478880241) - + 0.02861473054099849); + (((((l_l * 13.92070192741177 + l_r * 1.3267137495911709) - t2 * 64.012779834276941) + + t3 * 113.8555115663932) + - t4 * 71.740561014362669) + + ((((t5 * -8.3254487924926615 + t6 * 24.95980217422504) - t7 * 25.091153555588338) + + K_result_tmp * 18.584986031795061) + - b_K_result_tmp * 58.459437542105583)) + + ((((c_K_result_tmp * 43.56187476503856 + d_K_result_tmp * 25.511952716616278) + - e_K_result_tmp * 53.290945022355167) + + f_K_result_tmp * 23.72327578069881) + + 1.7271519902312811); K_result[16] = - (((((l_l * 5.2972848332551621 - l_r * 27.429859360067429) - t2 * 27.127116215748519) - + t3 * 47.878168322524381) - - t4 * 36.153594345135581) - + (((((t5 * 17.443979497381079 + t6 * 110.0345464204547) - t7 * 149.3114795084017) - + K_result_tmp * 37.676996153487543) - - b_K_result_tmp * 181.78752464772509) - + c_K_result_tmp * 154.21686219229)) - + (((d_K_result_tmp * 8.13571881088043 - e_K_result_tmp * 2.945041346048114) - + f_K_result_tmp * 33.4683373212623) - + 0.70001959812415449); + ((((((l_l * 35.0280622655384 - l_r * 124.5719890522153) - t2 * 150.88869027192169) + + t3 * 221.97326755539439) + - t4 * 94.055401007841169) + + t5 * 221.55042045516089) + + ((((t6 * -181.322133395468 - t7 * 6.3908584340698731) + + K_result_tmp * 155.070074821783) + - b_K_result_tmp * 550.21528057284047) + + c_K_result_tmp * 437.78024124401048)) + + (((d_K_result_tmp * 64.327983558699358 - e_K_result_tmp * 228.70519329653911) + + f_K_result_tmp * 233.2843736227486) + + 0.47141423698804658); K_result[17] = - (((((l_l * -2.6513487306387442 - l_r * 18.037859626648331) + t2 * 14.412687045731159) - - t3 * 12.434191988956851) - - t4 * 7.6245878901630686) - + ((((t5 * 84.35383577919491 - t6 * 153.9335207875358) + t7 * 91.743799676562475) - + K_result_tmp * 5.9187789781112938) - + b_K_result_tmp * 50.713128893562377)) - + ((((c_K_result_tmp * 33.854175270461248 - d_K_result_tmp * 90.860755680034828) - + e_K_result_tmp * 122.73785652708671) - - f_K_result_tmp * 85.053267051066925) - + 0.48462980939584238); + ((((((l_l * -44.815030362233173 - l_r * 22.67359233341892) + t2 * 300.63777655447711) + - t3 * 722.6443221554473) + + t4 * 615.86211252050509) + + t5 * 135.55840257767949) + + ((((t6 * -328.952448997027 + t7 * 288.48380332919538) + - K_result_tmp * 183.20751049808581) + + b_K_result_tmp * 215.10134447449559) + - c_K_result_tmp * 128.974584711217)) + + (((d_K_result_tmp * 338.35164171883753 - e_K_result_tmp * 228.66532312829861) + - f_K_result_tmp * 167.1989684123391) + + 0.14524761154754681); K_result[18] = - (((((l_l * -7.6662812062521306 + l_r * 86.64332891702847) - t2 * 5.0711482423355161) - + t3 * 62.102579908294658) - - t4 * 58.259796365305888) - + ((((t5 * -405.00361772778541 + t6 * 818.8772814087655) - t7 * 565.84129440775234) - + K_result_tmp * 122.0404229213346) - - b_K_result_tmp * 116.9914011133222)) - + ((((c_K_result_tmp * -146.00639918581891 - d_K_result_tmp * 252.15737405522279) - + e_K_result_tmp * 72.870754721170741) - + f_K_result_tmp * 332.55928318870139) - - 2.045707106826081); + ((((((l_l * -61.233539815926562 + l_r * 159.5801290651749) + t2 * 190.34084270442739) + - t3 * 251.27876537746511) + + t4 * 133.4885804108757) + - t5 * 794.44465857885507) + + ((((t6 * 1691.717444581247 - t7 * 1249.09402201776) + + K_result_tmp * 183.5228184950972) + + b_K_result_tmp * 16.160767658309179) + - c_K_result_tmp * 402.26934628943991)) + + (((d_K_result_tmp * -435.36829352398382 + e_K_result_tmp * 200.68166088781419) + + f_K_result_tmp * 419.72926424666178) + + 4.01139336562379); K_result[19] = - (((((l_l * -2.3255131457247682 - l_r * 21.225654779075239) - t2 * 4.8041773430892656) - - t3 * 2.431456755967782) - + t4 * 24.582564440978409) - + ((((t5 * -10.033306799041689 + t6 * 11.49419832788041) - t7 * 3.9373859134706959) - - K_result_tmp * 6.9870949680485657) - + b_K_result_tmp * 106.0025640537179)) - + ((((c_K_result_tmp * -78.868776410047161 + d_K_result_tmp * 54.345940883954533) - - e_K_result_tmp * 89.251445873726638) - - f_K_result_tmp * 38.089211539308117) - + 2.044158060186823); + (((((l_l * 21.2577287689819 - l_r * 12.9870023863814) - t2 * 349.89198925126891) + + t3 * 1166.3480762987249) + - t4 * 1216.610967429835) + + ((((t5 * -147.40174670149861 + t6 * 413.19859809021762) - t7 * 378.94485419476177) + + K_result_tmp * 129.87364317535341) + + b_K_result_tmp * 9.882885033112828)) + + ((((c_K_result_tmp * -19.331279683481981 - d_K_result_tmp * 620.86309445194036) + + e_K_result_tmp * 690.186941601567) + + f_K_result_tmp * 73.346939409885664) + + 3.2812475516442419); K_result[20] = - (((((l_l * 0.25813515449237068 - l_r * 1.991685745227957) - t2 * 0.72340081465349571) - - t3 * 0.1410253350059455) - + t4 * 0.38709288239498713) - + ((((t5 * 4.5238258868673782 - t6 * 2.4165966667437462) + t7 * 0.77889824111916373) - + K_result_tmp * 0.5669873455993214) - - b_K_result_tmp * 15.65918265225269)) - + ((((c_K_result_tmp * 10.34154274718372 + d_K_result_tmp * 8.2511713067634211) - - e_K_result_tmp * 5.12289167464622) - + f_K_result_tmp * 2.0333988771688061) - - 0.081300535630453488); + (((((l_l * 0.72462086482937327 - l_r * 12.838118637771791) - t2 * 4.1210848153270723) + + t3 * 0.62044551545321458) + + t4 * 8.1870002417434833) + + ((((t5 * 17.68908372284503 - t6 * 35.043641841758962) + t7 * 25.208415843362069) + + K_result_tmp * 17.23878502188936) + - b_K_result_tmp * 44.06040072734023)) + + ((((c_K_result_tmp * 22.11748790399276 + d_K_result_tmp * 1.4029420829172441) + - e_K_result_tmp * 24.402795404650441) + + f_K_result_tmp * 31.4736534261724) + - 0.0081646253371954729); K_result[21] = - (((((l_l * -0.6496084376171678 + l_r * 0.064060538620080987) + t2 * 0.15319711595788929) - + t3 * 6.1538887721810083) - - t4 * 9.72888858732645) - + ((((t5 * 0.83001962624743886 - t6 * 2.0059919875383) + t7 * 1.73040005752884) - + K_result_tmp * 0.50847684393085846) - + b_K_result_tmp * 14.0194728677426)) - + ((((c_K_result_tmp * -5.594666654232654 - d_K_result_tmp * 17.623741312671779) - + e_K_result_tmp * 23.66589919486816) - - f_K_result_tmp * 12.30395322310088) - + 0.002729575343444598); + ((((((l_l * -4.3734000722553219 - l_r * 0.78338018259563413) + t2 * 28.135271062744639) + - t3 * 71.3902205741409) + + t4 * 65.742993637374184) + + t5 * 6.5149907334735246) + + ((((t6 * -14.358109376589519 + t7 * 13.274897558630689) + - K_result_tmp * 37.007992109015269) + + b_K_result_tmp * 14.846418678293171) + - c_K_result_tmp * 11.31082359108575)) + + (((d_K_result_tmp * 76.331032334544673 - e_K_result_tmp * 66.795376335326623) + - f_K_result_tmp * 7.6665342045499481) + + 0.20272249447720819); K_result[22] = - (((((l_l * -0.073161745714453288 - l_r * 0.938973252579294) - t2 * 5.2438383168354026) - + t3 * 15.708350806611611) - - t4 * 11.37576169115421) - + ((((t5 * -4.78000014867496 + t6 * 24.19477048650754) - t7 * 16.647789922927359) - + K_result_tmp * 9.8243406018208486) - - b_K_result_tmp * 11.0895530901302)) - + ((((c_K_result_tmp * -25.546895378673572 - d_K_result_tmp * 19.034984398896281) - - e_K_result_tmp * 5.405871978560981) - + f_K_result_tmp * 45.6896537802703) - + 0.83952349752701394); + (((((l_l * -2.3363551008637682 + l_r * 11.24480445171284) + t2 * 5.3211463753799011) + - t3 * 0.57171085012863132) + - t4 * 2.8177250030630989) + + ((((t5 * -55.757882575886917 + t6 * 135.6439077284615) - t7 * 100.86739565575181) + - K_result_tmp * 5.3392747948147647) + + b_K_result_tmp * 19.330834180309449)) + + ((((c_K_result_tmp * -64.055828387260547 - d_K_result_tmp * 1.4059174267391) + - e_K_result_tmp * 18.543758013579) + + f_K_result_tmp * 47.537512468339571) + + 0.98774204065526794); K_result[23] = - (((((l_l * 3.7379527159754811 - l_r * 2.9064338875911861) - t2 * 17.72551739841867) - + t3 * 28.81301008142221) - - t4 * 16.49390613756561) - + ((((t5 * -1.433685008440267 + t6 * 5.8595958967904336) - t7 * 14.73704569383646) - + K_result_tmp * 8.8501264281054368) - - b_K_result_tmp * 21.786102608478359)) - + ((((c_K_result_tmp * 40.09534130182378 + d_K_result_tmp * 8.50776276108387) - - e_K_result_tmp * 11.44049203196354) - - f_K_result_tmp * 12.31666034251559) - - 0.01719178508400724); + (((((l_l * 8.6280928711034832 + l_r * 1.3210078423531131) - t2 * 68.0560870098354) + + t3 * 202.35316767558879) + - t4 * 208.59143546298671) + + ((((t5 * -21.71116305486127 + t6 * 42.441381932796283) - t7 * 38.827920246532912) + + K_result_tmp * 29.585958773682059) + + b_K_result_tmp * 6.3636945233856128)) + + ((((c_K_result_tmp * 10.00259009078974 - d_K_result_tmp * 128.5191776518343) + + e_K_result_tmp * 160.4427305563878) + - f_K_result_tmp * 18.873919049115859) + - 0.2166027208349745); K_result[24] = - (((((l_l * 0.39395398967460737 - l_r * 22.31504281772045) - t2 * 17.1137095559913) - + t3 * 41.012720713808832) - - t4 * 22.999044032147321) - + ((((t5 * 25.83816181171353 - t6 * 85.786788743370352) + t7 * 65.277973447427058) - + K_result_tmp * 103.8563188283494) - - b_K_result_tmp * 15.509350701766319)) - + ((((c_K_result_tmp * 6.874830876976616 - d_K_result_tmp * 166.0861494290217) - + e_K_result_tmp * 69.567862638225591) - + f_K_result_tmp * 54.2789691437973) - + 0.54290832206822459); + (((((l_l * -19.681762861883492 - l_r * 45.328716276368127) + t2 * 143.80630371220741) + - t3 * 369.070284413346) + + t4 * 308.957092948608) + + ((((t5 * 282.6697610019545 - t6 * 625.081801917176) + t7 * 474.89533947852368) + - K_result_tmp * 188.47052816038789) + + b_K_result_tmp * 220.00395181122269)) + + ((((c_K_result_tmp * -5.455374894097754 + d_K_result_tmp * 312.59372690641948) + - e_K_result_tmp * 118.0930398226493) + - f_K_result_tmp * 312.01080776932321) + + 0.009970749414400952); K_result[25] = - (((((l_l * -9.3121185838227 - l_r * 15.90891661324488) + t2 * 64.025011560449) - - t3 * 219.10982748723569) - + t4 * 256.696813709747) - + ((((t5 * 19.168159800935211 - t6 * 6.6781647121163639) + t7 * 26.660720978392511) - - K_result_tmp * 38.716894684700073) - - b_K_result_tmp * 138.5472259086842)) - + ((((c_K_result_tmp * -30.31039897392381 + d_K_result_tmp * 313.68037462772759) - - e_K_result_tmp * 439.9176421685662) - + f_K_result_tmp * 210.8260580402783) - + 0.89208135906197927); + ((((((l_l * -122.5503144964209 + l_r * 30.974749704140439) + t2 * 254.42067143844329) + - t3 * 295.52208824580532) + + t4 * 127.0397990835678) + - t5 * 129.61650048432341) + + ((((t6 * 201.59851772110559 - t7 * 89.020511742198821) + + K_result_tmp * 113.0416585624686) + + b_K_result_tmp * 45.434965561776792) + - c_K_result_tmp * 211.222207821207)) + + (((d_K_result_tmp * -426.21016587817888 + e_K_result_tmp * 290.68857401064241) + + f_K_result_tmp * 249.53977184634161) + + 0.58615150703872509); K_result[26] = - (((((l_l * -17.531404935947339 - l_r * 5.5379421147849488) + t2 * 86.877975173402476) - - t3 * 162.8652804614745) - + t4 * 124.03639814451761) - + ((((t5 * -65.151898126156809 + t6 * 6.3907904320355344) + t7 * 58.1802335450001) - - K_result_tmp * 47.981448308496923) - + b_K_result_tmp * 421.45566825329041)) - + ((((c_K_result_tmp * -306.84781257881059 - d_K_result_tmp * 81.888196573182839) - + e_K_result_tmp * 91.9600372018616) - - f_K_result_tmp * 167.19745475043371) - + 2.043252259788491); + (((((l_l * -18.876966178854939 + l_r * 21.908970202569989) - t2 * 63.3475340514244) + + t3 * 249.0953992317724) + - t4 * 225.1658232405249) + + (((((t5 * -299.21245192260182 + t6 * 971.40891090736091) - t7 * 943.37102493427437) + + K_result_tmp * 27.277082313115741) + - b_K_result_tmp * 442.40062329065239) + + c_K_result_tmp * 309.0500878910043)) + + (((d_K_result_tmp * 110.4530070746854 - e_K_result_tmp * 230.81777248746579) + + f_K_result_tmp * 308.59688846500728) + + 3.5678962832067311); K_result[27] = - (((((l_l * 14.9553066935295 + l_r * 68.003429108183923) - t2 * 46.809788102527612) - + t3 * 64.579803561288415) - - t4 * 44.57757731944951) - + ((((t5 * 35.706257783390761 - t6 * 14.703161553398941) - t7 * 33.940970292680113) - - K_result_tmp * 302.03583695239479) - - b_K_result_tmp * 169.7211351741139)) - + ((((c_K_result_tmp * 188.32434877881221 + d_K_result_tmp * 696.98712982797178) - - e_K_result_tmp * 508.53543179759657) - - f_K_result_tmp * 26.364143072420081) - - 2.26029321870165); + ((((((l_l * 152.51071513391241 - l_r * 48.247427464136663) - t2 * 821.6452053626864) + + t3 * 1754.27289936947) + - t4 * 1375.09973607764) + + t5 * 114.3735557189091) + + ((((t6 * -38.281592932773442 - t7 * 104.7194256881497) + + K_result_tmp * 251.14629426242871) + - b_K_result_tmp * 601.16537605693748) + + c_K_result_tmp * 527.73825629009127)) + + (((d_K_result_tmp * -6.504003133402434 - e_K_result_tmp * 163.29640177382629) + + f_K_result_tmp * 143.06019647824689) + + 3.6791597449555491); K_result[28] = - (((((l_l * 0.48235990137702212 - l_r * 1.089085121366868) - t2 * 1.6216826044552231) - - t3 * 1.340524465328029) - + t4 * 4.9655661234827244) - + ((((t5 * -2.2966994150783089 + t6 * 10.483703915088411) - t7 * 10.20090928184332) - + K_result_tmp * 5.5681353016833661) - - b_K_result_tmp * 19.228317334687461)) - + ((((c_K_result_tmp * 8.6157599887378176 + d_K_result_tmp * 10.25151372048242) - - e_K_result_tmp * 17.685666776294831) - + f_K_result_tmp * 12.35620566699143) - + 0.0030673700589297151); + (((((l_l * -0.74280003899078784 - l_r * 4.0630043396230624) + t2 * 9.0673120113459067) + - t3 * 22.346476016072149) + + t4 * 17.636807409256249) + + ((((t5 * 23.569607119077581 - t6 * 53.096474273163963) + t7 * 41.619562246014389) + - K_result_tmp * 37.121049157794857) + + b_K_result_tmp * 59.264684888143712)) + + ((((c_K_result_tmp * -34.365405218840522 + d_K_result_tmp * 26.8018898082148) + - e_K_result_tmp * 8.07655019275422) + - f_K_result_tmp * 27.990245808797571) + + 0.18353184691337771); K_result[29] = - (((((l_l * -1.911920897125398 + l_r * 0.24593426869670651) + t2 * 7.14798796912681) - - t3 * 13.34683038357106) - + t4 * 8.36966630134579) - + ((((t5 * -0.82327262510986665 + t6 * 5.6353471818981644) - t7 * 7.20030692817838) - - K_result_tmp * 2.3626434219982122) - - b_K_result_tmp * 10.84559032635412)) - + ((((c_K_result_tmp * 14.191595994996121 + d_K_result_tmp * 9.5820208368917452) - - e_K_result_tmp * 0.72146033628724082) - - f_K_result_tmp * 7.0720766273360356) - - 0.085054204897638916); + (((((l_l * -13.062222484765741 + l_r * 0.60395700831470744) + t2 * 23.320411568013672) + - t3 * 51.241865534952652) + + t4 * 44.4968389451877) + + ((((t5 * -1.91073014390169 + t6 * 0.65905440379417479) + t7 * 6.4310838932291272) + + K_result_tmp * 11.52586834601725) + - b_K_result_tmp * 8.2904503333864934)) + + ((((c_K_result_tmp * -17.535361064378641 - d_K_result_tmp * 23.55309179760766) + - e_K_result_tmp * 2.540551063508643) + + f_K_result_tmp * 36.44120267700913) + + 0.010494239782009789); K_result[30] = - (((((l_l * -4.1742566793689893 + l_r * 4.53872156768127) + t2 * 15.50734575111896) - - t3 * 21.734029451530979) - + t4 * 14.01623348892873) - + ((((t5 * -15.81279274016652 + t6 * 10.84830614627616) + t7 * 9.7763982601301826) - - K_result_tmp * 7.5835839567921228) - + b_K_result_tmp * 50.738989806783657)) - + ((((c_K_result_tmp * -66.155558012529809 - d_K_result_tmp * 23.528291640663021) - + e_K_result_tmp * 9.4980639030979326) - + f_K_result_tmp * 21.578793969913871) - + 0.01328455083350657); + (((((l_l * 0.775570566007561 + l_r * 7.9793537477295944) - t2 * 8.1322284186521259) + + t3 * 14.17843024026269) + - t4 * 5.1940905223490086) + + ((((t5 * -55.190748788217853 + t6 * 163.53649996938259) - t7 * 157.76498485588081) + + K_result_tmp * 10.451620348122731) + - b_K_result_tmp * 100.9634667146584)) + + ((((c_K_result_tmp * 93.767579428163913 + d_K_result_tmp * 27.70383189965721) + - e_K_result_tmp * 43.577948833498148) + + f_K_result_tmp * 32.746687393253019) + - 0.15096848012759381); K_result[31] = - (((((l_l * -1.1551293155815809 + l_r * 0.33674493816424428) - t2 * 8.7266330653949975) - + t3 * 35.817481600252279) - - t4 * 38.012160623059557) - + ((((t5 * -4.8098738057536732 + t6 * 11.551236579190411) - t7 * 10.063494830761011) - + K_result_tmp * 12.20219053016346) - - b_K_result_tmp * 5.6684943630282083)) - + ((((c_K_result_tmp * 4.9475479979805126 - d_K_result_tmp * 29.384960137862869) - + e_K_result_tmp * 28.1800964917464) - - f_K_result_tmp * 0.2356079701679093) - + 0.829686679336074); + (((((l_l * 10.299597858281469 - l_r * 0.60307705488308549) - t2 * 59.144656028305349) + + t3 * 135.48327074553481) + - t4 * 114.7352584410914) + + ((((t5 * -3.7932484652178768 + t6 * 22.354454878124589) - t7 * 32.980475141730757) + + K_result_tmp * 2.250022799781354) + - b_K_result_tmp * 25.811096782359861)) + + ((((c_K_result_tmp * 47.6800011379672 + d_K_result_tmp * 33.324610516860972) + - e_K_result_tmp * 21.077795490812949) + - f_K_result_tmp * 28.50201406871431) + + 0.94597601061640657); K_result[32] = - ((((((l_l * 41.585061490786657 + l_r * 6.0561184927155036) - t2 * 131.44125250470489) - + t3 * 276.60369310605978) - - t4 * 243.16860840798989) - + t5 * 138.8342320803927) - + ((((t6 * -536.99420235581056 + t7 * 506.83627363105433) - - K_result_tmp * 30.974532445559522) - + b_K_result_tmp * 284.90102053227321) - - c_K_result_tmp * 81.1773840485689)) - + (((d_K_result_tmp * -150.17204003042119 + e_K_result_tmp * 258.84938701550237) - - f_K_result_tmp * 270.52079892709418) - - 15.43664491638706); + ((((((l_l * 61.4791617650117 + l_r * 80.627627072686693) - t2 * 161.0303488742644) + + t3 * 218.199343979588) + - t4 * 133.68385153700589) + + t5 * 20.949801368735109) + + ((((t6 * -492.92909764466953 + t7 * 579.35839731929309) + - K_result_tmp * 147.72156257968621) + + b_K_result_tmp * 234.13895397613561) + - c_K_result_tmp * 25.51368880439103)) + + (((d_K_result_tmp * 240.99688708231469 - e_K_result_tmp * 78.926412289527974) + - f_K_result_tmp * 309.33283490759828) + - 29.00531359539254); K_result[33] = - ((((((l_l * 7.7812536457489934 + l_r * 34.58984430113123) + t2 * 162.2342532314228) - - t3 * 580.49606213351228) - + t4 * 615.709068989336) - + t5 * 1.864881445630753) - + ((((t6 * -152.9485031252396 + t7 * 246.92556925550139) - - K_result_tmp * 157.85401654892149) - + b_K_result_tmp * 241.32772316785261) - - c_K_result_tmp * 414.22555809740021)) - + (((d_K_result_tmp * 300.17161904155921 - e_K_result_tmp * 403.90374035890858) - + f_K_result_tmp * 176.2543843424576) - - 15.14240934276431); + (((((l_l * 91.586344410967428 + l_r * 51.438790046740017) - t2 * 67.561680546682609) + - t3 * 122.5007362572368) + + t4 * 149.67019310725431) + + ((((t5 * -123.5540859424261 + t6 * 63.509167500106813) + t7 * 12.9600491371037) + - K_result_tmp * 100.30818419599009) + + b_K_result_tmp * 501.371235626336)) + + ((((c_K_result_tmp * -242.51819758580319 - d_K_result_tmp * 238.76291644862741) + + e_K_result_tmp * 524.46940057174243) + - f_K_result_tmp * 409.59894208958917) + - 29.070595484347361); K_result[34] = - ((((((l_l * 155.13595827720121 - l_r * 418.37583361314842) - t2 * 543.373641613612) - + t3 * 691.76926597183729) - - t4 * 271.23706554060283) - + t5 * 1431.482565727471) - + ((((t6 * -2267.7472400909778 + t7 * 1429.60424102931) - + K_result_tmp * 27.746069443133429) - - b_K_result_tmp * 640.74522176670746) - + c_K_result_tmp * 541.46947755639383)) - + (((d_K_result_tmp * 656.83867263586546 - e_K_result_tmp * 736.97392885675254) - + f_K_result_tmp * 95.477527712626284) - - 15.81354296862791); + ((((((l_l * 102.9442561649395 - l_r * 429.66938174545351) - t2 * 390.23976648733662) + + t3 * 770.051188781492) + - t4 * 613.3738301492815) + + t5 * 1469.6580605025811) + + (((((t6 * -2569.217001164438 + t7 * 1779.4728516327159) + + K_result_tmp * 251.8176743933214) + - b_K_result_tmp * 190.65307162040151) + + c_K_result_tmp * 120.6653501339099) + - d_K_result_tmp * 573.12828775644573)) + + ((e_K_result_tmp * 500.46949187207451 + f_K_result_tmp * 129.64788874421319) + - 23.246816687726369); K_result[35] = - ((((((l_l * -412.44731155317493 + l_r * 157.575918973223) + t2 * 1547.814259521376) - - t3 * 2744.5204060610122) - + t4 * 1820.219560753239) - - t5 * 572.5530328321322) - + ((((t6 * 974.3885700408365 - t7 * 772.40933205050339) - - K_result_tmp * 114.0501719428118) - + b_K_result_tmp * 404.09123492440278) - + c_K_result_tmp * 196.5762382296258)) - + (((d_K_result_tmp * -54.763517385076867 + e_K_result_tmp * 598.99679823348617) - - f_K_result_tmp * 907.63909918035654) - - 16.1955171039867); + ((((((l_l * -463.76578684837932 + l_r * 119.0083826631943) + t2 * 1937.88090379133) + - t3 * 4164.768918568664) + + t4 * 3617.776447723892) + - t5 * 227.57432155006009) + + ((((t6 * 448.03498615688738 - t7 * 271.59991491041671) + - K_result_tmp * 277.59010915984771) + - b_K_result_tmp * 533.0917074158599) + + c_K_result_tmp * 33.126442567803828)) + + (((d_K_result_tmp * 1458.849340444136 - e_K_result_tmp * 2096.512151025026) + + f_K_result_tmp * 808.69479906607444) + - 22.186362784812211); K_result[36] = - (((((l_l * 4.8673282012038941 - l_r * 0.02810096066532166) - t2 * 15.310148167403151) - + t3 * 25.521677086020411) - - t4 * 17.4995686305953) - + ((((t5 * 9.638848303935486 - t6 * 30.50688725574598) + t7 * 25.60443722440948) - - K_result_tmp * 6.1293028770247213) - + b_K_result_tmp * 13.776513259656561)) - + ((((c_K_result_tmp * 1.203417106113112 + d_K_result_tmp * 7.4006684172429109) - - e_K_result_tmp * 0.078280734495663432) - - f_K_result_tmp * 18.943781238278682) - - 1.1179990644923421); + (((((l_l * 8.1956998172785624 - l_r * 0.41290000467804361) - t2 * 23.356759644418659) + + t3 * 29.901258795142422) + - t4 * 14.042869270292069) + + ((((t5 * 24.009287104151628 - t6 * 64.2482744105211) + t7 * 52.70401554890271) + - K_result_tmp * 15.839616380380811) + + b_K_result_tmp * 0.53793377410136745)) + + ((((c_K_result_tmp * 17.978125066120182 + d_K_result_tmp * 44.955192195662342) + - e_K_result_tmp * 33.526362248086393) + - f_K_result_tmp * 22.651982649704241) + - 1.8795962319190189); K_result[37] = - (((((l_l * 0.9497695471639569 + l_r * 3.5989617911173322) + t2 * 10.71591703155174) - - t3 * 39.263582735580172) - + t4 * 41.177770681494088) - + ((((t5 * -5.4710586523999183 + t6 * 1.242243077227225) + t7 * 6.6991911879272843) - - K_result_tmp * 15.509720256253139) - + b_K_result_tmp * 22.679641928653769)) - + ((((c_K_result_tmp * -27.77118741940037 + d_K_result_tmp * 28.346828103653149) - - e_K_result_tmp * 30.186470237116961) - + f_K_result_tmp * 2.5521836768642578) - - 1.1001494805476479); + (((((l_l * 0.42738681582884391 + l_r * 7.2593628765526264) + t2 * 20.315287275137489) + - t3 * 54.658902211033173) + + t4 * 44.318301435786651) + + ((((t5 * -19.839895746159321 + t6 * 22.824563831480319) - t7 * 9.2884948381240058) + - K_result_tmp * 15.0201912831711) + + b_K_result_tmp * 47.179229408366332)) + + ((((c_K_result_tmp * -33.2049888072443 - d_K_result_tmp * 5.97211366897347) + + e_K_result_tmp * 24.353501922480579) + - f_K_result_tmp * 23.985341311619251) + - 1.8746759553655661); K_result[38] = - (((((l_l * 7.8471235256337026 - l_r * 16.3594913949482) - t2 * 26.445121281465969) - + t3 * 34.7361106687739) - - t4 * 13.76921938954532) - + ((((t5 * 46.241413401076052 - t6 * 61.397993220193591) + t7 * 34.434033845919679) - + K_result_tmp * 6.39408193708345) - - b_K_result_tmp * 30.03968685748271)) - + ((((c_K_result_tmp * 13.579957902895771 + d_K_result_tmp * 17.222149285946809) - - e_K_result_tmp * 31.03259575564466) - + f_K_result_tmp * 20.449160129221291) - - 1.0657499207339971); + (((((l_l * 0.69844868951772143 - l_r * 18.122980671691639) - t2 * 6.3995723634071346) + + t3 * 21.02672352721272) + - t4 * 20.78704692624726) + + ((((t5 * 37.838307309865463 - t6 * 34.671200446394323) + t7 * 8.7191163785385033) + + K_result_tmp * 34.688026222654223) + - b_K_result_tmp * 42.569583700950453)) + + ((((c_K_result_tmp * 14.22934939509935 - d_K_result_tmp * 64.537792722380942) + + e_K_result_tmp * 40.014871333242709) + + f_K_result_tmp * 44.694082359554322) + - 0.74077563515986466); K_result[39] = - (((((l_l * -19.99529511026709 + l_r * 12.06171890570675) + t2 * 72.537743990509469) - - t3 * 125.62751900310739) - + t4 * 81.444256368884183) - + ((((t5 * -37.515685359414221 + t6 * 63.248612731420778) - t7 * 49.34794327831667) - - K_result_tmp * 12.44633289988889) - + b_K_result_tmp * 18.729910747704128)) - + ((((c_K_result_tmp * 12.43815684624545 + d_K_result_tmp * 13.218379348440161) - + e_K_result_tmp * 16.536193254616439) - - f_K_result_tmp * 45.349373542642482) - - 1.095498659286591); + (((((l_l * -19.65817486856465 + l_r * 1.95324570971728) + t2 * 54.8773028040935) + - t3 * 87.2954304920205) + + t4 * 65.524272050825573) + + ((((t5 * -4.4180525826165589 + t6 * 19.252678012537441) - t7 * 18.058407330542089) + + K_result_tmp * 17.10360950104976) + - b_K_result_tmp * 63.609993464011758)) + + ((((c_K_result_tmp * 27.647047539300079 + d_K_result_tmp * 8.1555150573178743) + - e_K_result_tmp * 49.9663888030974) + + f_K_result_tmp * 63.274879312340417) + - 0.72343737602855029); gain_ = Eigen::Map>(K_result); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index ea637854..86eafda2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -330,20 +330,16 @@ class WheelLegInfantry transmit_buffer_.add_can1_transmission( 0x200, std::bit_cast(control_commands)); - transmit_buffer_.add_can1_transmission( - 0x01, chassis_hip_motors[0].generate_angle_command()); - transmit_buffer_.add_can1_transmission( - 0x02, chassis_hip_motors[1].generate_angle_command()); + 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_angle_command()); - transmit_buffer_.add_can2_transmission( - 0x04, chassis_hip_motors[3].generate_angle_command()); + 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(); } 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 index 2c75b8e6..bc735283 100644 --- 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 @@ -6,10 +6,9 @@ namespace rmcs_msgs { enum class WheelLegMode : uint8_t { STOP = 0, SPIN = 1, - SPIN_2_FOLLOW = 2, - STEP_DOWN = 3, - LAUNCH_RAMP = 4, - BALANCELESS = 5, + FOLLOW = 2, + LAUNCH_RAMP = 3, + BALANCELESS = 4, }; } From ff6e30de00bb9e2515a027d9cc1d1984e0127110 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Wed, 4 Mar 2026 01:35:36 +0800 Subject: [PATCH 31/35] Another day of fooling around. --- .../config/wheelleg-infantry.yaml | 4 +- rmcs_ws/src/rmcs_core/plugins.xml | 3 + .../wheel_leg/desire_state_controller.cpp | 57 +- .../chassis/wheel_leg/desire_state_solver.hpp | 28 +- .../wheel_leg_chassis_controller.cpp | 2 +- .../wheel_leg/wheel_leg_controller.cpp | 47 +- .../src/controller/lqr/lqr_calculator.hpp | 843 +++++++++--------- .../src/hardware/wheelleg-infantry.cpp | 12 +- 8 files changed, 522 insertions(+), 474 deletions(-) diff --git a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml index 95882a46..7bbf8ded 100644 --- a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml @@ -21,9 +21,10 @@ rmcs_executor: # - 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::DesireStateController -> desire_state_controller # - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller - - rmcs_core::controller::chassis::WheelLegController -> wheel_leg_controller # - rmcs_auto_aim::AutoAimInitializer -> auto_aim_initializer @@ -47,7 +48,6 @@ value_broadcaster: - /chassis/yaw/velocity - /chassis/pitch/angle - /chassis/pitch/velocity - - /chassis/velocity - /chassis/left_wheel/control_torque - /chassis/right_wheel/control_torque diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index 2cb6b2bb..a8d3ddb7 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -25,6 +25,9 @@ Test plugin. + + + Test plugin. Test plugin. diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp index 489e6668..68b59770 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp @@ -9,23 +9,32 @@ namespace rmcs_core::controller::chassis { class DesireStateController : public rmcs_executor::Component , public rclcpp::Node { - public: DesireStateController() : Node( get_component_name(), rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { + register_input("/chassis/yaw/velocity", chassis_yaw_velocity_imu_); register_input("/chassis/control_velocity", chassis_control_velocity_); + // register_input("/chassis/control_power_limit", power_limit_); + register_input("/chassis/leg/extended", is_leg_extended_); - register_input("/chassis/control_power_limit", power_limit_); + register_output("/chassis/desire_state", desire_state_); + register_output("/chassis/desire_leg_length", desire_leg_length_); + register_output("/chassis/desire_roll_angle", desire_roll_angle_); reset(); } - void reset() {} + + void reset() { + *desire_state_ = reset_state(); + *desire_leg_length_ = nan_; + *desire_roll_angle_ = nan_; + } void update() override { auto control_velocity = calculate_chassis_control_velocity(); - auto desire_state = update_desire_states(control_velocity); + auto desire_state = update_desire_state(control_velocity); *desire_state_ = desire_state; } @@ -37,40 +46,62 @@ class DesireStateController return chassis_control_velocity; } - rmcs_msgs::WheelLegState update_desire_states(const Eigen::Vector3d& control_velocity) { + rmcs_msgs::WheelLegState update_desire_state(const Eigen::Vector3d& control_velocity) { rmcs_msgs::WheelLegState desire_state; + // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity + // auto& [vx, vz, wz] = control_velocity; - auto& [vx, vz, wz] = control_velocity; + // desire_leg_length_ += vz * leg_length_sensitivity_; + if (*is_leg_extended_) { + *desire_leg_length_ = 0.25; + } else { + *desire_leg_length_ = 0.16; + } - // distance :always 0 during velocity control + // Distance :always 0 during velocity control desire_state.distance = 0.0; // else measure.distance desire_state.velocity = 0.0; - // yaw angle: + // Yaw angle: desire_state.yaw_angle = 0.0; // else measure.yaw desire_state.yaw_velocity = 0.0; - // leg tilt angle: + // Leg tilt angle: desire_state.left_tilt_angle = -0.1; desire_state.right_tilt_angle = -0.1; - // pitch angle: + // Pitch angle: desire_state.body_pitch_angle = 0.0; desire_state.body_pitch_velocity = 0.0; return desire_state; } + void update_jump_state() {} + + void update_climb_state() {} + + 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; + // Leg length limits + static constexpr double max_leg_length_ = 0.36; + static constexpr double min_leg_length_ = 0.13; + // Maximum control velocities static constexpr double translational_velocity_max_ = 2.0; static constexpr double angular_velocity_max_ = 2.0; - InputInterface measure_state_; InputInterface chassis_control_velocity_; + + InputInterface chassis_yaw_velocity_imu_; InputInterface power_limit_; InputInterface is_leg_extended_; @@ -80,9 +111,7 @@ class DesireStateController OutputInterface desire_state_; - bool park_mode_ = false; - - rmcs_msgs::WheelLegState chassis_mode_; + // rmcs_msgs::WheelLegState chassis_mode_; }; } // namespace rmcs_core::controller::chassis 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 index cc642e35..a8e29e3b 100644 --- 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 @@ -19,8 +19,7 @@ class DesireStateSolver { desire_roll_angle_ = 0.0; } - rmcs_msgs::WheelLegState - update(rmcs_msgs::WheelLegMode mode, Eigen::Vector3d control_velocity) { + rmcs_msgs::WheelLegState update(Eigen::Vector3d control_velocity) { // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity auto& [vx, vz, wz] = control_velocity; @@ -32,15 +31,13 @@ class DesireStateSolver { constexpr double power_kp_w = 0.40; // power_limit_velocity_w = power_kp_w * std::sqrt(referee_->chassis_power_limit_); - chassis_mode_ = mode; - - switch (chassis_mode_) { - case rmcs_msgs::WheelLegMode::STOP: break; - case rmcs_msgs::WheelLegMode::SPIN: break; - case rmcs_msgs::WheelLegMode::FOLLOW: break; - case rmcs_msgs::WheelLegMode::LAUNCH_RAMP: break; - case rmcs_msgs::WheelLegMode::BALANCELESS: break; - } + // switch (chassis_mode_) { + // case rmcs_msgs::WheelLegMode::STOP: break; + // case rmcs_msgs::WheelLegMode::SPIN: break; + // case rmcs_msgs::WheelLegMode::FOLLOW: break; + // case rmcs_msgs::WheelLegMode::LAUNCH_RAMP: break; + // case rmcs_msgs::WheelLegMode::BALANCELESS: break; + // } if (std::abs(measure_state_.velocity) < 1e-2) { park_mode_ = true; @@ -57,8 +54,8 @@ class DesireStateSolver { desire_state_.yaw_velocity = 0.0; // leg tilt angle: - desire_state_.left_tilt_angle = -0.1; - desire_state_.right_tilt_angle = -0.1; + desire_state_.left_tilt_angle = 0.0; + desire_state_.right_tilt_angle = 0.0; desire_state_.body_pitch_angle = 0.0; @@ -90,16 +87,11 @@ class DesireStateSolver { const double initial_leg_length_; - double translate_velocity_; - double angular_velocity; - bool park_mode_ = false; double desire_leg_length_; double desire_roll_angle_; - rmcs_msgs::WheelLegMode chassis_mode_; - rmcs_msgs::WheelLegState measure_state_; rmcs_msgs::WheelLegState desire_state_; }; 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 index bf3e4ec9..219f6541 100644 --- 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 @@ -91,7 +91,7 @@ class WheelLegChassisController : rmcs_msgs::WheelLegMode::BALANCELESS; } - // change leg length + // Change leg length if (!last_keyboard_.q && keyboard.q) { leg_extended_ = !leg_extended_; } 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 index c2c98e32..218ee832 100644 --- 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 @@ -290,6 +290,14 @@ class WheelLegController velocity = calculate_velocity; + auto is_parked = std::abs(velocity) < 1e-2; + + // if (is_parked) { + // distance = last_distance_ + velocity * dt_; + // } else { + // distance = 0.0; + // } + distance = last_distance_ + velocity * dt_; last_distance_ = distance; @@ -321,17 +329,17 @@ class WheelLegController measure_state.body_pitch_angle = *chassis_pitch_angle_imu_; measure_state.body_pitch_velocity = filtered_gyro_velocity_.y(); - // measure_state.distance = 0.0; - // measure_state.velocity = 0.0; + measure_state.distance = 0.0; + measure_state.velocity = 0.0; - // measure_state.yaw_angle = 0.0; - // measure_state.yaw_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.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.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; @@ -348,7 +356,9 @@ class WheelLegController rmcs_msgs::WheelLegState calculate_desire_state( Eigen::Vector3d control_velocity, rmcs_msgs::WheelLegState measure_state) { rmcs_msgs::WheelLegState desire_state{}; - + desire_state = desire_state_solver_.update(std::move(control_velocity)); + desire_roll_angle_ = desire_state_solver_.update_desire_roll_angle(); + desire_leg_length_ = desire_state_solver_.update_desire_leg_length(0.13, 0.36); return desire_state; } @@ -390,10 +400,9 @@ class WheelLegController Eigen::Vector4d result{}; auto error_state = desire_state.vector() - measure_state.vector(); - auto gain = lqr_calculator_.update(leg_posture.leg_length.x(), leg_posture.leg_length.y()); - result = -gain * error_state; + result = -1.0 * gain * error_state; auto err_0 = error_state(0); auto err_1 = error_state(1); @@ -428,10 +437,10 @@ class WheelLegController auto output_8 = -gain_08 * err_8; auto output_9 = -gain_09 * err_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(), "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]", @@ -441,9 +450,9 @@ class WheelLegController // measure_state.right_tilt_velocity, measure_state.body_pitch_angle, // measure_state.body_pitch_velocity); - RCLCPP_INFO( - get_logger(), "x: %f, err: %f, output: %f, lw torque: %f, rw torque: %f", - measure_state.distance, error_state(0), output_0, result.x(), result.y()); + // RCLCPP_INFO( + // get_logger(), "x: %f, err: %f, output: %f, lw torque: %f, rw torque: %f", + // measure_state.distance, error_state(0), output_0, result.x(), result.y()); return result; } @@ -557,6 +566,8 @@ class WheelLegController double d_leg_length_, d_roll_angle_; + double desire_leg_length_, desire_roll_angle_; + Eigen::Vector2d last_leg_length_, last_tilt_angle_; Eigen::Vector2d last_dot_leg_length_, last_dot_tilt_angle_; double last_distance_; 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 index f520f9c5..4df23ad4 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp +++ b/rmcs_ws/src/rmcs_core/src/controller/lqr/lqr_calculator.hpp @@ -37,9 +37,11 @@ class LqrCalculator { 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-01 16:47:18 */ + /* 2026-03-03 22:47:45 */ t2 = l_l * l_l; t3 = std::pow(l_l, 3.0); t5 = l_r * l_r; @@ -53,461 +55,468 @@ class LqrCalculator { e_K_result_tmp = l_r * t3; f_K_result_tmp = t2 * t5; K_result[0] = - ((((((l_l * 92.355112446985217 - l_r * 273.86445506363663) - t2 * 224.254186023749) - + t3 * 88.871572657900288) - + t4 * 139.03867730571409) - + t5 * 1059.1334076957751) - + ((((t6 * -1713.582322380568 + t7 * 992.81463015698455) - - K_result_tmp * 49.716992277853123) - - b_K_result_tmp * 852.2018909512982) - + c_K_result_tmp * 1107.871721207649)) - + (((d_K_result_tmp * 877.76631886967994 - e_K_result_tmp * 757.77384415184133) - - f_K_result_tmp * 226.04938788522941) - - 0.71256354892448814); + (((((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 * -273.93209042699431 + l_r * 87.1954445417931) + t2 * 1063.649758788918) - - t3 * 1822.080583531972) - + t4 * 1197.084415865671) - - t5 * 227.18179370482679) - + ((((t6 * 176.5072201142425 + t7 * 52.626985306441931) - - K_result_tmp * 19.096085264645971) - + b_K_result_tmp * 634.11059136111714) - - c_K_result_tmp * 671.58679586745416)) - + (((d_K_result_tmp * -668.10764063013244 + e_K_result_tmp * 728.95367898348775) - + f_K_result_tmp * 19.330597009900949) - - 0.43326490086341518); + (((((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 * -169.01051203621179 + l_r * 157.94912265003771) + t2 * 409.75693102273408) - - t3 * 440.17392258420722) - + t4 * 203.41307893409541) - - t5 * 1410.070913028987) - + ((((t6 * 3668.204235391795 - t7 * 3090.2419270224232) - + K_result_tmp * 530.45507635166462) - - b_K_result_tmp * 766.6009383482052) - - c_K_result_tmp * 158.33857771268461)) - + (((d_K_result_tmp * -854.04868567405856 + e_K_result_tmp * 178.8600310246072) - + f_K_result_tmp * 1269.96976472993) - + 23.207128047454439); + (((((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 * 149.78735965507289 - l_r * 147.8698841180518) - t2 * 1496.200184138826) - + t3 * 3924.5391044762191) - - t4 * 3446.6209396608679) - + t5 * 186.87271957958919) - + (((((t6 * 144.2208512426229 - t7 * 378.82716097452891) - + K_result_tmp * 760.93640228602408) - - b_K_result_tmp * 1279.8774466167081) - + c_K_result_tmp * 787.11174974028756) - - d_K_result_tmp * 987.68485697186634)) - + ((e_K_result_tmp * 365.67457696782378 + f_K_result_tmp * 908.53018463380181) - + 22.483942550576931); + ((((((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 * 43.269453866615727 - l_r * 114.80831612833209) - t2 * 105.854559506629) - + t3 * 42.669181589675233) - + t4 * 65.787062747513673) - + (((((t5 * 451.24376532765962 - t6 * 763.6112387679841) + t7 * 462.32743900156117) - - K_result_tmp * 62.808969622301078) - - b_K_result_tmp * 304.7639016202404) - + c_K_result_tmp * 448.438009099465)) - + (((d_K_result_tmp * 443.936858957365 - e_K_result_tmp * 369.555533404123) - - f_K_result_tmp * 145.429502806394) - - 1.3855287343294009); + (((((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 * -116.0987786944432 + l_r * 42.400768303339511) + t2 * 447.5486993677203) - - t3 * 783.61417177682142) - + t4 * 523.30580265859476) - - t5 * 118.35673199708231) - + ((((t6 * 102.3525107169372 + t7 * 16.93592152337677) - - K_result_tmp * 32.999245323417853) - + b_K_result_tmp * 330.18195434976451) - - c_K_result_tmp * 348.58283653874241)) - + (((d_K_result_tmp * -265.22559675823692 + e_K_result_tmp * 303.82414683642389) - - f_K_result_tmp * 3.1891603029329092) - - 1.2748781040246611); + (((((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 * -86.242587295137454 + l_r * 71.764544206863746) + t2 * 193.7856638351403) - - t3 * 169.578789175179) - + t4 * 40.11978156373538) - + ((((t5 * -636.59304070297219 + t6 * 1687.490766398086) - t7 * 1440.71627320408) - + K_result_tmp * 293.41905516473793) - - b_K_result_tmp * 446.45269502879307)) - + ((((c_K_result_tmp * 1.287182725691272 - d_K_result_tmp * 505.80111961355118) - + e_K_result_tmp * 179.25054212280961) - + f_K_result_tmp * 644.25722998165952) - + 11.11928691495916); + ((((((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 * 71.328342686041111 - l_r * 80.482930304154763) - t2 * 690.9068365075957) - + t3 * 1817.5540207706349) - - t4 * 1606.12373761799) - + t5 * 104.2230208950443) - + (((((t6 * 65.129162631819469 - t7 * 200.50502327691581) - + K_result_tmp * 404.8722608472865) - - b_K_result_tmp * 706.06425897129088) - + c_K_result_tmp * 474.38984547602229) - - d_K_result_tmp * 530.58430793217167)) - + ((e_K_result_tmp * 232.703390591384 + f_K_result_tmp * 453.90587169892359) - + 10.825512719740139); + (((((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 * -13.7107548402327 + l_r * 32.889390181255777) + t2 * 63.010136408437987) - - t3 * 120.2908917827902) - + t4 * 81.192186023692656) - + ((((t5 * -86.295654417094283 + t6 * 104.9401668042791) - t7 * 50.67862388884231) - - K_result_tmp * 41.24249620769973) - + b_K_result_tmp * 103.5981301062985)) - + ((((c_K_result_tmp * -59.890869038501847 + d_K_result_tmp * 33.80188037169389) - + e_K_result_tmp * 19.12753263341865) - - f_K_result_tmp * 80.97574572858079) - - 4.5523915416053544); + ((((((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 * -34.31841822255145 + l_r * 14.861992357354479) + t2 * 86.597549381777526) - - t3 * 100.17688511043821) - + t4 * 43.838807602816154) - + ((((t5 * -53.04373752679102 + t6 * 86.762137700278856) - t7 * 52.971609857210773) - + K_result_tmp * 32.578577722318776) - + b_K_result_tmp * 10.77693951169841)) - + ((((c_K_result_tmp * -37.291486037735318 - d_K_result_tmp * 123.30394009629551) - + e_K_result_tmp * 93.576355983719836) - + f_K_result_tmp * 47.409339150360353) - + 4.5696230962113047); + (((((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 * -11.91422227139272 - l_r * 27.10235100220812) + t2 * 56.828975601220179) - - t3 * 146.68456970594369) - + t4 * 142.1147449488235) - + t5 * 154.3395319474929) - + ((((t6 * -303.00815664096717 + t7 * 218.64233138478039) - - K_result_tmp * 52.363722986539912) - - b_K_result_tmp * 32.932365924853627) - + c_K_result_tmp * 64.567068976981375)) - + (((d_K_result_tmp * 159.78300627157671 - e_K_result_tmp * 148.1391392880353) - - f_K_result_tmp * 22.805801758521429) - - 4.9638966135395766); + (((((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 * 31.461468399575121 + l_r * 7.0714134616560393) - t2 * 153.1939748607592) - + t3 * 286.66176831786368) - - t4 * 192.3952528553325) - + ((((t5 * -41.622794048448469 + t6 * 106.92112334509331) - t7 * 96.497579469107819) - + K_result_tmp * 38.724984322015622) - - b_K_result_tmp * 130.1571583349087)) - + ((((c_K_result_tmp * 92.751034357358009 + d_K_result_tmp * 53.504824436731951) - - e_K_result_tmp * 115.2693156111965) - + f_K_result_tmp * 61.291486557776317) - + 4.9914390609146686); + (((((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 * -5.919673061242066 + l_r * 12.737241060505831) + t2 * 28.153436962463921) - - t3 * 55.044946554856367) - + t4 * 37.815885195552752) - + ((((t5 * -27.134619749177968 + t6 * 24.23448368243384) - t7 * 5.5552340928564732) - - K_result_tmp * 20.807810167510588) - + b_K_result_tmp * 48.792726242241237)) - + ((((c_K_result_tmp * -27.43725637899627 + d_K_result_tmp * 18.57434686475797) - + e_K_result_tmp * 7.059835621642593) - - f_K_result_tmp * 39.034123637183868) - - 1.6142866129663971); + (((((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 * -13.090877616107059 + l_r * 6.2229163275557253) + t2 * 26.982382900743119) - - t3 * 20.516693811938559) - - t4 * 0.066306227643642884) - + ((((t5 * -23.62070281444289 + t6 * 40.791472692179539) - t7 * 26.334497532889159) - + K_result_tmp * 16.707728884426221) - + b_K_result_tmp * 1.4385780808627)) - + ((((c_K_result_tmp * -14.24827386102737 - d_K_result_tmp * 58.955692957258833) - + e_K_result_tmp * 45.448830552771028) - + f_K_result_tmp * 22.9521033794778) - + 1.617778838865062); + (((((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 * -3.04211669546084 - l_r * 12.385246956637429) + t2 * 12.52336612985486) - - t3 * 34.740141172306942) - + t4 * 36.038839915419331) - + ((((t5 * 65.036741890603878 - t6 * 122.9771371839622) + t7 * 85.231479654558) - - K_result_tmp * 22.765648957828152) - - b_K_result_tmp * 16.1753484701422)) - + ((((c_K_result_tmp * 32.4594886556945 + d_K_result_tmp * 65.664297399926809) - - e_K_result_tmp * 57.839965670120407) - - f_K_result_tmp * 11.24969820975552) - - 1.7172066697883861); + (((((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 * 13.92070192741177 + l_r * 1.3267137495911709) - t2 * 64.012779834276941) - + t3 * 113.8555115663932) - - t4 * 71.740561014362669) - + ((((t5 * -8.3254487924926615 + t6 * 24.95980217422504) - t7 * 25.091153555588338) - + K_result_tmp * 18.584986031795061) - - b_K_result_tmp * 58.459437542105583)) - + ((((c_K_result_tmp * 43.56187476503856 + d_K_result_tmp * 25.511952716616278) - - e_K_result_tmp * 53.290945022355167) - + f_K_result_tmp * 23.72327578069881) - + 1.7271519902312811); + (((((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 * 35.0280622655384 - l_r * 124.5719890522153) - t2 * 150.88869027192169) - + t3 * 221.97326755539439) - - t4 * 94.055401007841169) - + t5 * 221.55042045516089) - + ((((t6 * -181.322133395468 - t7 * 6.3908584340698731) - + K_result_tmp * 155.070074821783) - - b_K_result_tmp * 550.21528057284047) - + c_K_result_tmp * 437.78024124401048)) - + (((d_K_result_tmp * 64.327983558699358 - e_K_result_tmp * 228.70519329653911) - + f_K_result_tmp * 233.2843736227486) - + 0.47141423698804658); + ((((((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 * -44.815030362233173 - l_r * 22.67359233341892) + t2 * 300.63777655447711) - - t3 * 722.6443221554473) - + t4 * 615.86211252050509) - + t5 * 135.55840257767949) - + ((((t6 * -328.952448997027 + t7 * 288.48380332919538) - - K_result_tmp * 183.20751049808581) - + b_K_result_tmp * 215.10134447449559) - - c_K_result_tmp * 128.974584711217)) - + (((d_K_result_tmp * 338.35164171883753 - e_K_result_tmp * 228.66532312829861) - - f_K_result_tmp * 167.1989684123391) - + 0.14524761154754681); + ((((((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 * -61.233539815926562 + l_r * 159.5801290651749) + t2 * 190.34084270442739) - - t3 * 251.27876537746511) - + t4 * 133.4885804108757) - - t5 * 794.44465857885507) - + ((((t6 * 1691.717444581247 - t7 * 1249.09402201776) - + K_result_tmp * 183.5228184950972) - + b_K_result_tmp * 16.160767658309179) - - c_K_result_tmp * 402.26934628943991)) - + (((d_K_result_tmp * -435.36829352398382 + e_K_result_tmp * 200.68166088781419) - + f_K_result_tmp * 419.72926424666178) - + 4.01139336562379); + ((((((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 * 21.2577287689819 - l_r * 12.9870023863814) - t2 * 349.89198925126891) - + t3 * 1166.3480762987249) - - t4 * 1216.610967429835) - + ((((t5 * -147.40174670149861 + t6 * 413.19859809021762) - t7 * 378.94485419476177) - + K_result_tmp * 129.87364317535341) - + b_K_result_tmp * 9.882885033112828)) - + ((((c_K_result_tmp * -19.331279683481981 - d_K_result_tmp * 620.86309445194036) - + e_K_result_tmp * 690.186941601567) - + f_K_result_tmp * 73.346939409885664) - + 3.2812475516442419); + ((((((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 * 0.72462086482937327 - l_r * 12.838118637771791) - t2 * 4.1210848153270723) - + t3 * 0.62044551545321458) - + t4 * 8.1870002417434833) - + ((((t5 * 17.68908372284503 - t6 * 35.043641841758962) + t7 * 25.208415843362069) - + K_result_tmp * 17.23878502188936) - - b_K_result_tmp * 44.06040072734023)) - + ((((c_K_result_tmp * 22.11748790399276 + d_K_result_tmp * 1.4029420829172441) - - e_K_result_tmp * 24.402795404650441) - + f_K_result_tmp * 31.4736534261724) - - 0.0081646253371954729); + (((((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 * -4.3734000722553219 - l_r * 0.78338018259563413) + t2 * 28.135271062744639) - - t3 * 71.3902205741409) - + t4 * 65.742993637374184) - + t5 * 6.5149907334735246) - + ((((t6 * -14.358109376589519 + t7 * 13.274897558630689) - - K_result_tmp * 37.007992109015269) - + b_K_result_tmp * 14.846418678293171) - - c_K_result_tmp * 11.31082359108575)) - + (((d_K_result_tmp * 76.331032334544673 - e_K_result_tmp * 66.795376335326623) - - f_K_result_tmp * 7.6665342045499481) - + 0.20272249447720819); + (((((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 * -2.3363551008637682 + l_r * 11.24480445171284) + t2 * 5.3211463753799011) - - t3 * 0.57171085012863132) - - t4 * 2.8177250030630989) - + ((((t5 * -55.757882575886917 + t6 * 135.6439077284615) - t7 * 100.86739565575181) - - K_result_tmp * 5.3392747948147647) - + b_K_result_tmp * 19.330834180309449)) - + ((((c_K_result_tmp * -64.055828387260547 - d_K_result_tmp * 1.4059174267391) - - e_K_result_tmp * 18.543758013579) - + f_K_result_tmp * 47.537512468339571) - + 0.98774204065526794); + ((((((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 * 8.6280928711034832 + l_r * 1.3210078423531131) - t2 * 68.0560870098354) - + t3 * 202.35316767558879) - - t4 * 208.59143546298671) - + ((((t5 * -21.71116305486127 + t6 * 42.441381932796283) - t7 * 38.827920246532912) - + K_result_tmp * 29.585958773682059) - + b_K_result_tmp * 6.3636945233856128)) - + ((((c_K_result_tmp * 10.00259009078974 - d_K_result_tmp * 128.5191776518343) - + e_K_result_tmp * 160.4427305563878) - - f_K_result_tmp * 18.873919049115859) - - 0.2166027208349745); + ((((((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 * -19.681762861883492 - l_r * 45.328716276368127) + t2 * 143.80630371220741) - - t3 * 369.070284413346) - + t4 * 308.957092948608) - + ((((t5 * 282.6697610019545 - t6 * 625.081801917176) + t7 * 474.89533947852368) - - K_result_tmp * 188.47052816038789) - + b_K_result_tmp * 220.00395181122269)) - + ((((c_K_result_tmp * -5.455374894097754 + d_K_result_tmp * 312.59372690641948) - - e_K_result_tmp * 118.0930398226493) - - f_K_result_tmp * 312.01080776932321) - + 0.009970749414400952); + ((((((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 * -122.5503144964209 + l_r * 30.974749704140439) + t2 * 254.42067143844329) - - t3 * 295.52208824580532) - + t4 * 127.0397990835678) - - t5 * 129.61650048432341) - + ((((t6 * 201.59851772110559 - t7 * 89.020511742198821) - + K_result_tmp * 113.0416585624686) - + b_K_result_tmp * 45.434965561776792) - - c_K_result_tmp * 211.222207821207)) - + (((d_K_result_tmp * -426.21016587817888 + e_K_result_tmp * 290.68857401064241) - + f_K_result_tmp * 249.53977184634161) - + 0.58615150703872509); + ((((((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 * -18.876966178854939 + l_r * 21.908970202569989) - t2 * 63.3475340514244) - + t3 * 249.0953992317724) - - t4 * 225.1658232405249) - + (((((t5 * -299.21245192260182 + t6 * 971.40891090736091) - t7 * 943.37102493427437) - + K_result_tmp * 27.277082313115741) - - b_K_result_tmp * 442.40062329065239) - + c_K_result_tmp * 309.0500878910043)) - + (((d_K_result_tmp * 110.4530070746854 - e_K_result_tmp * 230.81777248746579) - + f_K_result_tmp * 308.59688846500728) - + 3.5678962832067311); + ((((((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 * 152.51071513391241 - l_r * 48.247427464136663) - t2 * 821.6452053626864) - + t3 * 1754.27289936947) - - t4 * 1375.09973607764) - + t5 * 114.3735557189091) - + ((((t6 * -38.281592932773442 - t7 * 104.7194256881497) - + K_result_tmp * 251.14629426242871) - - b_K_result_tmp * 601.16537605693748) - + c_K_result_tmp * 527.73825629009127)) - + (((d_K_result_tmp * -6.504003133402434 - e_K_result_tmp * 163.29640177382629) - + f_K_result_tmp * 143.06019647824689) - + 3.6791597449555491); + ((((((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 * -0.74280003899078784 - l_r * 4.0630043396230624) + t2 * 9.0673120113459067) - - t3 * 22.346476016072149) - + t4 * 17.636807409256249) - + ((((t5 * 23.569607119077581 - t6 * 53.096474273163963) + t7 * 41.619562246014389) - - K_result_tmp * 37.121049157794857) - + b_K_result_tmp * 59.264684888143712)) - + ((((c_K_result_tmp * -34.365405218840522 + d_K_result_tmp * 26.8018898082148) - - e_K_result_tmp * 8.07655019275422) - - f_K_result_tmp * 27.990245808797571) - + 0.18353184691337771); + (((((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 * -13.062222484765741 + l_r * 0.60395700831470744) + t2 * 23.320411568013672) - - t3 * 51.241865534952652) - + t4 * 44.4968389451877) - + ((((t5 * -1.91073014390169 + t6 * 0.65905440379417479) + t7 * 6.4310838932291272) - + K_result_tmp * 11.52586834601725) - - b_K_result_tmp * 8.2904503333864934)) - + ((((c_K_result_tmp * -17.535361064378641 - d_K_result_tmp * 23.55309179760766) - - e_K_result_tmp * 2.540551063508643) - + f_K_result_tmp * 36.44120267700913) - + 0.010494239782009789); + (((((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 * 0.775570566007561 + l_r * 7.9793537477295944) - t2 * 8.1322284186521259) - + t3 * 14.17843024026269) - - t4 * 5.1940905223490086) - + ((((t5 * -55.190748788217853 + t6 * 163.53649996938259) - t7 * 157.76498485588081) - + K_result_tmp * 10.451620348122731) - - b_K_result_tmp * 100.9634667146584)) - + ((((c_K_result_tmp * 93.767579428163913 + d_K_result_tmp * 27.70383189965721) - - e_K_result_tmp * 43.577948833498148) - + f_K_result_tmp * 32.746687393253019) - - 0.15096848012759381); + ((((((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 * 10.299597858281469 - l_r * 0.60307705488308549) - t2 * 59.144656028305349) - + t3 * 135.48327074553481) - - t4 * 114.7352584410914) - + ((((t5 * -3.7932484652178768 + t6 * 22.354454878124589) - t7 * 32.980475141730757) - + K_result_tmp * 2.250022799781354) - - b_K_result_tmp * 25.811096782359861)) - + ((((c_K_result_tmp * 47.6800011379672 + d_K_result_tmp * 33.324610516860972) - - e_K_result_tmp * 21.077795490812949) - - f_K_result_tmp * 28.50201406871431) - + 0.94597601061640657); + ((((((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 * 61.4791617650117 + l_r * 80.627627072686693) - t2 * 161.0303488742644) - + t3 * 218.199343979588) - - t4 * 133.68385153700589) - + t5 * 20.949801368735109) - + ((((t6 * -492.92909764466953 + t7 * 579.35839731929309) - - K_result_tmp * 147.72156257968621) - + b_K_result_tmp * 234.13895397613561) - - c_K_result_tmp * 25.51368880439103)) - + (((d_K_result_tmp * 240.99688708231469 - e_K_result_tmp * 78.926412289527974) - - f_K_result_tmp * 309.33283490759828) - - 29.00531359539254); + ((((((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 * 91.586344410967428 + l_r * 51.438790046740017) - t2 * 67.561680546682609) - - t3 * 122.5007362572368) - + t4 * 149.67019310725431) - + ((((t5 * -123.5540859424261 + t6 * 63.509167500106813) + t7 * 12.9600491371037) - - K_result_tmp * 100.30818419599009) - + b_K_result_tmp * 501.371235626336)) - + ((((c_K_result_tmp * -242.51819758580319 - d_K_result_tmp * 238.76291644862741) - + e_K_result_tmp * 524.46940057174243) - - f_K_result_tmp * 409.59894208958917) - - 29.070595484347361); + ((((((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 * 102.9442561649395 - l_r * 429.66938174545351) - t2 * 390.23976648733662) - + t3 * 770.051188781492) - - t4 * 613.3738301492815) - + t5 * 1469.6580605025811) - + (((((t6 * -2569.217001164438 + t7 * 1779.4728516327159) - + K_result_tmp * 251.8176743933214) - - b_K_result_tmp * 190.65307162040151) - + c_K_result_tmp * 120.6653501339099) - - d_K_result_tmp * 573.12828775644573)) - + ((e_K_result_tmp * 500.46949187207451 + f_K_result_tmp * 129.64788874421319) - - 23.246816687726369); + ((((((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 * -463.76578684837932 + l_r * 119.0083826631943) + t2 * 1937.88090379133) - - t3 * 4164.768918568664) - + t4 * 3617.776447723892) - - t5 * 227.57432155006009) - + ((((t6 * 448.03498615688738 - t7 * 271.59991491041671) - - K_result_tmp * 277.59010915984771) - - b_K_result_tmp * 533.0917074158599) - + c_K_result_tmp * 33.126442567803828)) - + (((d_K_result_tmp * 1458.849340444136 - e_K_result_tmp * 2096.512151025026) - + f_K_result_tmp * 808.69479906607444) - - 22.186362784812211); + ((((((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 * 8.1956998172785624 - l_r * 0.41290000467804361) - t2 * 23.356759644418659) - + t3 * 29.901258795142422) - - t4 * 14.042869270292069) - + ((((t5 * 24.009287104151628 - t6 * 64.2482744105211) + t7 * 52.70401554890271) - - K_result_tmp * 15.839616380380811) - + b_K_result_tmp * 0.53793377410136745)) - + ((((c_K_result_tmp * 17.978125066120182 + d_K_result_tmp * 44.955192195662342) - - e_K_result_tmp * 33.526362248086393) - - f_K_result_tmp * 22.651982649704241) - - 1.8795962319190189); + ((((((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 * 0.42738681582884391 + l_r * 7.2593628765526264) + t2 * 20.315287275137489) - - t3 * 54.658902211033173) - + t4 * 44.318301435786651) - + ((((t5 * -19.839895746159321 + t6 * 22.824563831480319) - t7 * 9.2884948381240058) - - K_result_tmp * 15.0201912831711) - + b_K_result_tmp * 47.179229408366332)) - + ((((c_K_result_tmp * -33.2049888072443 - d_K_result_tmp * 5.97211366897347) - + e_K_result_tmp * 24.353501922480579) - - f_K_result_tmp * 23.985341311619251) - - 1.8746759553655661); + ((((((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 * 0.69844868951772143 - l_r * 18.122980671691639) - t2 * 6.3995723634071346) - + t3 * 21.02672352721272) - - t4 * 20.78704692624726) - + ((((t5 * 37.838307309865463 - t6 * 34.671200446394323) + t7 * 8.7191163785385033) - + K_result_tmp * 34.688026222654223) - - b_K_result_tmp * 42.569583700950453)) - + ((((c_K_result_tmp * 14.22934939509935 - d_K_result_tmp * 64.537792722380942) - + e_K_result_tmp * 40.014871333242709) - + f_K_result_tmp * 44.694082359554322) - - 0.74077563515986466); + ((((((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 * -19.65817486856465 + l_r * 1.95324570971728) + t2 * 54.8773028040935) - - t3 * 87.2954304920205) - + t4 * 65.524272050825573) - + ((((t5 * -4.4180525826165589 + t6 * 19.252678012537441) - t7 * 18.058407330542089) - + K_result_tmp * 17.10360950104976) - - b_K_result_tmp * 63.609993464011758)) - + ((((c_K_result_tmp * 27.647047539300079 + d_K_result_tmp * 8.1555150573178743) - - e_K_result_tmp * 49.9663888030974) - + f_K_result_tmp * 63.274879312340417) - - 0.72343737602855029); + ((((((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); } diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index 86eafda2..ea637854 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -330,16 +330,20 @@ class WheelLegInfantry 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()); + transmit_buffer_.add_can1_transmission( + 0x01, chassis_hip_motors[0].generate_angle_command()); + transmit_buffer_.add_can1_transmission( + 0x02, chassis_hip_motors[1].generate_angle_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_.add_can2_transmission( + 0x03, chassis_hip_motors[2].generate_angle_command()); + transmit_buffer_.add_can2_transmission( + 0x04, chassis_hip_motors[3].generate_angle_command()); transmit_buffer_.trigger_transmission(); } From 0b154f64fa0e117819aba6cba35c9570ac44a95d Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sun, 8 Mar 2026 23:29:01 +0800 Subject: [PATCH 32/35] Porting the skipped parts of the old code --- .../wheel_leg/desire_state_controller.cpp | 1 + .../chassis/wheel_leg/desire_state_solver.hpp | 81 ++++++++++++++----- .../wheel_leg_chassis_controller.cpp | 12 ++- .../wheel_leg/wheel_leg_controller.cpp | 75 +++++++++++++++-- 4 files changed, 144 insertions(+), 25 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp index 68b59770..e60c6485 100644 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp +++ b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp @@ -23,6 +23,7 @@ class DesireStateController register_output("/chassis/desire_state", desire_state_); register_output("/chassis/desire_leg_length", desire_leg_length_); register_output("/chassis/desire_roll_angle", desire_roll_angle_); + reset(); } 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 index a8e29e3b..5cc8a860 100644 --- 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 @@ -1,8 +1,6 @@ #pragma once -#include "rmcs_msgs/wheel_leg_mode.hpp" #include "rmcs_msgs/wheel_leg_state.hpp" -#include #include namespace rmcs_core::controller { @@ -15,8 +13,12 @@ class DesireStateSolver { } 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) { @@ -25,20 +27,6 @@ class DesireStateSolver { // desire_leg_length_ += vz * leg_length_sensitivity_; - constexpr double power_kp_x = 0.26; - // power_limit_velocity_x = power_kp_x * std::sqrt(referee_->chassis_power_limit_); - - constexpr double power_kp_w = 0.40; - // power_limit_velocity_w = power_kp_w * std::sqrt(referee_->chassis_power_limit_); - - // switch (chassis_mode_) { - // case rmcs_msgs::WheelLegMode::STOP: break; - // case rmcs_msgs::WheelLegMode::SPIN: break; - // case rmcs_msgs::WheelLegMode::FOLLOW: break; - // case rmcs_msgs::WheelLegMode::LAUNCH_RAMP: break; - // case rmcs_msgs::WheelLegMode::BALANCELESS: break; - // } - if (std::abs(measure_state_.velocity) < 1e-2) { park_mode_ = true; } else { @@ -62,7 +50,14 @@ class DesireStateSolver { return desire_state_; } - double update_desire_leg_length(const double min, const double max) { + 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_; @@ -74,9 +69,57 @@ class DesireStateSolver { measure_state_ = measure_state; } - void update_power() {} + 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; + 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; + jump_active = false; + break; + } + } + } + + void update_climb_state() {} private: + enum class JumpState { + IDLE = 0, + EXTEND_LEGS = 1, + CONTRACT_LEGS = 2, + LEVITATE = 3, + LAND = 4, + }; + + 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; @@ -92,6 +135,8 @@ class DesireStateSolver { double desire_leg_length_; double desire_roll_angle_; + JumpState jump_state_ = JumpState::IDLE; + rmcs_msgs::WheelLegState measure_state_; rmcs_msgs::WheelLegState desire_state_; }; 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 index 219f6541..9d7d365e 100644 --- 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 @@ -41,7 +41,7 @@ class WheelLegChassisController register_output("/chassis/control_mode", mode_); register_output("/chassis/leg/extended", is_leg_extended_); - + register_output("/chassis/jump/active", is_jump_active_); register_output("/chassis/control_velocity", chassis_control_velocity_); } @@ -96,7 +96,15 @@ class WheelLegChassisController leg_extended_ = !leg_extended_; } + // Jump + if (!last_keyboard_.e && keyboard.e) { + jump_active_ = !jump_active_; + } + + // Climb + *is_leg_extended_ = leg_extended_; + *is_jump_active_ = jump_active_; *mode_ = mode; } @@ -228,8 +236,10 @@ class WheelLegChassisController OutputInterface mode_; OutputInterface is_leg_extended_; + OutputInterface is_jump_active_; bool leg_extended_ = false; + bool jump_active_ = false; bool spinning_forward_ = true; pid::PidCalculator following_velocity_controller_; 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 index 218ee832..e3f51357 100644 --- 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 @@ -50,6 +50,9 @@ class WheelLegController register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_mode", mode_); + register_input("/chassis/leg/extended", is_leg_extended_); + register_input("/chassis/jump/active", is_jump_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_); @@ -259,7 +262,58 @@ class WheelLegController return result; } - void detect_chassis_levitate() {} + void 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::STOP) + && (mode != rmcs_msgs::WheelLegMode::BALANCELESS); + + if (mode_active) { + if ((average_support_force < levitate_force) && allow_levitate_) { + levitate_active_ = true; + } else if (average_support_force > normal_force) { + if (levitate_active_) { + allow_levitate_ = false; + } + levitate_active_ = false; + } + + if (left_support_force < levitate_force && allow_left_levitate_) { + left_levitate_active_ = true; + } else if (left_support_force > normal_force) { + if (left_levitate_active_) { + allow_left_levitate_ = false; + } + left_levitate_active_ = false; + } + + if (right_support_force < levitate_force) { + right_levitate_active_ = true; + } else if (right_support_force > normal_force) { + if (right_levitate_active_) { + allow_right_levitate_ = 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; + } + } + + // void detect_chassis_fall() {} Eigen::Vector2d calculate_translational_distance( LegPosture leg_posture, Eigen::Vector2d wheel_velocities, Eigen::Vector4d hip_angles) { @@ -355,10 +409,16 @@ class WheelLegController 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(); - desire_leg_length_ = desire_state_solver_.update_desire_leg_length(0.13, 0.36); + return desire_state; } @@ -367,9 +427,9 @@ class WheelLegController Eigen::Vector2d result; auto roll_control_force = - roll_angle_pid_calculator_.update(d_roll_angle_ - *chassis_roll_angle_imu_); + roll_angle_pid_calculator_.update(desire_roll_angle_ - *chassis_roll_angle_imu_); auto leg_length_control_force = leg_length_pid_calculator_.update( - d_leg_length_ - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); + desire_leg_length_ - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); auto calculate_compensation_feedforward_force = [this](double coefficient) { return (body_mess_ / 2.0 + centroid_position_coefficient_ * leg_mess_) * coefficient; @@ -551,6 +611,9 @@ class WheelLegController InputInterface chassis_control_velocity_; InputInterface mode_; + InputInterface is_leg_extended_; + InputInterface is_jump_active_; + OutputInterface left_front_hip_control_torque_; OutputInterface left_back_hip_control_torque_; OutputInterface right_front_hip_control_torque_; @@ -564,9 +627,9 @@ class WheelLegController OutputInterface left_wheel_control_torque_; OutputInterface right_wheel_control_torque_; - double d_leg_length_, d_roll_angle_; - double desire_leg_length_, desire_roll_angle_; + bool levitate_active_{false}, left_levitate_active_{false}, right_levitate_active_{false}; + bool allow_levitate_{true}, allow_left_levitate_{true}, allow_right_levitate_{true}; Eigen::Vector2d last_leg_length_, last_tilt_angle_; Eigen::Vector2d last_dot_leg_length_, last_dot_tilt_angle_; From 15a69f29601bf49e1a8639a403bfba57a7643a75 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Tue, 10 Mar 2026 00:01:00 +0800 Subject: [PATCH 33/35] Delete `desire_state_controller.cpp`. --- .../config/wheelleg-infantry.yaml | 1 - rmcs_ws/src/rmcs_core/plugins.xml | 3 - .../controller/chassis/wheel_leg/README.md | 2 + .../wheel_leg/desire_state_controller.cpp | 123 ------------------ .../chassis/wheel_leg/desire_state_solver.hpp | 12 +- .../wheel_leg/wheel_leg_controller.cpp | 11 +- 6 files changed, 18 insertions(+), 134 deletions(-) delete mode 100644 rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp diff --git a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml index 7bbf8ded..2d4ba216 100644 --- a/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml +++ b/rmcs_ws/src/rmcs_bringup/config/wheelleg-infantry.yaml @@ -23,7 +23,6 @@ rmcs_executor: - rmcs_core::controller::chassis::WheelLegController -> wheel_leg_controller - rmcs_core::controller::chassis::WheelLegChassisController -> chassis_controller - # - rmcs_core::controller::chassis::DesireStateController -> desire_state_controller # - rmcs_core::controller::chassis::ChassisPowerController -> chassis_power_controller diff --git a/rmcs_ws/src/rmcs_core/plugins.xml b/rmcs_ws/src/rmcs_core/plugins.xml index a8d3ddb7..2cb6b2bb 100644 --- a/rmcs_ws/src/rmcs_core/plugins.xml +++ b/rmcs_ws/src/rmcs_core/plugins.xml @@ -25,9 +25,6 @@ Test plugin. - - - Test plugin. Test plugin. 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 index 51984762..53467fa2 100644 --- 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 @@ -1 +1,3 @@ # desire state + +desire state: State: [s ds phi d_phi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b] diff --git a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp b/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp deleted file mode 100644 index e60c6485..00000000 --- a/rmcs_ws/src/rmcs_core/src/controller/chassis/wheel_leg/desire_state_controller.cpp +++ /dev/null @@ -1,123 +0,0 @@ -#include -#include -#include -#include -#include - -namespace rmcs_core::controller::chassis { - -class DesireStateController - : public rmcs_executor::Component - , public rclcpp::Node { -public: - DesireStateController() - : Node( - get_component_name(), - rclcpp::NodeOptions{}.automatically_declare_parameters_from_overrides(true)) { - register_input("/chassis/yaw/velocity", chassis_yaw_velocity_imu_); - register_input("/chassis/control_velocity", chassis_control_velocity_); - // register_input("/chassis/control_power_limit", power_limit_); - - register_input("/chassis/leg/extended", is_leg_extended_); - - register_output("/chassis/desire_state", desire_state_); - register_output("/chassis/desire_leg_length", desire_leg_length_); - register_output("/chassis/desire_roll_angle", desire_roll_angle_); - - reset(); - } - - void reset() { - *desire_state_ = reset_state(); - *desire_leg_length_ = nan_; - *desire_roll_angle_ = nan_; - } - - void update() override { - auto control_velocity = calculate_chassis_control_velocity(); - auto desire_state = update_desire_state(control_velocity); - - *desire_state_ = desire_state; - } - -private: - 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 update_desire_state(const Eigen::Vector3d& control_velocity) { - rmcs_msgs::WheelLegState desire_state; - // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity - // auto& [vx, vz, wz] = control_velocity; - - // desire_leg_length_ += vz * leg_length_sensitivity_; - if (*is_leg_extended_) { - *desire_leg_length_ = 0.25; - } else { - *desire_leg_length_ = 0.16; - } - - // Distance :always 0 during velocity control - desire_state.distance = 0.0; // else measure.distance - 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.1; - desire_state.right_tilt_angle = -0.1; - - // Pitch angle: - desire_state.body_pitch_angle = 0.0; - desire_state.body_pitch_velocity = 0.0; - - return desire_state; - } - - void update_jump_state() {} - - void update_climb_state() {} - - 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; - - // Leg length limits - static constexpr double max_leg_length_ = 0.36; - static constexpr double min_leg_length_ = 0.13; - - // Maximum control velocities - static constexpr double translational_velocity_max_ = 2.0; - static constexpr double angular_velocity_max_ = 2.0; - - InputInterface chassis_control_velocity_; - - InputInterface chassis_yaw_velocity_imu_; - InputInterface power_limit_; - - InputInterface is_leg_extended_; - - OutputInterface desire_leg_length_; - OutputInterface desire_roll_angle_; - - OutputInterface desire_state_; - - // rmcs_msgs::WheelLegState chassis_mode_; -}; - -} // namespace rmcs_core::controller::chassis - -#include - -PLUGINLIB_EXPORT_CLASS( - rmcs_core::controller::chassis::DesireStateController, rmcs_executor::Component) \ No newline at end of file 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 index 5cc8a860..94c5cb04 100644 --- 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 @@ -86,6 +86,7 @@ class DesireStateSolver { } case JumpState::CONTRACT_LEGS: { desire_leg_length_ = 0.14; + jump_state_ = JumpState::LEVITATE; break; } case JumpState::LEVITATE: { @@ -103,8 +104,6 @@ class DesireStateSolver { } } - void update_climb_state() {} - private: enum class JumpState { IDLE = 0, @@ -114,6 +113,14 @@ class DesireStateSolver { LAND = 4, }; + enum class ClimbState { + IDLE = 0, + DETECT = 1, + CONTRACT_LEGS = 2, + EXTEND_LEGS = 3, + LAND = 4, + }; + constexpr static rmcs_msgs::WheelLegState reset_state() { return rmcs_msgs::WheelLegState{nan_, nan_, nan_, nan_, nan_, nan_, nan_, nan_, nan_, nan_}; } @@ -136,6 +143,7 @@ class DesireStateSolver { double desire_roll_angle_; JumpState jump_state_ = JumpState::IDLE; + ClimbState climb_state_ = ClimbState::IDLE; rmcs_msgs::WheelLegState measure_state_; rmcs_msgs::WheelLegState desire_state_; 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 index e3f51357..5ef0fe22 100644 --- 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 @@ -346,11 +346,12 @@ class WheelLegController auto is_parked = std::abs(velocity) < 1e-2; - // if (is_parked) { - // distance = last_distance_ + velocity * dt_; - // } else { - // distance = 0.0; - // } + // When the vehicle stops the position control is activated. + // if (is_parked) { + // distance = last_distance_ + velocity * dt_; + // } else { + // distance = 0.0; + // } distance = last_distance_ + velocity * dt_; last_distance_ = distance; From 900bdaf77adeea7027a67409392a8aa13ade26f1 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Sat, 14 Mar 2026 00:43:26 +0800 Subject: [PATCH 34/35] Update `desire_state_solver.hpp` and replace `chassis_controller.cpp` with `wheel_leg_chassis_controller.cpp`. --- .../controller/chassis/wheel_leg/README.md | 8 +- .../chassis/wheel_leg/desire_state_solver.hpp | 51 ++- .../wheel_leg_chassis_controller.cpp | 83 +++-- .../wheel_leg/wheel_leg_controller.cpp | 291 ++++++++++++------ .../src/hardware/device/dm_motor.hpp | 6 + .../src/hardware/wheelleg-infantry.cpp | 8 +- .../include/rmcs_msgs/wheel_leg_mode.hpp | 5 +- 7 files changed, 301 insertions(+), 151 deletions(-) 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 index 53467fa2..5557cb38 100644 --- 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 @@ -1,3 +1,9 @@ # desire state -desire state: State: [s ds phi d_phi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b] +desire state: [s ds phi d_phi theta_l d_theta_l theta_r d_theta_r theta_b d_theta_b] + +## Wheel leg mode: +balanceless: 锁死关节，仅控轮子移动 +follow： yaw轴角度、角速度 +spin： yaw轴角速度 +rescue: 关闭lqr，仅关节 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 index 94c5cb04..c7f685fe 100644 --- 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 @@ -22,9 +22,6 @@ class DesireStateSolver { } rmcs_msgs::WheelLegState update(Eigen::Vector3d control_velocity) { - // x-axis translational velocity, z-axis vertical velocity, z-axis angular velocity - auto& [vx, vz, wz] = control_velocity; - // desire_leg_length_ += vz * leg_length_sensitivity_; if (std::abs(measure_state_.velocity) < 1e-2) { @@ -34,7 +31,7 @@ class DesireStateSolver { } // distance :always 0 during velocity control - desire_state_.distance = 0.0; // else measure.distance + desire_state_.distance = 0.0; desire_state_.velocity = 0.0; // yaw angle: @@ -43,13 +40,21 @@ class DesireStateSolver { // 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) { @@ -57,7 +62,6 @@ class DesireStateSolver { } else { desire_leg_length_ = 0.16; } - desire_leg_length_ = std::clamp(desire_leg_length_, min, max); return desire_leg_length_; @@ -65,10 +69,6 @@ class DesireStateSolver { double update_desire_roll_angle() const { return desire_roll_angle_; } - void update_measure_state(rmcs_msgs::WheelLegState& measure_state) { - measure_state_ = measure_state; - } - void update_jump_state(bool jump_active, double leg_length) { switch (jump_state_) { case JumpState::IDLE: { @@ -86,7 +86,9 @@ class DesireStateSolver { } case JumpState::CONTRACT_LEGS: { desire_leg_length_ = 0.14; - jump_state_ = JumpState::LEVITATE; + if (leg_length <= 0.15) { + jump_state_ = JumpState::LEVITATE; + } break; } case JumpState::LEVITATE: { @@ -98,12 +100,16 @@ class DesireStateSolver { } case JumpState::LAND: { jump_state_ = JumpState::IDLE; - jump_active = false; 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, @@ -121,6 +127,27 @@ class DesireStateSolver { 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; // or + 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_}; } @@ -143,7 +170,7 @@ class DesireStateSolver { double desire_roll_angle_; JumpState jump_state_ = JumpState::IDLE; - ClimbState climb_state_ = ClimbState::IDLE; + // ClimbState climb_state_ = ClimbState::IDLE; rmcs_msgs::WheelLegState measure_state_; rmcs_msgs::WheelLegState desire_state_; 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 index 9d7d365e..1ed71031 100644 --- 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 @@ -36,12 +36,20 @@ class WheelLegChassisController 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/leg/extended", is_leg_extended_); - register_output("/chassis/jump/active", is_jump_active_); + + 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_); } @@ -73,6 +81,12 @@ class WheelLegChassisController 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) { @@ -85,30 +99,37 @@ class WheelLegChassisController mode = mode == rmcs_msgs::WheelLegMode::LAUNCH_RAMP ? rmcs_msgs::WheelLegMode::FOLLOW : rmcs_msgs::WheelLegMode::LAUNCH_RAMP; - } else if (!last_keyboard_.z && keyboard.z) { + } 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_.q && keyboard.q) { + if (!last_keyboard_.z && keyboard.z) { leg_extended_ = !leg_extended_; } // Jump - if (!last_keyboard_.e && keyboard.e) { - jump_active_ = !jump_active_; - } + jump_active_ = !last_keyboard_.e && keyboard.e; // Climb + climb_active_ = !last_keyboard_.q && keyboard.q; + + // Rescue tip-over + rescue_tip_over_ = !last_keyboard_.r && keyboard.r; + *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; @@ -117,7 +138,7 @@ class WheelLegChassisController } void reset_all_controls() { - *mode_ = rmcs_msgs::WheelLegMode::STOP; + *mode_ = rmcs_msgs::WheelLegMode::BALANCELESS; *chassis_control_velocity_ = {nan, nan, nan}; } @@ -144,45 +165,38 @@ class WheelLegChassisController 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::STOP: break; + case rmcs_msgs::WheelLegMode::BALANCELESS: + case rmcs_msgs::WheelLegMode::RESCUE_TIP_OVER: break; 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::BALANCELESS: { - double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - - // err: [0, 2pi) -> [0, alignment) -> signed. - // In step-down mode, two sides of the chassis can be used for alignment. - // TODO: Dynamically determine the split angle based on chassis velocity. - constexpr double alignment = std::numbers::pi; - while (err > alignment / 2) { - chassis_control_angle -= alignment; - if (chassis_control_angle < 0) - chassis_control_angle += 2 * std::numbers::pi; - err -= alignment; - } - - angular_velocity = following_velocity_controller_.update(err); - } break; case rmcs_msgs::WheelLegMode::LAUNCH_RAMP: { double err = calculate_unsigned_chassis_angle_error(chassis_control_angle); - // err: [0, 2pi) -> signed - // In launch ramp mode, only one direction can be used for alignment. + // 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_; @@ -232,17 +246,26 @@ class WheelLegChassisController 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 leg_extended_ = false; - bool jump_active_ = false; 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_; }; 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 index 5ef0fe22..f70bbba4 100644 --- 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 @@ -42,7 +42,9 @@ class WheelLegController , 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_(350.0, 0.0, 0.0) { + , leg_length_pid_calculator_(350.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) { 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_); @@ -50,8 +52,14 @@ class WheelLegController register_input("/chassis/control_velocity", chassis_control_velocity_); register_input("/chassis/control_mode", mode_); - register_input("/chassis/leg/extended", is_leg_extended_); - register_input("/chassis/jump/active", is_jump_active_); + 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_); @@ -112,20 +120,30 @@ class WheelLegController auto wheel_velocities = calculate_wheel_velocities(); auto leg_posture = calculate_leg_posture(hip_angles); - auto distance = calculate_translational_distance(leg_posture, wheel_velocities, 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 desire_state = calculate_desire_state(chassis_control_velocity, measure_state); - 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); - update_hip_control_angles(); + if (*is_balanceless_) { + update_hip_zero_point_control_angles(); + calculate_balanceless_control_torques(chassis_control_velocity, wheel_velocities); + return; + } + if (*is_rescue_tip_over_) { + update_hip_rescue_tip_over_control_torques(); + return; + } + + // update_hip_zero_point_control_angles(); update_hip_and_wheel_torques(leg_forces, control_torques); // *left_front_hip_control_torque_ = 0.0; @@ -138,8 +156,8 @@ class WheelLegController // *left_back_hip_control_torque_, *right_back_hip_control_torque_, // *right_front_hip_control_torque_); - // *left_wheel_control_torque_ = 0.0; - // *right_wheel_control_torque_ = 0.0; + *left_wheel_control_torque_ = 0.0; + *right_wheel_control_torque_ = 0.0; } private: @@ -201,8 +219,6 @@ class WheelLegController result.leg_length = Eigen::Vector2d{left_leg_length, right_leg_length}; result.tilt_angle = Eigen::Vector2d{left_tilt_angle, right_tilt_angle}; - // RCLCPP_INFO(get_logger(), "left angle: %f", left_tilt_angle); - 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_; @@ -233,7 +249,7 @@ class WheelLegController 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 + // 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()) @@ -262,61 +278,8 @@ class WheelLegController return result; } - void 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::STOP) - && (mode != rmcs_msgs::WheelLegMode::BALANCELESS); - - if (mode_active) { - if ((average_support_force < levitate_force) && allow_levitate_) { - levitate_active_ = true; - } else if (average_support_force > normal_force) { - if (levitate_active_) { - allow_levitate_ = false; - } - levitate_active_ = false; - } - - if (left_support_force < levitate_force && allow_left_levitate_) { - left_levitate_active_ = true; - } else if (left_support_force > normal_force) { - if (left_levitate_active_) { - allow_left_levitate_ = false; - } - left_levitate_active_ = false; - } - - if (right_support_force < levitate_force) { - right_levitate_active_ = true; - } else if (right_support_force > normal_force) { - if (right_levitate_active_) { - allow_right_levitate_ = 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; - } - } - - // void detect_chassis_fall() {} - - Eigen::Vector2d calculate_translational_distance( - LegPosture leg_posture, Eigen::Vector2d wheel_velocities, Eigen::Vector4d hip_angles) { + Eigen::Vector2d + calculate_translational_distance(LegPosture leg_posture, Eigen::Vector2d wheel_velocities) { Eigen::Vector2d result; auto& [distance, velocity] = result; @@ -339,19 +302,19 @@ class WheelLegController // 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_}); + // 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 (is_parked) { - // distance = last_distance_ + velocity * dt_; - // } else { - // distance = 0.0; - // } + 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; @@ -363,17 +326,17 @@ class WheelLegController calculate_measure_state(Eigen::Vector2d distance, LegPosture leg_posture) { rmcs_msgs::WheelLegState measure_state; - filtered_gyro_velocity_ = chassis_gyro_velocity_filter_.update( - Eigen::Vector3d{ - *chassis_roll_velocity_imu_, *chassis_pitch_velocity_imu_, - *chassis_yaw_velocity_imu_}); + // filtered_gyro_velocity_ = chassis_gyro_velocity_filter_.update( + // Eigen::Vector3d{ + // *chassis_roll_velocity_imu_, *chassis_pitch_velocity_imu_, + // *chassis_yaw_velocity_imu_}); leg_posture.tilt_angle.array() += *chassis_pitch_angle_imu_; measure_state.distance = distance.x(); measure_state.velocity = distance.y(); measure_state.yaw_angle = *chassis_yaw_angle_imu_; - measure_state.yaw_velocity = filtered_gyro_velocity_.z(); + 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(); @@ -382,19 +345,19 @@ class WheelLegController 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 = filtered_gyro_velocity_.y(); + measure_state.body_pitch_velocity = *chassis_pitch_velocity_imu_; - measure_state.distance = 0.0; - measure_state.velocity = 0.0; + // measure_state.distance = 0.0; + // measure_state.velocity = 0.0; - measure_state.yaw_angle = 0.0; - measure_state.yaw_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.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.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; @@ -402,6 +365,69 @@ class WheelLegController 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; @@ -410,7 +436,6 @@ class WheelLegController 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); @@ -460,9 +485,21 @@ class WheelLegController LegPosture leg_posture) { Eigen::Vector4d result{}; - auto error_state = desire_state.vector() - measure_state.vector(); + 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 your legs vertical,and only remain legs' outputs. + // set zero + error_state(0, 0) = 0.0; // distance + error_state(1, 0) = 0.0; // velocity + error_state(2, 0) = 0.0; // yaw angle + error_state(3, 0) = 0.0; // yaw velocity + + error_state(8, 0) = 0.0; // body pitch angle + error_state(9, 0) = 0.0; // body pitch velocity + } + result = -1.0 * gain * error_state; auto err_0 = error_state(0); @@ -498,10 +535,10 @@ class WheelLegController auto output_8 = -gain_08 * err_8; auto output_9 = -gain_09 * err_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(), "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]", @@ -518,13 +555,32 @@ class WheelLegController return result; } - void update_hip_control_angles() { - *left_front_hip_control_angle_ = 0.2; - *left_back_hip_control_angle_ = -0.2; - *right_front_hip_control_angle_ = -0.2; - *right_back_hip_control_angle_ = 0.2; + void calculate_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() { reset_control_angles(); } + 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] = @@ -535,6 +591,11 @@ class WheelLegController auto right_hip_control_torque = right_leg_vmc_solver_.update_joint_torque(right_leg_force, right_leg_control_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); @@ -542,6 +603,8 @@ class WheelLegController *left_back_hip_control_torque_ = clamp_hip_control_torque(-left_hip_control_torque.y()); *right_back_hip_control_torque_ = clamp_hip_control_torque(right_hip_control_torque.y()); *right_front_hip_control_torque_ = clamp_hip_control_torque(-right_hip_control_torque.x()); + + reset_control_angles(); } static double clamp_wheel_control_torque(const double& torque) { @@ -552,6 +615,20 @@ class WheelLegController return std::clamp(torque, -15., 15.); } + 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(); @@ -563,6 +640,8 @@ class WheelLegController 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; @@ -612,8 +691,14 @@ class WheelLegController 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_; @@ -630,11 +715,13 @@ class WheelLegController double desire_leg_length_, desire_roll_angle_; bool levitate_active_{false}, left_levitate_active_{false}, right_levitate_active_{false}; - bool allow_levitate_{true}, allow_left_levitate_{true}, allow_right_levitate_{true}; + + bool about_to_fall_{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_; @@ -645,6 +732,8 @@ class WheelLegController 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_; lqr::LqrCalculator lqr_calculator_; }; } // namespace rmcs_core::controller::chassis 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 index 804dff32..df151b66 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/device/dm_motor.hpp @@ -87,6 +87,12 @@ class DmMotor : public librmcs::device::DmMotor { 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(); diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index ea637854..c08487f1 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -331,9 +331,9 @@ class WheelLegInfantry 0x200, std::bit_cast(control_commands)); transmit_buffer_.add_can1_transmission( - 0x01, chassis_hip_motors[0].generate_angle_command()); + 0x01, chassis_hip_motors[0].generate_disable_command()); transmit_buffer_.add_can1_transmission( - 0x02, chassis_hip_motors[1].generate_angle_command()); + 0x02, chassis_hip_motors[1].generate_disable_command()); control_commands[0] = 0; control_commands[1] = chassis_wheel_motors_[1].generate_command(); @@ -341,9 +341,9 @@ class WheelLegInfantry 0x200, std::bit_cast(control_commands)); transmit_buffer_.add_can2_transmission( - 0x03, chassis_hip_motors[2].generate_angle_command()); + 0x03, chassis_hip_motors[2].generate_disable_command()); transmit_buffer_.add_can2_transmission( - 0x04, chassis_hip_motors[3].generate_angle_command()); + 0x04, chassis_hip_motors[3].generate_disable_command()); transmit_buffer_.trigger_transmission(); } 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 index bc735283..1f278199 100644 --- 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 @@ -4,11 +4,10 @@ namespace rmcs_msgs { enum class WheelLegMode : uint8_t { - STOP = 0, + BALANCELESS = 0, SPIN = 1, FOLLOW = 2, LAUNCH_RAMP = 3, - BALANCELESS = 4, + RESCUE_TIP_OVER = 4, }; - } From bc87d1dc52f23fbc3fc6c2651b01803b62fbbac4 Mon Sep 17 00:00:00 2001 From: miNu50Ne Date: Tue, 17 Mar 2026 00:50:48 +0800 Subject: [PATCH 35/35] Update special wheelleg mode controller. --- rmcs_ws/src/rmcs_core/librmcs | 2 +- .../controller/chassis/wheel_leg/README.md | 19 +- .../chassis/wheel_leg/desire_state_solver.hpp | 2 +- .../wheel_leg_chassis_controller.cpp | 5 +- .../wheel_leg/wheel_leg_controller.cpp | 202 ++++++++++-------- .../src/hardware/wheelleg-infantry.cpp | 12 +- .../include/rmcs_msgs/wheel_leg_mode.hpp | 1 - 7 files changed, 140 insertions(+), 103 deletions(-) diff --git a/rmcs_ws/src/rmcs_core/librmcs b/rmcs_ws/src/rmcs_core/librmcs index 6f79757c..1347b210 160000 --- a/rmcs_ws/src/rmcs_core/librmcs +++ b/rmcs_ws/src/rmcs_core/librmcs @@ -1 +1 @@ -Subproject commit 6f79757c8c3b0a0668fde40f28db2e2ba7bb2e93 +Subproject commit 1347b2109d0c93dbc3cfe5daaa6acd7754987f81 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 index 5557cb38..c301b00d 100644 --- 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 @@ -1,9 +1,22 @@ -# desire state +#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] -## Wheel leg mode: +## reset +启动时收腿至最短后再启动lqr计算。 + +## Wheel-leg Mode balanceless: 锁死关节，仅控轮子移动 follow： yaw轴角度、角速度 spin： yaw轴角速度 -rescue: 关闭lqr，仅关节 + +### 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 index c7f685fe..5abba902 100644 --- 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 @@ -137,7 +137,7 @@ class DesireStateSolver { desire_state_.distance = 0.0; desire_state_.velocity = vx; - desire_state_.yaw_angle = 0.0; // or + desire_state_.yaw_angle = 0.0; // desire_state_.yaw_velocity = wz; desire_state_.left_tilt_angle = 0.0; 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 index 1ed71031..e5432c39 100644 --- 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 @@ -117,7 +117,7 @@ class WheelLegChassisController climb_active_ = !last_keyboard_.q && keyboard.q; // Rescue tip-over - rescue_tip_over_ = !last_keyboard_.r && keyboard.r; + rescue_tip_over_ = !last_keyboard_.g && keyboard.g; *is_balanceless_ = mode == rmcs_msgs::WheelLegMode::BALANCELESS; *is_leg_extended_ = leg_extended_; @@ -177,7 +177,6 @@ class WheelLegChassisController switch (*mode_) { case rmcs_msgs::WheelLegMode::BALANCELESS: - case rmcs_msgs::WheelLegMode::RESCUE_TIP_OVER: break; case rmcs_msgs::WheelLegMode::SPIN: { angular_velocity = 0.6 * (spinning_forward_ ? angular_velocity_max : -angular_velocity_max); @@ -230,7 +229,7 @@ class WheelLegChassisController // Maximum control velocities static constexpr double translational_velocity_max = 2.0; - static constexpr double angular_velocity_max = 16.0; + static constexpr double angular_velocity_max = 2.0; InputInterface joystick_right_; InputInterface joystick_left_; 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 index f70bbba4..725328e4 100644 --- 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 @@ -42,9 +42,10 @@ class WheelLegController , 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_(350.0, 0.0, 0.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) { + , 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_); @@ -66,6 +67,11 @@ class WheelLegController 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_); @@ -132,29 +138,26 @@ class WheelLegController 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_hip_zero_point_control_angles(); - calculate_balanceless_control_torques(chassis_control_velocity, wheel_velocities); - return; - } + // if (*is_balanceless_) { + // update_balanceless_control_torques(chassis_control_velocity, wheel_velocities); + // return; + // } - if (*is_rescue_tip_over_) { - update_hip_rescue_tip_over_control_torques(); - 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_zero_point_control_angles(); 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; - - // RCLCPP_INFO( - // get_logger(), "%f, %f, %f, %f", *left_front_hip_control_torque_, - // *left_back_hip_control_torque_, *right_back_hip_control_torque_, - // *right_front_hip_control_torque_); + *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; @@ -219,6 +222,8 @@ class WheelLegController 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_; @@ -284,7 +289,6 @@ class WheelLegController auto& [distance, velocity] = result; auto wheel_velocity = (wheel_velocities.x() + wheel_velocities.y()) / 2.0; - leg_posture.tilt_angle.array() += *chassis_pitch_angle_imu_; auto left_leg_velocity = leg_posture.leg_length.x() * std::cos(leg_posture.tilt_angle.x()) @@ -326,12 +330,6 @@ class WheelLegController calculate_measure_state(Eigen::Vector2d distance, LegPosture leg_posture) { rmcs_msgs::WheelLegState measure_state; - // filtered_gyro_velocity_ = chassis_gyro_velocity_filter_.update( - // Eigen::Vector3d{ - // *chassis_roll_velocity_imu_, *chassis_pitch_velocity_imu_, - // *chassis_yaw_velocity_imu_}); - leg_posture.tilt_angle.array() += *chassis_pitch_angle_imu_; - measure_state.distance = distance.x(); measure_state.velocity = distance.y(); @@ -347,8 +345,8 @@ class WheelLegController 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.distance = 0.0; + measure_state.velocity = 0.0; // measure_state.yaw_angle = 0.0; // measure_state.yaw_velocity = 0.0; @@ -452,10 +450,12 @@ class WheelLegController 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_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0); + 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; @@ -463,8 +463,8 @@ class WheelLegController auto gravity_feedforward_control_force = calculate_compensation_feedforward_force(g_); auto inertial_feedforward_control_force = calculate_compensation_feedforward_force( - (leg_posture.leg_length.x() + leg_posture.leg_length.y()) / 2.0 / (2 * wheel_distance_) - * measure_state.yaw_velocity * measure_state.velocity); + leg_length / (2 * wheel_distance_) * measure_state.yaw_velocity + * measure_state.velocity); // F_ψ // F_bl_l = 1 1 1 -1 * F_l @@ -489,56 +489,34 @@ class WheelLegController auto gain = lqr_calculator_.update(leg_posture.leg_length.x(), leg_posture.leg_length.y()); if (levitate_active_) { - // When levitating, only keep your legs vertical,and only remain legs' outputs. + // When levitating, only keep legs vertical,and only remain legs' outputs. // set zero - error_state(0, 0) = 0.0; // distance - error_state(1, 0) = 0.0; // velocity - error_state(2, 0) = 0.0; // yaw angle - error_state(3, 0) = 0.0; // yaw velocity + 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.0; // body pitch angle - error_state(9, 0) = 0.0; // body pitch velocity + error_state(8) = 0.0; // body pitch angle + error_state(9) = 0.0; // body pitch velocity } result = -1.0 * gain * error_state; - auto err_0 = error_state(0); - auto err_1 = error_state(1); - auto err_2 = error_state(2); - auto err_3 = error_state(3); - auto err_4 = error_state(4); - auto err_5 = error_state(5); - auto err_6 = error_state(6); - auto err_7 = error_state(7); - auto err_8 = error_state(8); - auto err_9 = error_state(9); - - auto gain_00 = gain(0, 0); - auto gain_01 = gain(0, 1); - auto gain_02 = gain(0, 2); - auto gain_03 = gain(0, 3); - auto gain_04 = gain(0, 4); - auto gain_05 = gain(0, 5); - auto gain_06 = gain(0, 6); - auto gain_07 = gain(0, 7); - auto gain_08 = gain(0, 8); - auto gain_09 = gain(0, 9); - - auto output_0 = -gain_00 * err_0; - auto output_1 = -gain_01 * err_1; - auto output_2 = -gain_02 * err_2; - auto output_3 = -gain_03 * err_3; - auto output_4 = -gain_04 * err_4; - auto output_5 = -gain_05 * err_5; - auto output_6 = -gain_06 * err_6; - auto output_7 = -gain_07 * err_7; - auto output_8 = -gain_08 * err_8; - auto output_9 = -gain_09 * err_9; + 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(), "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]", @@ -548,14 +526,10 @@ class WheelLegController // measure_state.right_tilt_velocity, measure_state.body_pitch_angle, // measure_state.body_pitch_velocity); - // RCLCPP_INFO( - // get_logger(), "x: %f, err: %f, output: %f, lw torque: %f, rw torque: %f", - // measure_state.distance, error_state(0), output_0, result.x(), result.y()); - return result; } - void calculate_balanceless_control_torques( + 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; @@ -579,7 +553,46 @@ class WheelLegController reset_control_torques(); } - void update_hip_rescue_tip_over_control_torques() { reset_control_angles(); } + 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; @@ -591,6 +604,11 @@ class WheelLegController 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; @@ -599,10 +617,15 @@ class WheelLegController *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_back_hip_control_torque_ = clamp_hip_control_torque(right_hip_control_torque.y()); + *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(); } @@ -612,7 +635,7 @@ class WheelLegController } static double clamp_hip_control_torque(const double& torque) { - return std::clamp(torque, -15., 15.); + return std::clamp(torque, -10., 10.); } void reset_control_torques() { @@ -669,6 +692,11 @@ class WheelLegController 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_; @@ -716,8 +744,9 @@ class WheelLegController double desire_leg_length_, desire_roll_angle_; bool levitate_active_{false}, left_levitate_active_{false}, right_levitate_active_{false}; - bool about_to_fall_{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_; @@ -734,6 +763,7 @@ class WheelLegController 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 diff --git a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp index c08487f1..86eafda2 100644 --- a/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp +++ b/rmcs_ws/src/rmcs_core/src/hardware/wheelleg-infantry.cpp @@ -330,20 +330,16 @@ class WheelLegInfantry transmit_buffer_.add_can1_transmission( 0x200, std::bit_cast(control_commands)); - transmit_buffer_.add_can1_transmission( - 0x01, chassis_hip_motors[0].generate_disable_command()); - transmit_buffer_.add_can1_transmission( - 0x02, chassis_hip_motors[1].generate_disable_command()); + 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_disable_command()); - transmit_buffer_.add_can2_transmission( - 0x04, chassis_hip_motors[3].generate_disable_command()); + 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(); } 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 index 1f278199..c5b9cfdb 100644 --- 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 @@ -8,6 +8,5 @@ enum class WheelLegMode : uint8_t { SPIN = 1, FOLLOW = 2, LAUNCH_RAMP = 3, - RESCUE_TIP_OVER = 4, }; }