update AuboRobot

.gitignore

@@ -0,0 +1,7 @@
+/.idea
+/cmake-build-debug
+/cmake-build-sigma-debuggit
+/build
+/log
+/third_party/osqp/
+/third_party/OsqpEigen/

.gitmodules

@@ -0,0 +1,9 @@
+[submodule "third_party/grpc"]
+	path = third_party/grpc
+	url = https://github.com/grpc/grpc.git
+[submodule "third_party/osqp"]
+	path = third_party/osqp
+	url = https://github.com/osqp/osqp.git
+[submodule "third_party/OsqpEigen"]
+	path = third_party/osqp-eigen
+	url = https://github.com/robotology/osqp-eigen.git

CMakeLists.txt

@@ -0,0 +1,98 @@
+cmake_minimum_required(VERSION 3.22.0)
+
+project(cmvr_es)
+
+# 设置C++标准
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
+set(CMAKE_CXX_FLAGS -pthread)
+set(CMAKE_CXX_STANDARD_REQUIRED True)
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+
+# 查找依赖
+list(APPEND CMAKE_PREFIX_PATH "$ENV{HOME}./local")
+
+###########################################################
+## DEPENDENCIES
+#############################################################
+find_package(jsoncpp REQUIRED)
+find_package(protobuf REQUIRED)
+find_package(gRPC REQUIRED)
+find_package(PkgConfig REQUIRED)
+pkg_check_modules(GLOG REQUIRED libglog)
+
+############################################################
+# PROTO
+############################################################
+
+set(PROTO_IMPORT_DIR ${PROJECT_SOURCE_DIR}/protos)
+set(PROTO_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/_protobuf)
+file(MAKE_DIRECTORY ${PROTO_BINARY_DIR})
+file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto)
+add_library(proto-objects OBJECT ${PROTO_FILES})
+message(STATUS "proto files will be loaded in ${PROTO_IMPORT_DIR}")
+message(STATUS "proto files will be generated in ${PROTO_BINARY_DIR}")
+
+protobuf_generate(
+        TARGET proto-objects
+        IMPORT_DIRS ${PROTO_IMPORT_DIR}
+        PROTOC_OUT_DIR ${PROTO_BINARY_DIR}
+        USAGE_REQUIREMENT INTERFACE
+)
+protobuf_generate(
+        TARGET proto-objects
+        LANGUAGE grpc
+        GENERATE_EXTENSIONS .grpc.pb.h .grpc.pb.cc
+        PLUGIN "protoc-gen-grpc=$ENV{HOME}/.local/bin/grpc_cpp_plugin"
+        USAGE_REQUIREMENT INTERFACE
+        IMPORT_DIRS ${PROTO_IMPORT_DIR}
+        PROTOC_OUT_DIR ${PROTO_BINARY_DIR}
+)
+target_include_directories(proto-objects PUBLIC ${PROTO_BINARY_DIR} ${PROTO_IMPORT_DIR})
+target_link_libraries(proto-objects PUBLIC protobuf::libprotobuf gRPC::grpc++ gRPC::grpc++_reflection)
+
+############################################################
+# include
+############################################################
+include_directories(
+        /usr/include/opencv4
+        /usr/include/eigen3
+        /usr/local/include/osqp
+        /usr/local/share/osqp/codegen_files/inc/private/
+        $ENV{HOME}/.local/include
+        /usr/local/include/
+        ${PROTO_BINARY_DIR}
+        ${PROJECT_SOURCE_DIR}/include
+        ${PROJECT_SOURCE_DIR}/src/devices
+        ${PROJECT_SOURCE_DIR}/third_party
+)
+
+############################################################
+# subdirectory
+############################################################
+
+add_subdirectory(src)
+add_subdirectory(example)
+#add_subdirectory(test)
+
+############################################################
+# executables
+############################################################
+
+add_executable(cmvr_es src/main.cpp)
+target_include_directories(cmvr_es PRIVATE ${GLOG_INCLUDE_DIRS})
+target_link_libraries(cmvr_es PRIVATE
+        proto-objects
+        service
+        ${GLOG_LIBRARIES}
+        gflags
+        jsoncpp_lib
+        cmvr_es::utils
+        cmvr_es::service
+        cmvr_es::monitor
+        cmvr_es::hardware_manager
+        cmvr_es::httpclient
+        cmvr_es::device::canbus
+        cmvr_es::device::ti5motor
+        #cmvr_es::ctrl::controller
+)

LICENSE

@@ -0,0 +1,21 @@
+The MIT License (MIT)
+
+Copyright (c) 2017 FullStory, Inc
+
+Permission is hereby granted, free of charge, to any person obtaining a copy
+of this software and associated documentation files (the "Software"), to deal
+in the Software without restriction, including without limitation the rights
+to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+copies of the Software, and to permit persons to whom the Software is
+furnished to do so, subject to the following conditions:
+
+The above copyright notice and this permission notice shall be included in all
+copies or substantial portions of the Software.
+
+THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
+AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
+SOFTWARE.

README.md

@@ -1,92 +1,152 @@
-# cmvr-es
+# CMVR-ES
 
-
+## Overview
-
-## Getting started
-
-To make it easy for you to get started with GitLab, here's a list of recommended next steps.
-
-Already a pro? Just edit this README.md and make it your own. Want to make it easy? [Use the template at the bottom](#editing-this-readme)!
-
-## Add your files
-
-- [ ] [Create](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#create-a-file) or [upload](https://docs.gitlab.com/ee/user/project/repository/web_editor.html#upload-a-file) files
-- [ ] [Add files using the command line](https://docs.gitlab.com/ee/gitlab-basics/add-file.html#add-a-file-using-the-command-line) or push an existing Git repository with the following command:
-
-```
-cd existing_repo
-git remote add origin http://192.168.1.100:18088/smart_bench/cmvr-es.git
-git branch -M main
-git push -uf origin main
-```
-
-## Integrate with your tools
-
-- [ ] [Set up project integrations](http://192.168.1.100:18088/smart_bench/cmvr-es/-/settings/integrations)
-
-## Collaborate with your team
-
-- [ ] [Invite team members and collaborators](https://docs.gitlab.com/ee/user/project/members/)
-- [ ] [Create a new merge request](https://docs.gitlab.com/ee/user/project/merge_requests/creating_merge_requests.html)
-- [ ] [Automatically close issues from merge requests](https://docs.gitlab.com/ee/user/project/issues/managing_issues.html#closing-issues-automatically)
-- [ ] [Enable merge request approvals](https://docs.gitlab.com/ee/user/project/merge_requests/approvals/)
-- [ ] [Set auto-merge](https://docs.gitlab.com/ee/user/project/merge_requests/merge_when_pipeline_succeeds.html)
-
-## Test and Deploy
-
-Use the built-in continuous integration in GitLab.
-
-- [ ] [Get started with GitLab CI/CD](https://docs.gitlab.com/ee/ci/quick_start/index.html)
-- [ ] [Analyze your code for known vulnerabilities with Static Application Security Testing(SAST)](https://docs.gitlab.com/ee/user/application_security/sast/)
-- [ ] [Deploy to Kubernetes, Amazon EC2, or Amazon ECS using Auto Deploy](https://docs.gitlab.com/ee/topics/autodevops/requirements.html)
-- [ ] [Use pull-based deployments for improved Kubernetes management](https://docs.gitlab.com/ee/user/clusters/agent/)
-- [ ] [Set up protected environments](https://docs.gitlab.com/ee/ci/environments/protected_environments.html)
-
-***
-
-# Editing this README
-
-When you're ready to make this README your own, just edit this file and use the handy template below (or feel free to structure it however you want - this is just a starting point!). Thank you to [makeareadme.com](https://www.makeareadme.com/) for this template.
-
-## Suggestions for a good README
-Every project is different, so consider which of these sections apply to yours. The sections used in the template are suggestions for most open source projects. Also keep in mind that while a README can be too long and detailed, too long is better than too short. If you think your README is too long, consider utilizing another form of documentation rather than cutting out information.
-
-## Name
-Choose a self-explaining name for your project.
-
-## Description
-Let people know what your project can do specifically. Provide context and add a link to any reference visitors might be unfamiliar with. A list of Features or a Background subsection can also be added here. If there are alternatives to your project, this is a good place to list differentiating factors.
-
-## Badges
-On some READMEs, you may see small images that convey metadata, such as whether or not all the tests are passing for the project. You can use Shields to add some to your README. Many services also have instructions for adding a badge.
-
-## Visuals
-Depending on what you are making, it can be a good idea to include screenshots or even a video (you'll frequently see GIFs rather than actual videos). Tools like ttygif can help, but check out Asciinema for a more sophisticated method.
 
 ## Installation
-Within a particular ecosystem, there may be a common way of installing things, such as using Yarn, NuGet, or Homebrew. However, consider the possibility that whoever is reading your README is a novice and would like more guidance. Listing specific steps helps remove ambiguity and gets people to using your project as quickly as possible. If it only runs in a specific context like a particular programming language version or operating system or has dependencies that have to be installed manually, also add a Requirements subsection.
 
-## Usage
+### 1. Git submodules install 
-Use examples liberally, and show the expected output if you can. It's helpful to have inline the smallest example of usage that you can demonstrate, while providing links to more sophisticated examples if they are too long to reasonably include in the README.
 
-## Support
+```
-Tell people where they can go to for help. It can be any combination of an issue tracker, a chat room, an email address, etc.
+git submodule update --init --recursive
+```
 
-## Roadmap
+### 2. Dependency install
-If you have ideas for releases in the future, it is a good idea to list them in the README.
 
-## Contributing
+```shell
-State if you are open to contributions and what your requirements are for accepting them.
+# basic
+sudo apt-get install build-essential -y
+sudo apt-get install liblttng-ust-dev -y
+sudo apt-get install lttng-tools -y
+sudo apt install libboost-all-dev libssl-dev -y
 
-For people who want to make changes to your project, it's helpful to have some documentation on how to get started. Perhaps there is a script that they should run or some environment variables that they need to set. Make these steps explicit. These instructions could also be useful to your future self.
+# json
+sudo apt install libjsoncpp-dev -y
 
-You can also document commands to lint the code or run tests. These steps help to ensure high code quality and reduce the likelihood that the changes inadvertently break something. Having instructions for running tests is especially helpful if it requires external setup, such as starting a Selenium server for testing in a browser.
+# google
+sudo apt install libgoogle-glog-dev -y
 
-## Authors and acknowledgment
+# modbus
-Show your appreciation to those who have contributed to the project.
+sudo apt install libmodbus-dev -y
 
-## License
+# vision
-For open source projects, say how it is licensed.
+sudo apt install libeigen3-dev -y
+sudo apt install libopencv-dev python3-opencv -y
 
-## Project status
+# fcl
-If you have run out of energy or time for your project, put a note at the top of the README saying that development has slowed down or stopped completely. Someone may choose to fork your project or volunteer to step in as a maintainer or owner, allowing your project to keep going. You can also make an explicit request for maintainers.
+sudo apt install -y libfcl-dev
+
+# flann
+sudo apt-get install libflann-dev -y
+
+# ffmpeg (deprecated)
+#sudo apt install ffmpeg
+
+#alas
+sudo apt-get install libasound2-dev -y
+
+# gstreamer
+sudo apt install gstreamer1.0-plugins-good gstreamer1.0-plugins-bad -y
+sudo apt install gstreamer1.0-plugins-ugly gstreamer1.0-libav -y
+
+# tinyxml2
+sudo apt install libtinyxml2-dev -y
+```
+
+### 3. gRPC install (build from source)
+version: https://grpc.io/docs/languages/cpp/quickstart/（grpc官网地址 v1.73.0）
+#### 安装完成之后要配置环境变量
+vim ~/.bashrc
+在末尾添加
+
+### 4. ffmpeg install (build from source)
+#### 4.1 install dependencies
+```shell
+sudo apt-get update
+sudo apt install build-essential yasm nasm git -y
+sudo apt install libx264-dev libx265-dev libvpx-dev libfdk-aac-dev libmp3lame-dev libopus-dev -y
+```
+
+#### 4.2 build from source
+```shell
+cd assets
+tar xjvf ffmpeg-4.4.tar.bz2
+cd ffmpeg-4.4
+
+# set configurations
+./configure \
+    --extra-libs="-lpthread -lm" \
+    --enable-gpl \
+    --enable-libass \
+    --enable-libfdk-aac \
+    --enable-libfreetype \
+    --enable-libmp3lame \
+    --enable-libopus \
+    --enable-libvorbis \
+    --enable-libvpx \
+    --enable-libx264 \
+    --enable-libx265 \
+    --enable-pic \
+    --enable-shared \
+    --enable-nonfree \
+
+# build and install
+make -j4
+sudo make install
+```
+
+
+### 5. librealsense2 2.55.1 install
+```shell
+sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
+sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
+sudo apt-get install librealsense2-dkms=1.3.24-0ubuntu1 -y
+sudo apt-get install \
+librealsense2=2.55.1-0~realsense.12473 \
+librealsense2-utils=2.55.1-0~realsense.12473 \
+librealsense2-dev=2.55.1-0~realsense.12473 \
+librealsense2-dbg=2.55.1-0~realsense.12473 -y
+```
+
+### 5. OSQPEigen install(build from source)
+#### 5.1 install osqp (v0.6.3)
+```shell
+git clone --branch v0.6.3 https://github.com/osqp/osqp.git
+cd osqp
+git submodule init
+git submodule update
+mkdir build
+cd build
+cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..
+make -j
+sudo make install
+```
+#### 5.2 install OsqpEigen (v0.10.1)
+```shell
+git clone --branch v0.10.1 https://github.com/robotology/osqp-eigen.git
+cd osqp-eigen
+git submodule init
+git submodule update
+mkdir build
+cd build
+cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..
+make -j
+sudo make install
+```
+
+#### 6 mech-eye-sdk
+```shell
+cd assets
+sudo dkpg -i Mech-Eye_API_2.5.1_amd64.deb
+```
+
+### 串口冲突问题
+```shell
+sudo systemctl stop brltty
+sudo systemctl disable brltty
+sudo apt remove brltty
+```
+
+### 开放端口
+```shell
+sudo iptables -A INPUT -p tcp --dport 50052 -j ACCEPT
+```

config/cabin_robot.xml

@@ -0,0 +1,144 @@
+<CMVR-ES>
+    <Constants rootDir="/home/xtkuang/projects/cmvr-es"/>
+    <Logger dir="../log" level="info" bufSize="5" logSize="1024"/>
+
+    <DeviceManager name="cmvr_es" ver="0.1" description="cmvr edge system version 0.1">
+        <Devices>
+            <AGV>
+            </AGV>
+
+            <Battery>
+            </Battery>
+
+            <Camera>
+<!--                <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
+<!--                <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
+<!--                <RealsenseCamera id="cam3" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
+<!--                <RealsenseCamera id="cam4" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
+<!--                <MechMind id="cam5" ip="10.148.108.111" align="true" _2dtype="color"/>-->
+<!--                <RealsenseCamera id="cam6" serial="243122075389" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
+            </Camera>
+
+            <DexHand>
+<!--                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000">-->
+<!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
+<!--                </RH56DFTP>-->
+<!--                <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="192.168.1.224" port="6000">-->
+<!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
+<!--                </RH56DFTP>-->
+            </DexHand>
+
+            <Robot>
+<!--                <LeftArm id="left_arm" devtype="ti5Robot"  />-->
+<!--                <RightArm />-->
+<!--                <Neck/>-->
+                <Humanoid id="hc01" dof="14"
+                          urdf="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
+                          baseLink="PELVIS_S"
+                          jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
+                          linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
+                          bufferSize="50"
+                          verbose="false">
+                    <CanManger id="" devId="">
+                        <LeftArmCan id = " " devId = " " channelId ="0" enable="true" toolFrame="L_FINGER_TIP">
+                            <Motor id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        </LeftArmCan>
+                        <RightArmCan id = " " devId = " " channelId ="1"  enable="true" toolFrame="R_FINGER_TIP">
+                            <Motor id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
+                            <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
+                        </RightArmCan>
+                        <HeadCan id = " " devId = " " channelId ="2" enable="false">
+                            <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        </HeadCan>
+                        <WaistCan id = " " devId = " " channelId ="3" enable="false">
+                            <Motor id="14" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                            <Motor id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        </WaistCan>
+                    </CanManger>
+
+                </Humanoid>
+            </Robot>
+
+            <BioHead>
+<!--                <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">-->
+<!--                    &lt;!&ndash; 配置左眉毛，舵机通道 0~3 &ndash;&gt;-->
+<!--                    <EyeBrow serial="64:0~3" offest="90 90 90 90"-->
+<!--                             jLmtUp="170 170 170 170" jLmtLow="10 10 10 10"/>-->
+
+<!--                    &lt;!&ndash; 配置眼睛，舵机通道 4~9 &ndash;&gt;-->
+<!--                    <Eye serial="64:4~9" offest="90 90 90 90 90 90"-->
+<!--                         jLmtUp="170 170 170 170 170 170" jLmtLow="10 10 10 10 10 10"/>-->
+
+<!--                    &lt;!&ndash; 配置嘴巴，舵机通道 0~8 &ndash;&gt;-->
+<!--                    <Mouth serial="65:0~8" offest="90 90 90 90 90 90 90 90 90"-->
+<!--                           jLmtUp="170 170 170 170 170 170 170 170 170" jLmtLow="10 10 10 10 10 10 10 10 10"/>-->
+<!--                </esp32>-->
+            </BioHead >
+
+            <Microphone>
+<!--                <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
+                <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>
+            </Microphone>
+
+            <Speaker>
+                <ffmpegSpeaker id="spk1" serial="" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="80"/>
+            </Speaker>
+
+            <Canbus>
+<!--                <rightArmCan id="can1"  brand="SOCKET_CAN_RAW"  type="USB_CARD" channel_id="CHANNEL_ID_ZERO" interface="NATIVE" baudrate="BCAN_BAUDRATE_500K"/>-->
+            </Canbus>
+
+        </Devices>
+
+        <HighLevelController>
+            <BioHeadExpre headId="bio_head" />
+            <CartesianWBC urdf="" />
+            <ScreenTouch robotID="" DexhandID="" />
+        </HighLevelController>
+    </DeviceManager>
+
+    <MonitorManager>
+        <DiskMonitor id="file_monitor" freq="1">
+<!--            <Folder fileDir="/home/share/assets/audio" maxVolume="1000"/>-->
+<!--            <Folder fileDir="/home/share/assets/image" maxVolume="1000"/>-->
+<!--            <Folder fileDir="/home/share/assets/video" maxVolume="1000"/>-->
+<!--            <Folder fileDir="../log" maxVolume="1000"/>-->
+        </DiskMonitor>
+
+        <JointMonitor id="robot_joint_monitor" freq="200">
+            <RobotJoint robotID="left_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
+        </JointMonitor>
+
+    </MonitorManager>
+
+    <gRPCServer port="50052">
+
+    </gRPCServer>
+
+</CMVR-ES>

config/dev_config.xml

@@ -0,0 +1,145 @@
+    <CMVR-ES>
+    <Constants rootDir="/home/xtkuang/projects/cmvr-es"/>
+    <Logger dir="../log" level="info" bufSize="5" logSize="1024"/>
+
+    <HardwareManager>
+        <CanManager>
+            <CanGroup id="LeftArmCAN" channelId="0" enable="true">
+                <MotorProtocolManager>
+                    <Ti5MotorProtocol id="23" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="24" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="25" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="26" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="27" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="28" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="1" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                </MotorProtocolManager>
+            </CanGroup>
+            <CanGroup id="RightArmCAN" channelId="1" enable="true">
+                <MotorProtocolManager>
+                    <Ti5MotorProtocol id="16" jointName="R_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="17" jointName="R_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="18" jointName="R_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="19" jointName="R_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="20" jointName="R_WRIST_P" limitQLb="-3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
+                </MotorProtocolManager>
+            </CanGroup>
+            <CanGroup id="NeckCAN" channelId ="2" enable="true">
+                <MotorProtocolManager>
+                    <Ti5MotorProtocol id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                </MotorProtocolManager>
+            </CanGroup>
+            <CanGroup id = "WaistCAN" channelId ="3" enable="false">
+                <MotorProtocolManager>
+                    <Ti5MotorProtocol id="14" jointName="WAIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    <Ti5MotorProtocol id="15" jointName="WAIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                </MotorProtocolManager>
+            </CanGroup>
+        </CanManager>
+
+        <SerialManager>
+            <RS485Manager>
+            </RS485Manager>
+            <IOManager>
+            </IOManager>
+        </SerialManager>
+    </HardwareManager>
+
+    <DeviceManager>
+        <Devices>
+            <AGV>
+            </AGV>
+            <Battery>
+            </Battery>
+
+            <Camera>
+<!--                <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
+<!--                <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
+<!--                <RealsenseCamera id="left_hand_cam" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
+<!--                <RealsenseCamera id="right_hand_cam" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
+            </Camera>
+
+            <DexHand>
+<!--                <RH56DFTP id="left_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>-->
+<!--                <RH56DFTP id="right_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>-->
+            </DexHand>
+
+            <Robot >
+                <HumanoidRobot id="hc01" urdfPath="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
+                               jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
+                               linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM">
+                    <ControllerManager>
+                        <ComponentGroup id="leftArm" CanGroupID="LeftArmCAN">
+                            <JointPositionCtrl defaultSpeed="" defaultAcc=""/>
+                            <CartesianController baseLine="" eeLink="" defaultSpeed="" defaultAcc=""/>
+                        </ComponentGroup>
+                        <ComponentGroup id="rightArm" CanGroupID="RightArmCAN">
+                            <JointPositionCtrl/>
+                        </ComponentGroup>
+                        <ComponentGroup id="WholeBody" CanGroupID="LeftArmCAN,RightArmCAN,NeckCAN,WaistCAN">
+                            <JointPositionCtrl/>
+                        </ComponentGroup>
+                    </ControllerManager>
+                </HumanoidRobot>
+                <AuboRobot id="hc02" ip="127.0.0.1" usrname="aubo" password="123456">
+
+                </AuboRobot>
+            </Robot>
+
+            <BioHead>
+
+            </BioHead >
+
+            <Microphone>
+<!--                <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
+<!--                <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>-->
+            </Microphone>
+
+            <Speaker>
+<!--                <ffmpegSpeaker id="spk1" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="80"/>-->
+            </Speaker>
+
+        </Devices>
+
+        <HighLevelController>
+            <BioHeadExpre headId="bio_head" />
+            <CartesianWBC urdf="" />
+            <ScreenTouch robotID="" DexhandID="" />
+        </HighLevelController>
+    </DeviceManager>
+
+    <MonitorManager>
+        <DiskMonitor id="file_monitor" freq="1">
+            <!--            <Folder fileDir="/home/share/assets/audio" maxVolume="1000"/>-->
+            <!--            <Folder fileDir="/home/share/assets/image" maxVolume="1000"/>-->
+            <!--            <Folder fileDir="/home/share/assets/video" maxVolume="1000"/>-->
+            <!--            <Folder fileDir="../log" maxVolume="1000"/>-->
+        </DiskMonitor>
+
+        <JointMonitor id="robot_joint_monitor" freq="200">
+            <RobotJoint robotID="left_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
+        </JointMonitor>
+    </MonitorManager>
+
+    <Server>
+        <gRPCServer port="50052"/>
+    </Server>
+
+</CMVR-ES>

config/dev_config_v2.xml

@@ -0,0 +1,140 @@
+    <CMVR-ES>
+    <Constants rootDir="/home/xtkuang/projects/cmvr-es"/>
+    <Logger dir="../log" level="info" bufSize="5" logSize="1024"/>
+
+    <HardwareManager>
+        <CanManager>
+            <CanGroup id="LeftArmCAN" channelId="0" enable="true">
+                <MotorProtocolManager>
+                    <Ti5MotorProtocol/>
+                </MotorProtocolManager>
+            </CanGroup>
+            <CanGroup id="RightArmCAN" channelId="1" enable="true">
+                <MotorProtocolManager>
+                    <Ti5MotorProtocol/>
+                </MotorProtocolManager>
+            </CanGroup>
+            <CanGroup id="NeckCAN" channelId ="2" enable="true">
+                <MotorProtocolManager>
+                    <Ti5MotorProtocol/>
+                </MotorProtocolManager>
+            </CanGroup>
+            <CanGroup id = "WaistCAN" channelId ="3" enable="false">
+                <MotorProtocolManager>
+                    <Ti5MotorProtocol/>
+                </MotorProtocolManager>
+            </CanGroup>
+        </CanManager>
+
+        <SerialManager>
+            <RS485Manager>
+            </RS485Manager>
+            <IOManager>
+            </IOManager>
+        </SerialManager>
+    </HardwareManager>
+
+    <DeviceManager>
+        <Devices>
+            <AGV>
+            </AGV>
+            <Battery>
+            </Battery>
+
+            <Camera>
+<!--                <UVCCamera id="cam1" serial="/dev/video6" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
+<!--                <UVCCamera id="cam2" serial="/dev/video14" w="640" h="480" fps="30" mode="video" codec="H265"/>-->
+<!--                <RealsenseCamera id="left_hand_cam" serial="243122072252" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
+<!--                <RealsenseCamera id="right_hand_cam" serial="243122075614" w="1280" h="720" fps="30" mode="video" stream_mode="rgbd" align_mode="color" codec="H265"/>-->
+            </Camera>
+
+            <DexHand>
+<!--                <RH56DFTP id="left_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>-->
+<!--                <RH56DFTP id="right_hand" default_force="500" default_speed="500" ip_address="192.168.1.223" port="6000"/>-->
+            </DexHand>
+
+            <Robot >
+                <HumanoidRobot id="hc01" urdfPath="/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf"
+                               jointNames="L_SHOULDER_P,L_SHOULDER_R,L_SHOULDER_Y,L_ELBOW_R,L_WRIST_P,L_WRIST_Y,L_WRIST_R,R_SHOULDER_P,R_SHOULDER_R,R_SHOULDER_Y,R_ELBOW_R,R_WRIST_P,R_WRIST_Y,R_WRIST_R"
+                               linkNames="PELVIS_S,L_SHOULDER_P_S,L_SHOULDER_R_S,L_SHOULDER_Y_S,L_ELBOW_R_S,L_WRIST_P_S,L_WRIST_Y_S,L_WRIST_R_S,R_SHOULDER_P_S,R_SHOULDER_R_S,R_SHOULDER_Y_S,R_ELBOW_R_S,R_WRIST_P_S,R_WRIST_Y_S,R_WRIST_R_S,R_FINGER_TIP,R_CAM"
+                               baseLink="PELVIS_S">
+                    <MotorManager>
+                        <Motor id="23" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_SHOULDER_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        <Motor id="24" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_SHOULDER_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        <Motor id="25" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_SHOULDER_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        <Motor id="26" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_ELBOW_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        <Motor id="27" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_WRIST_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        <Motor id="28" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_WRIST_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                        <Motor id="1" canGroupId="LeftArmCAN" protocolType="Ti5MotorProtocol" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
+                    </MotorManager>
+                    <ControllerManager>
+                        <ComponentGroup id="leftArm" CanGroupID="LeftArmCAN">
+                            <JointPositionCtrl defaultSpeed="" defaultAcc=""/>
+                            <CartesianController baseLine="" eeLink="" defaultSpeed="" defaultAcc=""/>
+                        </ComponentGroup>
+                        <ComponentGroup id="rightArm" CanGroupID="RightArmCAN">
+                            <JointPositionCtrl/>
+                        </ComponentGroup>
+                        <ComponentGroup id="WholeBody" CanGroupID="LeftArmCAN,RightArmCAN,NeckCAN,WaistCAN">
+                            <JointPositionCtrl/>
+                        </ComponentGroup>
+                    </ControllerManager>
+                </HumanoidRobot>
+<!--                <AuboRobot id="hc02" ip="127.0.0.1" usrname="aubo" password="123456">-->
+
+<!--                </AuboRobot>-->
+            </Robot>
+
+            <BioHead>
+
+            </BioHead >
+
+            <Microphone>
+<!--                <ffmpegMicPhone id="mic1" alsa="hw:0" channels="2" sampleRate="44100" volume="80"/>-->
+<!--                <ffmpegMicPhone id="mic2" alsa="hw:1" channels="1" sampleRate="44100" volume="80"/>-->
+            </Microphone>
+
+            <Speaker>
+<!--                <ffmpegSpeaker id="spk1" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="50000" volume="80"/>-->
+            </Speaker>
+
+        </Devices>
+
+        <HighLevelController>
+            <BioHeadExpre headId="bio_head" />
+            <CartesianWBC urdf="" />
+            <ScreenTouch robotID="" DexhandID="" />
+        </HighLevelController>
+    </DeviceManager>
+
+    <MonitorManager>
+        <DiskMonitor id="file_monitor" freq="1">
+            <!--            <Folder fileDir="/home/share/assets/audio" maxVolume="1000"/>-->
+            <!--            <Folder fileDir="/home/share/assets/image" maxVolume="1000"/>-->
+            <!--            <Folder fileDir="/home/share/assets/video" maxVolume="1000"/>-->
+            <!--            <Folder fileDir="../log" maxVolume="1000"/>-->
+        </DiskMonitor>
+
+        <JointMonitor id="robot_joint_monitor" freq="200">
+            <RobotJoint robotID="left_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="left_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="0" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="1" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="2" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="3" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="4" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="6" maxTemp="80" maxCurrent="5" maxVel="2"/>
+            <RobotJoint robotID="right_arm" motorID="7" maxTemp="80" maxCurrent="5" maxVel="2"/>
+        </JointMonitor>
+    </MonitorManager>
+
+    <Server>
+        <gRPCServer port="50053"/>
+    </Server>
+
+</CMVR-ES>

config/robot_description/hc_description/dual_arm.urdf

@@ -0,0 +1,613 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="dual_arm">
+  <link name="PELVIS_S">
+    <inertial>
+      <origin xyz="3.78529087037144E-05 3.81781425684836E-07 0.0386396273530852" rpy="0 0 0" />
+      <mass value="2.10624590271277" />
+      <inertia
+        ixx="0.00165706865324979"
+        ixy="3.13663044821662E-09"
+        ixz="6.84209533216046E-07"
+        iyy="0.00136736758630241"
+        iyz="-1.01014607878816E-10"
+        izz="0.00168295663089359" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="L_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282134 0.0704593083751867 1.15261874861217E-06" rpy="0 0 0" />
+      <mass value="0.880737519698403" />
+      <inertia
+        ixx="0.000583190308378148"
+        ixy="-1.53153074560473E-05"
+        ixz="6.58466471754072E-09"
+        iyy="0.000445532440067177"
+        iyz="6.37001404038516E-09"
+        izz="0.000465648071069952" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_P" type="revolute">
+    <origin xyz="0 0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="L_SHOULDER_P_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="L_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975857 0.091739327988169 -1.67085072999562E-08" rpy="0 0 0" />
+      <mass value="0.594788424442483" />
+      <inertia
+        ixx="0.000380970396712656"
+        ixy="4.80751844573946E-05"
+        ixz="-1.34913746586865E-11"
+        iyy="0.000320962178597474"
+        iyz="-1.00039763442409E-09"
+        izz="0.000414771047901155" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_R" type="revolute">
+    <origin xyz="0.035 0.0765 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_P_S" />
+    <child link="L_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2" upper="2" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="L_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014801 0.0863620459174068 9.50748668682166E-09"
+        rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia
+        ixx="0.000327003834287552"
+        ixy="-1.8057438966771E-05"
+        ixz="7.28778136267901E-10"
+        iyy="0.000213830709361531"
+        iyz="1.49273668517817E-10"
+        izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_Y" type="revolute">
+    <origin xyz="-0.035 0.1475 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_R_S" />
+    <child link="L_SHOULDER_Y_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-2.18" upper="0" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="L_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303443 0.0603199964106564 2.99655911639718E-07" rpy="0 0 0" />
+      <mass value="0.393571904406493" />
+      <inertia
+        ixx="0.00017277119386253"
+        ixy="2.34549801867355E-05"
+        ixz="-1.90560556659271E-09"
+        iyy="0.000155340245267897"
+        iyz="8.82493073600007E-09"
+        izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_ELBOW_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_ELBOW_R" type="revolute">
+    <origin xyz="0.034 0.1025 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_Y_S" />
+    <child link="L_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2.05" upper="0" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="L_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39659173115092E-10 0.0675972604744393 0.019200551565574" rpy="0 0 0" />
+      <mass value="0.442332465815496" />
+      <inertia
+        ixx="0.000476754055455895"
+        ixy="-4.61505824713347E-15"
+        ixz="6.03808112272229E-18"
+        iyy="0.000103466585850499"
+        iyz="-4.88426705501198E-05"
+        izz="0.000482956345754213" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_P" type="revolute">
+    <origin xyz="-0.034 0.0965 0" rpy="0 0 0" />
+    <parent link="L_ELBOW_R_S" />
+    <child link="L_WRIST_P_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="0" upper="3.14" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="L_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887331864 -5.06426789392833E-10 -0.0341253783666609" rpy="0 0 0" />
+      <mass value="0.23573847772002" />
+      <inertia
+        ixx="5.10686940455785E-05"
+        ixy="7.45706654358429E-16"
+        ixz="6.2619568923368E-06"
+        iyy="6.00636019624519E-05"
+        iyz="1.27989654155383E-16"
+        izz="5.32182903609189E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_Y" type="revolute">
+    <origin xyz="0 0.1525 0.039" rpy="0 0 0" />
+    <parent link="L_WRIST_P_S" />
+    <child link="L_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+
+  <link name="L_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0161477357620243 0.0955046558857351 -0.00499392489444117" rpy="0 0 0" />
+      <mass value="0.504894112043562" />
+      <inertia
+        ixx="0.000186833711781125"
+        ixy="-3.99488705622731E-06"
+        ixz="-1.51920720398336E-06"
+        iyy="0.000179960980467837"
+        iyz="3.6992669535716E-06"
+        izz="0.000280756101568308" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_R" type="revolute">
+    <origin xyz="0.0258 0 -0.039" rpy="0 0 0" />
+    <parent link="L_WRIST_Y_S" />
+    <child link="L_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-1.57" upper="0.26" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
+      <mass value="0.880737519698404" />
+      <inertia
+        ixx="0.000583190308378149"
+        ixy="1.53153074560471E-05"
+        ixz="-6.58466471736542E-09"
+        iyy="0.000445532440067178"
+        iyz="6.37001403998779E-09"
+        izz="0.000465648071069953" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_P" type="revolute">
+    <origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="R_SHOULDER_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
+      <mass value="0.59478842444248" />
+      <inertia
+        ixx="0.000380970396712653"
+        ixy="-4.80751844573946E-05"
+        ixz="1.34913746041655E-11"
+        iyy="0.000320962178597472"
+        iyz="-1.00039763417375E-09"
+        izz="0.000414771047901152" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_R" type="revolute">
+    <origin xyz="0.035 -0.0765 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_P_S" />
+    <child link="R_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2" upper="2" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia
+        ixx="0.000327003834287551"
+        ixy="1.80574389667711E-05"
+        ixz="-7.28778136077729E-10"
+        iyy="0.000213830709361532"
+        iyz="1.49273668428957E-10"
+        izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material
+        name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_Y" type="revolute">
+    <origin xyz="-0.035 -0.1475 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_R_S" />
+    <child link="R_SHOULDER_Y_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="0" upper="3.14" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
+      <mass value="0.393571904406492" />
+      <inertia
+        ixx="0.000172771193862529"
+        ixy="-2.34549801867353E-05"
+        ixz="1.90560556668771E-09"
+        iyy="0.000155340245267897"
+        iyz="8.82493073602971E-09"
+        izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_ELBOW_R" type="revolute">
+    <origin xyz="0.034 -0.1025 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_Y_S" />
+    <child link="R_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="2.05" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
+      <mass value="0.442332465815497" />
+      <inertia
+        ixx="0.000476754055455896"
+        ixy="-4.61462558956188E-15"
+        ixz="-5.91762926004267E-18"
+        iyy="0.0001034665858505"
+        iyz="4.88426705501212E-05"
+        izz="0.000482956345754214" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_P" type="revolute">
+    <origin xyz="-0.034 -0.0965 0" rpy="0 0 0" />
+    <parent link="R_ELBOW_R_S" />
+    <child link="R_WRIST_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-3.14" upper="0" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
+      <mass value="0.235738477720019" />
+      <inertia
+        ixx="5.1068694045578E-05"
+        ixy="7.45771487459288E-16"
+        ixz="6.26195689233664E-06"
+        iyy="6.00636019624515E-05"
+        iyz="1.27828046342987E-16"
+        izz="5.32182903609188E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_Y" type="revolute">
+    <origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_P_S" />
+    <child link="R_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+
+  <link name="R_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
+      <mass value="0.504366058218534" />
+      <inertia
+        ixx="0.000185559065855467"
+        ixy="5.75889766751509E-06"
+        ixz="2.40683898454438E-06"
+        iyy="0.000131246007872084"
+        iyz="1.60533277040148E-06"
+        izz="0.000271800951396149" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+      <material
+        name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_R" type="revolute">
+    <origin xyz="0.03 0 -0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_Y_S" />
+    <child link="R_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_FINGER_TIP">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.004"/>
+      </geometry>
+      <material name="">
+        <color rgba="0 1 1 1"/> <!-- 绿色 -->
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_FINGER_TIP_FIXED" type="fixed">
+    <origin xyz="0 -0.270 -0.005" rpy="0 0 -1.5707963267"/>
+    <parent link="R_WRIST_R_S"/>
+    <child link="R_FINGER_TIP"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="R_CAM">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.004"/>
+      </geometry>
+      <material name="">
+        <color rgba="0 1 0 1"/> <!-- 绿色 -->
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_CAM_FIXED" type="fixed">
+    <origin xyz="-0.01212 -0.17655 0.07506" rpy="-1.5707963267 0 3.1415926535"/>
+    <parent link="R_WRIST_R_S"/>
+    <child link="R_CAM"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+</robot>

config/robot_description/hc_description/left_arm.urdf

@@ -0,0 +1,275 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="left_arm">
+  <link name="base_link">
+    <visual>
+      <geometry>
+        <cylinder radius="0.05" length="1.2"/>
+      </geometry>
+      <material name="gray">
+        <color rgba="0.5 0.5 0.5 1.0"/>
+      </material>
+    </visual>
+
+    <collision>
+      <geometry>
+        <cylinder radius="0.2" length="1.2"/>
+      </geometry>
+    </collision>
+
+    <inertial>
+      <mass value="20.0"/>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <inertia ixx="1.0" ixy="0.0" ixz="0.0" iyy="1.0" iyz="0.0" izz="1.0"/>
+    </inertial>
+  </link>
+
+  <joint name="base_fixed" type="fixed">
+    <origin rpy="0 0 0.0" xyz="0 0 0.6"/>
+    <parent link="base_link"/>
+    <child link="PELVIS_S"/>
+    <axis xyz="0 0 0"/>
+  </joint>
+
+  <link name="PELVIS_S">
+    <inertial>
+      <origin xyz="3.78529087037144E-05 3.81781425684836E-07 0.0386396273530852" rpy="0 0 0" />
+      <mass value="2.10624590271277" />
+      <inertia ixx="0.00165706865324979" ixy="3.13663044821662E-09" ixz="6.84209533216046E-07" iyy="0.00136736758630241" iyz="-1.01014607878816E-10" izz="0.00168295663089359" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_P" type="revolute">
+    <origin xyz="0 0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="L_SHOULDER_P_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-3.14" upper="3.14" effort="100" velocity="3.351" />
+  </joint>
+
+  <link name="L_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282134 0.0704593083751867 1.15261874861217E-06" rpy="0 0 0" />
+      <mass value="0.880737519698403" />
+      <inertia ixx="0.000583190308378148" ixy="-1.53153074560473E-05" ixz="6.58466471754072E-09" iyy="0.000445532440067177" iyz="6.37001404038516E-09" izz="0.000465648071069952" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_P_S.STL" />
+      </geometry>
+      <material
+        name="">
+        <color rgba="0.898039215686275 0.917647058823529 0.929411764705882 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_R" type="revolute">
+    <origin xyz="0.035 0.0765 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_P_S" />
+    <child link="L_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-3.14" upper="3.14" effort="100" velocity="3.351" />
+  </joint>
+
+  <link name="L_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975857 0.091739327988169 -1.67085072999562E-08" rpy="0 0 0" />
+      <mass value="0.594788424442483" />
+      <inertia ixx="0.000380970396712656" ixy="4.80751844573946E-05" ixz="-1.34913746586865E-11" iyy="0.000320962178597474" iyz="-1.00039763442409E-09" izz="0.000414771047901155" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_SHOULDER_Y" type="revolute">
+    <origin xyz="-0.035 0.1475 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_R_S" />
+    <child link="L_SHOULDER_Y_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-3.14" upper="3.14" effort="49" velocity="3.8758" />
+  </joint>
+
+  <link name="L_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014801 0.0863620459174068 9.50748668682166E-09" rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia ixx="0.000327003834287552" ixy="-1.8057438966771E-05" ixz="7.28778136267901E-10" iyy="0.000213830709361531" iyz="1.49273668517817E-10" izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_SHOULDER_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_ELBOW_R" type="revolute">
+    <origin xyz="0.034 0.1025 0" rpy="0 0 0" />
+    <parent link="L_SHOULDER_Y_S" />
+    <child link="L_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-3.14" upper="3.14" effort="49" velocity="3.8758" />
+  </joint>
+
+  <link name="L_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303443 0.0603199964106564 2.99655911639718E-07" rpy="0 0 0" />
+      <mass value="0.393571904406493" />
+      <inertia ixx="0.00017277119386253" ixy="2.34549801867355E-05" ixz="-1.90560556659271E-09" iyy="0.000155340245267897" iyz="8.82493073600007E-09" izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_ELBOW_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_P" type="revolute">
+    <origin xyz="-0.034 0.0965 0" rpy="0 0 0" />
+    <parent link="L_ELBOW_R_S" />
+    <child link="L_WRIST_P_S" />
+    <axis xyz="0 1 0" />
+    <limit lower="-3.14" upper="3.14" effort="26" velocity="4.19" />
+  </joint>
+
+  <link name="L_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39659173115092E-10 0.0675972604744393 0.019200551565574" rpy="0 0 0" />
+      <mass value="0.442332465815496" />
+      <inertia ixx="0.000476754055455895" ixy="-4.61505824713347E-15" ixz="6.03808112272229E-18" iyy="0.000103466585850499" iyz="-4.88426705501198E-05" izz="0.000482956345754213" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.698039215686274 0.698039215686274 0.698039215686274 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_Y" type="revolute">
+    <origin xyz="0 0.1525 0.039" rpy="0 0 0" />
+    <parent link="L_WRIST_P_S" />
+    <child link="L_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-3.14" upper="3.14" effort="26" velocity="4.71" />
+  </joint>
+
+  <link name="L_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887331864 -5.06426789392833E-10 -0.0341253783666609" rpy="0 0 0" />
+      <mass value="0.23573847772002" />
+      <inertia ixx="5.10686940455785E-05" ixy="7.45706654358429E-16" ixz="6.2619568923368E-06" iyy="6.00636019624519E-05" iyz="1.27989654155383E-16" izz="5.32182903609189E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="L_WRIST_R" type="revolute">
+    <origin xyz="0.0258 0 -0.039" rpy="0 0 0" />
+    <parent link="L_WRIST_Y_S" />
+    <child link="L_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-3.14" upper="3.14" effort="26" velocity="4.71" />
+  </joint>
+
+  <link name="L_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0161477357620243 0.0955046558857351 -0.00499392489444117" rpy="0 0 0" />
+      <mass value="0.504894112043562" />
+      <inertia ixx="0.000186833711781125" ixy="-3.99488705622731E-06" ixz="-1.51920720398336E-06" iyy="0.000179960980467837" iyz="3.6992669535716E-06" izz="0.000280756101568308" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/L_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+
+</robot>

config/robot_description/hc_description/right_arm.urdf

@@ -0,0 +1,316 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<robot name="right_arm">
+  <link name="PELVIS_S">
+    <inertial>
+      <origin xyz="3.78529e-05 3.81781e-07 0.0386396" rpy="0 0 0"/>
+      <mass value="2.10624590271277"/>
+      <inertia 
+        ixx="0.00165706865324979" ixy="3.13663e-09"  ixz="6.84210e-07"
+        iyy="0.00136736758630241" iyz="-1.01015e-10" izz="0.00168295663089359"/>
+    </inertial>
+
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL"/>
+      </geometry>
+      <material name="light_gray">
+        <color rgba="0.698 0.698 0.698 1"/>
+      </material>
+    </visual>
+
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <mesh filename="meshes/PELVIS_S.STL"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <link name="R_SHOULDER_P_S">
+    <inertial>
+      <origin xyz="-0.00982258725282141 -0.0704593093873431 -1.15069920925137E-06" rpy="0 0 0" />
+      <mass value="0.880737519698404" />
+      <inertia
+        ixx="0.000583190308378149"
+        ixy="1.53153074560471E-05"
+        ixz="-6.58466471736542E-09"
+        iyy="0.000445532440067178"
+        iyz="6.37001403998779E-09"
+        izz="0.000465648071069953" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin
+        xyz="0 0 0"
+        rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_P" type="revolute">
+    <origin xyz="0 -0.0945 0.042" rpy="0 0 0" />
+    <parent link="PELVIS_S" />
+    <child link="R_SHOULDER_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-1.57" upper="1.57" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_R_S">
+    <inertial>
+      <origin xyz="-0.0346025282975784 -0.09173932900033 1.86280643132974E-08" rpy="0 0 0" />
+      <mass value="0.59478842444248" />
+      <inertia
+        ixx="0.000380970396712653" ixy="-4.80751844573946E-05"  ixz="1.34913746041655E-11"
+        iyy="0.000320962178597472" iyz="-1.00039763417375E-09" izz="0.000414771047901152" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+  
+  <joint name="R_SHOULDER_R" type="revolute">
+    <origin xyz="0.035 -0.0765 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_P_S" />
+    <child link="R_SHOULDER_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-2" upper="2" effort="120" velocity="3.351" />
+  </joint>
+
+  <link name="R_SHOULDER_Y_S">
+    <inertial>
+      <origin xyz="-0.00440976862014946 -0.0863620469295699 -7.58791862676134E-09" rpy="0 0 0" />
+      <mass value="0.563406026626801" />
+      <inertia
+        ixx="0.000327003834287551"
+        ixy="1.80574389667711E-05"
+        ixz="-7.28778136077729E-10"
+        iyy="0.000213830709361532"
+        iyz="1.49273668428957E-10"
+        izz="0.000297341029639189" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_SHOULDER_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_SHOULDER_Y" type="revolute">
+    <origin xyz="-0.035 -0.1475 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_R_S" />
+    <child link="R_SHOULDER_Y_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="0" upper="3.14" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_ELBOW_R_S">
+    <inertial>
+      <origin xyz="-0.0335624237303424 -0.0603199974228191 -2.97736340082455E-07" rpy="0 0 0" />
+      <mass value="0.393571904406492" />
+      <inertia
+        ixx="0.000172771193862529"
+        ixy="-2.34549801867353E-05"
+        ixz="1.90560556668771E-09"
+        iyy="0.000155340245267897"
+        iyz="8.82493073602971E-09"
+        izz="0.00018104232734159" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_ELBOW_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_ELBOW_R" type="revolute">
+    <origin xyz="0.034 -0.1025 0" rpy="0 0 0" />
+    <parent link="R_SHOULDER_Y_S" />
+    <child link="R_ELBOW_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="0" upper="2.18" effort="80" velocity="3.8758" />
+  </joint>
+
+  <link name="R_WRIST_P_S">
+    <inertial>
+      <origin xyz="-1.39656577968772E-10 -0.0675972614865965 0.0192005515655728" rpy="0 0 0" />
+      <mass value="0.442332465815497" />
+      <inertia
+        ixx="0.000476754055455896"
+        ixy="-4.61462558956188E-15"
+        ixz="-5.91762926004267E-18"
+        iyy="0.0001034665858505"
+        iyz="4.88426705501212E-05"
+        izz="0.000482956345754214" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_P_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_P" type="revolute">
+    <origin xyz="-0.034 -0.0965 0" rpy="0 0 0" />
+    <parent link="R_ELBOW_R_S" />
+    <child link="R_WRIST_P_S" />
+    <axis xyz="0 -1 0" />
+    <limit lower="-3.14" upper="0" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
+      <mass value="0.235738477720019" />
+      <inertia
+        ixx="5.1068694045578E-05"
+        ixy="7.45771487459288E-16"
+        ixz="6.26195689233664E-06"
+        iyy="6.00636019624515E-05"
+        iyz="1.27828046342987E-16"
+        izz="5.32182903609188E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_Y" type="revolute">
+    <origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_P_S" />
+    <child link="R_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="0" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+
+  <link name="R_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
+      <mass value="0.204366058218534" />
+      <inertia
+        ixx="0.000185559065855467"
+        ixy="5.75889766751509E-06"
+        ixz="2.40683898454438E-06"
+        iyy="0.000131246007872084"
+        iyz="1.60533277040148E-06"
+        izz="0.000271800951396149" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_R" type="revolute">
+    <origin xyz="0.03 0 -0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_Y_S" />
+    <child link="R_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_FINGER_TIP">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.002"/>
+      </geometry>
+      <material name="">
+        <color rgba="0 1 0 1"/> <!-- 绿色 -->
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_FINGER_TIP_FIXED" type="fixed">
+    <origin xyz="-0.01212 -0.17655 0.07506" rpy="3.14159 0 -1.5708"/>
+    <parent link="R_WRIST_R_S"/>
+    <child link="R_FINGER_TIP"/>
+  </joint>
+</robot>

example/CMakeLists.txt

@@ -0,0 +1,80 @@
+find_package(glog REQUIRED)
+find_package(protobuf REQUIRED)
+find_package(PkgConfig REQUIRED)
+find_package(fcl REQUIRED)
+find_package(OpenCV REQUIRED)
+
+find_package(pybind11 REQUIRED)
+
+include_directories(${CMAKE_SOURCE_DIR}/include)
+
+add_executable(solve_fk solve_fk.cpp)
+target_link_libraries(solve_fk PRIVATE cmvr_es::utils)
+
+add_executable(solve_ik solve_ik.cpp)
+target_link_libraries(solve_ik PRIVATE cmvr_es::utils)
+
+add_executable(wbc_example wbc_example.cpp)
+target_link_libraries(wbc_example PRIVATE cmvr_es::utils)
+
+add_executable(follow_traj_example follow_traj_example.cpp)
+target_link_libraries(follow_traj_example PRIVATE cmvr_es::utils)
+
+set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${CMAKE_BINARY_DIR}/bin)
+
+add_executable(moveJ ${CMAKE_CURRENT_SOURCE_DIR}/moveJ.cpp)
+
+target_link_libraries(moveJ PRIVATE
+        cmvr_es::device::canbus
+        cmvr_es::device::ti5motor
+        cmvr_es::device::humanoid_robot
+        pthread glog::glog
+        proto-objects
+        ccd
+        fcl
+        cmvr_es::device_manager
+        ${OpenCV_LIBS})
+
+
+add_executable(torqueOff ${CMAKE_CURRENT_SOURCE_DIR}/torqueOff.cpp)
+
+target_link_libraries(torqueOff PRIVATE
+        cmvr_es::device::canbus
+        cmvr_es::device::ti5motor
+        cmvr_es::device::humanoid_robot
+        pthread glog::glog
+        proto-objects
+        ccd
+        fcl
+        cmvr_es::device_manager
+        ${OpenCV_LIBS})
+
+add_executable(torqueOn ${CMAKE_CURRENT_SOURCE_DIR}/torqueOn.cpp)
+target_link_libraries(torqueOn PRIVATE
+        cmvr_es::device::canbus
+        cmvr_es::device::ti5motor
+        cmvr_es::device::humanoid_robot
+        pthread
+        glog::glog
+        proto-objects
+        ccd
+        fcl
+        cmvr_es::device_manager
+        ${OpenCV_LIBS})
+
+
+
+# ---------------- pybind11 模块 ----------------
+set(CMAKE_POSITION_INDEPENDENT_CODE ON)
+pybind11_add_module(robot_wrapper ${CMAKE_CURRENT_SOURCE_DIR}/robot_wrapper.cpp)
+target_link_libraries(robot_wrapper PRIVATE
+        cmvr_es::device::canbus
+        cmvr_es::device::ti5motor
+        cmvr_es::device::humanoid_robot
+        pthread
+        glog::glog
+        proto-objects
+        ccd
+        fcl
+        cmvr_es::device_manager
+        ${OpenCV_LIBS})

example/follow_traj_example.cpp

@@ -0,0 +1,234 @@
+// cartesian_controller_test.cpp
+// ---------------------------------------------------------------------------
+// Refactored test for CartesianController with trajectory timing & logging
+//   * Left arm: circle in Y‑Z plane (normal‑X), r = 0.05m
+//   * Right arm: square in Y‑Z plane (normal‑X), side = 0.05m
+//   * Prints joint angles (q[DOF]) and EE positions (x,y,z) every step
+// ---------------------------------------------------------------------------
+
+#include <chrono>
+#include <iostream>
+#include <memory>
+#include <vector>
+#include <cmath>
+#include <iomanip>
+
+#include "utils/dynamics/robot.h"
+#include "utils/solver/qp_solver.h"
+
+// ---------- CONFIG ---------------------------------------------------------
+constexpr int DOF = 14;                                    // robot DOF
+using RobotT      = cmvr::dyn::Robot<DOF>;
+using StateT      = cmvr::dyn::State<DOF>;
+using ControllerT = cmvr::ctrl::CartesianController<DOF>;
+
+static const char* kUrdfPath = "/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf";
+static const char* kBaseLink = "PELVIS_S";
+static const char* kLeftEE   = "L_WRIST_R_S";
+static const char* kRightEE  = "R_WRIST_R_S";
+
+// ---------- TRAJECTORY GENERATORS -----------------------------------------
+std::vector<Eigen::Matrix4d> generateCircle(const Eigen::Matrix4d& T_center,
+                                            double radius, int n_points)
+{
+    std::vector<Eigen::Matrix4d> poses; poses.reserve(n_points);
+    const double x  = T_center(0,3);
+    const double y0 = T_center(1,3);
+    const double z0 = T_center(2,3);
+    const Eigen::Matrix3d R = T_center.block<3,3>(0,0);
+    for (int i = 0; i < n_points; ++i) {
+        double th = 2.0 * M_PI * i / n_points;
+        double y = y0 + radius * std::cos(th);
+        double z = z0 + radius * std::sin(th);
+        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+        T.block<3,3>(0,0) = R;
+        T(0,3) = x; T(1,3) = y; T(2,3) = z;
+        poses.push_back(T);
+    }
+    return poses;
+}
+
+std::vector<Eigen::Matrix4d> generateSquare(const Eigen::Matrix4d& T_center,
+                                            double side, int n_points)
+{
+    std::vector<Eigen::Matrix4d> poses; poses.reserve(n_points);
+    const double half = side / 2.0;
+    const double x  = T_center(0,3);
+    const double y0 = T_center(1,3);
+    const double z0 = T_center(2,3);
+    const Eigen::Matrix3d R = T_center.block<3,3>(0,0);
+
+    for (int i = 0; i < n_points; ++i) {
+        double u = static_cast<double>(i) / n_points;       // 0‑1
+        double seg = u * 4.0;                               // 4 edges
+        double y, z;
+        if (seg < 1.0) {               // −Y → +Z
+            y = -half;                 z = -half + seg*side;
+        } else if (seg < 2.0) {        // +Z → +Y
+            y = -half + (seg-1)*side;  z =  half;
+        } else if (seg < 3.0) {        // +Y → −Z
+            y =  half;                 z =  half - (seg-2)*side;
+        } else {                       // −Z → −Y
+            y =  half - (seg-3)*side;  z = -half;
+        }
+        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+        T.block<3,3>(0,0) = R;
+        T(0,3) = x; T(1,3) = y0 + y; T(2,3) = z0 + z;
+        poses.push_back(T);
+    }
+    return poses;
+}
+
+std::vector<Eigen::Matrix4d> generateLine(const Eigen::Matrix4d& T_center, double length, int n_points)
+{
+    std::vector<Eigen::Matrix4d> poses; poses.reserve(n_points);
+    double delta_x = length / (double)n_points;
+    for (int i = 0; i < n_points; ++i) {
+        const Eigen::Matrix3d R = T_center.block<3,3>(0,0);
+        Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
+        T.block<3,3>(0,0) = R;
+        T(0,3) = T_center(0,3) + delta_x * (i+1);
+        T(1,3) = T_center(1,3);
+        T(2,3) = T_center(2,3);
+        poses.push_back(T);
+    }
+    return poses;
+}
+
+// ---------- CSV HELPERS ----------------------------------------------------
+// 写入表头
+template<int DOF>
+void writeCsvHeader(std::ofstream& csv)
+{
+    csv << std::fixed << std::setprecision(6);
+    csv << "idx";
+    for (int d = 0; d < DOF; ++d) csv << ", q" << d;
+    csv << ", pLx, pLy, pLz, pRx, pRy, pRz\n";
+}
+
+// 写入一行数据
+template<int DOF>
+void writeCsvRow(std::ofstream& csv,
+                 int idx,
+                 const Eigen::Vector<double, DOF>& q,
+                 const Eigen::Vector3d& pL,
+                 const Eigen::Vector3d& pR)
+{
+    csv << idx;
+    for (int d = 0; d < DOF; ++d)
+        csv << ", " << q[d];
+    csv << ',' << pL.x() << ',' << pL.y() << ',' << pL.z();
+    csv << ',' << pR.x() << ',' << pR.y() << ',' << pR.z();
+    csv << '\n';
+}
+
+// ---------- MAIN TEST ROUTINE ---------------------------------------------
+void runTrajectoryTest(int n_points_circle = 200,
+                       int n_points_square = 200,
+                       double dt = 0.002)
+{
+    // 1. Load robot ---------------------------------------------------------
+    auto rcfg  = cmvr::dyn::LoadRobotFromURDF(kUrdfPath, kBaseLink);
+    auto robot = std::make_shared<RobotT>(rcfg);
+
+    std::vector<std::string> link_names = {
+            kBaseLink,
+            "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", kLeftEE,
+            "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", kRightEE
+    };
+    std::vector<std::string> joint_names = {
+            "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
+            "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
+    };
+    auto state = robot->MakeState(link_names, joint_names);
+
+    Eigen::Vector<double, DOF> q_init;
+    q_init << -0.1818, -0.573093, -1.15019, -1.82123, 2.14858, 0.436445, 0.0384556,
+    0.0734063, 1.16235, 1.72241, 1.81379, -2.70904, 0.0389824, 0.356423;
+    state->SetQ(q_init);
+    robot->ComputeForwardKinematics(state);
+
+    // EE centers -----------------------------------------------------------
+    const auto base_idx  = robot->GetLinkIdx(kBaseLink);
+    const auto left_idx  = robot->GetLinkIdx(kLeftEE);
+    const auto right_idx = robot->GetLinkIdx(kRightEE);
+    const Eigen::Matrix4d T_left_center  = robot->GetTransformation(state, base_idx, left_idx);
+    const Eigen::Matrix4d T_right_center = robot->GetTransformation(state, base_idx, right_idx);
+
+    // Trajectories ---------------------------------------------------------
+    const auto circle_traj = generateCircle (T_left_center , 0.1, n_points_circle);
+    // const auto circle_traj = generateLine(T_left_center, 0.1, 200);
+    // const auto square_traj = generateSquare(T_right_center, 0.1, n_points_square);
+    const auto square_traj = generateLine(T_right_center, 0.15, n_points_square);
+    std::cout << "--- square_traj (" << square_traj.size() << " waypoints) ---\n";
+    std::cout << "--- end square_traj ---\n";
+    const int N = std::max(circle_traj.size(), square_traj.size());
+
+    // Controller -----------------------------------------------------------
+    ControllerT ctrl(robot);
+
+    // Print CSV header -----------------------------------------------------
+    std::cout << std::fixed << std::setprecision(6);
+    std::cout << "idx";
+    for (int d = 0; d < DOF; ++d) std::cout << ", q" << d;
+    std::cout << ", pLx, pLy, pLz, pRx, pRy, pRz\n";
+
+    // Write CSV ------------------------------------------------------------
+    std::string csv_path = "../../joint_positions.csv";
+    std::ofstream csv(csv_path);
+    if (!csv) throw std::runtime_error("cannot open " + csv_path);
+
+    writeCsvHeader<DOF>(csv);   // ← 写表头
+
+    // Timing --------------------------------------------------------------
+    namespace chrono = std::chrono;
+    const auto t_start = chrono::high_resolution_clock::now();
+
+    for (int k = 0; k < N; ++k) {
+        const Eigen::Matrix4d& T_left  = circle_traj [k % circle_traj.size()];
+        const Eigen::Matrix4d& T_right = square_traj [k % square_traj.size()];
+
+        std::vector<cmvr::ctrl::PoseTarget> targets = {
+                {kLeftEE , T_left , 0.5, 1.0},
+                {kRightEE, T_right, 0.5, 1.0}
+        };
+
+        Eigen::Vector<double, DOF> q_cmd;
+        bool ok = ctrl.compute(state, kBaseLink, targets, dt,
+                               ControllerT::Mode::Position, q_cmd, 60, 1e-4);
+        if (!ok) std::cerr << "[WARN] IK failed at step " << k << '\n';
+
+        state->SetQ(q_cmd);
+        robot->ComputeForwardKinematics(state);
+
+        // Current EE positions -------------------------------------------
+        const Eigen::Matrix4d T_L_now = robot->GetTransformation(state, base_idx, left_idx);
+        const Eigen::Matrix4d T_R_now = robot->GetTransformation(state, base_idx, right_idx);
+        const Eigen::Vector3d pL = T_L_now.block<3,1>(0,3);
+        const Eigen::Vector3d pR = T_R_now.block<3,1>(0,3);
+
+        // CSV‑style print --------------------------------------------------
+        std::cout << k;
+        for (int d = 0; d < DOF; ++d) std::cout << ", " << q_cmd[d];
+        std::cout << ", " << pL.transpose() << ", " << pR.transpose() << '\n';
+        writeCsvRow<DOF>(csv, k, q_cmd, pL, pR);
+    }
+
+    const auto t_end = chrono::high_resolution_clock::now();
+    const double total_ms = chrono::duration<double, std::milli>(t_end - t_start).count();
+    csv.close();
+
+    // Summary -------------------------------------------------------------
+    std::cout << "---------- Trajectory Test Summary ----------\n";
+    std::cout << "Total points : " << N << "\n";
+    std::cout << "Total time   : " << total_ms << " ms\n";
+    std::cout << "Average / pt : " << total_ms / N << " ms" << std::endl;
+}
+
+// ---------- MAIN ----------------------------------------------------------
+int main() {
+    try { runTrajectoryTest(); }
+    catch (const std::exception &e) {
+        std::cout << e.what();
+    }
+}

example/moveJ.cpp

@@ -0,0 +1,73 @@
+//
+// Created by lgv on 2025/8/13.
+//
+
+#include <iostream>
+#include <vector>
+#include <string>
+#include <cstdlib> // for std::atof
+
+#include <glog/logging.h>
+#include "device_manager/device_manager.h"
+#include <libgen.h>
+
+using namespace cmvr::device;
+
+int main(int argc, char* argv[]) {
+    // 需要传入 side + 7 个关节值
+    if (argc != 9) {
+         LOG(ERROR) << "Usage: " << argv[0] << " <side:left|right> J1 J2 J3 J4 J5 J6 J7" << std::endl;
+        return 1;
+    }
+
+    std::string side = argv[1];
+    if (side != "left" && side != "right") {
+         LOG(ERROR) << "side must be 'left' or 'right'" << std::endl;
+        return 1;
+    }
+
+    // 输入的 7 个关节
+    std::vector<double> arm_q(7);
+    for (int i = 0; i < 7; ++i) {
+        arm_q[i] = std::atof(argv[i + 2]);
+    }
+
+    // 左右臂关节名称
+    std::vector<std::string> left_joint_names = {
+        "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
+        "L_ELBOW_R",    "L_WRIST_P",    "L_WRIST_Y",    "L_WRIST_R"
+    };
+    std::vector<std::string> right_joint_names = {
+        "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
+        "R_ELBOW_R",    "R_WRIST_P",    "R_WRIST_Y",    "R_WRIST_R"
+    };
+
+    std::vector<JointPositionCmd> cmd;
+    cmd.reserve(7);
+
+    if (side == "left") {
+        for (size_t i = 0; i < 7; ++i)
+            cmd.push_back({left_joint_names[i], arm_q[i]});
+    } else {
+        for (size_t i = 0; i < 7; ++i)
+            cmd.push_back({right_joint_names[i], arm_q[i]});
+    }
+
+    // 获取 robot
+    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
+    const XmlNode config(config_path);
+
+    if (!config.hasChild("DeviceManager")) {
+        LOG(ERROR) << "Device Manager node not found";
+        return 1;
+    }
+
+    auto dmgr_cfg = config.getChild("DeviceManager");
+    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
+
+    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
+
+    robot->moveJ(cmd, 0.8);
+
+    return 0;
+}

example/robot_wrapper.cpp

@@ -0,0 +1,170 @@
+//
+// Created by lgv on 2025/8/15.
+//
+#ifdef MAX_ITER
+#undef MAX_ITER
+#endif
+
+#include <opencv2/opencv.hpp>
+#include <pybind11/pybind11.h>
+#include <pybind11/stl.h>
+#include <mutex>
+
+#include "devices/abstract_robot.h"
+#include "device_manager/device_manager.h"
+#include "cmvr/msgs/geometry.pb.h"
+
+namespace py = pybind11;
+using namespace cmvr::device;
+
+class PyRobotWrapper {
+public:
+    // 构造时只需要传入 config_path 和 robot_name（只在第一次初始化有效）
+    PyRobotWrapper(const std::string& config_path, const std::string& robot_name) {
+        std::lock_guard<std::mutex> lock(init_mutex); // 线程安全
+        if (!robot_) {
+            const XmlNode config(config_path);
+            if (!config.hasChild("DeviceManager")) {
+                throw std::runtime_error("Device Manager node not found");
+            }
+            auto dmgr_cfg = config.getChild("DeviceManager");
+            dmgr_ = &DeviceManager::getInstance(dmgr_cfg);
+            robot_ = dmgr_->getDevice<AbstractRobot>(robot_name);
+        }
+    }
+
+    void moveJ(const std::string &side, const std::vector<double> &q, double speed=0.8) {
+        if (q.size() != 7)
+            throw std::runtime_error("Expected 7 joint values");
+
+        const auto& joint_names = getJointNames(side);
+
+        std::vector<JointPoint> cmd = {{"WAIST_Y", 0}, {"WAIST_P", 0}};
+        for (size_t i = 0; i < 7; ++i)
+            cmd.push_back({joint_names[i], q[i]});
+
+        robot_->moveJ(cmd, speed);
+    }
+
+    void torqueOn() {
+        checkRobotInit();
+        robot_->torqueOn();
+    }
+
+    void torqueOn(const std::string& joint_name) {
+        checkRobotInit();
+        robot_->torqueOn(joint_name);
+    }
+
+    void torqueOff() {
+        checkRobotInit();
+        robot_->torqueOff();
+    }
+
+    void torqueOff(const std::string& joint_name) {
+        checkRobotInit();
+        robot_->torqueOff(joint_name);
+    }
+
+    void calibrateZeroQ(const std::string &joint_name) {
+        checkRobotInit();
+        robot_->calibrateZeroQ(joint_name);
+    }
+
+    std::vector<double> getJointQ(const std::string& side) const {
+        checkRobotInit();
+
+        auto joint_qs = robot_->getJointQ();  // unordered_map<std::string,double>
+        const auto& joint_names = getJointNames(side);
+
+        std::vector<double> values;
+        for (const auto& name : joint_names) {
+            values.push_back(joint_qs.at(name));
+        }
+
+        return values;
+    }
+
+    std::vector<double> ik(const std::string& side,
+                           const std::string &base_link,
+                           const std::string &ee_link,
+                           double x, double y, double z,
+                           double rx, double ry, double rz)
+    {
+        checkRobotInit();
+
+        cmvr::msgs::Pose3d pose;
+        pose.mutable_position()->set_x(x);
+        pose.mutable_position()->set_y(y);
+        pose.mutable_position()->set_z(z);
+
+        pose.mutable_euler()->set_rx(rx);
+        pose.mutable_euler()->set_ry(ry);
+        pose.mutable_euler()->set_rz(rz);
+
+        auto js_cmd = robot_->ik(base_link, ee_link, pose);
+
+        if (js_cmd.size() < 14) {
+            throw std::runtime_error("IK solution size is smaller than expected");
+        }
+
+        if (side == "left") {
+            return std::vector<double>(js_cmd.begin(), js_cmd.begin() + 7);
+        } else if (side == "right") {
+            return std::vector<double>(js_cmd.begin() + 7, js_cmd.begin() + 14);
+        } else {
+            throw std::runtime_error("Side must be 'left' or 'right'");
+        }
+    }
+
+private:
+    static DeviceManager* dmgr_;
+    static std::shared_ptr<AbstractRobot> robot_;
+    static std::mutex init_mutex;
+
+    static const std::vector<std::string> LEFT_JOINTS;
+    static const std::vector<std::string> RIGHT_JOINTS;
+
+    void checkRobotInit() const {
+        if (!robot_)
+            throw std::runtime_error("Robot not initialized");
+    }
+
+    const std::vector<std::string>& getJointNames(const std::string& side) const {
+        if (side == "left") return LEFT_JOINTS;
+        if (side == "right") return RIGHT_JOINTS;
+        throw std::runtime_error("Side must be 'left' or 'right'");
+    }
+};
+
+// 静态成员初始化
+DeviceManager* PyRobotWrapper::dmgr_ = nullptr;
+std::shared_ptr<AbstractRobot> PyRobotWrapper::robot_ = nullptr;
+std::mutex PyRobotWrapper::init_mutex;
+
+const std::vector<std::string> PyRobotWrapper::LEFT_JOINTS = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
+                                                              "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
+const std::vector<std::string> PyRobotWrapper::RIGHT_JOINTS = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
+                                                               "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
+
+// Python 绑定
+PYBIND11_MODULE(robot_wrapper, m) {
+    py::class_<PyRobotWrapper>(m, "Robot")
+        .def(py::init<const std::string&, const std::string&>(),
+             py::arg("config_path"), py::arg("robot_name"),
+             "Initialize the robot wrapper with config and robot name")
+        .def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"), py::arg("speed")=0.8,
+             "Move robot joints in joint space with optional speed")
+        .def("torqueOn", py::overload_cast<>(&PyRobotWrapper::torqueOn))
+        .def("torqueOn", py::overload_cast<const std::string&>(&PyRobotWrapper::torqueOn))
+        .def("torqueOff", py::overload_cast<>(&PyRobotWrapper::torqueOff))
+        .def("torqueOff", py::overload_cast<const std::string&>(&PyRobotWrapper::torqueOff))
+        .def("calibrateZeroQ", &PyRobotWrapper::calibrateZeroQ)
+        .def("getJointQ", &PyRobotWrapper::getJointQ, py::arg("side"),
+             "Get joint positions for the specified side ('left' or 'right')")
+        .def("ik", &PyRobotWrapper::ik,
+             py::arg("side"), py::arg("base_link"), py::arg("ee_link"),
+             py::arg("x"), py::arg("y"), py::arg("z"),
+             py::arg("rx"), py::arg("ry"), py::arg("rz"),
+             "Compute inverse kinematics and return 7 joint values for the given side");
+}

example/solve_fk.cpp

@@ -0,0 +1,101 @@
+#include <iostream>
+#include <fstream>
+#include <memory>
+#include <vector>
+#include <string>
+#include <iomanip>
+
+#include "utils/dynamics/robot.h"
+
+constexpr int DOF = 14;
+using RobotT = cmvr::dyn::Robot<DOF>;
+using StateT = cmvr::dyn::State<DOF>;
+
+static const char* kUrdfPath = "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf";
+
+int main(int argc, char** argv) {
+    if (argc < 10) {
+        std::cerr << "用法: solve_fk <base_line> <target_link> <q1> <q2> <q3> <q4> <q5> <q6> <q7> [-o <filename>]\n";
+        return 1;
+    }
+
+    bool write_file = false;
+    std::string output_file;
+
+    if (argc == 12 && std::string(argv[10]) == "-o") {
+        write_file = true;
+        output_file = argv[11];
+    }
+
+    std::string arm = argv[1];
+    std::vector<std::string> link_names = {
+        "PELVIS_S",
+        "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S",
+        "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_CAM", "R_FINGER_TIP"
+    };
+    std::vector<std::string> joint_names = {
+        "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
+        "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
+    };
+
+    std::string base_link = argv[1];
+    std::string target_link = argv[2];
+    bool find_base_link = false;
+    bool find_target_link = false;
+    for (const auto& link_name : link_names) {
+        if (base_link == link_name) {
+            find_base_link = true;
+        }
+        if (target_link == link_name) {
+            find_target_link = true;
+        }
+    }
+    if (!find_base_link) {
+        std::cerr << "base link 无效" << std::endl;
+        return -1;
+    }
+    if (!find_target_link) {
+        std::cerr << "target link 无效" << std::endl;
+        return -1;
+    }
+
+    Eigen::Vector<double, DOF> q;
+    q.setZero();
+    for (int i = 0; i < 7; ++i) {
+        if (target_link[0] == 'L') {
+            q[i] = std::stod(argv[i+3]);
+        }
+        else if (target_link[0] == 'R') {
+            q[i+7] = std::stod(argv[i+3]);
+        }
+    }
+
+    auto rcfg = cmvr::dyn::LoadRobotFromURDF(kUrdfPath, "PELVIS_S");
+    auto robot = std::make_shared<RobotT>(rcfg);
+    auto state = robot->MakeState(link_names, joint_names);
+    state->SetQ(q);
+    robot->ComputeForwardKinematics(state);
+
+    auto base_idx = robot->GetLinkIdx(base_link);
+    auto ee_idx   = robot->GetLinkIdx(target_link);
+    Eigen::Matrix4d T = robot->GetTransformation(state, base_idx, ee_idx);
+
+    std::cout << std::fixed << std::setprecision(10);
+    std::cout << "变换矩阵T:\n" << T << "\n";
+
+    if (write_file) {
+        std::ofstream fout(output_file);
+        if (!fout) {
+            std::cerr << "无法创建输出文件 " << output_file << "\n";
+            return 1;
+        }
+        fout << std::fixed << std::setprecision(6);
+        fout << T(0,0) << "," << T(0,1) << "," << T(0,2) << "," << T(0,3) << ","
+             << T(1,0) << "," << T(1,1) << "," << T(1,2) << "," << T(1,3) << ","
+             << T(2,0) << "," << T(2,1) << "," << T(2,2) << "," << T(2,3) << "\n";
+        fout.close();
+        std::cout << "结果已保存到 " << output_file << "\n";
+    }
+
+    return 0;
+}

example/solve_ik.cpp

@@ -0,0 +1,130 @@
+//
+// Created by xtkuang on 2025/7/31.
+//
+
+#include "utils/dynamics/robot.h"
+#include "utils/solver/qp_solver.h"
+#include <Eigen/Dense>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+#include <cmath>
+#include <glog/logging.h>
+
+constexpr int DOF = 14;
+using RobotT = cmvr::dyn::Robot<DOF>;
+using ControllerT = cmvr::ctrl::CartesianController<DOF>;
+
+// 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Z→Y→X
+Eigen::Matrix3d eulerToRotationMatrix(double rx, double ry, double rz)
+{
+    Eigen::Matrix3d R_x;
+    R_x << 1, 0, 0,
+           0, cos(rx), -sin(rx),
+           0, sin(rx), cos(rx);
+
+    Eigen::Matrix3d R_y;
+    R_y << cos(ry), 0, sin(ry),
+           0, 1, 0,
+          -sin(ry), 0, cos(ry);
+
+    Eigen::Matrix3d R_z;
+    R_z << cos(rz), -sin(rz), 0,
+           sin(rz), cos(rz), 0,
+           0, 0, 1;
+
+    // ✅ 按照输入顺序 X → Y → Z 旋转
+    return R_x * R_y * R_z;
+}
+
+int main(int argc, char **argv)
+{
+    if (argc != 9)
+    {
+        std::cerr << "用法: " << argv[0] << " <base link> <target link> <x> <y> <z> <rx> <ry> <rz>\n";
+        return -1;
+    }
+
+    std::vector<std::string> link_names = {
+        "PELVIS_S",
+        "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S",
+        "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_CAM", "R_FINGER_TIP"
+    };
+
+    std::vector<std::string> joint_names = {
+        "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
+        "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
+    };
+
+    std::string BASE_LINK = argv[1];
+    std::string target_link = argv[2];
+    bool find_base_link = false;
+    bool find_target_link = false;
+    for (auto link_name : link_names) {
+        if (BASE_LINK == link_name) {
+            find_base_link = true;
+        }
+        if (target_link == link_name) {
+            find_target_link = true;
+        }
+    }
+    if (!find_base_link) {
+        std::cerr << "base link 无效" << std::endl;
+        return -1;
+    }
+    if (!find_target_link) {
+        std::cerr << "target link 无效" << std::endl;
+        return -1;
+    }
+
+    double x = std::stod(argv[3]);
+    double y = std::stod(argv[4]);
+    double z = std::stod(argv[5]);
+    double rx = std::stod(argv[6]) * M_PI / 180.0;
+    double ry = std::stod(argv[7]) * M_PI / 180.0;
+    double rz = std::stod(argv[8]) * M_PI / 180.0;
+
+    // 1. 加载机器人模型
+    auto rcfg = LoadRobotFromURDF(
+        "/home/lgv/cmvr/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
+        /*root*/ "PELVIS_S");
+    auto robot = std::make_shared<RobotT>(rcfg);
+
+    auto state = robot->MakeState(link_names, joint_names);
+
+    // 初始关节角度
+    Eigen::Vector<double, DOF> q_init;
+    q_init << -0.3647738137, -1.4556045962, 1.6057029118, -1.8483036779, 2.9024825461, 0.1850049007, -0.5602506899,
+             -0.23905230057945082, 1.2838583858530206, 1.8486727698400174, 1.3036511045462158, -2.824727848601091, -0.02012495463116552, -0.202540520242054;
+    state->SetQ(q_init);
+    robot->ComputeForwardKinematics(state);
+
+    // 2. 构造目标位姿矩阵
+    Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
+    T_target.block<3,3>(0,0) = eulerToRotationMatrix(rx, ry, rz);  // 输入为弧度
+    T_target(0,3) = x;
+    T_target(1,3) = y;
+    T_target(2,3) = z;
+
+    // 3. 创建控制器
+    ControllerT ctrl(robot);
+    constexpr double DT = 0.002; // 2ms
+
+    std::vector<cmvr::ctrl::PoseTarget> targets = {
+        {target_link, T_target, 0.5, 1.0}
+    };
+
+    Eigen::Vector<double, DOF> q_cmd;
+    bool ok = ctrl.compute(state, BASE_LINK, targets, DT, ControllerT::Mode::Position, q_cmd, 10000, 1e-6);
+
+    // ✅ 格式化输出
+    std::cout << "[IK Solve] success=" << std::boolalpha << ok << "\n";
+    std::cout << "left:  ";
+    for(int i=0; i<7; ++i) std::cout << q_cmd[i] << (i<6?" ":"");
+    std::cout << "\nright: ";
+    for(int i=7; i<14; ++i) std::cout << q_cmd[i] << (i<13?" ":"");
+    std::cout << std::endl;
+
+    return 0;
+}

example/torqueOff.cpp

@@ -0,0 +1,34 @@
+//
+// Created by lgv on 2025/8/13.
+//
+
+#include <iostream>
+#include <vector>
+#include <string>
+#include <cstdlib> // for std::atof
+
+#include <glog/logging.h>
+#include "device_manager/device_manager.h"
+#include <libgen.h>
+
+using namespace cmvr::device;
+int main(int argc, char* argv[]) {
+
+    // 这里按你的原代码结构获取 robot
+    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
+    const XmlNode config(config_path);
+
+    if (!config.hasChild("DeviceManager")) {
+        LOG(ERROR) << "Device Manager node not found";
+        return 1;
+    }
+
+    auto dmgr_cfg = config.getChild("DeviceManager");
+    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
+
+    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
+
+    robot->torqueOff();
+
+    return 0;
+}

example/torqueOn.cpp

@@ -0,0 +1,36 @@
+//
+// Created by lgv on 2025/8/13.
+//
+
+#include <iostream>
+#include <vector>
+#include <string>
+#include <cstdlib> // for std::atof
+
+#include <glog/logging.h>
+#include "device_manager/device_manager.h"
+#include <libgen.h>
+
+using namespace cmvr::device;
+int main(int argc, char* argv[]) {
+
+    // 这里按你的原代码结构获取 robot
+    std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml";
+    const XmlNode config(config_path);
+
+    if (!config.hasChild("DeviceManager")) {
+        LOG(ERROR) << "Device Manager node not found";
+        return 1;
+    }
+
+    auto dmgr_cfg = config.getChild("DeviceManager");
+    auto &dmgr = DeviceManager::getInstance(dmgr_cfg);
+
+    auto robot = dmgr.getDevice<AbstractRobot>("hc01");
+
+    robot->eStop();
+
+    std::this_thread::sleep_for(std::chrono::milliseconds(500));
+
+    return 0;
+}

example/wbc_example.cpp

@@ -0,0 +1,94 @@
+//
+// Created by xtkuang on 2025/7/9.
+//
+
+#include "utils/dynamics/robot.h"
+#include "utils/solver/qp_solver.h"
+
+constexpr int DOF = 14;
+using RobotT = cmvr::dyn::Robot<DOF>;
+//using StateT = cmvr::dyn::State<DOF>;
+using ControllerT = cmvr::ctrl::CartesianController<DOF>;
+
+int main(int argc, char **argv) {
+    auto rcfg  = LoadRobotFromURDF(
+            "/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
+            /*root*/ "PELVIS_S");
+
+    auto robot = std::make_shared<RobotT>(rcfg);
+
+    std::vector<std::string> link_names = {
+        "PELVIS_S",
+        "L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S",
+        "R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S"
+    };
+
+    std::vector<std::string> joint_names = {
+        "L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
+        "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
+    };
+
+    auto state = robot->MakeState(link_names, joint_names);
+    Eigen::Vector<double, DOF> q_init;
+    q_init << -0.3647738137, -1.4556045962, 1.6057029118, -1.8483036779, 2.9024825461, 0.1850049007, -0.5602506899,
+    -0.02679378, 0.9625459, 2.0013871, 1.4209224, -2.552366, 0.34969908, 0.10842471;
+    state->SetQ(q_init);
+    robot->ComputeForwardKinematics(state);
+    auto base_idx = robot->GetLinkIdx("PELVIS_S");
+    auto target_idx_left = robot->GetLinkIdx("L_WRIST_R_S");
+    auto target_idx_right = robot->GetLinkIdx("R_WRIST_R_S");
+    auto T_l = robot->GetTransformation(state, base_idx, target_idx_left);
+    auto T_r = robot->GetTransformation(state, base_idx, target_idx_right);
+    std::cout << "q_init: " << std::endl;
+    std::cout << "PELVIS_S -> L_WRIST_R_S:" << std::endl;
+    std::cout << T_l << std::endl << std::endl;
+    std::cout << "PELVIS_S -> R_WRIST_R_S:" << std::endl;
+    std::cout << T_r << std::endl << std::endl;
+
+    Eigen::Matrix4d T_target_left, T_target_right;
+    Eigen::Vector<double, DOF> q_result;
+
+    T_target_left <<
+    0.      , 1.        , 0.        , 0.2,
+    -1.     , 0.        , 0.0       , 0.3,
+    0.      , 0.        , 1.        , -0.15,
+    0,          0,          0,          1;
+
+    T_target_right <<
+    0.       , -1       , 0.       , 0.2      ,
+    1.       , 0.       , 0.       , -0.3     ,
+    0.       , 0.       , 1.       , -0.15    ,
+    0.       , 0.       , 0.       , 1        ;
+
+    ControllerT ctrl(robot);          // d_safe / lambda 用默认即可
+    constexpr char BASE_LINK[] = "PELVIS_S";
+    constexpr double DT = 0.002;      // 控制周期 2 ms
+    std::vector<cmvr::ctrl::PoseTarget> targets = {
+            // {"L_WRIST_R_S", T_target_left, 0.5, 1.0},
+            {"R_WRIST_R_S", T_target_right, 0.5, 1.0}
+    };
+
+    /* --- 3.1 位置控制（输出关节位置） --- */
+    {
+        state->SetQ(q_init);
+        robot->ComputeForwardKinematics(state);
+        Eigen::Vector<double, DOF> q_cmd;
+        bool ok = ctrl.compute(
+                state, BASE_LINK, targets, DT,
+                ControllerT::Mode::Position, q_cmd,  80, 1e-6);
+
+        std::cout << "\n[Position] success=" << std::boolalpha << ok
+                  << "\nq_cmd = " << q_cmd.transpose() << "\n";
+        state->SetQ(q_cmd);
+        robot->ComputeForwardKinematics(state);
+        T_l = robot->GetTransformation(state, base_idx, target_idx_left);
+        T_r = robot->GetTransformation(state, base_idx, target_idx_right);
+        std::cout << "PELVIS_S -> L_WRIST_R_S:" << std::endl;
+        std::cout << T_l << std::endl << std::endl;
+        std::cout << "PELVIS_S -> R_WRIST_R_S" << ":" << std::endl;
+        std::cout << T_r << std::endl << std::endl;
+    }
+
+    return 0;
+}
+

include/device_manager/device_factory.h

@@ -0,0 +1,42 @@
+//
+// Created by xtkuang on 2025/5/13.
+//
+
+#ifndef CMVR_ES_DEVICE_FACTORY_H
+#define CMVR_ES_DEVICE_FACTORY_H
+#pragma once
+
+#include "devices/abstract_agv.h"
+#include "devices/abstract_battery.h"
+#include "devices/abstract_camera.h"
+#include "devices/abstract_dexhand.h"
+#include "devices/abstract_gripper.h"
+#include "devices/abstract_robot.h"
+#include "devices/abstract_microphone.h"
+#include "devices/abstract_speaker.h"
+#include "devices/abstract_biohead.h"
+
+namespace cmvr::device {
+
+    class DeviceFactory {
+    public:
+        DeviceFactory() = default;
+
+        template <typename DeviceType>
+        std::shared_ptr<DeviceType> create(const XmlNode& cfg);
+
+    private:
+        std::shared_ptr<AbstractAGV> create_agv_(const XmlNode& cfg);
+        std::shared_ptr<AbstractBattery> create_battery_(const XmlNode& cfg);
+        std::shared_ptr<AbstractCamera> create_camera_(const XmlNode& cfg);
+        std::shared_ptr<AbstractDexHand> create_dexhand_(const XmlNode& cfg);
+        std::shared_ptr<AbstractGripper> create_gripper_(const XmlNode& cfg);
+        std::shared_ptr<AbstractMicrophone> create_mic_(const XmlNode& cfg);
+        std::shared_ptr<AbstractRobot> create_robot_(const XmlNode& cfg);
+        std::shared_ptr<AbstractSpeaker> create_speaker_(const XmlNode& cfg);
+        std::shared_ptr<AbstractBiohead> create_biohead_(const XmlNode& cfg);
+    };
+}
+
+
+#endif //CMVR_ES_DEVICE_FACTORY_H

include/device_manager/device_manager.h

@@ -0,0 +1,77 @@
+//
+// Created by xtkuang on 2025/5/6.
+//
+
+#ifndef CMVR_ES_DEVICE_MANAGER_H
+#define CMVR_ES_DEVICE_MANAGER_H
+
+#include <list>
+#include <variant>
+#include <sys/utsname.h>
+#include <unordered_map>
+#include "device_factory.h"
+#include "system_monitor.h"
+#include "../utils/base/thread_pool.h"
+
+namespace cmvr::device {
+
+    using DeviceVariant = std::variant<
+        std::shared_ptr<AbstractAGV>,
+        std::shared_ptr<AbstractBattery>,
+        std::shared_ptr<AbstractCamera>,
+        std::shared_ptr<AbstractDexHand>,
+        std::shared_ptr<AbstractGripper>,
+        std::shared_ptr<AbstractMicrophone>,
+        std::shared_ptr<AbstractRobot>,
+        std::shared_ptr<AbstractSpeaker>,
+        std::shared_ptr<AbstractBiohead>
+    >;
+
+    typedef struct {
+        std::string version;
+        std::string name;
+        std::string description;
+        std::string os;
+        std::string kernel_version;
+        std::string architecture;
+    } SystemInfo;
+
+    class DeviceManager {
+    public:
+        DeviceManager(const DeviceManager&) = delete;
+        DeviceManager& operator=(const DeviceManager&) = delete;
+
+        static DeviceManager& getInstance(const XmlNode &cfg);
+        static DeviceManager& getInstance();
+        static void destroyInstance();
+
+        void start();
+        void restart();
+        void stop();
+
+        void updateParam(const std::string& id, const std::pair<std::string, std::string> &param);
+        void getDeviceList(std::list<std::pair<std::string, std::string>> &device_list);
+
+        void getSystemInfo(SystemInfo &info) const;
+        void getSystemStatus(device::SystemStatus &status) const;
+
+        template <class DeviceType>
+        std::shared_ptr<DeviceType> getDevice(const std::string &device_id);
+
+    private:
+        static std::once_flag init_flag_;
+        static std::shared_ptr<DeviceManager> instance_;
+
+        XmlNode cfg_;
+        SystemInfo info_;
+        std::unordered_map<std::string, DeviceVariant> devices_;
+        std::unique_ptr<DeviceFactory> dev_factory_;
+        std::unique_ptr<device::SystemMonitor> sys_monitor_;
+
+        explicit DeviceManager(const XmlNode &cfg);
+        void init_devices_();
+        void get_os_info_();
+    };
+} // cmvr
+
+#endif //CMVR_ES_DEVICE_MANAGER_H

include/device_manager/system_monitor.h

@@ -0,0 +1,45 @@
+//
+// Created by xtkuang on 2025/5/13.
+//
+
+#ifndef CMVR_ES_MONITOR_H
+#define CMVR_ES_MONITOR_H
+
+#include <fstream>
+#include <sstream>
+#include <string>
+#include <cmath>
+#include <mutex>
+#include <unistd.h>
+#include <sys/statvfs.h>
+#include <glog/logging.h>
+#include "../utils/base/timer.h"
+#include "rapidxml/xml_parser.h"
+
+namespace cmvr::device {
+    struct SystemStatus {
+        float cpu_usage;
+        float used_memory;
+        float total_memory;
+        float used_disk;
+        float total_disk;
+        float cpu_temperature;
+    };
+
+    class SystemMonitor {
+    public:
+        SystemMonitor()=default;
+        ~SystemMonitor()=default;
+        void getStatus(SystemStatus &status);
+    private:
+        SystemStatus status_{};
+        void get_cpu_();
+        void get_memory_();
+        void get_disk_();
+    };
+
+}
+
+
+
+#endif //CMVR_ES_MONITOR_H

include/devices/abstract_agv.h

@@ -0,0 +1,36 @@
+//
+// Created by xtkuang on 2025/5/8.
+//
+
+#ifndef CMVR_ES_ABSTRACT_AGV_H
+#define CMVR_ES_ABSTRACT_AGV_H
+#pragma once
+
+#include "abstract_device.h"
+
+namespace cmvr::device{
+    class AbstractAGV: public AbstractDevice {
+    public:
+        explicit AbstractAGV(const XmlNode &config): AbstractDevice(config) {};
+        ~AbstractAGV() override=default;
+
+        virtual void getState(AGVState &state) {}
+
+        // navigation
+        virtual void eStop() {}
+        virtual void goHome() {}
+        virtual void moveto(math::Pose2d &location, double speed_ratio) {}
+        virtual void setVelocity(math::Vec3 linear, math::Vec3 angular) {}
+
+        // map
+        virtual void initMap(float resolution, int width, int height) {}
+        virtual void updateMap() {}
+        virtual void saveMap(const std::string& file_path) {}
+        virtual void loadMap(const std::string& file_path) {}
+
+    protected:
+        AGVState state_;
+    };
+}
+
+#endif //CMVR_ES_ABSTRACT_AGV_H

include/devices/abstract_battery.h

@@ -0,0 +1,25 @@
+//
+// Created by xtkuang on 2025/5/8.
+//
+
+#ifndef CMVR_ES_ABSTRACT_BATTERY_H
+#define CMVR_ES_ABSTRACT_BATTERY_H
+#pragma once
+
+#include "abstract_device.h"
+
+namespace cmvr::device{
+    class AbstractBattery: public AbstractDevice {
+    public:
+        explicit AbstractBattery(const XmlNode &config): AbstractDevice(config) {}
+        ~AbstractBattery() override =default;
+
+        virtual void getState(BatteryState &state) {}
+
+    protected:
+        BatteryState state_{};
+
+    };
+}
+
+#endif //CMVR_ES_ABSTRACT_BATTERY_H

include/devices/abstract_biohead.h

@@ -0,0 +1,83 @@
+#ifndef ABSTRACT_BIOHEAD_H
+#define ABSTRACT_BIOHEAD_H
+#pragma once
+#include "abstract_device.h"
+
+namespace cmvr::device {
+
+    // 面部表情状态结构体
+    struct FacialExpressionState {
+        // 眉毛
+        float left_eyebrow_outside_y = 0.0f;  // 左眉毛外侧垂直位置
+        float left_eyebrow_inside_y = 0.0f;   // 左眉毛内侧垂直位置
+        float right_eyebrow_outside_y = 0.0f; // 右眉毛外侧垂直位置
+        float right_eyebrow_inside_y = 0.0f;  // 右眉毛内侧垂直位置
+
+        // 眼睑
+        float left_eye_upper_lid_y = 0.0f;    // 左眼上眼睑垂直位置
+        float left_eye_lower_lid_y = 0.0f;    // 左眼下眼睑垂直位置
+        float right_eye_upper_lid_y = 0.0f;   // 右眼上眼睑垂直位置
+        float right_eye_lower_lid_y = 0.0f;   // 右眼下眼睑垂直位置
+
+        // 眼球
+        float left_eye_ball_x = 0.0f;         // 左眼球水平位置（X坐标）
+        float left_eye_ball_y = 0.0f;         // 左眼球垂直位置（Y坐标）
+        float right_eye_ball_x = 0.0f;        // 右眼球水平位置（X坐标）
+        float right_eye_ball_y = 0.0f;        // 右眼球垂直位置（Y坐标）
+
+        // 鼻子
+        float left_nose_y = 0.0f;             // 左鼻孔垂直位置
+        float right_nose_y = 0.0f;            // 右鼻孔垂直位置
+
+        // 嘴巴
+        float upper_lip_y = 0.0f;             // 上唇的垂直位置
+        float upper_lip_z = 0.0f;             // 上唇的前后位置（Z坐标）
+        float lower_lip_y = 0.0f;             // 下唇的垂直位置
+        float lower_lip_z = 0.0f;             // 下唇的前后位置（Z坐标）
+
+        // 左唇角
+        float upper_left_lip_x = 0.0f;        // 左唇角上部水平位置（X坐标）
+        float upper_left_lip_y = 0.0f;        // 左唇角上部垂直位置（Y坐标）
+        float left_corner_lip_x = 0.0f;       // 左嘴角水平位置（X坐标）
+        float left_corner_lip_y = 0.0f;       // 左嘴角垂直位置（Y坐标）
+        float lower_left_lip_x = 0.0f;        // 左唇角下部水平位置（X坐标）
+        float lower_left_lip_y = 0.0f;        // 左唇角下部垂直位置（Y坐标）
+
+        // 右唇角
+        float upper_right_lip_x = 0.0f;       // 右唇角上部水平位置（X坐标）
+        float upper_right_lip_y = 0.0f;       // 右唇角上部垂直位置（Y坐标）
+        float right_corner_lip_x = 0.0f;      // 右嘴角水平位置（X坐标）
+        float right_corner_lip_y = 0.0f;      // 右嘴角垂直位置（Y坐标）
+        float lower_right_lip_x = 0.0f;       // 右唇角下部水平位置（X坐标）
+        float lower_right_lip_y = 0.0f;       // 右唇角下部垂直位置（Y坐标）
+        // Jaw
+        float jaw_x = 0.0f;
+        float jaw_y = 0.0f;
+    };
+
+    // 抽象头部类
+    class AbstractBiohead : public AbstractDevice {
+    public:
+        explicit AbstractBiohead(const XmlNode &config) : AbstractDevice(config) {}
+        ~AbstractBiohead() override = default;
+
+        virtual void getState(RobotState &state){};
+        virtual void eStop() {};
+        virtual void setExpressionPose(FacialExpressionState& expression_state, double vel=0, double acc=0) {}
+        virtual void streamFacialPose(FacialExpressionState& expression_state, double vel, double acc) {}
+        FacialExpressionState expression_state_;
+        std::atomic<bool> emergency_stop_requested = false;
+
+
+    };
+
+} // namespace cmvr::device
+
+#endif // ABSTRACT_BIOHEAD_H
+
+
+
+
+
+
+

include/devices/abstract_camera.h

@@ -0,0 +1,74 @@
+#ifndef CMVR_ES_ABSTRACT_CAMERA_H
+#define CMVR_ES_ABSTRACT_CAMERA_H
+#pragma once
+
+#include <opencv2/opencv.hpp>
+#include "abstract_device.h"
+#include <Eigen/Core>
+
+namespace cmvr::device {
+    struct Rs2Intrinsics
+    {
+        float   cx;
+        float   cy;
+        float   fx;
+        float   fy;
+        float   coeffs[5];
+
+    };
+    struct StreamFrameData
+    {
+        //原始图像数据
+        cv::Mat rgbImage;
+        //深度图原始数据
+        cv::Mat depthImage;
+        //彩色图像帧
+        std::vector<uint8_t> rgbFrame;
+        //深度图像帧
+        std::vector<uint8_t> depthFrame;
+        //编码格式
+        std::string codec = ".h264";
+        Rs2Intrinsics intrinsics;
+        int width;
+        int height;
+        int fps;
+        bool bKey;
+        bool depthKey;
+    };
+    class AbstractCamera : public AbstractDevice {
+    public:
+        // 录制状态
+        enum class RecordingState {
+            STOPPED,
+            RECORDING,
+            PAUSED
+        };
+    public:
+        explicit AbstractCamera(const XmlNode &cfg): AbstractDevice(cfg) {}
+        ~AbstractCamera() override = default;
+
+        inline void getState(CameraState &state) {state = state_;}
+        virtual void getRGBImage(cv::Mat &color, Rs2Intrinsics& intrinsics) {}
+        virtual void getDepthImage(cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
+        virtual void getRGBDImages(cv::Mat &color, cv::Mat &depth, Rs2Intrinsics& intrinsics) {}
+        virtual void startRecording(const std::string &video_path) {}
+        virtual void stopRecording() {}
+        virtual void pauseRecording() {}
+        virtual void resumeRecording() {}
+        virtual void getEncodedFrame(StreamFrameData& frame_data, size_t& index) {}
+
+        virtual bool startStreaming() {return true;}
+        virtual void stopStreaming() {}
+        virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
+    protected:
+        CameraState state_{};
+        void clear_error_() {
+            this->state_.is_error = false;
+            this->state_.error_message.clear();
+        }
+    };
+
+}
+
+
+#endif // CMVR_ES_ABSTRACT_CAMERA_H

include/devices/abstract_canbus.h

@@ -0,0 +1,107 @@
+//
+// Created by lgv on 2025/7/14.
+//
+
+#pragma once
+#include "abstract_device.h"
+#include "cmvr/msgs/error_code.pb.h"
+#include "canbus/common/byte.h"
+namespace cmvr::device {
+
+    /**
+    * @class CanFrame
+    * @brief The class which defines the information to send and receive.
+    */
+    struct CanFrame {
+        /// Message id
+        uint32_t id;
+        /// Message length
+        uint8_t len;
+        /// Message content
+        uint8_t data[8];
+        /// Time stamp
+        struct timeval timestamp;
+
+        /**
+         * @brief Constructor
+         */
+        CanFrame() : id(0), len(0), timestamp{0} {
+            std::memset(data, 0, sizeof(data));
+        }
+
+        /**
+         * @brief CanFrame string including essential information about the message.
+         * @return The info string.
+         */
+        std::string CanFrameString() const {
+            std::stringstream output_stream("");
+            output_stream << "id:0x" << Byte::byte_to_hex(id)
+                    << ",len:" << static_cast<int>(len) << ",data:";
+            for (uint8_t i = 0; i < len; ++i) {
+                output_stream << Byte::byte_to_hex(data[i]);
+            }
+            output_stream << ",";
+            return output_stream.str();
+        }
+    };
+
+    const int CAN_RESULT_SUCC = 0;
+    const int CAN_ERROR_BASE = 2000;
+    const int CAN_ERROR_OPEN_DEVICE_FAILED = CAN_ERROR_BASE + 1;
+    const int CAN_ERROR_FRAME_NUM = CAN_ERROR_BASE + 2;
+    const int CAN_ERROR_SEND_FAILED = CAN_ERROR_BASE + 3;
+    const int CAN_ERROR_RECV_FAILED = CAN_ERROR_BASE + 4;
+
+    class AbstractCanbus : public AbstractDevice {
+    public:
+        AbstractCanbus(const XmlNode &cfg) : AbstractDevice(cfg) {}
+        ~AbstractCanbus() {}
+        /**
+         * @brief Send messages
+         * @param frames The messages to send.
+         * @param frame_num The amount of messages to send.
+        * @return The status of the sending action
+        */
+        virtual cmvr::msgs::ErrorCode send(const std::vector<CanFrame> &frames,
+                                               int32_t *const frame_num) = 0;
+
+        /**
+         * @brief Send a single message.
+         * @param frames A single-element vector containing only one message.
+         * @return The status of the sending single message action
+         */
+        virtual cmvr::msgs::ErrorCode sendSingleFrame(
+            const std::vector<CanFrame> &frames) {
+            CHECK_EQ(frames.size(), 1U)
+                << "frames size not equal to 1, actual frame size :" << frames.size();
+            int32_t n = 1;
+            return send(frames, &n);
+        }
+
+        /**
+         * @brief Receive messages
+         * @param frames The messages to receive.
+         * @param frame_num The amount of messages to receive.
+         * @return The status of the receiving action which is defined by
+         *         apollo::common::ErrorCode.
+         */
+        virtual cmvr::msgs::ErrorCode receive(std::vector<CanFrame> *const frames,
+                                                  int32_t *const frame_num) = 0;
+
+        /**
+         * @brief Get the error string.
+         * @param status The status to get the error string.
+         */
+        virtual std::string getErrorString(const int32_t status) = 0;
+    protected:
+        /// The CAN client is started.
+        bool is_started_ = false;
+
+        /// CAN clientstatus
+        cmvr::msgs::ErrorCode status_;
+
+        //
+        bool enable_log_{false};
+
+    };
+    }

include/devices/abstract_device.h

@@ -0,0 +1,42 @@
+//
+// Created by xtkuang on 2025/5/6.
+//
+
+#ifndef CMVR_ES_ABSTRACT_DEVICES_H
+#define CMVR_ES_ABSTRACT_DEVICES_H
+
+#include <thread>
+#include <jsoncpp/json/json.h>
+#include "rapidxml/xml_parser.h"
+#include "state_define.h"
+
+#pragma warning(disable:4996)
+#define GLOG_USE_GLOG_EXPORT
+#include <glog/logging.h>
+
+
+namespace cmvr::device {
+    class AbstractDevice {
+    public:
+        explicit AbstractDevice(const XmlNode& config) {cfg_ = config;}
+        virtual ~AbstractDevice() = default;
+
+        [[maybe_unused]] XmlNode getConfig() {return cfg_;}
+        virtual void getState() {}
+        virtual void init() {}
+        virtual void start() {}
+        virtual void stop() {}
+        virtual void update() {}
+
+        virtual void updateParams(const std::pair<std::string, std::string>& param) {}
+        virtual void customCommand(const Json::Value &cmd, Json::Value &feedback) {}
+
+    protected:
+        XmlNode cfg_;
+        std::string id_;   // 设备名称
+    };
+}
+
+
+
+#endif //CMVR_ES_ABSTRACT_DEVICES_H

include/devices/abstract_dexhand.h

@@ -0,0 +1,227 @@
+//
+// Created by xtkuang on 2025/5/8.
+//
+
+#ifndef CMVR_ES_ABSTRACT_DEXHAND_H
+#define CMVR_ES_ABSTRACT_DEXHAND_H
+#pragma once
+
+#include "abstract_device.h"
+
+namespace cmvr::device{
+    // 单个触觉点数据（16位无符号整数）
+    using TactilePoint = uint16_t;
+
+    // 手指触觉数据结构体（支持从数组任意位置解析）
+    struct FingerTactileData {
+        std::vector<std::vector<TactilePoint>> data; // 触觉数据二维数组
+        int rows;                                     // 行数
+        int cols;                                     // 列数
+        int byteSize;                                 // 总字节数（rows * cols * 2字节/点）
+        std::string name;                             // 部位名称
+
+        // 初始化数据数组
+        void init() {
+            data.resize(rows, std::vector<TactilePoint>(cols, 0));
+        }
+
+        // 从 std::vector<uint16_t> 中赋值数据
+        void assignFromVector(const std::vector<uint16_t>& values, int startIndex = 0) {
+            int index = startIndex;
+            for (int i = 0; i < rows; ++i) {
+                for (int j = 0; j < cols; ++j) {
+                    if (index < values.size()) {
+                        data[i][j] = values[index];
+                        index++;
+                    }
+                }
+            }
+        }
+    };
+
+    // 手掌触觉数据结构体（支持从数组任意位置解析）
+    struct PalmTactileData {
+        std::vector<std::vector<TactilePoint>> data; // 触觉数据二维数组
+        int rows = 8;                                 // 行数（掌心固定为8行）
+        int cols = 14;                                // 列数（掌心固定为14列）
+        int byteSize = 224;                           // 总字节数（8*14*2=224）
+        std::string name = "掌心";                    // 部位名称
+
+        // 初始化数据数组
+        void init() {
+            data.resize(rows, std::vector<TactilePoint>(cols, 0));
+        }
+
+        // 从 std::vector<uint16_t> 中赋值数据
+        void assignFromVector(const std::vector<uint16_t>& values, int startIndex = 0) {
+            int index = startIndex;
+            for (int j = 0; j < cols; ++j) {
+                for (int i = rows - 1; i >= 0; --i) {
+                    if (index < values.size()) {
+                        data[i][j] = values[index];
+                        index++;
+                    }
+                }
+            }
+        }
+    };
+
+    // 手指枚举（方便按手指类型解析）
+    enum FingerType {
+        PINKY,    // 小拇指
+        RING,     // 无名指
+        MIDDLE,   // 中指
+        INDEX,    // 食指
+        THUMB     // 大拇指
+    };
+
+    // 整只手的触觉传感器数据
+    struct HandTactileSensors {
+        // 五指的触觉数据（每根手指包含指端、指尖、指腹）
+        struct {
+            FingerTactileData tip;   // 指端
+            FingerTactileData finger; // 指尖
+            FingerTactileData pad;    // 指腹
+        } pinky, ring, middle, index; // 小拇指、无名指、中指、食指
+
+        // 大拇指（特殊：多一个指中）
+        struct {
+            FingerTactileData tip;    // 指端
+            FingerTactileData finger;  // 指尖
+            FingerTactileData middle;  // 指中
+            FingerTactileData pad;     // 指腹
+        } thumb;
+
+        // 掌心触觉数据
+        PalmTactileData palm;
+
+        // 初始化所有传感器数据（设置相对偏移量）
+        void init() {
+            // 小拇指（总370byte）
+            pinky.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "小拇指指端"};
+            pinky.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "小拇指指尖"};
+            pinky.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "小拇指指腹"};
+
+            // 无名指（总370byte）
+            ring.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "无名指指端"};
+            ring.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "无名指指尖"};
+            ring.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "无名指指腹"};
+
+            // 中指（总370byte）
+            middle.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "中指指端"};
+            middle.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "中指指尖"};
+            middle.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "中指指腹"};
+
+            // 食指（总370byte）
+            index.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "食指指端"};
+            index.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "食指指尖"};
+            index.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "食指指腹"};
+
+            // 大拇指（总420byte）
+            thumb.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指端"};
+            thumb.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指尖"};
+            thumb.middle = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指中"};
+            thumb.pad = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指指腹"};
+
+            // 掌心（总224byte）
+            palm = { .name = "掌心"};
+
+            // 初始化所有数据数组
+            pinky.tip.init();
+            pinky.finger.init();
+            pinky.pad.init();
+
+            ring.tip.init();
+            ring.finger.init();
+            ring.pad.init();
+
+            middle.tip.init();
+            middle.finger.init();
+            middle.pad.init();
+
+            index.tip.init();
+            index.finger.init();
+            index.pad.init();
+
+            thumb.tip.init();
+            thumb.finger.init();
+            thumb.middle.init();
+            thumb.pad.init();
+
+            palm.init();
+        }
+        // 按手指类型解析整个手指的数据
+        void parseFinger(FingerType fingerType, const std::vector<uint16_t>& values) {
+            switch (fingerType) {
+                case PINKY:
+                    pinky.tip.assignFromVector(values);
+                    pinky.finger.assignFromVector(values, 9);
+                    pinky.pad.assignFromVector(values, 9 + 96);
+                    break;
+
+                case RING:
+                    ring.tip.assignFromVector(values);
+                    ring.finger.assignFromVector(values, 9);
+                    ring.pad.assignFromVector(values, 9 + 96);
+                    break;
+
+                case MIDDLE:
+                    middle.tip.assignFromVector(values);
+                    middle.finger.assignFromVector(values, 9);
+                    middle.pad.assignFromVector(values, 9 + 96);
+                    break;
+
+                case INDEX:
+                    index.tip.assignFromVector(values);
+                    index.finger.assignFromVector(values, 9);
+                    index.pad.assignFromVector(values, 9 + 96);
+                    break;
+
+                case THUMB:
+                    thumb.tip.assignFromVector(values);
+                    thumb.finger.assignFromVector(values, 9);
+                    thumb.middle.assignFromVector(values, 9 + 96);
+                    thumb.pad.assignFromVector(values, 9 + 96 + 9);
+                    break;
+            }
+        }
+
+        // 解析手心（掌心）数据
+        void parsePalm(const std::vector<uint16_t>& values) {
+            palm.assignFromVector(values);
+        }
+
+        // 获取手指名称（用于调试）
+        std::string getFingerName(FingerType fingerType) {
+            switch (fingerType) {
+                case PINKY: return "小拇指";
+                case RING: return "无名指";
+                case MIDDLE: return "中指";
+                case INDEX: return "食指";
+                case THUMB: return "大拇指";
+                default: return "未知";
+            }
+        }
+    };
+    class AbstractDexHand: public AbstractDevice{
+    public:
+        explicit AbstractDexHand(const XmlNode& cfg): AbstractDevice(cfg) {}
+        ~AbstractDexHand() override = default;
+
+        virtual void getState(DexHandState &state) {}
+
+        // control
+        virtual void setPositions(const std::vector<int>& finger_joint_targets) {}
+        virtual void setAngles(const std::vector<int>& finger_joint_angles) {}
+        virtual void setVelocities(const std::vector<int>& finger_joint_velocities) {}
+        virtual void setPresetAct(int action_id) {}
+        virtual void execPresetAct(int action_id) {}
+        virtual void setForce(const std::vector<int>& finger_joint_force) {}
+        virtual HandTactileSensors& getSensorData() { return hand_tactile_sensors_;}
+    protected:
+        DexHandState status_;
+        HandTactileSensors hand_tactile_sensors_;
+    };
+}
+
+#endif //CMVR_ES_ABSTRACT_DEXHAND_H

include/devices/abstract_gripper.h

@@ -0,0 +1,30 @@
+//
+// Created by xtkuang on 2025/5/8.
+//
+
+#ifndef CMVR_ES_ABSTRACT_GRIPPER_H
+#define CMVR_ES_ABSTRACT_GRIPPER_H
+#pragma once
+
+#include "abstract_device.h"
+
+namespace cmvr::device{
+
+    class AbstractGripper: public AbstractDevice {
+    public:
+        explicit AbstractGripper(const XmlNode &cfg): AbstractDevice(cfg) {}
+        ~AbstractGripper() override = default;
+
+        virtual void getState(GripperState &state) {}
+
+        virtual void openGripper(float vel) {}
+        virtual void closeGripper(float vel) {}
+        virtual void setPosition(float position, float vel) {}
+        virtual void setForce(float value) {}
+
+    protected:
+        GripperState state_;
+    };
+}
+
+#endif //CMVR_ES_ABSTRACT_GRIPPER_H

include/devices/abstract_microphone.h

@@ -0,0 +1,30 @@
+//
+// Created by xtkuang on 2025/5/8.
+//
+
+#ifndef CMVR_ES_ABSTRACT_MICROPHONE_H
+#define CMVR_ES_ABSTRACT_MICROPHONE_H
+#pragma once
+
+#include "abstract_device.h"
+
+namespace cmvr::device{
+    class AbstractMicrophone: public AbstractDevice {
+    public:
+        explicit AbstractMicrophone(const XmlNode &config): AbstractDevice(config) {}
+        ~AbstractMicrophone() override = default;
+
+        virtual void getState(MicrophoneState &state) {}
+        virtual void startRecording(const std::string& outputFilePath) {}
+        virtual void stopRecording() {}
+        virtual void pause() {}
+        virtual void resume() {}
+        virtual void setVolume(const int volume) {}
+        virtual int getVolume() {return 0;}
+
+    protected:
+        MicrophoneState state_{};
+    };
+}
+
+#endif //CMVR_ES_ABSTRACT_MICROPHONE_H

include/devices/abstract_motor.h

@@ -0,0 +1,180 @@
+//
+// Created by xtkuang on 2025/7/30.
+//
+
+#ifndef CMVR_ES_ABSTRACT_MOTOR_H
+#define CMVR_ES_ABSTRACT_MOTOR_H
+
+#pragma once
+
+#include "abstract_device.h"
+#include "utils/dynamics/joint.h"
+#include "hardware/can/motor_protocol/abstractmotorprotocol.h"
+#include <mutex>
+
+namespace cmvr::device{
+
+    class AbstractMotor: public AbstractDevice {
+
+        typedef struct {
+
+
+        } MotorProfile;
+
+        // xml 使用
+        struct MotorInfo {
+            int id;
+            std::string joint_name;
+            float limitQ;
+            float limitQd;
+            std::string protocol_type;
+            std::string canGroupId;
+        };
+
+        typedef struct {
+            double position;
+            double velocity;
+            double acceleration;
+            double torque;
+            double temperature;
+            double voltage;
+            double current;
+            std::string error_msg;
+        } JointStatus;
+
+    public:
+        explicit AbstractMotor(const XmlNode &config): AbstractDevice(config) {}
+        AbstractMotor(const XmlNode &config,uint8_t node_id): AbstractDevice(config) ,node_id_(node_id) {}
+        ~AbstractMotor() override = default;
+
+        virtual void setMode(msgs::RunMode mode) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            protocol_->setMode(node_id_, mode);
+        }
+
+        virtual msgs::RunMode getMode() {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            return protocol_->getMode(node_id_);
+        }
+
+        void torqueOff() {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            protocol_->torqueOff(node_id_);
+        }
+
+        virtual void setLimitQ(double ub, double lb) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            protocol_->setLimitQ(node_id_, ub,lb);
+        }
+        virtual void setLimitQd(double qd) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            protocol_->setLimitQd(node_id_, qd);
+        }
+        virtual void setLimitQdd(double u_qdd,double l_qdd) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            protocol_->setLimitQdd(node_id_, u_qdd,l_qdd);
+        }
+        // virtual void setLimitTau(double tau) = 0;
+        // virtual void setLimitCurrent(double tau) = 0;
+        virtual void brake() {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            protocol_->brake(node_id_);
+        }
+        /**
+         *
+         * @param q unit : rad
+         */
+        virtual void setQ(double q) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            protocol_->setQ(node_id_, q);
+        }
+
+        virtual bool calibrateZeroQ() {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            return  protocol_->calibrateZeroQ(node_id_);
+        }
+
+        virtual bool reachedTargetQ() {
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            return  protocol_->reachedTargetQ(node_id_);
+        }
+        // rad /s
+        virtual void setQd(double qd) {
+            std::scoped_lock lock(mtx_);
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            return  protocol_->setQd(node_id_,qd);
+        }
+        // virtual void setQdd(double qdd) = 0;  // rad /s^2
+        // virtual void setTau(double tau) = 0;   // N m
+        // virtual void clear_err() = 0;
+        // virtual void getStatus() = 0;
+        // virtual MotorProfile getProfile() = 0;
+
+        virtual  double getQ() {
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            return  protocol_->getQ(node_id_);
+        }
+
+        virtual  double getQd() {
+            if (!protocol_) {
+                throw std::runtime_error("Protocol not set for motor");
+            }
+            return  protocol_->getQd(node_id_);
+        }
+
+
+        // 使用的通讯协议
+        virtual void setProtocol(std::shared_ptr<hardware::AbstractMotorProtocol> protocol) {
+            std::scoped_lock lock(mtx_);
+            protocol_ = std::move(protocol);
+            protocol_->initNode(node_id_);
+        }
+
+        uint8_t id() const {
+            return node_id_;
+        }
+        std::string jointName() const {
+            return info_.joint_name;
+        }
+    protected:
+        mutable std::mutex mtx_;
+        MotorInfo info_{};
+        uint8_t node_id_;
+        // 使用的通讯协议
+        std::shared_ptr<hardware::AbstractMotorProtocol> protocol_;
+    };
+}
+#endif //CMVR_ES_ABSTRACT_MOTOR_H

include/devices/abstract_robot.h

@@ -0,0 +1,131 @@
+//
+// Created by xtkuang on 2025/5/8.
+//
+
+#ifndef CMVR_ES_ABSTRACT_ROBOT_H
+#define CMVR_ES_ABSTRACT_ROBOT_H
+#pragma once
+
+#include "jsoncpp/json/json.h"
+#include "abstract_device.h"
+#include "utils/solver/qp_solver.h"
+#include "cmvr/msgs/geometry.pb.h"
+#include "cmvr/msgs/motor.pb.h"
+#include "controller/controller_manager.h"
+
+namespace cmvr::device{
+
+     struct JointPoint{
+        std::string joint_name;
+        double rad;             // rad
+        double vel;             // rad / s
+
+        JointPoint(const std::string& name, double r, double v)
+        : joint_name(name), rad(r), vel(v) {}
+         JointPoint(const std::string& name, double r)
+        : joint_name(name), rad(r), vel(0) {}
+         JointPoint()
+       : joint_name(""), rad(0), vel(0) {}
+    } ;
+
+    typedef struct {
+        std::string joint_name;
+        double vel;             // rad / s
+    } JointVelocityCommand;
+
+    typedef struct {
+        std::string joint_name;
+        double current;         //
+    } JointCurrentCommand;
+
+    class AbstractRobot: public AbstractDevice {
+    public:
+        explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {}
+        ~AbstractRobot() override=default;
+
+        virtual int getDOF() { throw std::runtime_error("Not implemented"); }
+
+        virtual std::vector<std::string> getJointNames() { throw std::runtime_error("Not implemented"); }
+
+        virtual std::unordered_map<std::string,double> getJointQ() const { throw std::runtime_error("Not implemented"); }
+
+        /**
+         *
+         * @param joint_qs 函数会根据 joint_qs 的 joint name 去获取值
+         */
+        virtual void getJointQ(std::unordered_map<std::string,double> &joint_qs) const {throw std::runtime_error("Not implemented");}
+        virtual std::vector<std::string> getLinkNames() { throw std::runtime_error("Not implemented"); }
+
+        virtual void getJointsState(std::vector<JointState>& states)  = 0;
+        virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); }
+
+        virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}
+
+        virtual void torqueOn() { throw std::runtime_error("Not implemented"); }
+        virtual void torqueOn(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }
+
+        virtual void torqueOff() { throw std::runtime_error("Not implemented"); }
+        virtual void torqueOff(const std::string &joint_name) { throw std::runtime_error("Not implemented"); }
+
+        virtual void eStop() { throw std::runtime_error("Not implemented"); }
+
+        virtual void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
+
+        virtual void moveJ(std::vector<JointPoint> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
+        virtual void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
+        virtual void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.5, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
+
+        virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
+
+        virtual void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1)  { throw std::runtime_error("Not implemented"); }
+
+        virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
+
+        virtual void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
+
+        virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
+        virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); }
+
+        virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
+
+        virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
+
+        virtual void followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
+
+        virtual void followJointTrajectory(std::vector<std::vector<JointPoint>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
+
+        virtual void followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) { throw std::runtime_error("Not implemented"); }
+
+        virtual void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) { throw std::runtime_error("Not implemented"); }
+
+        virtual void servoJ(std::vector<double> &joints, double dt) { throw std::runtime_error("Not implemented"); }
+
+        virtual void servoJ(std::vector<JointPoint> &joints, double dt) { throw std::runtime_error("Not implemented"); }
+        virtual void servoJ(std::vector<JointPoint> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); }
+        virtual void servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
+        virtual void servoDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose,double vel = 0.1, double acc = 0.1) { throw std::runtime_error("Not implemented"); }
+
+
+        virtual void servoL(math::Pose3d &pose, double dt) { throw std::runtime_error("Not implemented"); }
+
+        virtual void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { throw std::runtime_error("Not implemented"); }
+
+        virtual void calibrateZeroQ(const std::string &joint_name) = 0;
+
+        void setToolFrame(const std::string& toolFrame)
+        {
+            toolFrame_ = toolFrame;
+        }
+
+        virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
+        virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0;
+    protected:
+        int dof_{};
+        RobotState state_{};
+        std::string toolFrame_;
+
+        std::shared_ptr<ControllerManager> controller_manager_;
+    };
+}
+
+#endif //CMVR_ES_ABSTRACT_ROBOT_H

include/devices/abstract_speaker.h

@@ -0,0 +1,30 @@
+//
+// Created by xtkuang on 2025/5/8.
+//
+
+#ifndef CMVR_ES_ABSTRACT_SPEAKER_H
+#define CMVR_ES_ABSTRACT_SPEAKER_H
+#pragma once
+
+#include "abstract_device.h"
+
+namespace cmvr::device{
+    class AbstractSpeaker: public AbstractDevice {
+    public:
+        explicit AbstractSpeaker(const XmlNode &config): AbstractDevice(config) {}
+        ~AbstractSpeaker() override = default;
+
+        virtual void getState(SpeakerState &state) {}
+        virtual void play(const std::string &file_path) {}
+        virtual void setVolume(int volume) {}
+        [[nodiscard]] virtual int getVolume() const {return 0;}
+        virtual void pause() {}
+        virtual void resume() {}
+
+    protected:
+        SpeakerState state_{};
+
+    };
+}
+
+#endif //CMVR_ES_ABSTRACT_SPEAKER_H

include/devices/state_define.h

@@ -0,0 +1,156 @@
+//
+// Created by xtkuang on 2025/5/7.
+//
+
+#ifndef CMVR_ES_STATE_DEFINE_H
+#define CMVR_ES_STATE_DEFINE_H
+
+#include <atomic>
+#include <cmath>
+#include <iostream>
+#include <vector>
+#include <unordered_map>
+#include <set>
+
+#include "utils/math/geometry.h"
+
+
+namespace cmvr::device{
+    enum Lifecycle{
+        INIT,   // 初始化：设备刚创建或刚上电，正在初始化
+        READY,  // 就绪：设备已初始化完成，可以开始工作
+        RUNNING,// 运行中：设备正在正常工作或采集
+        ERROR,  // 错误：设备发生故障或异常，不能正常工作
+        STOP,   // 停止：设备已停止工作，但没有严重错误
+        ESTOP   // 急停：设备处于紧急停止状态，通常需要人工干预才能恢复
+    };
+
+    enum AudioFormat {
+        WAV,
+        MP3,
+        AAC,
+        OGG,
+        FLAC,
+        UNKNOWN
+    };
+
+    // ------------------------------------- AGV -------------------------------------
+    typedef struct{
+
+    } AGVState;
+
+    // ------------------------------------- robot -------------------------------------
+    typedef enum {
+        FORWARD, BACKWARD,
+        X_POSITIVE,   // X轴正向
+        X_NEGATIVE,   // X轴负向
+        Y_POSITIVE,   // Y轴正向
+        Y_NEGATIVE,   // Y轴负向
+        Z_POSITIVE,   // Z轴正向
+        Z_NEGATIVE,   // Z轴负向
+       // 角度方向也可以添加
+        ROTATE_X,     // 绕X轴旋转
+        ROTATE_Y,     // 绕Y轴旋转
+        ROTATE_Z      // 绕Z轴旋转
+    } RobotJointIndexDirection;
+
+
+
+    /// robot cartesian index
+    typedef enum {
+        X, Y, Z, RX, RY, RZ
+    } RobotCartesian;
+
+    typedef struct {
+        std::string name;
+        bool is_ready;
+        bool power_on;
+        double position;
+        double velocity;
+        double current;
+        double torque;
+        double target_position;
+        double target_velocity;
+        int target_feedback_gain;
+        double target_feedforward_torque;
+        double temperature;
+        double voltage;
+    } JointState;
+
+    typedef struct{
+        std::vector<double> temperature;
+        std::vector<double> voltage;
+        std::vector<double> joint_positions;
+        bool error;
+        std::string error_msg;
+    } RobotState;
+
+    struct CameraState {
+        bool is_initialized;     ///< 是否已初始化
+        bool is_opened;
+        bool is_streaming;      ///< 是否正在采集视频数据
+        bool is_recording;
+        bool is_error;
+        std::string error_message;
+        int fps;
+        int width;
+        int height;
+    };
+
+    /// @brief 从 XML 或其他配置中读取的相机网络参数
+    struct CameraConfig {
+        std::string ipAddress;               ///< 相机 IP 地址
+    };
+
+
+    typedef struct{
+        bool is_initialized;     ///< 是否已初始化
+        bool is_running;
+        bool is_recording;
+        bool is_error;
+        int volume;                 // 0~100, 支持软音量调节
+        std::string error_message;
+    } MicrophoneState;
+
+    typedef struct{
+
+    } GripperState;
+
+    typedef struct{
+        int angle;              //自由度角度
+        int speed;              //自由度速度
+        int force;              //实际受力
+        int position;           //执行器位置
+        int current;            //执行器实际电流
+        int temperature;        //执行器温度
+        int error;     //执行器故障信息
+        std::vector<std::string> error_message;
+    } RH56DFTPDexHand;
+
+    typedef struct{
+        bool is_initialized;                //是否初始化
+        RH56DFTPDexHand hands[6];           // 包含6个自由度状态
+    } DexHandState;
+
+    typedef struct {
+        float voltage;            //< 电压（V）
+        float current;            //< 电流（A）
+        float temperature;        //< 温度（°C）
+        int charge_percentage;    //< 剩余电量（%）
+        bool is_charging;         //< 是否正在充电
+        Lifecycle health;         //< 电池健康状态
+        std::string vendor_info;  //< 厂商信息（可选）
+    } BatteryState;
+
+    typedef struct {
+        bool is_initialized;
+        bool is_running;
+        bool is_decoding;
+        bool is_paused;
+        int volume;                 // 0~100, 支持软音量调节
+    } SpeakerState;
+
+}
+
+
+#endif //CMVR_ES_STATE_DEFINE_H

include/hardware/can/can_group.h

@@ -0,0 +1,35 @@
+//
+// Created by linbo on 2025/10/22.
+//
+
+#pragma once
+
+#include "motor_protocol/motorprotocolmanager.h"
+
+namespace cmvr::hardware
+{
+    class CanGroup
+    {
+    public:
+        explicit CanGroup(const XmlNode &cfg);
+        ~CanGroup() = default;
+
+        void init();
+
+        std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
+        uint8_t getNodeId(const std::string& joint_name);
+
+        const MotorProtocolManager::MotorInfo& getMotorInfo(const std::string& joint_name);
+    private:
+        XmlNode cfg_;
+        std::string id_;
+        unsigned int channelId_;
+        bool enable_;
+        std::shared_ptr<MotorProtocolManager> motor_protocol_manager;
+
+        std::shared_ptr<device::AbstractCanbus> can_client_{nullptr};
+        std::shared_ptr<device::CanReceiver<msgs::RobotDetail>> can_receiver_{nullptr};
+        std::shared_ptr<device::CanSender<msgs::RobotDetail>> can_sender_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail>> message_manager_{nullptr};
+    };
+}

include/hardware/can/can_manager.h

@@ -0,0 +1,28 @@
+//
+// Created by linbo on 2025/10/22.
+//
+
+#ifndef CMVR_ES_CAN_MANAGER_H
+#define CMVR_ES_CAN_MANAGER_H
+#include <unordered_map>
+#include <string>
+
+#include "rapidxml/rapidxml.hpp"
+#include "can_group.h"
+namespace cmvr::hardware
+{
+    class CanManager
+    {
+
+    public:
+        explicit CanManager(const XmlNode &cfg);
+        ~CanManager() = default;
+
+        std::shared_ptr<CanGroup> getCanGroup(const std::string &canGroupID);
+    private:
+        //CanGroup
+        std::unordered_map<std::string,std::shared_ptr<CanGroup>> canGroups_;
+    };
+}
+
+#endif //CMVR_ES_CAN_MANAGER_H

include/hardware/can/motor_protocol/abstractmotorprotocol.h

@@ -0,0 +1,65 @@
+//
+// Created by linbo on 2025/10/22.
+//
+
+#ifndef CMVR_ES_ABSTRACTMOTORPROTOCOL_H
+#define CMVR_ES_ABSTRACTMOTORPROTOCOL_H
+#include "rapidxml/xml_parser.h"
+#include "cmvr/msgs/canopen.pb.h"
+#include "cmvr/msgs/motor.pb.h"
+namespace cmvr::hardware
+{
+    class AbstractMotorProtocol
+    {
+
+    public:
+        enum class CommProto : uint8_t {
+            CANOPEN = 1,
+            CUSTOM = 2
+        };
+    public:
+        explicit AbstractMotorProtocol(const XmlNode &cfg)
+        {
+            cfg_ = cfg;
+            id_ = cfg.getAttrString("id");
+            joint_name_ = cfg.getAttrString("joint_name");
+            limitQLb_ = cfg.getAttrDefault("limitQLb_",3.14f);
+            limitQUb_ = cfg.getAttrDefault("limitQUb_",3.14f);
+            limitQd = cfg.getAttrDefault("limitQd",3.0f);
+        }
+        virtual ~AbstractMotorProtocol() = default;
+
+        /**
+         * @brief 用于初始化与通讯协议相关的设置
+         * @param node_id 电机Id
+         * @return
+         */
+        virtual bool initNode(uint8_t node_id) = 0;
+
+        virtual void setQ(uint8_t node_id, double angle_rad) = 0;
+        virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
+        virtual msgs::RunMode getMode(uint8_t node_id) = 0;
+        virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;
+        virtual void setLimitQd(uint8_t node_id,double qd) = 0;
+        virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0;
+        virtual bool calibrateZeroQ(uint8_t node_id) = 0;
+        virtual bool reachedTargetQ(uint8_t node_id) = 0;
+        virtual void setQd(uint8_t node_id, double qd) = 0;
+        virtual void setQdd(uint8_t node_id,double qdd) = 0;
+        virtual void brake(uint8_t node_id) = 0;
+        virtual void torqueOff(uint8_t node_id) = 0;
+
+        virtual double getQ(uint8_t node_id) = 0;
+        virtual double getQd(uint8_t node_id) = 0;
+        CommProto comm_proto{CommProto::CANOPEN};
+    protected:
+        XmlNode cfg_;
+        std::string id_;
+        std::string joint_name_;
+        double limitQLb_;
+        double limitQUb_;
+        double limitQd;
+    };
+}
+
+#endif //CMVR_ES_ABSTRACTMOTORPROTOCOL_H

include/hardware/can/motor_protocol/motorprotocolmanager.h

@@ -0,0 +1,54 @@
+//
+// Created by linbo on 2025/10/22.
+//
+
+#ifndef CMVR_ES_MOTORPROTOCOLMANAGER_H
+#define CMVR_ES_MOTORPROTOCOLMANAGER_H
+
+#include <unordered_map>
+#include <memory>
+#include "abstractmotorprotocol.h"
+#include "devices/abstract_canbus.h"
+#include "canbus/can_comm/can_sender.h"
+#include "canbus/can_comm/can_receiver.h"
+#include "canbus/can_comm/message_manager.h"
+#include "cmvr/msgs/robot_detail.pb.h"
+
+namespace cmvr::hardware
+{
+    class MotorProtocolManager
+    {
+    public:
+        struct MotorInfo
+        {
+            uint8_t node_id;        //电机ID
+            std::string jointName;  //关节名称
+            double limitQLb;        //逆时针限位
+            double limitQUb;        //顺时针限位
+            double limitQd;         //加速度限制
+            std::shared_ptr<AbstractMotorProtocol> motorProtocol;
+        };
+    public:
+        explicit MotorProtocolManager(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
+                                std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
+        ~MotorProtocolManager() = default;
+
+        uint8_t getNodeId(const std::string& joint_name);
+        //根据协议类型获取协议实例对象
+        std::shared_ptr<AbstractMotorProtocol> getMotorProtocol(const std::string &protocolType);
+
+        const MotorInfo& getMotorInfo(const std::string& joint_name);
+    private:
+        std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
+
+        std::shared_ptr<AbstractMotorProtocol> ti5Protocol_{nullptr};
+        //协议类型，协议实例对象
+        std::unordered_map<std::string, std::shared_ptr<AbstractMotorProtocol>> motor_protocols_;
+        //协议类型，电机参数
+        std::unordered_map<std::string, std::vector<MotorInfo>> motors_;
+    };
+}
+
+
+#endif //CMVR_ES_MOTORPROTOCOLMANAGER_H

include/hardware/can/motor_protocol/ti5motorprotocol.h

@@ -0,0 +1,110 @@
+//
+// Created by linbo on 2025/10/22.
+//
+
+#pragma once
+
+#include "rapidxml/xml_parser.h"
+#include "abstractmotorprotocol.h"
+#include "cmvr/msgs/motor.pb.h"
+#include "motor/motor_protocol_interface.h"
+#include "devices/abstract_canbus.h"
+#include "canbus/can_comm/can_receiver.h"
+#include "canbus/can_comm/can_sender.h"
+#include "canbus/can_comm/message_manager.h"
+#include "cmvr/msgs/error_code.pb.h"
+#include "cmvr/msgs/robot_detail.pb.h"
+
+#include "canbus/canopen/sdo_request_protocol.h"
+#include "canbus/canopen/sync_protocol.h"
+#include "canbus/canopen/nmt_request_protocol.h"
+#include "motor/ti5_motor/canopen/protocol/ti5_motor_rpdo1.h"
+#include <cmath>
+namespace cmvr::hardware{
+    class Ti5MotorProtocol final : public AbstractMotorProtocol {
+    public:
+        explicit Ti5MotorProtocol(const XmlNode &cfg);
+        Ti5MotorProtocol(const XmlNode &cfg,std::shared_ptr<device::CanSender<msgs::RobotDetail>> sender,
+                                std::shared_ptr<device::MessageManager<msgs::RobotDetail>> manager);
+        ~Ti5MotorProtocol() override;
+
+        bool initNode(uint8_t node_id) override;
+
+        void setMode(uint8_t node_id, msgs::RunMode mode) override;
+
+        void setQ(uint8_t node_id, double angle_rad) override;
+        void setLimitQ(uint8_t node_id, double ub, double lb) override;
+        void setLimitQd(uint8_t node_id, double qd) override;
+        void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override;
+        bool calibrateZeroQ(uint8_t node_id) override;
+        void brake(uint8_t node_id) override;
+        bool reachedTargetQ(uint8_t node_id) override;
+
+        double getQ(uint8_t node_id) override;
+        double getQd(uint8_t node_id) override;
+
+        void setQd(uint8_t node_id, double qd) override;
+        void setQdd(uint8_t node_id, double qdd) override;
+
+        void torqueOff(uint8_t node_id) override;
+
+        void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10);
+        void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index,
+                      msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10);
+
+        void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
+        void configPdo(uint8_t node_id);
+
+        std::unique_ptr<msgs::RobotDetail> GetRobotDetail() {
+            auto data_ptr = std::make_unique<msgs::RobotDetail>();
+            message_manager_->GetSensorData(data_ptr.get());
+            return data_ptr;
+        }
+
+        msgs::RunMode getMode(uint8_t node_id) override {
+            return cur_mode_[node_id];
+        }
+
+
+    private:
+        void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
+        void setPPTargetPosByPdo(uint8_t node_id, int32_t pos);
+        void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos);
+        void configTPDO1(uint8_t node_id);
+        void configTPDO2(uint8_t node_id);
+        // 目标位置 607A + 控制字 6040
+        void configRPDO1(uint8_t node_id, bool start);
+        bool waitUntil(std::function<bool()> condition, int timeout_ms) {
+            auto start = std::chrono::steady_clock::now();
+            while (!condition()) {
+                std::this_thread::sleep_for(std::chrono::milliseconds(10));
+                if (std::chrono::steady_clock::now() - start > std::chrono::milliseconds(timeout_ms))
+                    return false;
+            }
+            return true;
+        }
+    private:
+        static constexpr double GearRatio = 101.0; // 电机减速比
+        static constexpr double RADTODEG = 180.0 / M_PI;
+        std::shared_ptr<device::AbstractCanbus> can_client_{nullptr};
+
+        // key node_id
+        std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
+        std::unordered_map<uint8_t,uint32_t> last_Qd_{};
+        std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
+        std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
+        std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
+
+        // nmt
+        device::NmtRequestProtocol<msgs::RobotDetail> *nmt_command_{nullptr};
+
+        //sync
+        device::SyncProtocol<msgs::RobotDetail> *sync_command_{nullptr};
+
+        // sdo
+        std::map<uint8_t, device::SdoRequestProtocol<msgs::RobotDetail> *> sdo_commands_{};
+
+        // rpdo1
+        std::map<uint8_t, device::motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
+    };
+}

include/hardware/serial/RS485/esp32_serial_port.h

@@ -0,0 +1,33 @@
+//
+// Created by tankaitao on 2025/6/27.
+//
+
+#ifndef SERIAL_PORT_H
+#define SERIAL_PORT_H
+#include <string>
+#include <vector>
+#include <cstdint>
+
+
+namespace cmvr
+{
+    class SerialPort{
+    public:
+        SerialPort()=default;
+        ~SerialPort();
+
+        bool open(const std::string &port_name,unsigned int baud_rate);
+        void close();
+        bool is_open();
+        bool wakeupESP32(const std::string &port_name);
+        bool send(const std::vector<uint8_t>& data);
+        bool sendRawServoData(const std::vector<uint8_t>& raw);
+        static void printPacket(const std::vector<uint8_t>& packet);
+    private:
+        int fd_ = -1;
+    };
+}
+
+
+
+#endif //SERIAL_PORT_H

include/hardware/serial/serial_manager.h

@@ -0,0 +1,21 @@
+//
+// Created by linbo on 2025/10/22.
+//
+
+#ifndef CMVR_ES_SERIAL_MANAGER_H
+#define CMVR_ES_SERIAL_MANAGER_H
+#include "rapidxml/xml_parser.h"
+namespace cmvr::hardware
+{
+    class SerialManager
+    {
+    public:
+        explicit SerialManager(const XmlNode &cfg);
+        ~SerialManager() = default;
+
+    private:
+
+    };
+}
+
+#endif //CMVR_ES_SERIAL_MANAGER_H

include/hardware/serial_interface.h

@@ -0,0 +1,120 @@
+//
+// Created by xtkuang on 2025/5/6.
+//
+
+#ifndef CMVR_ES_SERIAL_INTERFACE_H
+#define CMVR_ES_SERIAL_INTERFACE_H
+
+#include <iostream>
+#include <sstream>
+#include <string>
+#include <vector>
+#include <boost/asio.hpp>
+#include <iomanip>
+#include <thread>
+
+using namespace std;
+using namespace boost::asio;
+
+namespace cmvr {
+    // 定义通信模式枚举
+    enum class CommunicationMode {
+        RS485,
+        CAN
+    };
+
+    // 寄存器字典
+    const struct {
+        const char* name;
+        int address;
+    } regdict[] = {
+        {"defaultSpeedSet",1032},
+        {"defaultForceSet",1044},
+        {"posSet",1474},
+        {"angleSet", 1486},
+        {"forceSet", 1498},
+        {"speedSet", 1522},
+        {"angleAct", 1546},
+        {"posAct",1534},
+        {"forceAct", 1582},
+        {"current",1594},
+        {"error",1606},
+        {"temperature",1618},
+        {"finger_one_touch",3000},
+        {"finger_two_touch",3370},
+        {"finger_the_touch",3740},
+        {"finger_or_touch",4110},
+        {"finger_fiv_touch",4480},
+        {"finger_palm_touch",4900}
+    };
+
+    // 串口通信接口类
+    class serial_interface {
+    public:
+        serial_interface(const string& port, unsigned int baudrate, CommunicationMode mode);
+
+        void write(const vector<unsigned char>& data);
+        vector<unsigned char> read(size_t length);
+
+        bool isInitialized();
+
+        CommunicationMode getCommunicationMode();
+
+    private:
+        void on_timeout(const boost::system::error_code& ec);
+        void on_read_complete(const boost::system::error_code& ec
+            , size_t bytes_transferred, boost::system::error_code* out_ec, size_t* out_bytes_transferred);
+
+    private:
+        bool is_initialized = false;
+        io_context io;
+        std::shared_ptr<serial_port> serial;
+        CommunicationMode comm_mode;
+        steady_timer timer;
+        bool timeout;
+    };
+
+    // 根据寄存器名称获取地址
+    int getAddressByName(const string& reg_name);
+
+    // 写寄存器函数
+    void write_register(serial_interface& serial, int address_decimal, const string& id_value, const vector<int>& values_to_write);
+
+    /**
+     * 从串口读取寄存器数据
+     *
+     * @param serial 串口通信接口引用
+     * @param address_decimal 十进制表示的寄存器起始地址
+     * @param id_value 设备ID字符串值
+     * @param register_length 寄存器长度（字节数）
+     * @param length_to_read 需要读取的数据长度（字节数）
+     * @return 包含读取字节数据的向量
+     */
+    std::vector<unsigned char> read_register(serial_interface& serial, int address_decimal, const string& id_value
+            ,size_t register_length, size_t length_to_read);
+    std::vector<int> read_register_can(serial_interface& serial, int address_decimal, const std::string& id_value
+        , size_t register_length, size_t length_to_read, bool parse_as_short = true);
+    std::vector<unsigned char> read_register_can_single(serial_interface& serial, int address_decimal
+        , const string& id_value, size_t register_length, size_t length_to_read);
+    /**
+     * 将字节数据解析为整数数组
+     *
+     * @param data 待解析的字节数据向量
+     * @param start_byte 开始解析的起始字节位置
+     * @param register_length 每个寄存器的长度（字节数）
+     * @param per_byte 每个数据点占用的字节数
+     * @return 解析后的整数值向量
+     */
+    std::vector<int> parse_register(const std::vector<unsigned char>& data,int start_byte, size_t register_length,int per_byte);
+
+    /**
+     * 将错误码解析为错误描述列表
+     *
+     * @param errorCode 待解析的错误码
+     * @return 包含错误描述信息的字符串向量
+     * @note 每个错误码可能对应多个错误描述
+     */
+    std::vector<std::string> parse_error(unsigned char errorCode);
+} // cmvr
+
+#endif //CMVR_ES_SERIAL_INTERFACE_H

include/hardware_manager/hardware_factory.h

@@ -0,0 +1,25 @@
+//
+// Created by linbo on 2025/10/21.
+//
+
+#ifndef CMVR_ES_HARDWARE_FACTORY_H
+#define CMVR_ES_HARDWARE_FACTORY_H
+
+#include "rapidxml/xml_parser.h"
+#include "hardware/can/can_manager.h"
+namespace cmvr::hardware {
+
+    class HardwareFactory {
+
+
+    public:
+        HardwareFactory() = default;
+
+        template<typename HardwareType>
+        std::shared_ptr<HardwareType> create(const XmlNode& cfg);
+    private:
+        std::shared_ptr<CanManager> create_can_(const XmlNode& cfg);
+    };
+}
+
+#endif //CMVR_ES_HARDWARE_FACTORY_H

include/hardware_manager/hardware_manager.h

@@ -0,0 +1,40 @@
+//
+// Created by linbo on 2025/10/21.
+//
+
+#ifndef CMVR_ES_HARDWARE_MANAGER_H
+#define CMVR_ES_HARDWARE_MANAGER_H
+#include "rapidxml/xml_parser.h"
+#include <glog/logging.h>
+#include <mutex>
+#include "hardware_factory.h"
+#include "hardware/can/can_manager.h"
+namespace cmvr::hardware
+{
+    class HardWareManager
+    {
+    public:
+        HardWareManager(const HardWareManager&) = delete;
+        HardWareManager& operator=(const HardWareManager&) = delete;
+
+        static HardWareManager& getInstance(const XmlNode &cfg);
+        static HardWareManager& getInstance();
+        static void destroyInstance();
+
+        void initHardWare();
+
+        std::shared_ptr<CanGroup> getCanGroup(const std::string& canGroupID);
+
+
+    private:
+        explicit HardWareManager(const XmlNode &cfg);
+    private:
+        XmlNode cfg_;
+        static std::once_flag init_flag_;
+        static std::shared_ptr<HardWareManager> instance_;
+        std::shared_ptr<HardwareFactory> hardware_factory_;
+        std::shared_ptr<CanManager> can_manager_;
+    };
+}
+
+#endif //CMVR_ES_HARDWARE_MANAGER_H

include/monitor/abstract_monitor.h

@@ -0,0 +1,42 @@
+//
+// Created by xtkuang on 2025/6/6.
+//
+
+#ifndef ABSTRACT_MONITOR_H
+#define ABSTRACT_MONITOR_H
+
+#include <mutex>
+#include <glog/logging.h>
+#include "rapidxml/xml_parser.h"
+#include "../utils/base/timer.h"
+
+namespace cmvr::monitor{
+    class AbstractMonitor {
+    public:
+        explicit AbstractMonitor(const XmlNode &cfg){
+            cfg_ = cfg;
+            freq_ = cfg_.getAttrDefault("freq", 1);
+            timer_ = std::make_unique<FDTimer>();
+            timer_->start(std::chrono::milliseconds(1000/freq_), [this](){run();});
+        }
+
+        virtual ~AbstractMonitor() {timer_->stop();}
+
+        void run() {
+            if (!check_()) {
+                execution_();
+            }
+        }
+
+    protected:
+        XmlNode cfg_;
+        int freq_;
+        std::unique_ptr<FDTimer> timer_;
+
+        virtual bool check_() = 0;
+        virtual bool execution_() = 0;
+    };
+
+}
+
+#endif //ABSTRACT_MONITOR_H

include/monitor/monitor_factory.h

@@ -0,0 +1,27 @@
+//
+// Created by xtkuang on 2025/6/6.
+//
+
+#ifndef MONITOR_FACTORY_H
+#define MONITOR_FACTORY_H
+
+
+#include "monitor/abstract_monitor.h"
+
+namespace cmvr::monitor
+{
+    class MonitorFactory
+    {
+    public:
+        MonitorFactory() = default;
+        template <typename MonitorType>
+        std::shared_ptr<MonitorType> create(const XmlNode& cfg);
+        std::shared_ptr<AbstractMonitor> create(const XmlNode& cfg);
+    };
+
+
+}
+
+
+
+#endif //MONITOR_FACTORY_H

include/monitor/monitor_manager.h

@@ -0,0 +1,37 @@
+//
+// Created by xtkuang on 2025/6/9.
+//
+
+#ifndef MONITOR_MANAGER_H
+#define MONITOR_MANAGER_H
+
+#include <unordered_map>
+#include "rapidxml/xml_parser.h"
+#include "monitor/monitor_factory.h"
+
+namespace cmvr::monitor {
+
+    class MonitorManager {
+    public:
+        MonitorManager(const MonitorManager&) = delete;
+        MonitorManager& operator=(const MonitorManager&) = delete;
+        static MonitorManager& getInstance(const XmlNode &cfg);
+        static MonitorManager& getInstance();
+        static void destroyInstance();
+
+    private:
+        explicit MonitorManager (const XmlNode &cfg);
+        void init_monitors();
+    private:
+        static std::once_flag init_flag_;
+        static std::shared_ptr<MonitorManager> instance_;
+
+        XmlNode cfg_;
+        std::shared_ptr<MonitorFactory> monitor_factory_;
+        std::unordered_map<std::string, std::shared_ptr<AbstractMonitor>> monitors_;
+    };
+}
+
+
+
+#endif //MONITOR_MANAGER_H

include/service/grpc_service/grpc_camera_service.h

@@ -0,0 +1,41 @@
+//
+// Created by xtkuang on 2025/6/1.
+//
+
+#ifndef GRPC_CAMERA_SERVICE_H
+#define GRPC_CAMERA_SERVICE_H
+
+#include "cmvr/api/camera_service.grpc.pb.h"
+#include "../../utils/base/grpc_utils.h"
+#include "device_manager/device_manager.h"
+
+namespace cmvr::service {
+
+    class gRPCCameraServiceImpl final: public api::CameraService::Service {
+    public:
+        gRPCCameraServiceImpl();
+        ~gRPCCameraServiceImpl() override = default;
+        grpc::Status GetStatus(grpc::ServerContext* context, const api::GetCameraStateCommand_Request* request, api::GetCameraStateCommand_Feedback* response) override;
+        grpc::Status StartCamera(grpc::ServerContext* context, const api::StartCameraCommand_Request* request, api::StartCameraCommand_Feedback* response) override;
+        grpc::Status StopCamera(grpc::ServerContext* context, const api::StopCameraCommand_Request* request, api::StopCameraCommand_Feedback* response) override;
+        grpc::Status GetRGBImage(grpc::ServerContext* context, const api::GetRGBImageCommand_Request* request, api::GetRGBImageCommand_Feedback* response) override;
+        grpc::Status GetDepthImage(grpc::ServerContext* context, const api::GetDepthImageCommand_Request* request, api::GetDepthImageCommand_Feedback* response) override;
+        grpc::Status GetRGBDImages(grpc::ServerContext* context, const api::GetRGBDImagesCommand_Request* request, api::GetRGBDImagesCommand_Feedback* response) override;
+        grpc::Status StartRecording(grpc::ServerContext* context, const api::StartCameraRecordingCommand_Request* request, api::StartCameraRecordingCommand_Feedback* response) override;
+        grpc::Status StopRecording(grpc::ServerContext* context, const api::StopCameraRecordingCommand_Request* request, api::StopCameraRecordingCommand_Feedback* response) override;
+        grpc::Status GetDepthImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetDepthImageStreamCommand_Feedback, cmvr::api::GetDepthImageStreamCommand_Request>* stream) override;
+        grpc::Status GetRGBDImagesStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBDImagesStreamCommand_Feedback, cmvr::api::GetRGBDImagesStreamCommand_Request>* stream) override;
+        grpc::Status GetRGBImageStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback, cmvr::api::GetRGBImageStreamCommand_Request>* stream) override;
+    private:
+        device::DeviceManager& dmgr_;
+
+        //双向流读写线程
+        std::shared_ptr<std::thread> read_thread_ = nullptr;
+        std::shared_ptr<std::thread> write_thread_ = nullptr;
+        std::atomic<bool> running_{false};
+    };
+}
+
+
+
+#endif //GRPC_CAMERA_SERVICE_H

include/service/grpc_service/grpc_dexhand_service.h

@@ -0,0 +1,33 @@
+//
+// Created by linbo on 2025/7/3.
+//
+
+#ifndef GRPC_DEXHAND_SERVICE_H
+#define GRPC_DEXHAND_SERVICE_H
+
+#include "cmvr/api/dexhand_service.grpc.pb.h"
+#include "../../utils/base/grpc_utils.h"
+#include "device_manager/device_manager.h"
+namespace cmvr::service {
+    class gRPCDexHandServiceImpl final: public api::DexHandService::Service {
+    public:
+        gRPCDexHandServiceImpl();
+        ~gRPCDexHandServiceImpl() override = default;
+        grpc::Status GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request,api::GetDexHandStateCommand_Feedback* response) override;
+        grpc::Status SetDexHandPos(grpc::ServerContext* context, const cmvr::api::SetDexHandPositionsCommand_Request* request, cmvr::api::SetDexHandPositionsCommand_Feedback* response) override;
+        grpc::Status SetDexHandAngle(grpc::ServerContext* context, const cmvr::api::SetDexHandAnglesCommand_Request* request, cmvr::api::SetDexHandAnglesCommand_Feedback* response) override;
+        grpc::Status SetDexHandForce(grpc::ServerContext* context, const cmvr::api::SetDexHandForceCommand_Request* request, cmvr::api::SetDexHandForceCommand_Feedback* response) override;
+        grpc::Status SetDexHandSpeed(grpc::ServerContext* context, const cmvr::api::SetDexHandSpeedCommand_Request* request, cmvr::api::SetDexHandSpeedCommand_Feedback* response) override;
+        grpc::Status SetDexHandPresetAct(grpc::ServerContext* context, const cmvr::api::SetDexHandPresetActCommand_Request* request, cmvr::api::SetDexHandPresetActCommand_Feedback* response) override;
+        grpc::Status GetSensorData(grpc::ServerContext* context, const cmvr::api::GetSensorDataCommand_Request* request, cmvr::api::GetSensorDataCommand_Feedback* response) override;
+        grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetSensorDataStreamCommand_Feedback, cmvr::api::GetSensorDataStreamCommand_Request>* stream) override;
+    private:
+        device::DeviceManager& dmgr_;
+        // 静态成员声明（关键！必须在类内声明）
+        static const std::unordered_map<std::string, int> kIdToIndex;  // 字符串id→索引
+        static const std::vector<std::string> kIndexToId;              // 索引→字符串id
+    };
+
+}
+
+#endif //GRPC_DEXHAND_SERVICE_H

include/service/grpc_service/grpc_head_service.h

@@ -0,0 +1,35 @@
+#ifndef BIO_HEAD_SERVICE_H
+#define BIO_HEAD_SERVICE_H
+
+#include "cmvr/api/biohead_service.grpc.pb.h"
+#include "device_manager/device_manager.h"
+
+namespace cmvr::service
+{
+    class gRPCMBioHeadServiceImpl : public api::BioHeadService::Service {
+    public:
+        gRPCMBioHeadServiceImpl();
+        ~gRPCMBioHeadServiceImpl() override = default;
+
+        grpc::Status SetExpression(grpc::ServerContext* context,
+                                   const api::SetFacialExpression_Request* request,
+                                   api::SetFacialExpression_Feedback* response) override;
+
+        grpc::Status StreamExpression(grpc::ServerContext* context,
+                                             grpc::ServerReaderWriter<api::StreamFacialExpression_Feedback, api::StreamFacialExpression_Request>* stream) override;
+
+
+        grpc::Status GetSystemStatus(grpc::ServerContext* context,
+                                     const api::GetStatus_Request* request,
+                                     api::GetStatus_Feedback* response) override;
+
+        grpc::Status EmergencyStop(grpc::ServerContext* context,
+                                   const api::EmergencyStop_Request* request,
+                                   api::EmergencyStop_Feedback* response) override;
+
+    private:
+        device::DeviceManager& dmgr_;
+    };
+}  // namespace cmvr::service
+
+#endif  // BIO_HEAD_SERVICE_H

include/service/grpc_service/grpc_humanoid_robot_service.h

@@ -0,0 +1,38 @@
+//
+// Created by lgv on 2025/8/15.
+//
+
+#pragma once
+
+#include "device_manager/device_manager.h"
+#include "cmvr/api/humanoid_robot_service.grpc.pb.h"
+
+namespace cmvr {
+    namespace service {
+        class gRPCHumanoidRobotServiceImpl final : public api::HumanoidRobotService::Service {
+        public:
+            gRPCHumanoidRobotServiceImpl();
+            ~gRPCHumanoidRobotServiceImpl() = default;
+            grpc::Status torqueOff(grpc::ServerContext *context,
+                const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override;
+            grpc::Status torqueOn(grpc::ServerContext *context,
+                const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override;
+
+            grpc::Status moveJ(grpc::ServerContext *context, const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) override;
+
+            grpc::Status moveL(grpc::ServerContext* context, const cmvr::api::MoveL_Request* request, cmvr::api::MoveL_Response* response) override;
+
+            grpc::Status speedJ(grpc::ServerContext* context, const cmvr::api::SpeedJ_Request* request, cmvr::api::SpeedJ_Response* response) override;
+
+            grpc::Status speedL(grpc::ServerContext* context, const cmvr::api::SpeedL_Request* request, cmvr::api::SpeedL_Response* response) override;
+
+            grpc::Status getJointState(grpc::ServerContext *context, const cmvr::api::JointRequest *request, cmvr::api::JointResponse *response) override;
+        private:
+            device::DeviceManager& dmgr_;
+        };
+
+
+
+    }
+
+}

include/service/grpc_service/grpc_microphone_service.h

@@ -0,0 +1,30 @@
+//
+// Created by linbo on 2025/6/13.
+// Created by xtkuang on 2025/6/13.
+//
+
+#ifndef GRPC_MICROPHONE_SERVICE_H
+#define GRPC_MICROPHONE_SERVICE_H
+#include "cmvr/api/microphone_service.grpc.pb.h"
+#include "device_manager/device_manager.h"
+#include "../../utils/base/grpc_utils.h"
+
+namespace cmvr::service
+{
+
+    class gRPCMicroPhoneServiceImpl: public api::MicPhoneService::Service {
+    public:
+        gRPCMicroPhoneServiceImpl();
+        ~gRPCMicroPhoneServiceImpl() override = default;
+        grpc::Status GetStatus(grpc::ServerContext* context, const api::GetMicStateCommand_Request* request,api::GetMicStateCommand_Feedback* response) override;
+        grpc::Status StartRecord(grpc::ServerContext* context, const api::StartMicRecordingCommand_Request* request,api::StartMicRecordingCommand_Feedback* response) override;
+        grpc::Status StopRecord(grpc::ServerContext* context, const api::StopMicRecordingCommand_Request* request,api::StopMicRecordingCommand_Feedback* response) override;
+        grpc::Status PauseRecord(grpc::ServerContext* context, const api::PauseMicRecordingCommand_Request* request,api::PauseMicRecordingCommand_Feedback* response) override;
+        grpc::Status ResumeRecord(grpc::ServerContext* context, const api::ResumeMicRecordingCommand_Request* request,api::ResumeMicRecordingCommand_Feedback* response) override;
+        grpc::Status SetVolume(grpc::ServerContext* context, const api::SetMicPhoneVolumeCommand_Request* request,api::SetMicPhoneVolumeCommand_Feedback* response) override;
+        grpc::Status GetVolume(grpc::ServerContext* context, const api::GetMicPhoneVolumeCommand_Request* request,api::GetMicPhoneVolumeCommand_Feedback* response) override;
+    private:
+        device::DeviceManager& dmgr_;
+    };
+}
+#endif //GRPC_MICROPHONE_SERVICE_H

include/service/grpc_service/grpc_speaker_service.h

@@ -0,0 +1,30 @@
+//
+// Created by xtkuang on 2025/6/10.
+//
+
+#ifndef GRPC_SPEAKER_SERVICE_H
+#define GRPC_SPEAKER_SERVICE_H
+
+#include "cmvr/api/speaker_service.grpc.pb.h"
+#include "device_manager/device_manager.h"
+#include "../../utils/base/grpc_utils.h"
+
+namespace cmvr::service {
+    class gRPCSpeakerServiceImpl: public api::SpeakerService::Service {
+    public:
+        gRPCSpeakerServiceImpl();
+        ~gRPCSpeakerServiceImpl() override = default;
+        grpc::Status GetStatus(grpc::ServerContext* context, const api::GetSpeakerStateCommand_Request* request,api::GetSpeakerStateCommand_Feedback* response) override;
+        grpc::Status PlayAudio(grpc::ServerContext* context, const api::PlayAudioCommand_Request* request,api::PlayAudioCommand_Feedback* response) override;
+        grpc::Status StopPlayback(grpc::ServerContext* context, const api::StopSpeakerCommand_Request* request,api::StopSpeakerCommand_Feedback* response) override;
+        grpc::Status PausePlayback(grpc::ServerContext* context, const api::PauseSpeakerCommand_Request* request,api::PauseSpeakerCommand_Feedback* response) override;
+        grpc::Status ResumePlayback(grpc::ServerContext* context, const api::ResumeSpeakerCommand_Request* request,api::ResumeSpeakerCommand_Feedback* response) override;
+        grpc::Status SetVolume(grpc::ServerContext* context, const api::SetSpeakerVolumeCommand_Request* request,api::SetSpeakerVolumeCommand_Feedback* response) override;
+        grpc::Status GetVolume(grpc::ServerContext* context, const api::GetSpeakerVolumeCommand_Request* request,api::GetSpeakerVolumeCommand_Feedback* response) override;
+    private:
+        device::DeviceManager& dmgr_;
+    };
+}
+
+
+#endif //GRPC_SPEAKER_SERVICE_H

include/service/grpc_service/grpc_system_service.h

@@ -0,0 +1,26 @@
+//
+// Created by xtkuang on 2025/6/6.
+//
+
+#ifndef GRPC_SYSTEM_SERVICE_H
+#define GRPC_SYSTEM_SERVICE_H
+
+#include "cmvr/api/system_service.grpc.pb.h"
+#include "../../utils/base/grpc_utils.h"
+#include "device_manager/device_manager.h"
+
+namespace cmvr::service
+{
+    class gRPCSystemServiceImpl: public api::SystemService::Service {
+    public:
+        gRPCSystemServiceImpl();
+        ~gRPCSystemServiceImpl() override = default;
+        grpc::Status GetSystemInfo(grpc::ServerContext* context, const api::GetSystemInfoCommand_Request* request, api::GetSystemInfoCommand_Feedback* response) override;
+        grpc::Status GetSystemStatus(grpc::ServerContext* context, const api::GetSystemStatusCommand_Request* request, api::GetSystemStatusCommand_Feedback* response) override;
+        grpc::Status UpdateParams(grpc::ServerContext* context, const cmvr::api::UpdateParamsCommand_Request* request, cmvr::api::UpdateParamsCommand_Feedback* response) override;
+    private:
+        device::DeviceManager& dmgr_;
+    };
+}
+
+#endif //GRPC_SYSTEM_SERVICE_H

include/service/http_service/httpclient.h

@@ -0,0 +1,88 @@
+//
+// Created by linbo on 2025/6/24.
+//
+
+#ifndef HTTPCLIENT_H
+#define HTTPCLIENT_H
+#include <string>
+#include <map>
+#include <vector>
+#include <stdexcept>
+#include <memory>
+#include <utility>
+
+// 控制是否使用libcurl的宏
+#ifdef USE_LIBCURL
+#include <curl/curl.h>
+#endif
+
+using namespace std;
+
+namespace cmvr {
+
+    // HTTP响应结构
+    struct HttpResponse {
+        int statusCode;
+        std::string statusText;
+        std::map<std::string, std::string> headers;
+        std::string body;
+    };
+
+    // HTTP异常类
+    class HttpException : public std::runtime_error {
+    public:
+        HttpException(const std::string& message) : std::runtime_error(message) {}
+    };
+
+    // HTTP客户端接口
+    class HttpClientImpl {
+    public:
+        virtual ~HttpClientImpl() = default;
+
+        virtual void setBaseUrl(const std::string& baseUrl) = 0;
+        virtual void setTimeout(int timeout) = 0;
+        virtual void addHeader(const std::string& key, const std::string& value) = 0;
+        virtual void clearHeaders() = 0;
+
+        virtual HttpResponse get(const std::string& path,
+                                const std::map<std::string, std::string>& params = {}) = 0;
+
+        virtual HttpResponse post(const std::string& path,
+                                 const std::map<std::string, std::string>& params = {},
+                                 const std::string& body = "",
+                                 const std::string& contentType = "application/x-www-form-urlencoded") = 0;
+
+        virtual HttpResponse postJson(const std::string& path,
+                                     const std::map<std::string, std::string>& params = {},
+                                     const std::string& jsonBody = "") = 0;
+    };
+
+    // HTTP客户端类
+    class HttpClient {
+    public:
+        HttpClient();
+        ~HttpClient() = default;
+
+        void setBaseUrl(const std::string& baseUrl);
+        void setTimeout(int timeout);
+        void addHeader(const std::string& key, const std::string& value);
+        void clearHeaders();
+
+        HttpResponse get(const std::string& path,
+                        const std::map<std::string, std::string>& params = {});
+
+        HttpResponse post(const std::string& path,
+                         const std::map<std::string, std::string>& params = {},
+                         const std::string& body = "",
+                         const std::string& contentType = "application/x-www-form-urlencoded");
+
+        HttpResponse postJson(const std::string& path,
+                             const std::map<std::string, std::string>& params = {},
+                             const std::string& jsonBody = "");
+
+    private:
+        std::unique_ptr<HttpClientImpl> m_impl;
+    };
+
+}
+#endif //HTTPCLIENT_H

include/utils/base/abstract_interpolation.h

@@ -0,0 +1,72 @@
+#ifndef INTERPOLATION_BASE_H
+#define INTERPOLATION_BASE_H
+
+#include "utils/math/geometry.h"
+#include <vector>
+#include <stdexcept>
+
+namespace cmvr::math {
+
+    /**
+     * 插值基类，用于机械臂位姿插值
+     * 支持三维位姿(Pose3d)和二维位姿(Pose2d)的插值
+     */
+    class AbstractInterpolation {
+    public:
+        // 构造函数
+        AbstractInterpolation(const std::vector<Pose3d>& waypoints);
+        AbstractInterpolation(const std::vector<Pose2d>& waypoints);
+
+        // 析构函数
+        virtual ~AbstractInterpolation() = default;
+
+        /**
+         * 插值计算接口
+         * @param t 插值参数，范围[0, 1]，0表示起点，1表示终点
+         * @return 插值得到的位姿
+         */
+        virtual Pose3d interpolate_3d(double t) const = 0;
+        virtual Pose2d interpolate_2d(double t) const = 0;
+
+        /**
+         * 生成轨迹点序列
+         * @param num_points 生成的轨迹点数量，包括起点和终点
+         * @return 轨迹点序列
+         */
+        std::vector<Pose3d> generate_trajectory_3d(size_t num_points) const;
+        std::vector<Pose2d> generate_trajectory_2d(size_t num_points) const;
+
+    protected:
+        // 路径点存储
+        std::vector<Pose3d> waypoints_3d_;
+        std::vector<Pose2d> waypoints_2d_;
+
+        /**
+         * 辅助函数：线性插值单个数值
+         * @param a 起始值
+         * @param b 目标值
+         * @param t 插值参数[0,1]
+         * @return 插值结果
+         */
+        static double linear_interpolate(double a, double b, double t);
+
+        /**
+         * 辅助函数：四元数球面线性插值(slerp)
+         * @param q1 起始四元数
+         * @param q2 目标四元数
+         * @param t 插值参数[0,1]
+         * @return 插值结果
+         */
+        static Quat slerp(const Quat& q1, const Quat& q2, double t);
+
+        /**
+         * 检查t的范围并进行约束
+         * @param t 原始参数
+         * @return 约束在[0,1]范围内的参数
+         */
+        static double clamp_t(double t);
+    };
+
+} // namespace cmvr::math
+
+#endif // INTERPOLATION_BASE_H

include/utils/base/grpc_utils.h

@@ -0,0 +1,17 @@
+//
+// Created by xtkuang on 2025/6/1.
+//
+
+#ifndef GRPC_UTILS_H
+#define GRPC_UTILS_H
+
+#include <grpcpp/grpcpp.h>
+
+inline void setCurrentTimestamp(google::protobuf::Timestamp* ts) {
+    auto now = std::chrono::system_clock::now();
+    auto duration = now.time_since_epoch();
+    ts->set_seconds(std::chrono::duration_cast<std::chrono::seconds>(duration).count());
+    ts->set_nanos(std::chrono::duration_cast<std::chrono::nanoseconds>(duration).count() % 1000000000);
+}
+
+#endif //GRPC_UTILS_H

include/utils/base/logger.h

@@ -0,0 +1,53 @@
+//
+// Created by xtkuang on 2025/6/3.
+//
+
+#ifndef LOGGER_H
+#define LOGGER_H
+
+#include <sys/stat.h>
+#include <unistd.h>
+#include <glog/logging.h>
+#include "rapidxml/xml_parser.h"
+
+using namespace std;
+
+inline bool initLogger(const XmlNode &cfg) {
+     google::InitGoogleLogging("cmvr_es");
+
+        std::string temp = cfg.getAttrString("dir");
+        if (temp.empty()) {
+            std::cerr << "Error: no directory specified" << std::endl;
+            return false;
+        } else {
+            // 判断目录是否存在
+            if (access(temp.c_str(), F_OK) != 0) {
+                // 创建目录
+                if (mkdir(temp.c_str(), 0755) != 0) {
+                    std::cerr << "Failed to create directory: " << temp << std::endl;
+                    return false;
+                }
+            }
+            FLAGS_log_dir = temp;
+        }
+
+        temp = cfg.getAttrDefault("level", "info");
+        if (temp == "info") {
+            FLAGS_minloglevel = google::INFO;
+        } else if (temp == "warn") {
+            FLAGS_minloglevel = google::WARNING;
+        } else if (temp == "error") {
+            FLAGS_minloglevel = google::ERROR;
+        } else if (temp == "fatal") {
+            FLAGS_minloglevel = google::FATAL;
+        }
+
+        FLAGS_logbufsecs = cfg.getAttrDefault("bufSize", 5);
+        FLAGS_max_log_size = cfg.getAttrDefault("logSize", 1024);
+        FLAGS_logtostderr = false;
+        FLAGS_alsologtostderr = true;
+
+        return true;
+}
+
+#endif //LOGGER_H

include/utils/base/os.h

@@ -0,0 +1,33 @@
+//
+// Created by xtkuang on 2025/6/11.
+//
+
+#ifndef OS_H
+#define OS_H
+
+#include <vector>
+#include <iostream>
+#include <sys/stat.h>
+
+inline bool pathExists(const std::string& path) {
+    struct stat info;
+    return stat(path.c_str(), &info) == 0;
+}
+
+inline std::vector<std::string> splitString(const std::string& pattern, const std::string& delimiter) {
+    std::vector<std::string> result;
+    if (delimiter.empty()) {
+        result.push_back(pattern); // 若分隔符为空，则原样返回
+        return result;
+    }
+    size_t start = 0;
+    size_t end;
+    while ((end = pattern.find(delimiter, start)) != std::string::npos) {
+        result.push_back(pattern.substr(start, end - start));
+        start = end + delimiter.length();
+    }
+    result.push_back(pattern.substr(start));
+    return result;
+}
+
+#endif //OS_H

include/utils/base/ring_buffer.h

@@ -0,0 +1,136 @@
+//
+// Created by xtkuang on 2025/5/30.
+//
+
+#ifndef CMVR_ES_RING_BUFFER_H
+#define CMVR_ES_RING_BUFFER_H
+
+#pragma once
+#include <deque>
+#include <mutex>
+#include <vector>
+#include <atomic>
+#include <optional>
+
+template<typename T>
+class RingBuffer {
+public:
+    explicit RingBuffer(size_t capacity) : capacity_(capacity) {}
+
+    void push(const T& item) {
+        std::lock_guard<std::mutex> lock(mutex_);
+        if (buffer_.size() >= capacity_) {
+            buffer_.pop_front();
+        }
+        buffer_.push_back(item);
+    }
+
+    std::vector<T> getAll() const {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return std::vector<T>(buffer_.begin(), buffer_.end());
+    }
+
+    void clear() {
+        std::lock_guard<std::mutex> lock(mutex_);
+        buffer_.clear();
+    }
+
+    size_t size() const {
+        std::lock_guard<std::mutex> lock(mutex_);
+        return buffer_.size();
+    }
+
+private:
+    size_t capacity_;
+    std::deque<T> buffer_;
+    mutable std::mutex mutex_;
+};
+
+template<typename T>
+class SPMCRingBuffer {
+public:
+    explicit SPMCRingBuffer(size_t capacity)
+        : buffer_(capacity), capacity_(capacity),
+          head_(0), tail_(0) {}
+
+    // 写入操作（仅支持单个生产者）
+    void push(const T& item) {
+        size_t head = head_.load(std::memory_order_relaxed);
+        size_t tail = tail_.load(std::memory_order_acquire);
+        buffer_[head % capacity_] = item;
+        head = head + 1;
+        head_.store(head, std::memory_order_release);
+        if (head - tail >= capacity_) {
+            // 队列满，覆盖最旧的数据
+            tail_.store(tail + 1, std::memory_order_release);
+        }
+    }
+
+    // 单消费者使用（内部 tail_）
+    std::optional<T> pop() {
+        size_t tail = tail_.load(std::memory_order_relaxed);
+        size_t head = head_.load(std::memory_order_acquire);
+        if (tail >= head) return std::nullopt;
+        T value = buffer_[tail % capacity_];
+        tail_.store(tail + 1, std::memory_order_release);
+        return value;
+    }
+
+    std::optional<T> getLast() {
+        size_t tail = tail_.load(std::memory_order_relaxed);
+        size_t head = head_.load(std::memory_order_acquire);
+        if (tail >= head) return std::nullopt;
+        T value = buffer_[head_ % capacity_];
+        return value;
+    }
+
+    // 多消费者使用（每个读者独立维护 reader_tail）
+    std::optional<T> pop(size_t& reader_tail) const {
+        size_t head = head_.load(std::memory_order_acquire);
+        if (reader_tail >= head) return std::nullopt;
+        if (head > reader_tail + capacity_) {
+            // 数据已被覆盖，跳过无效读取区间
+            reader_tail = head - capacity_;
+            return std::nullopt;
+        }
+        T value = buffer_[reader_tail % capacity_];
+        reader_tail++;
+        return value;
+    }
+
+    size_t size() const {
+        return head_.load(std::memory_order_acquire) - tail_.load(std::memory_order_acquire);
+    }
+
+    size_t getHead() const {
+        return head_.load(std::memory_order_acquire);
+    }
+
+    size_t getTail() const {
+        return tail_.load(std::memory_order_acquire);
+    }
+
+    bool empty() const {
+        return size() == 0;
+    }
+
+    bool full() const {
+        return size() >= capacity_;
+    }
+
+    void clear() {
+        head_.store(0, std::memory_order_release);
+        tail_.store(0, std::memory_order_release);
+    }
+
+private:
+    std::vector<T> buffer_;
+    const size_t capacity_;
+
+    std::atomic<size_t> head_;  // 共享写指针
+    std::atomic<size_t> tail_;  // 共享读指针（仅用于 SPSC 模式）
+};
+
+
+
+#endif //CMVR_ES_RING_BUFFER_H

include/utils/base/thread_pool.h

@@ -0,0 +1,81 @@
+//
+// Created by xtkuang on 2025/5/13.
+//
+
+#ifndef CMVR_ES_THREAD_POOL_H
+#define CMVR_ES_THREAD_POOL_H
+
+
+#include <thread>
+#include <vector>
+#include <functional>
+#include <atomic>
+#include <mutex>
+#include <condition_variable>
+#include <memory>
+#include <chrono>
+#include <future>
+#include <boost/lockfree/queue.hpp>
+#pragma warning(disable:4996)
+#define GLOG_USE_GLOG_EXPORT
+#include <glog/logging.h>
+
+class InterruptFlag {
+public:
+    void request_stop();
+    [[nodiscard]] bool stop_requested() const;
+
+private:
+    std::atomic<bool> flag_ {false};
+};
+
+// 提交任务（带 future 返回）
+class TaskHandle {
+public:
+    using TaskFuncType = std::function<void()>;
+    using ReturnType = void;
+
+    TaskHandle(std::shared_ptr<std::promise<ReturnType>> promise)
+            : future_(promise->get_future()) {}
+
+    std::future<ReturnType>& get_future() { return future_; }
+
+private:
+    std::future<ReturnType> future_;
+};
+
+
+class ThreadPool {
+public:
+    ThreadPool(size_t min_threads, size_t max_threads);
+    ~ThreadPool();
+
+    // 提交任务（不带返回值）
+    void submit(std::function<void(InterruptFlag&)> task);
+
+    TaskHandle submit_with_future(std::function<TaskHandle::TaskFuncType()> f);
+
+    void shutdown();
+
+private:
+    void worker_loop(size_t id);
+    void monitor_loop();
+
+    boost::lockfree::queue<std::function<void(InterruptFlag&)>*> task_queue_;
+    std::atomic<size_t> pending_tasks_;
+
+    std::vector<std::thread> threads_;
+    std::vector<std::unique_ptr<InterruptFlag>> flags_;
+
+    std::mutex control_mutex_;
+    std::condition_variable control_cv_;
+    std::atomic<bool> shutdown_requested_ = false;
+
+    const size_t min_threads_;
+    const size_t max_threads_;
+
+    std::thread monitor_thread_;
+};
+
+
+#endif //CMVR_ES_THREAD_POOL_H

include/utils/base/timer.h

@@ -0,0 +1,37 @@
+//
+// Created by xtkuang on 2025/5/14.
+//
+
+#ifndef CMVR_ES_TIMER_H
+#define CMVR_ES_TIMER_H
+
+#include <sys/timerfd.h>
+#include <unistd.h>
+#include <functional>
+#include <thread>
+#include <atomic>
+#include <system_error>
+#include <iostream>
+#include <cstring>
+#include <utility>
+
+class FDTimer {
+public:
+    using Callback = std::function<void()>;
+
+    FDTimer();
+    ~FDTimer();
+    void start(std::chrono::nanoseconds interval, Callback callback);
+    void stop();
+    [[nodiscard]] bool is_running() const;
+
+private:
+    static timespec toTimespec(std::chrono::nanoseconds ns);
+
+    int fd_;
+    std::atomic<bool> running_;
+    Callback callback_;
+    std::thread worker_thread_;
+};
+
+#endif //CMVR_ES_TIMER_H

include/utils/dynamics/inertial.h

@@ -0,0 +1,54 @@
+//
+// Created by xtkuang on 2025/7/8.
+//
+
+#ifndef INERTIAL_H
+#define INERTIAL_H
+
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+
+#include "utils/math/se3.h"
+
+namespace cmvr::dyn {
+
+class Inertial;
+
+class Inertial {
+public:
+    using MatrixType = Eigen::Matrix<double, 6, 6>;
+
+    static MatrixType I(double mass);
+
+    static MatrixType I(double mass, double ixx, double iyy, double izz,
+                        const math::SE3::MatrixType& T = math::SE3::Identity());
+
+    static MatrixType I(double mass, double ixx, double iyy, double izz,
+                        const Eigen::Vector3d& com);
+
+    static MatrixType I(double mass, double ixx, double iyy, double izz, double ixy, double ixz, double iyz,
+                        const math::SE3::MatrixType& T = math::SE3::Identity());
+
+    static MatrixType I(double mass, double ixx, double iyy, double izz, double ixy, double ixz, double iyz,
+                        const Eigen::Vector3d& com);
+
+    static MatrixType I(double mass, const Eigen::Vector3d& inertia,
+                        const math::SE3::MatrixType& T = math::SE3::Identity());
+
+    static MatrixType I(double mass, const Eigen::Matrix<double, 6, 1>& inertia,
+                        const math::SE3::MatrixType& T = math::SE3::Identity());
+
+    static MatrixType Transform(const math::SE3::MatrixType& T, const MatrixType& I);
+
+    static Eigen::Vector3d GetCOM(const MatrixType& I);
+
+    static double GetMass(const MatrixType& I);
+
+    // [I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{xz}, I_{yz}]^T
+    static Eigen::Vector<double, 6> GetInertia(const MatrixType& I);
+};
+
+}
+#endif //INERTIAL_H

include/utils/dynamics/joint.h

@@ -0,0 +1,108 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef JOINT_H
+#define JOINT_H
+
+#include <eigen3/Eigen/Core>
+#include <memory>
+#include "utils/math/constants.h"
+#include "utils/math/liegroup.h"
+#include "utils/dynamics/link.h"
+
+namespace cmvr::dyn {
+class Joint : public std::enable_shared_from_this<Joint> {
+public:
+    template<int DOF>
+    friend class Robot;
+
+    static std::shared_ptr<Joint> Make(std::string name, math::se3v::MatrixType S);
+
+    static std::shared_ptr<Joint> MakeRevoluteJoint(std::string name,
+                                                    const math::SE3::MatrixType &T = math::SE3::Identity(),
+                                                    const Eigen::Vector3d &axis = {0, 0, 1});
+
+    static std::shared_ptr<Joint> MakePrismaticJoint(std::string name,
+                                                     const math::SE3::MatrixType &T = math::SE3::Identity(),
+                                                     const Eigen::Vector3d &axis = {0, 0, 1});
+
+    static std::shared_ptr<Joint> MakeFixedJoint(std::string name);
+
+    std::string GetName() const;
+
+    void ConnectLinks(const std::shared_ptr<Link> &parent_link, const std::shared_ptr<Link> &child_link,
+                      const math::SE3::MatrixType &T_pj = math::SE3::Identity(),
+                      const math::SE3::MatrixType &T_jc = math::SE3::Identity());
+
+    void Disconnect();
+
+    void SetLimitQ(double lower, double upper);
+
+    void SetLimitQdot(double lower, double upper);
+
+    void SetLimitQddot(double lower, double upper);
+
+    void SetLimitTorque(double value);
+
+    double GetLimitQLower() const;
+
+    double GetLimitQUpper() const;
+
+    double GetLimitQdotLower() const;
+
+    double GetLimitQdotUpper() const;
+
+    double GetLimitQddotLower() const;
+
+    double GetLimitQddotUpper() const;
+
+    double GetLimitTorque() const;
+
+    void SetLimitQLower(double val);
+
+    void SetLimitQUpper(double val);
+
+    void SetLimitQdotLower(double val);
+
+    void SetLimitQdotUpper(double val);
+
+    void SetLimitQddotLower(double val);
+
+    void SetLimitQddotUpper(double val);
+
+    std::weak_ptr<Link> GetParentLink();
+
+    std::shared_ptr<Link> GetChildLink();
+
+    std::shared_ptr<const Link> GetChildLink() const;
+
+    bool IsFixed() const;
+
+private:
+    explicit Joint(std::string name);
+
+    Joint(std::string name, math::se3v::MatrixType S);
+
+private:
+    std::string name_{};
+    bool fixed_;
+    math::se3v::MatrixType S_;
+    double limit_torque_{(std::numeric_limits<double>::max)()};
+    double limit_q_lower_{-(std::numeric_limits<double>::max)()};
+    double limit_q_upper_{(std::numeric_limits<double>::max)()};
+    double limit_qdot_lower_{-(std::numeric_limits<double>::max)()};
+    double limit_qdot_upper_{(std::numeric_limits<double>::max)()};
+    //  double limit_qddot_lower_{-(std::numeric_limits<double>::max)()};
+    //  double limit_qddot_upper_{(std::numeric_limits<double>::max)()};
+    double limit_qddot_lower_{-10.};
+    double limit_qddot_upper_{10.}; // (rad/s^2)
+
+    std::weak_ptr<Link> parent_link_;
+    std::shared_ptr<Link> child_link_{nullptr};
+    math::SE3::MatrixType T_pj_, T_jc_;
+};
+}
+
+
+#endif //JOINT_H

include/utils/dynamics/link.h

@@ -0,0 +1,143 @@
+//
+// Created by xtkuang on 2025/7/8.
+//
+
+#ifndef LINK_H
+#define LINK_H
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+#include <memory>
+
+#include "utils/dynamics/inertial.h"
+#include "utils/math/liegroup.h"
+
+namespace cmvr::dyn {
+template<int DOF>
+class Robot;
+class Link;
+class Joint;
+class Collision;
+struct CollisionResult;
+class Geom;
+class GeomCapsule;
+
+enum class GeomType { kCapsule = 0 };
+
+class Link : public std::enable_shared_from_this<Link> {
+public:
+    template<int DOF>
+    friend class Robot;
+
+    friend class Joint;
+
+    static std::shared_ptr<Link> Make(std::string name, Inertial::MatrixType I = Inertial::I(1.));
+
+    std::string GetName() const;
+
+    std::weak_ptr<Joint> GetParentJoint();
+
+    std::vector<std::shared_ptr<Joint> > GetChildJointList();
+
+    const std::vector<std::shared_ptr<Joint> > &GetChildJointList() const;
+
+    void AddCollision(const std::shared_ptr<Collision> &collision);
+
+    std::vector<std::shared_ptr<Collision> > GetCollisions();
+
+    const std::vector<std::shared_ptr<Collision> > &GetCollisions() const;
+
+private:
+    Link(std::string name, Inertial::MatrixType I);
+
+private:
+    std::string name_{};
+    Inertial::MatrixType I_{};
+
+    std::weak_ptr<Joint> parent_joint_;
+    std::vector<std::shared_ptr<Joint> > child_joints_;
+
+    std::vector<std::shared_ptr<Collision> > collisions_;
+};
+
+class Collision : public std::enable_shared_from_this<Collision> {
+public:
+    explicit Collision(std::string name);
+
+    void SetOrigin(const math::SE3::MatrixType &T);
+
+    math::SE3::MatrixType GetOrigin() const;
+
+    void AddGeom(const std::shared_ptr<Geom> &geom);
+
+    std::vector<std::shared_ptr<Geom> > GetGeoms();
+
+    const std::vector<std::shared_ptr<Geom> > &GetGeoms() const;
+
+private:
+    std::string name_;
+    math::SE3::MatrixType T_{math::SE3::Identity()};
+    std::vector<std::shared_ptr<Geom> > geoms_;
+};
+
+class Geom : public std::enable_shared_from_this<Geom> {
+public:
+    Geom(unsigned int coltype = 0, unsigned int colaffinity = 0) : coltype_(coltype), colaffinity_(colaffinity) {
+    }
+
+    virtual ~Geom() = default;
+
+    virtual GeomType GetType() const = 0;
+
+    unsigned int GetColtype() const { return coltype_; }
+
+    unsigned int GetColaffinity() const { return colaffinity_; }
+
+    virtual std::optional<CollisionResult> ComputeMinimumDistance(const math::SE3::MatrixType &T,
+                                                                  const Geom &other_geom,
+                                                                  const math::SE3::MatrixType &other_T) const = 0;
+
+    bool Filter(const Geom &other_geom) const {
+        return (coltype_ & other_geom.colaffinity_) || (other_geom.coltype_ & colaffinity_);
+    }
+
+protected:
+    unsigned int coltype_;
+    unsigned int colaffinity_;
+};
+
+class GeomCapsule : public Geom {
+public:
+    GeomCapsule(double length, double radius, unsigned int coltype = 0, unsigned int colaffinity = 0);
+
+    GeomCapsule(Eigen::Vector3d sp, Eigen::Vector3d ep, double radius, unsigned int coltype = 0,
+                unsigned int colaffinity = 0);
+
+    GeomType GetType() const override;
+
+    std::optional<CollisionResult> ComputeMinimumDistance(const math::SE3::MatrixType &T, const Geom &other_geom,
+                                                          const math::SE3::MatrixType &other_T) const override;
+
+    Eigen::Vector3d GetStartPoint() const;
+
+    Eigen::Vector3d GetEndPoint() const;
+
+    double GetRadius() const;
+
+private:
+    Eigen::Vector3d sp_;
+    Eigen::Vector3d ep_;
+    double radius_;
+};
+
+struct CollisionResult {
+    std::string link1;
+    std::string link2;
+    Eigen::Vector3d position1;
+    Eigen::Vector3d position2;
+    double distance;
+};
+
+}
+#endif //LINK_H

include/utils/dynamics/robot.h

@@ -0,0 +1,288 @@
+//
+// Created by xtkuang on 2025/7/8.
+//
+
+#ifndef ROBOT_H
+#define ROBOT_H
+
+
+#pragma once
+
+#include <Eigen/Core>
+#include <iostream>
+#include <memory>
+#include <queue>
+#include <string>
+#include <type_traits>
+#include <utility>
+#include <vector>
+#include <iomanip>
+#include <limits>
+
+#include "inertial.h"
+#include "joint.h"
+#include "link.h"
+#include "state.h"
+#include "utils/math/liegroup.h"
+#include "utils/math/qp_solver.h"
+
+namespace cmvr::dyn {
+/**********************************************************
+ * FORWARD DECLARATION
+ **********************************************************/
+
+//
+template<int DOF>
+class Robot;
+class Link;
+class Joint;
+
+/**********************************************************
+ * MOBILE BASE
+ **********************************************************/
+
+enum class MobileBaseType {
+    None, //
+    Differential, //
+    Mecanum //
+};
+
+struct MobileBase {
+    MobileBaseType type{MobileBaseType::None};
+    math::SE3::MatrixType T; // front = x-axis
+    std::vector<std::string> joints;
+    std::vector<double> params;
+};
+
+struct MobileBaseDifferential : public MobileBase {
+    unsigned int right_wheel_idx{};
+    unsigned int left_wheel_idx{};
+    double wheel_base{0.};
+    double wheel_radius{0.};
+};
+
+struct MobileBaseMecanum : public MobileBase {
+    unsigned int fr_wheel_idx{};
+    unsigned int fl_wheel_idx{};
+    unsigned int rr_wheel_idx{};
+    unsigned int rl_wheel_idx{};
+    double L_x{0.};
+    double L_y{0.};
+    double wheel_radius{0.};
+};
+
+/**********************************************************
+ * ROBOT
+ **********************************************************/
+
+struct RobotConfiguration {
+    std::string name;
+    std::shared_ptr<Link> base_link;
+    std::shared_ptr<MobileBase> mobile_base;
+};
+
+template<int DOF>
+class Robot {
+public:
+    template<typename T, int N>
+    using ContainerType = typename std::conditional_t<(N > 0), std::array<T, (unsigned int) N>, std::vector<T> >;
+
+    struct Link_ {
+        struct SubLink_ {
+            std::shared_ptr<Link> link;
+            Inertial::MatrixType J_wrt_p;
+            math::SE3::MatrixType M_wrt_p;
+            math::SE3::MatrixType M_wrt_base;
+        };
+
+        std::vector<SubLink_> links;
+        Inertial::MatrixType J{Inertial::MatrixType::Zero()}; // Link_ inertial wrt base link
+        math::SE3::MatrixType M{math::SE3::Identity()}; // Base link to Link_ frame
+        Inertial::MatrixType I{Inertial::MatrixType::Zero()}; // Link_ inertial wrt Link_ frame
+        int depth{0};
+        int parent_joint_idx{-1};
+        std::vector<int> child_joint_idx{};
+
+        void SetBaseLink(const std::shared_ptr<Link> &link, const math::SE3::MatrixType &T) {
+            // Rest link information
+            links.clear();
+            J = Inertial::MatrixType::Zero();
+            M = T;
+            AddLink(link, math::SE3::Identity());
+        }
+
+        int AddLink(const std::shared_ptr<Link> &link, const math::SE3::MatrixType &M_wrt_p) {
+            Inertial::MatrixType J_wrt_p = Inertial::Transform(M_wrt_p, link->I_);
+            int idx = links.size();
+            SubLink_ l;
+            l.link = link;
+            l.J_wrt_p = J_wrt_p;
+            l.M_wrt_p = M_wrt_p;
+            l.M_wrt_base = M * M_wrt_p;
+            links.push_back(l);
+            I += J_wrt_p;
+            J += Inertial::Transform(M, J_wrt_p);
+            return idx;
+        }
+    };
+
+    struct Joint_ {
+        std::shared_ptr<Joint> joint{nullptr};
+        math::se3v::MatrixType S{math::se3v::MatrixType::Zero()}; // zero pose
+
+        int parent_link_idx{};
+        int child_link_idx{};
+    };
+
+    struct LinkIdx_ {
+        int link_idx;
+        int sub_link_idx;
+    };
+
+    explicit Robot(const RobotConfiguration &robot_configuration);
+    
+    std::shared_ptr<Link> GetBase();
+    
+    std::vector<std::string> GetLinkNames() const;
+    
+    std::vector<std::string> GetJointNames() const;
+    
+    std::shared_ptr<Link> GetLink(const std::string &name) const;
+    
+    std::shared_ptr<Link> GetLink(std::shared_ptr<State<DOF>> state, int index) const;
+
+    LinkIdx_ GetLinkIdx(const std::string& name) const;
+
+    template<typename LinkContainer = std::vector<std::string>, typename JointContainer = std::vector<std::string> >
+    std::shared_ptr<State<DOF>> MakeState(const LinkContainer &link_names, const JointContainer &joint_names);
+
+    int GetDOF() const;
+
+    int GetNumberOfJoints() const;
+
+    void ComputeForwardKinematics(std::shared_ptr<State<DOF>> state);
+
+    void ComputeDiffForwardKinematics(std::shared_ptr<State<DOF>> state);
+
+    void Compute2ndDiffForwardKinematics(std::shared_ptr<State<DOF>> state);
+
+    void ComputeInverseDynamics(std::shared_ptr<State<DOF>> state);
+
+    Eigen::Vector<double, DOF> ComputeGravityTerm(std::shared_ptr<State<DOF>> state);
+
+    Eigen::Matrix<double, DOF, DOF> ComputeMassMatrix(std::shared_ptr<State<DOF>> state);
+
+    Eigen::Matrix<double, 6, 6> ComputeReflectiveInertia(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to);
+
+    math::SE3::MatrixType ComputeTransformation(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to);
+
+    math::se3v::MatrixType ComputeBodyVelocity(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to);
+
+    Eigen::Matrix<double, 6, DOF> ComputeSpaceJacobian(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to);
+
+    Eigen::Matrix<double, 6, DOF> ComputeBodyJacobian(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to);
+
+    double ComputeMass(std::shared_ptr<State<DOF>> state, unsigned int target_link);
+
+    Eigen::Vector3d ComputeCenterOfMass(std::shared_ptr<State<DOF>> state, unsigned int ref_link, unsigned int target_link);
+
+    Eigen::Vector3d ComputeCenterOfMass(std::shared_ptr<State<DOF>> state, unsigned int ref_link, const std::vector<unsigned int> &target_links);
+
+    Eigen::Matrix<double, 3, DOF> ComputeCenterOfMassJacobian(std::shared_ptr<State<DOF>> state, unsigned int ref_link, unsigned int target_link);
+
+    Inertial::MatrixType ComputeTotalInertial(std::shared_ptr<State<DOF>> state, unsigned int ref_link);
+
+    Eigen::Vector3d ComputeCenterOfMass(std::shared_ptr<State<DOF>> state, unsigned int ref_link);
+
+    Eigen::Matrix<double, 3, DOF> ComputeCenterOfMassJacobian(std::shared_ptr<State<DOF>> state, unsigned int ref_link);
+
+    std::vector<CollisionResult> DetectCollisionsOrNearestLinks(std::shared_ptr<State<DOF>> state, int collision_threshold = 0);
+
+    // TODO: ComputeBodyJacobianDot
+    // TODO: ComputeBodyAcceleration
+    // TODO: InverseDiffDynamics
+
+    Eigen::Vector<double, DOF> GetLimitQLower(const std::shared_ptr<State<DOF>> &state);
+
+    Eigen::Vector<double, DOF> GetLimitQUpper(const std::shared_ptr<State<DOF>> &state);
+
+    Eigen::Vector<double, DOF> GetLimitQdotLower(const std::shared_ptr<State<DOF>> &state);
+
+    Eigen::Vector<double, DOF> GetLimitQdotUpper(const std::shared_ptr<State<DOF>> &state);
+
+    Eigen::Vector<double, DOF> GetLimitQddotLower(const std::shared_ptr<State<DOF>> &state);
+
+    Eigen::Vector<double, DOF> GetLimitQddotUpper(const std::shared_ptr<State<DOF>> &state);
+
+    Eigen::Vector<double, DOF> GetLimitTorque(const std::shared_ptr<State<DOF>> &state);
+
+    Eigen::Vector<double, DOF> GetJointProperty(const std::shared_ptr<State<DOF>> &state, const std::function<double(std::shared_ptr<Joint>)> &getter);
+
+    void ComputeMobilityInverseDiffKinematics(std::shared_ptr<State<DOF>> state, //
+                                              const Eigen::Vector2d &linear_velocity, // (m/s)
+                                              double angular_velocity // (rad/s)
+    );
+
+    void ComputeMobilityInverseDiffKinematics(std::shared_ptr<State<DOF>> state, //
+                                              const math::se2v::MatrixType &body_velocity // w, x, y
+    );
+
+    math::se2v::MatrixType ComputeMobilityDiffKinematics( //
+        std::shared_ptr<State<DOF>> state //
+    );
+
+    static int CountJoints(const std::shared_ptr<Link> &base_link, bool include_fixed=false);
+
+    math::SE3::MatrixType GetLinkT(std::shared_ptr<State<DOF>> state, const LinkIdx_ &idx);
+
+    math::se3v::MatrixType GetLinkV(std::shared_ptr<State<DOF>> state, const LinkIdx_ &idx);
+
+    math::SE3::MatrixType GetJointT(std::shared_ptr<State<DOF>> state, int joint_idx);
+
+    math::se3v::MatrixType GetJointV(std::shared_ptr<State<DOF>> state, int joint_idx);
+
+    math::se3v::MatrixType GetJointVdot(std::shared_ptr<State<DOF>> state, int joint_idx);
+
+    math::SE3::MatrixType GetTransformation(std::shared_ptr<State<DOF>> state, const LinkIdx_ &from, const LinkIdx_ &to);
+
+    Eigen::Matrix<double, 6, DOF> GetSpaceJacobian(std::shared_ptr<State<DOF>> state, const LinkIdx_ &from, const LinkIdx_ &to);
+
+    Eigen::Matrix<double, 6, DOF> GetBodyJacobian(std::shared_ptr<State<DOF>> state, const LinkIdx_ &from, const LinkIdx_ &to);
+
+protected:
+    Robot() = default;
+
+    void Build(const RobotConfiguration &rc);
+
+private:
+    ContainerType<Link_, DOF + 1> links_;
+    ContainerType<Joint_, DOF> joints_;
+
+    std::unordered_map<std::string, LinkIdx_> link_idx_; // link name to (parent idx, sub link idx)
+    std::unordered_map<std::string, unsigned int> joint_idx_; // joint name to joint idx
+
+    size_t n_links_{};
+    size_t n_joints_{};
+
+    /**
+     * MOBILE BASE
+     */
+    std::shared_ptr<MobileBase> mobile_base_;
+};
+
+RobotConfiguration LoadRobotFromURDFData(const std::string &model, const std::string &base_link_name);
+
+RobotConfiguration LoadRobotFromURDF(const std::string &path, const std::string &base_link_name);
+
+inline std::string to_string(GeomType type) {
+    switch (type) {
+        case GeomType::kCapsule:
+            return "capsule";
+    }
+    return "unknown";
+}
+}
+
+#include "robot.tpp"
+
+#endif //ROBOT_H

include/utils/dynamics/robot.tpp

@@ -0,0 +1,828 @@
+#pragma once
+
+#include "robot.h"
+using namespace cmvr::dyn;
+
+
+template<int DOF>
+Robot<DOF>::Robot(const RobotConfiguration &robot_configuration) {
+    Build(robot_configuration);
+}
+
+template<int DOF>
+std::shared_ptr<Link> Robot<DOF>::GetBase() {
+    return links_[0].links[0].link;
+}
+
+template<int DOF>
+std::vector<std::string> Robot<DOF>::GetLinkNames() const {
+    std::vector<std::string> names;
+    for (const auto &[n, idx]: link_idx_) {
+        names.push_back(n);
+    }
+    return names;
+}
+
+template<int DOF>
+std::vector<std::string> Robot<DOF>::GetJointNames() const {
+    std::vector<std::string> names;
+    for (const auto &[n, idx]: joint_idx_) {
+        names.push_back(n);
+    }
+    return names;
+}
+
+template<int DOF>
+std::shared_ptr<Link> Robot<DOF>::GetLink(const std::string &name) const {
+    if (link_idx_.find(name) == link_idx_.end()) {
+        return nullptr;
+    }
+    const auto &idx = link_idx_.at(name);
+    return links_[idx.link_idx].links[idx.sub_link_idx].link;
+}
+
+template<int DOF>
+std::shared_ptr<Link> Robot<DOF>::GetLink(std::shared_ptr<State<DOF> > state, int index) const {
+    if (index >= static_cast<int>(state->utr_link_map.size())) {
+        return nullptr;
+    }
+    const auto &idx = state->utr_link_map[index];
+    return links_[idx.link_idx].links[idx.sub_link_idx].link;
+}
+
+template<int DOF>
+typename Robot<DOF>::LinkIdx_ Robot<DOF>::GetLinkIdx(const std::string& name) const {
+    auto it = link_idx_.find(name);
+    if (it == link_idx_.end()) {
+        throw std::runtime_error("Link name not found: " + name);
+    }
+    return it->second;
+}
+
+template<int DOF>
+template<typename LinkContainer, typename JointContainer>
+std::shared_ptr<State<DOF>> Robot<DOF>::MakeState(const LinkContainer &link_names, const JointContainer &joint_names) {
+    static_assert(std::is_same_v<typename LinkContainer::value_type, std::string> ||
+                  std::is_same_v<typename LinkContainer::value_type, std::string_view>,
+                  "LinkContainer value_type must be std::string");
+    static_assert(std::is_same_v<typename JointContainer::value_type, std::string> ||
+                  std::is_same_v<typename JointContainer::value_type, std::string_view>,
+                  "JointContainer value_type must be std::string");
+
+    std::vector<bool> flag;
+    flag.resize(n_joints_);
+    std::fill(flag.begin(), flag.end(), false);
+
+    auto state = std::shared_ptr<State<DOF>>(new State < DOF > (n_joints_));
+    for (int i = 0; i < static_cast<int>(joint_names.size()); i++) {
+        const auto &name = joint_names[i];
+
+        auto it = std::find_if(joints_.begin(), joints_.end(),
+                               [name](const auto &j) { return j.joint->name_ == name; });
+        if (it == joints_.end()) {
+            throw std::runtime_error("Cannot find the joint with name");
+        }
+        state->utr_joint_map[i] = it - joints_.begin();
+        state->joint_names[i] = it->joint->name_;
+        flag[state->utr_joint_map[i]] = true;
+    }
+    for (unsigned int i = joint_names.size(); i < n_joints_; i++) {
+        auto it = std::find(flag.begin(), flag.end(), false);
+        state->utr_joint_map[i] = it - flag.begin();
+        state->joint_names[i] = (joints_.begin() + (it - flag.begin()))->joint->name_;
+        *it = true;
+    }
+    for (int i = 0; i < state->utr_joint_map.size(); i++) {
+        state->rtu_joint_map[state->utr_joint_map[i]] = i;
+    }
+
+    std::unordered_map<std::string, bool> name_flag;
+    state->link_names.clear();
+    state->utr_link_map.clear();
+    for (const auto &name: link_names) {
+        auto it = link_idx_.find(name);
+        if (it == link_idx_.end()) {
+            throw std::runtime_error("The link with the given name does not exist");
+        }
+        state->utr_link_map.push_back(it->second);
+        state->link_names.push_back(name);
+        name_flag[name] = true;
+    }
+    for (const auto &[k, v]: link_idx_) {
+        if (name_flag.find(k) == name_flag.end()) {
+            state->utr_link_map.push_back(v);
+            state->link_names.push_back(k);
+        }
+    }
+    for (auto i = 0; i < state->link_names.size(); i++) {
+        const auto &n = state->link_names[i];
+        auto it = link_idx_.find(n);
+        if (it == link_idx_.end()) {
+            throw std::runtime_error("Fatal error; link names in state should be found in link idx");
+        }
+        if (it->second.link_idx == 0 && it->second.sub_link_idx == 0) {
+            state->base_link_user_idx = i;
+        }
+    }
+
+    return state;
+}
+
+template<int DOF>
+int Robot<DOF>::GetDOF() const {
+    return n_joints_;
+}
+
+template<int DOF>
+int Robot<DOF>::GetNumberOfJoints() const {
+    return n_joints_;
+}
+
+template<int DOF>
+void Robot<DOF>::ComputeForwardKinematics(std::shared_ptr<State<DOF>> state) {
+    for (auto i = 0; i < n_joints_; i++) {
+        state->E[i] = math::SE3::Exp(joints_[i].S, state->q(i));
+
+        int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx;
+        state->T[i] = GetJointT(state, parent_joint_idx) * state->E[i];
+
+        state->S.col(i) = math::SE3::Ad(GetJointT(state, parent_joint_idx), joints_[i].S);
+    }
+}
+
+template<int DOF>
+void Robot<DOF>::ComputeDiffForwardKinematics(std::shared_ptr<State<DOF>> state) {
+    // In Body Frame
+    for (auto i = 0; i < n_joints_; i++) {
+        state->V.col(i) = joints_[i].S * state->qdot(i);
+
+        int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx;
+        state->V.col(i) += math::SE3::InvAd(state->E[i], GetJointV(state, parent_joint_idx));
+    }
+}
+
+template<int DOF>
+void Robot<DOF>::Compute2ndDiffForwardKinematics(std::shared_ptr<State<DOF>> state) {
+    for (auto i = 0; i < n_joints_; i++) {
+        state->Vdot.col(i) = joints_[i].S * state->qddot(i);
+
+        int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx;
+        state->Vdot.col(i) += math::SE3::InvAd(state->E[i], GetJointVdot(state, parent_joint_idx)) +
+                math::SE3::ad(state->V.col(i), joints_[i].S * state->qdot(i));
+    }
+}
+
+template<int DOF>
+void Robot<DOF>::ComputeInverseDynamics(std::shared_ptr<State<DOF>> state) {
+    for (int i = n_joints_ - 1; i >= 0; i--) {
+        Eigen::Vector<double, 6> F;
+        F.setZero();
+
+        for (const auto &child_joint_idx: links_[joints_[i].child_link_idx].child_joint_idx) {
+            F += math::SE3::InvAd(state->E[child_joint_idx]).transpose() * state->F.col(child_joint_idx);
+        }
+        F += links_[joints_[i].child_link_idx].J * state->Vdot.col(i) -
+                math::SE3::adTranspose(state->V.col(i), links_[joints_[i].child_link_idx].J * state->V.col(i));
+
+        state->F.col(i) = F;
+        state->tau(i) = joints_[i].S.dot(F);
+    }
+}
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::ComputeGravityTerm(std::shared_ptr<State<DOF>> state) {
+    Eigen::Vector<double, DOF> gravity_term;
+    gravity_term.resize(n_joints_);
+    std::vector<Eigen::Vector<double, 6> > Vdot{}, Fs{};
+    Vdot.resize(n_joints_);
+    Fs.resize(n_joints_);
+
+    // Calculate Vdot
+    for (auto i = 0; i < n_joints_; i++) {
+        int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx;
+        if (parent_joint_idx < 0) {
+            Vdot[i] = math::SE3::InvAd(state->E[i], state->Vdot0);
+        } else {
+            Vdot[i] = math::SE3::InvAd(state->E[i], Vdot[parent_joint_idx]);
+        }
+    }
+
+    //
+    for (int i = n_joints_ - 1; i >= 0; i--) {
+        Eigen::Vector<double, 6> F;
+        F.setZero();
+
+        for (const auto &child_joint_idx: links_[joints_[i].child_link_idx].child_joint_idx) {
+            F += math::SE3::InvAd(state->E[child_joint_idx]).transpose() * Fs[child_joint_idx];
+        }
+        F += links_[joints_[i].child_link_idx].J * Vdot[i];
+
+        Fs[i] = F;
+        gravity_term(state->rtu_joint_map[i]) = joints_[i].S.dot(F);
+    }
+
+    return gravity_term;
+}
+
+template<int DOF>
+Eigen::Matrix<double, DOF, DOF> Robot<DOF>::ComputeMassMatrix(std::shared_ptr<State<DOF>> state) {
+    Eigen::Matrix<double, DOF, DOF> M(n_joints_, n_joints_);
+    M.setZero();
+    std::vector<Eigen::Matrix<double, 6, DOF> > J;
+    J.resize(n_joints_);
+
+    for (auto i = 0; i < n_joints_; i++) {
+        int parent_joint_idx = links_[joints_[i].parent_link_idx].parent_joint_idx;
+        if (parent_joint_idx < 0) {
+            J[i].setZero();
+            J[i].col(state->rtu_joint_map[i]) = joints_[i].S;
+        } else {
+            J[i] = math::SE3::Ad(state->E[i].inverse()) * J[parent_joint_idx];
+            J[i].col(state->rtu_joint_map[i]) = joints_[i].S;
+        }
+    }
+
+    for (auto i = 0; i < n_joints_; i++) {
+        M += J[i].transpose() * links_[joints_[i].child_link_idx].J * J[i];
+    }
+
+    return M;
+}
+
+template<int DOF>
+Eigen::Matrix<double, 6, 6> Robot<DOF>::ComputeReflectiveInertia(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to) {
+    if (from >= state->utr_link_map.size() || to >= state->utr_link_map.size()) {
+        throw std::runtime_error("Out of range state link");
+    }
+    const auto &J = ComputeBodyJacobian(state, from, to);
+    Eigen::Matrix<double, DOF, DOF> m_inv = ComputeMassMatrix(state).completeOrthogonalDecomposition().
+            pseudoInverse();
+    return (J * m_inv * J.transpose()).completeOrthogonalDecomposition().pseudoInverse();
+}
+
+template<int DOF>
+cmvr::math::SE3::MatrixType Robot<DOF>::ComputeTransformation(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to) {
+    if (from >= state->utr_link_map.size() || to >= state->utr_link_map.size()) {
+        throw std::runtime_error("Out of range state link");
+    }
+    return GetTransformation(state, state->utr_link_map[from], state->utr_link_map[to]);
+}
+
+template<int DOF>
+cmvr::math::se3v::MatrixType Robot<DOF>::ComputeBodyVelocity(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to) {
+    if (from >= state->utr_link_map.size() || to >= state->utr_link_map.size()) {
+        throw std::runtime_error("Out of range state link");
+    }
+    math::SE3::MatrixType T_from_to = ComputeTransformation(state, from, to);
+    math::se3v::MatrixType V_from = GetLinkV(state, state->utr_link_map[from]);
+    math::se3v::MatrixType V_to = GetLinkV(state, state->utr_link_map[to]);
+    return V_to - math::SE3::InvAd(T_from_to, V_from);
+}
+
+template<int DOF>
+Eigen::Matrix<double, 6, DOF> Robot<DOF>::ComputeSpaceJacobian(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to) {
+    return GetSpaceJacobian(state, state->utr_link_map[from], state->utr_link_map[to]);
+}
+
+template<int DOF>
+Eigen::Matrix<double, 6, DOF> Robot<DOF>::ComputeBodyJacobian(std::shared_ptr<State<DOF>> state, unsigned int from, unsigned int to) {
+    math::SE3::MatrixType T = ComputeTransformation(state, from, to);
+    return math::SE3::InvAd(T) * ComputeSpaceJacobian(state, from, to);
+}
+
+template<int DOF>
+double Robot<DOF>::ComputeMass(std::shared_ptr<State<DOF>> state, unsigned int target_link) {
+    const auto &idx = state->utr_link_map[target_link];
+    return Inertial::GetMass(links_[idx.link_idx].links[idx.sub_link_idx].link->I_);
+}
+
+template<int DOF>
+Eigen::Vector3d Robot<DOF>::ComputeCenterOfMass(std::shared_ptr<State<DOF>> state, unsigned int ref_link, unsigned int target_link) {
+    const auto &idx = state->utr_link_map[target_link];
+    return math::SE3::Multiply(ComputeTransformation(state, ref_link, target_link),
+                               Inertial::GetCOM(links_[idx.link_idx].links[idx.sub_link_idx].link->I_));
+}
+
+template<int DOF>
+Eigen::Vector3d Robot<DOF>::ComputeCenterOfMass(std::shared_ptr<State<DOF>> state, unsigned int ref_link, const std::vector<unsigned int> &target_links) {
+    Eigen::Vector3d com{Eigen::Vector3d::Zero()};
+    double mass = 0;
+    for (auto target_link: target_links) {
+        const auto &idx = state->utr_link_map[target_link];
+        const auto &p = Inertial::GetCOM(links_[idx.link_idx].links[idx.sub_link_idx].link->I_);
+        double m = Inertial::GetMass(links_[idx.link_idx].links[idx.sub_link_idx].link->I_);
+        com += math::SE3::Multiply(ComputeTransformation(state, ref_link, target_link), p) * m;
+        mass += m;
+    }
+    return com / mass;
+}
+
+template<int DOF>
+Eigen::Matrix<double, 3, DOF> Robot<DOF>::ComputeCenterOfMassJacobian(std::shared_ptr<State<DOF>> state,
+                                                          unsigned int ref_link,
+                                                          unsigned int target_link) {
+    using namespace math;
+    const auto &idx = state->utr_link_map[target_link];
+    return SE3::GetRotation(ComputeTransformation(state, ref_link, target_link)) *
+           (SE3::InvAd(SE3::T(Inertial::GetCOM(links_[idx.link_idx].links[idx.sub_link_idx].link->I_))) *
+            ComputeBodyJacobian(state, ref_link, target_link))
+           .block(3, 0, 3, n_joints_);
+}
+
+template<int DOF>
+Inertial::MatrixType Robot<DOF>::ComputeTotalInertial(std::shared_ptr<State<DOF>> state, unsigned int ref_link) {
+    Inertial::MatrixType I{Inertial::MatrixType::Zero()};
+    for (const auto &link: links_) {
+        I += Inertial::Transform(GetJointT(state, link.parent_joint_idx), link.J);
+    }
+
+    return Inertial::Transform(math::SE3::Inverse(GetLinkT(state, state->utr_link_map[ref_link])), I);
+}
+
+template<int DOF>
+Eigen::Vector3d Robot<DOF>::ComputeCenterOfMass(std::shared_ptr<State<DOF>> state, unsigned int ref_link) {
+    Eigen::Vector3d sum{Eigen::Vector3d::Zero()};
+    double mass = 0;
+    for (const auto &link: links_) {
+        sum += Inertial::GetMass(link.J) *
+                math::SE3::Multiply(GetJointT(state, link.parent_joint_idx), Inertial::GetCOM(link.J));
+        mass += Inertial::GetMass(link.J);
+    }
+    return math::SE3::Multiply(math::SE3::Inverse(GetLinkT(state, state->utr_link_map[ref_link])), sum / mass);
+}
+
+template<int DOF>
+Eigen::Matrix<double, 3, DOF> Robot<DOF>::ComputeCenterOfMassJacobian(std::shared_ptr<State<DOF>> state,
+                                                          unsigned int ref_link) {
+    using namespace math;
+    Eigen::Matrix<double, 3, DOF> J;
+    J.resize(3, n_joints_);
+    J.setZero();
+    double mass = 0;
+    for (auto i = 0; i < links_.size(); i++) {
+        //      const auto& link = links_[i];
+        double m = Inertial::GetMass(links_[i].I);
+        J += m * SE3::GetRotation(GetTransformation(state, state->utr_link_map[ref_link], {i, 0})) *
+                (SE3::InvAd(SE3::T(Inertial::GetCOM(links_[i].I))) *
+                 GetBodyJacobian(state, state->utr_link_map[ref_link], {i, 0}))
+                .block(3, 0, 3, n_joints_);
+        mass += m;
+    }
+    return J / mass;
+}
+
+template<int DOF>
+std::vector<CollisionResult> Robot<DOF>::DetectCollisionsOrNearestLinks(std::shared_ptr<State<DOF>> state, int collision_threshold) {
+    std::vector<CollisionResult> dis;
+    int n = state->GetLinkNames().size();
+    for (int i = 0; i < n; i++) {
+        auto link1 = GetLink(state, i);
+        if (!link1) {
+            throw std::runtime_error("Index error");
+        }
+        auto link1_T = ComputeTransformation(state, 0, i);
+        for (int j = i + 1; j < n; j++) {
+            auto link2 = GetLink(state, j);
+            if (!link2) {
+                throw std::runtime_error("Index error");
+            }
+            auto link2_T = ComputeTransformation(state, 0, j);
+
+            for (auto link1_col: link1->GetCollisions()) {
+                for (auto link2_col: link2->GetCollisions()) {
+                    for (auto link1_geom: link1_col->GetGeoms()) {
+                        for (auto link2_geom: link2_col->GetGeoms()) {
+                            if (link1_geom->GetType() == GeomType::kCapsule && //
+                                link2_geom->GetType() == GeomType::kCapsule && //
+                                link1_geom->Filter(*link2_geom)) {
+                                auto collision_result =
+                                        link1_geom->ComputeMinimumDistance(
+                                            link1_T * link1_col->GetOrigin(), *link2_geom, //
+                                            link2_T * link2_col->GetOrigin());
+                                if (collision_result.has_value()) {
+                                    auto v = collision_result.value();
+                                    v.link1 = link1->GetName();
+                                    v.link2 = link2->GetName();
+                                    dis.push_back(v);
+                                }
+                            }
+                        }
+                    }
+                }
+            }
+        }
+    }
+
+    std::sort(dis.begin(), dis.end(),
+              [](const CollisionResult &r1, const CollisionResult &r2) { return r1.distance < r2.distance; });
+
+    std::vector<CollisionResult> rv;
+    for (const auto &d: dis) {
+        if (d.distance > 0 && static_cast<int>(rv.size()) >= collision_threshold) {
+            break;
+        }
+        rv.push_back(d);
+    }
+    return rv;
+}
+
+// TODO: ComputeBodyJacobianDot
+// TODO: ComputeBodyAcceleration
+// TODO: InverseDiffDynamics
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::GetLimitQLower(const std::shared_ptr<State<DOF>> &state) {
+    return GetJointProperty(state, [](auto j) { return j->GetLimitQLower(); });
+}
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::GetLimitQUpper(const std::shared_ptr<State<DOF>> &state) {
+    return GetJointProperty(state, [](auto j) { return j->GetLimitQUpper(); });
+}
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::GetLimitQdotLower(const std::shared_ptr<State<DOF>> &state) {
+    return GetJointProperty(state, [](auto j) { return j->GetLimitQdotLower(); });
+}
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::GetLimitQdotUpper(const std::shared_ptr<State<DOF>> &state) {
+    return GetJointProperty(state, [](auto j) { return j->GetLimitQdotUpper(); });
+}
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::GetLimitQddotLower(const std::shared_ptr<State<DOF>> &state) {
+    return GetJointProperty(state, [](auto j) { return j->GetLimitQddotLower(); });
+}
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::GetLimitQddotUpper(const std::shared_ptr<State<DOF>> &state) {
+    return GetJointProperty(state, [](auto j) { return j->GetLimitQddotUpper(); });
+}
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::GetLimitTorque(const std::shared_ptr<State<DOF>> &state) {
+    return GetJointProperty(state, [](auto j) { return j->GetLimitTorque(); });
+}
+
+template<int DOF>
+Eigen::Vector<double, DOF> Robot<DOF>::GetJointProperty(const std::shared_ptr<State<DOF>> &state,
+    const std::function<double(std::shared_ptr<Joint>)> &getter) {
+    Eigen::Vector<double, DOF> prop;
+    prop.resize(n_joints_);
+    for (auto i = 0; i < n_joints_; i++) {
+        prop(state->rtu_joint_map[i]) = getter(joints_[i].joint);
+    }
+    return prop;
+}
+
+template<int DOF>
+void Robot<DOF>::ComputeMobilityInverseDiffKinematics(std::shared_ptr<State<DOF>> state, //
+    const Eigen::Vector2d &linear_velocity, // (m/s)
+    double angular_velocity // (rad/s)
+) {
+    math::se2v::MatrixType S;
+    S(0) = angular_velocity;
+    S.tail<2>() = linear_velocity;
+    ComputeMobilityInverseDiffKinematics(state, S);
+}
+
+template<int DOF>
+void Robot<DOF>::ComputeMobilityInverseDiffKinematics(std::shared_ptr<State<DOF>> state, //
+    const math::se2v::MatrixType &body_velocity // w, x, y
+) {
+    math::se3v::MatrixType S{math::se3v::MatrixType::Zero()};
+    S.block<3, 1>(2, 0) = body_velocity;
+
+    S = math::SE3::InvAd(mobile_base_->T, S);
+
+    switch (mobile_base_->type) {
+        case MobileBaseType::None: {
+            break;
+        }
+        case MobileBaseType::Differential: {
+            auto mb = std::static_pointer_cast<MobileBaseDifferential>(mobile_base_);
+            double v_right = S(3) + mb->wheel_base / 2 * S(2);
+            double v_left = S(3) - mb->wheel_base / 2 * S(2);
+            state->qdot(mb->right_wheel_idx) = -v_right / mb->wheel_radius;
+            state->qdot(mb->left_wheel_idx) = -v_left / mb->wheel_radius;
+            break;
+        }
+        case MobileBaseType::Mecanum: {
+            double v_x = S(3);
+            double v_y = S(4);
+            double w = S(2);
+            // std::cout<<"V_x: "<< v_x<<std::endl;
+            // std::cout<<"V_y: "<< v_y<<std::endl;
+            // std::cout<<"w: "<< w<<std::endl;
+
+            auto mb = std::static_pointer_cast<MobileBaseMecanum>(mobile_base_);
+            double w_1 = 1 / mb->wheel_radius * (v_x - v_y - (mb->L_x + mb->L_y) * w);
+            double w_2 = 1 / mb->wheel_radius * (v_x + v_y + (mb->L_x + mb->L_y) * w);
+            double w_3 = 1 / mb->wheel_radius * (v_x + v_y - (mb->L_x + mb->L_y) * w);
+            double w_4 = 1 / mb->wheel_radius * (v_x - v_y + (mb->L_x + mb->L_y) * w);
+            state->qdot(mb->fl_wheel_idx) = w_1;
+            state->qdot(mb->fr_wheel_idx) = w_2;
+            state->qdot(mb->rl_wheel_idx) = w_3;
+            state->qdot(mb->rr_wheel_idx) = w_4;
+
+            // std::cout<<"mb->fl_wheel_idx: "<<mb->fl_wheel_idx<<std::endl;
+            // std::cout<<"mb->fr_wheel_idx: "<<mb->fr_wheel_idx<<std::endl;
+            // std::cout<<"mb->rl_wheel_idx: "<<mb->rl_wheel_idx<<std::endl;
+            // std::cout<<"mb->rr_wheel_idx: "<<mb->rr_wheel_idx<<std::endl;
+
+            break;
+        }
+    }
+}
+
+template<int DOF>
+cmvr::math::se2v::MatrixType Robot<DOF>::ComputeMobilityDiffKinematics( //
+    std::shared_ptr<State<DOF>> state //
+) {
+    math::se3v::MatrixType S{math::se3v::MatrixType::Zero()};
+
+    switch (mobile_base_->type) {
+        case MobileBaseType::None: {
+            break;
+        }
+        case MobileBaseType::Differential: {
+            auto mb = std::static_pointer_cast<MobileBaseDifferential>(mobile_base_);
+            double w_r = -state->qdot(mb->right_wheel_idx);
+            double w_l = -state->qdot(mb->left_wheel_idx);
+            S.block<3, 1>(2, 0) = math::se2v::MatrixType{
+                (w_r - w_l) * mb->wheel_radius / mb->wheel_base,
+                (w_r + w_l) * mb->wheel_radius / 2, 0
+            };
+            break;
+        }
+        case MobileBaseType::Mecanum: {
+            auto mb = std::static_pointer_cast<MobileBaseMecanum>(mobile_base_);
+            double w_1 = state->qdot(mb->fl_wheel_idx);
+            double w_2 = state->qdot(mb->fr_wheel_idx);
+            double w_3 = state->qdot(mb->rl_wheel_idx);
+            double w_4 = state->qdot(mb->rr_wheel_idx);
+            double v_x = mb->wheel_radius / 4 * (w_1 + w_2 + w_3 + w_4);
+            double v_y = mb->wheel_radius / 4 * (-w_1 + w_2 + w_3 - w_4);
+            double w = mb->wheel_radius / (4 * (mb->L_x + mb->L_y)) * (-w_1 + w_2 - w_3 + w_4);
+            S.block<3, 1>(2, 0) = math::se2v::MatrixType{w, v_x, v_y};
+            break;
+        }
+    }
+    S = math::SE3::Ad(mobile_base_->T, S);
+    return S.block<3, 1>(2, 0);
+}
+
+template<int DOF>
+int Robot<DOF>::CountJoints(const std::shared_ptr<Link> &base_link, bool include_fixed) {
+    int n_joints = 0;
+    std::queue<std::shared_ptr<Link> > que;
+    que.push(base_link);
+    while (!que.empty()) {
+        std::shared_ptr<Link> link = que.front();
+        que.pop();
+
+        for (const auto &joint: link->GetChildJointList()) {
+            que.push(joint->GetChildLink());
+
+            if (joint->IsFixed() && !include_fixed);
+            else {
+                n_joints++;
+            }
+        }
+    }
+    return n_joints;
+}
+
+template<int DOF>
+void Robot<DOF>::Build(const RobotConfiguration &rc) {
+    const auto &base = rc.base_link;
+    int n_joints = CountJoints(base);
+    if constexpr (DOF < 0) {
+        n_links_ = n_joints + 1;
+        n_joints_ = n_joints;
+
+        links_.resize(n_links_);
+        joints_.resize(n_joints_);
+    } else {
+        if (n_joints != DOF) {
+            throw std::runtime_error("DOF does not match the number of joints");
+        }
+        n_links_ = DOF + 1;
+        n_joints_ = DOF;
+    }
+
+    struct QueueItem {
+        int parent_joint_idx{};
+        int parent_link_idx{};
+        std::shared_ptr<Link> link;
+        int depth{};
+        bool merge_parent_link{};
+        math::SE3::MatrixType M_wrt_base;
+        math::SE3::MatrixType M_wrt_p;
+    };
+
+    std::queue<QueueItem> que;
+    int link_idx = 0;
+    int joint_idx = 0;
+    {
+        {
+            QueueItem item;
+            item.parent_joint_idx = -1;
+            item.parent_link_idx = link_idx++;
+            item.link = base;
+            item.depth = 0;
+            item.merge_parent_link = false;
+            item.M_wrt_base = math::SE3::Identity();
+            item.M_wrt_p = math::SE3::Identity();
+            que.push(item);
+        }
+
+        while (!que.empty()) {
+            auto e = que.front();
+            que.pop();
+
+            if (!e.merge_parent_link) {
+                links_[e.parent_link_idx].SetBaseLink(e.link, e.M_wrt_base);
+                links_[e.parent_link_idx].depth = e.depth;
+                links_[e.parent_link_idx].parent_joint_idx = e.parent_joint_idx;
+                link_idx_[e.link->name_] = {e.parent_link_idx, 0};
+            } else {
+                int sub_link_idx = links_[e.parent_link_idx].AddLink(e.link, e.M_wrt_p);
+                link_idx_[e.link->name_] = {e.parent_link_idx, sub_link_idx};
+            }
+
+            for (const auto &joint: e.link->child_joints_) {
+                if (!joint->fixed_) {
+                    math::SE3::MatrixType M_wrt_base = e.M_wrt_base * joint->T_pj_;
+                    {
+                        Joint_ j;
+                        j.joint = joint;
+                        j.S = math::SE3::Ad(M_wrt_base, joint->S_);
+                        j.parent_link_idx = e.parent_link_idx;
+                        j.child_link_idx = link_idx;
+                        joints_[joint_idx] = j;
+                    }
+                    joint_idx_[joint->name_] = joint_idx;
+                    links_[e.parent_link_idx].child_joint_idx.push_back(joint_idx);
+                    {
+                        QueueItem item;
+                        item.parent_joint_idx = joint_idx++;
+                        item.parent_link_idx = link_idx++;
+                        item.link = joint->child_link_;
+                        item.depth = e.depth + 1;
+                        item.merge_parent_link = false;
+                        item.M_wrt_base = M_wrt_base;
+                        item.M_wrt_p = math::SE3::Identity();
+                        que.push(item);
+                    }
+                } else {
+                    QueueItem item;
+                    item.parent_joint_idx = e.parent_joint_idx;
+                    item.parent_link_idx = e.parent_link_idx;
+                    item.link = joint->child_link_;
+                    item.depth = e.depth;
+                    item.merge_parent_link = true;
+                    item.M_wrt_base = e.M_wrt_base * joint->T_pj_ * joint->T_jc_;
+                    item.M_wrt_p = e.M_wrt_p * joint->T_pj_ * joint->T_jc_;
+                    que.push(item);
+                }
+            }
+        }
+    }
+
+    if (rc.mobile_base) {
+        switch (rc.mobile_base->type) {
+            case MobileBaseType::None: {
+                auto mb = std::make_shared<MobileBase>();
+                *std::static_pointer_cast<MobileBase>(mb) = *rc.mobile_base;
+                mobile_base_ = mb;
+                break;
+            }
+            case MobileBaseType::Differential: {
+                auto mb = std::make_shared<MobileBaseDifferential>();
+                *std::static_pointer_cast<MobileBase>(mb) = *rc.mobile_base;
+                if (rc.mobile_base->joints.size() != 2) {
+                    throw std::runtime_error("Differential type mobile should have two joints.");
+                }
+                if (joint_idx_.find(rc.mobile_base->joints[0]) == joint_idx_.end()) {
+                    throw std::runtime_error("Right wheel has invalid parameter.");
+                }
+                if (joint_idx_.find(rc.mobile_base->joints[1]) == joint_idx_.end()) {
+                    throw std::runtime_error("Left wheel has invalid parameter.");
+                }
+                if (rc.mobile_base->params.size() != 2) {
+                    throw std::runtime_error("Differential type mobile should have two parameters.");
+                }
+                mb->right_wheel_idx = joint_idx_[rc.mobile_base->joints[0]];
+                mb->left_wheel_idx = joint_idx_[rc.mobile_base->joints[1]];
+                mb->wheel_base = rc.mobile_base->params[0];
+                mb->wheel_radius = rc.mobile_base->params[1];
+                mobile_base_ = mb;
+                break;
+            }
+            case MobileBaseType::Mecanum:
+                auto mb = std::make_shared<MobileBaseMecanum>();
+                *std::static_pointer_cast<MobileBase>(mb) = *rc.mobile_base;
+                if (rc.mobile_base->joints.size() != 4) {
+                    throw std::runtime_error(
+                        "Mecanum type mobile should have four joints. (front-right, front-left, rear-right, rear-left)");
+                }
+                if (joint_idx_.find(rc.mobile_base->joints[0]) == joint_idx_.end()) {
+                    throw std::runtime_error("Front-right wheel has invalid parameter.");
+                }
+                if (joint_idx_.find(rc.mobile_base->joints[1]) == joint_idx_.end()) {
+                    throw std::runtime_error("Front-left wheel has invalid parameter.");
+                }
+                if (joint_idx_.find(rc.mobile_base->joints[2]) == joint_idx_.end()) {
+                    throw std::runtime_error("Rear-right wheel has invalid parameter.");
+                }
+                if (joint_idx_.find(rc.mobile_base->joints[3]) == joint_idx_.end()) {
+                    throw std::runtime_error("Rear-left wheel has invalid parameter.");
+                }
+                if (rc.mobile_base->params.size() != 3) {
+                    throw std::runtime_error(
+                        "Mecanum type mobile should have three parameters. (Lx, Ly, wheel radius)");
+                }
+                mb->fr_wheel_idx = joint_idx_[rc.mobile_base->joints[0]];
+                mb->fl_wheel_idx = joint_idx_[rc.mobile_base->joints[1]];
+                mb->rr_wheel_idx = joint_idx_[rc.mobile_base->joints[2]];
+                mb->rl_wheel_idx = joint_idx_[rc.mobile_base->joints[3]];
+                mb->L_x = rc.mobile_base->params[0];
+                mb->L_y = rc.mobile_base->params[1];
+                mb->wheel_radius = rc.mobile_base->params[2];
+                mobile_base_ = mb;
+                break;
+        }
+    }
+}
+
+template<int DOF>
+cmvr::math::SE3::MatrixType Robot<DOF>::GetLinkT(std::shared_ptr<State<DOF>> state, const LinkIdx_ &idx) {
+    return GetJointT(state, links_[idx.link_idx].parent_joint_idx) *
+           links_[idx.link_idx].links[idx.sub_link_idx].M_wrt_base;
+}
+
+template<int DOF>
+cmvr::math::se3v::MatrixType Robot<DOF>::GetLinkV(std::shared_ptr<State<DOF>> state, const LinkIdx_ &idx) {
+    return math::SE3::InvAd(links_[idx.link_idx].links[idx.sub_link_idx].M_wrt_base,
+                            GetJointV(state, links_[idx.link_idx].parent_joint_idx));
+}
+
+template<int DOF>
+cmvr::math::SE3::MatrixType Robot<DOF>::GetJointT(std::shared_ptr<State<DOF>> state, int joint_idx) {
+    if (joint_idx < 0)
+        return math::SE3::Identity();
+    return state->T[joint_idx];
+}
+
+template<int DOF>
+cmvr::math::se3v::MatrixType Robot<DOF>::GetJointV(std::shared_ptr<State<DOF>> state, int joint_idx) {
+    if (joint_idx < 0)
+        return state->V0;
+    return state->V.col(joint_idx);
+}
+
+template<int DOF>
+cmvr::math::se3v::MatrixType Robot<DOF>::GetJointVdot(std::shared_ptr<State<DOF>> state, int joint_idx) {
+    if (joint_idx < 0)
+        return state->Vdot0;
+    return state->Vdot.col(joint_idx);
+}
+
+template<int DOF>
+cmvr::math::SE3::MatrixType Robot<DOF>::GetTransformation(std::shared_ptr<State<DOF>> state, const LinkIdx_ &from, const LinkIdx_ &to) {
+    return math::SE3::Inverse(GetLinkT(state, from)) * GetLinkT(state, to);
+}
+
+template<int DOF>
+Eigen::Matrix<double, 6, DOF> Robot<DOF>::GetSpaceJacobian(std::shared_ptr<State<DOF>> state, const LinkIdx_ &from, const LinkIdx_ &to) {
+    Eigen::Matrix<double, 6, DOF> J;
+    J.resize(6, n_joints_);
+    J.setZero();
+
+    // TODO: Need to optimize
+
+    int from_link_idx = from.link_idx;
+    int to_link_idx = to.link_idx;
+    while (from_link_idx != to_link_idx) {
+        if (links_[from_link_idx].depth > links_[to_link_idx].depth) {
+            int pj = links_[from_link_idx].parent_joint_idx;
+            J.col(state->rtu_joint_map[pj]) = -state->S.col(pj);
+            from_link_idx = joints_[pj].parent_link_idx;
+        } else {
+            int pj = links_[to_link_idx].parent_joint_idx;
+            J.col(state->rtu_joint_map[pj]) = state->S.col(pj);
+            to_link_idx = joints_[pj].parent_link_idx;
+        }
+    }
+
+    return math::SE3::InvAd(GetLinkT(state, from)) * J;
+}
+
+template<int DOF>
+Eigen::Matrix<double, 6, DOF> Robot<DOF>::GetBodyJacobian(std::shared_ptr<State<DOF>> state, const LinkIdx_ &from,
+    const LinkIdx_ &to) {
+    math::SE3::MatrixType T = GetTransformation(state, from, to);
+    return math::SE3::InvAd(T) * GetSpaceJacobian(state, from, to);
+}

include/utils/dynamics/state.h

@@ -0,0 +1,141 @@
+//
+// Created by xtkuang on 2025/7/8.
+//
+
+#ifndef STATE_H
+#define STATE_H
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+#include "utils/math/se3.h"
+
+namespace cmvr::dyn {
+template<int DOF>
+class Robot;
+
+template<int DOF>
+class State {
+public:
+    template<int>
+    friend class Robot;
+
+    template<typename T, int N>
+    using ContainerType = typename std::conditional_t<(N > 0), std::array<T, (unsigned int) N>, std::vector<T> >;
+
+    unsigned int GetBaseLinkIdx() const { // NOLINT
+        return base_link_user_idx;
+    }
+
+    template<typename Derived>
+    void SetQ(const Eigen::MatrixBase<Derived> &new_q) {
+        Set<Derived::RowsAtCompileTime>(q, new_q.eval());
+    }
+
+    Eigen::Vector<double, DOF> GetQ() { return q(utr_joint_map); }
+
+    template<typename Derived>
+    void SetQdot(const Eigen::MatrixBase<Derived> &new_qdot) {
+        Set<Derived::RowsAtCompileTime>(qdot, new_qdot.eval());
+    }
+
+    Eigen::Vector<double, DOF> GetQdot() { return qdot(utr_joint_map); }
+
+    template<typename Derived>
+    void SetQddot(const Eigen::MatrixBase<Derived> &new_qddot) {
+        Set<Derived::RowsAtCompileTime>(qddot, new_qddot.eval());
+    }
+
+    Eigen::Vector<double, DOF> GetQddot() { return qddot(utr_joint_map); }
+
+    template<typename Derived>
+    void SetTau(const Eigen::MatrixBase<Derived> &new_tau) {
+        Set<Derived::RowsAtCompileTime>(tau, new_tau.eval());
+    }
+
+    Eigen::Vector<double, DOF> GetTau() { return tau(utr_joint_map); }
+
+    void SetV0(const math::se3v::MatrixType &new_V0) { V0 = new_V0; }
+
+    // Vdot of root link in root frame
+    void SetVdot0(const math::se3v::MatrixType &new_Vdot0) { Vdot0 = new_Vdot0; }
+
+    void SetGravity(const math::se3v::MatrixType &gravity) { Vdot0 = -gravity; }
+
+    ContainerType<std::string, DOF> GetJointNames() const { return joint_names; }
+
+    [[nodiscard]] std::vector<std::string> GetLinkNames() const { return link_names; }
+
+private:
+    explicit State(int dof) {
+        if (!(DOF < 0 || dof == DOF)) {
+            throw std::runtime_error("State initialization failed");
+        }
+
+        if constexpr (DOF < 0) {
+            q.resize(dof);
+            qdot.resize(dof);
+            qddot.resize(dof);
+            tau.resize(dof);
+            E.resize(dof + 1, math::SE3::Identity());
+            S.resize(6, dof);
+            T.resize(dof + 1, math::SE3::Identity());
+            V.resize(6, dof);
+            Vdot.resize(6, dof);
+            F.resize(6, dof);
+            joint_names.resize(dof);
+            utr_joint_map.resize(dof);
+            rtu_joint_map.resize(dof);
+        } else {
+            E.fill(math::SE3::Identity());
+            T.fill(math::SE3::Identity());
+        }
+
+        q.setZero();
+        qdot.setZero();
+        qddot.setZero();
+        S.setZero();
+        V.setZero();
+        Vdot0.setZero();
+        F.setZero();
+    }
+
+    template<int N>
+    void Set(Eigen::Vector<double, DOF> &s, const Eigen::Vector<double, N> &i) {
+        if (s.size() == i.size()) {
+            s = i(rtu_joint_map);
+        } else if (s.size() > i.size()) {
+            s.template head<>(i.size()) = i(rtu_joint_map.template head<>(i.size()));
+        } else {
+            throw std::runtime_error("i.size cannot be greater than s.size");
+        }
+    }
+
+    ContainerType<std::string, DOF> joint_names;
+    Eigen::Vector<unsigned int, DOF> utr_joint_map; // user joint idx -> robot joint idx
+    Eigen::Vector<unsigned int, DOF> rtu_joint_map; // robot joint idx -> user joint idx
+
+    unsigned int base_link_user_idx;
+    std::vector<std::string> link_names;
+    std::vector<typename Robot<DOF>::LinkIdx_> utr_link_map; // user link idx -> robot dummy/sub link idx
+
+public:
+    Eigen::Vector<double, DOF> q;
+    Eigen::Vector<double, DOF> qdot;
+    Eigen::Vector<double, DOF> qddot;
+    Eigen::Vector<double, DOF> tau;
+    math::se3v::MatrixType V0{math::se3v::MatrixType::Zero()};
+    math::se3v::MatrixType Vdot0{math::se3v::MatrixType::Zero()};
+
+    ContainerType<math::SE3::MatrixType, DOF> E{}; // exponential mapping
+    Eigen::Matrix<double, 6, DOF> S; // S(q_0,...q_i) in root frame
+
+    ContainerType<math::SE3::MatrixType, DOF> T{}; // product of exponential
+    Eigen::Matrix<double, 6, DOF> V;
+    Eigen::Matrix<double, 6, DOF> Vdot;
+    Eigen::Matrix<double, 6, DOF> F;
+};
+
+}
+
+#endif //STATE_H

include/utils/math/constants.h

@@ -0,0 +1,52 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef CONSTANCE_H
+#define CONSTANCE_H
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+
+namespace cmvr::math {
+
+    constexpr double kPi = 3.1415926535897932384626433832795;                ///< \pi
+    constexpr double kPiHalf = 1.5707963267948966192313216916398;            ///< \frac{\pi}{2}
+    constexpr double kPiDouble = 6.283185307179586476925286766559;           ///< \pi*2
+    constexpr double kInvPiDouble = 0.15915494309189533576888376337251;      ///< \frac{1}{\pi*2}
+    constexpr double kPiDividedBySqrt2 = 2.2214414690791831235079404950303;  ///< \frac{\pi}{\sqrt{2}}
+    constexpr double kPiSquare = 9.8696044010893586188344909998762;          ///< \pi^2
+
+    constexpr double kRad2Deg = 57.295779513082320876798154814105;   ///< \frac{180}{\pi}
+    constexpr double kDeg2Rad = 0.01745329251994329576923690768489;  /// \frac{\pi}{180}
+
+    static const Eigen::Matrix<double, 1, 4> kAffineConstant = {0, 0, 0, 1};
+
+    static const double kDoubleEpsilon = std::numeric_limits<double>::epsilon();
+
+    /// Calculate sine and cosine simultaneously
+    inline void fsincos(double theta,   ///< Angle in radians
+                        double& sine,   ///< Variable for storing a sine value
+                        double& cosine  ///< Variable for storing a sine value
+    ) {
+        using std::sin;
+        using std::sqrt;
+
+        theta -= (int)(theta * kInvPiDouble) * kPiDouble;
+        if (theta < 0)
+            theta += kPiDouble;
+
+        sine = sin(theta);
+        if (theta < kPiHalf) {
+            cosine = sqrt(1 - sine * sine);
+            return;
+        } else if (theta < kPi + kPiHalf) {
+            cosine = -sqrt(1 - sine * sine);
+            return;
+        }
+        cosine = sqrt(1 - sine * sine);
+    }
+}
+
+#endif //CONSTANCE_H

include/utils/math/geometry.h

@@ -0,0 +1,57 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef GEOMETRY_H
+#define GEOMETRY_H
+
+namespace cmvr::math {
+    typedef struct {
+        double x;
+        double y;
+    } Vec2;
+
+    typedef struct {
+        double x;
+        double y;
+        double z;
+    } Vec3;
+
+    /// quat
+    typedef struct {
+        double w;
+        double x;
+        double y;
+        double z;
+    } Quat;
+
+    /// position 3d
+    typedef struct {
+        double x;        //* unit: m
+        double y;
+        double z;
+    } Position;
+
+    /// euler angel
+    typedef struct {
+        double rx; //* unit: rad
+        double ry;
+        double rz;
+    } Euler;
+
+    /// pose 3d
+    typedef struct {
+        Position position;     ///< 位置，单位：m
+        Quat quaternion;       ///< 四元数
+        Euler euler;           ///< 欧拉角，单位：rad
+    } Pose3d;
+
+    typedef struct {
+        double x;        //* unit: m
+        double y;
+        double theta;
+    } Pose2d;
+}
+
+
+#endif //GEOMETRY_H

include/utils/math/liegroup.h

@@ -0,0 +1,15 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef LIEGROUP_H
+#define LIEGROUP_H
+
+#pragma once
+
+#include "se2.h"
+
+#include "se3.h"
+#include "so3.h"
+
+#endif //LIEGROUP_H

include/utils/math/qp_solver.h

@@ -0,0 +1,126 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef QP_SOLVER_H
+#define QP_SOLVER_H
+
+#pragma once
+
+#include <exception>
+#include <memory>
+#include <optional>
+
+#include <eigen3/Eigen/Core>
+#include <OsqpEigen/OsqpEigen.h>
+
+namespace cmvr::math {
+
+class QPSolverException : public std::exception {
+public:
+    static constexpr unsigned int kStatusOffset = 100;
+
+    explicit QPSolverException(int error_code);
+
+    const char* what() const noexcept override;
+
+    int code() const noexcept;
+
+    static std::string GenerateMessage(int code);
+
+private:
+    int error_code_;
+    std::string message_;
+};
+
+class QPSolverImpl {
+public:
+    QPSolverImpl() = default;
+    ~QPSolverImpl() = default;
+
+    void SetupImpl(int n_var, int n_const, double time_limit);
+    void InitFunctionImpl();
+    void AddCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b);
+    void SetCostFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &b);
+    void SetConstraintsFunctionImpl(const Eigen::MatrixXd &A, const Eigen::VectorXd &lb, const Eigen::VectorXd &ub);
+    void SetPrimalVariableImpl(const Eigen::VectorXd &pv);
+    void ResetIsFirstImpl();
+    Eigen::VectorXd SolveImpl();
+    Eigen::MatrixXd GetACostImpl() const;
+    Eigen::VectorXd GetBCostImpl() const;
+    Eigen::MatrixXd GetAConstImpl() const;
+    Eigen::VectorXd GetLowerBoundImpl() const;
+    Eigen::VectorXd GetUpperBoundImpl() const;
+
+private:
+    OsqpEigen::Solver solver_;
+    int n_var_{};
+    int n_const_{};
+    int err_code_{};
+
+    int is_first_{};
+
+    int n_hessian_element_{};
+
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_cost_;
+    Eigen::Matrix<double, Eigen::Dynamic, 1> b_cost_;
+
+    Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> A_const_;
+    Eigen::Matrix<double, Eigen::Dynamic, 1> lb_;
+    Eigen::Matrix<double, Eigen::Dynamic, 1> ub_;
+
+    Eigen::SparseMatrix<double> hessian_;
+    Eigen::Matrix<double, Eigen::Dynamic, 1> gradient_;
+
+    Eigen::SparseMatrix<double> linearMatrix_;
+    Eigen::Matrix<double, Eigen::Dynamic, 1> lowerBound_;
+    Eigen::Matrix<double, Eigen::Dynamic, 1> upperBound_;
+
+    Eigen::Matrix<double, Eigen::Dynamic, 1> primal_variable_for_warmstart_;
+};
+
+
+class QPSolver {
+public:
+    QPSolver();
+
+    ~QPSolver();
+
+    void Setup(int n_var, int n_const, double time_limit = 2e-3);
+
+    void InitFunction();
+
+    void AddCostFunction(const Eigen::MatrixXd& A, const Eigen::VectorXd& b);
+
+    void SetCostFunction(const Eigen::MatrixXd& A, const Eigen::VectorXd& b);
+
+    void SetConstraintsFunction(const Eigen::MatrixXd& A, const Eigen::VectorXd& lb, const Eigen::VectorXd& ub);
+
+    void SetPrimalVariable(const Eigen::VectorXd& pv);
+
+    void ResetIsFirst();
+
+    /**
+     * Solve the QP problem
+     * @return Solution
+     * @throw QPSolverException
+     */
+    Eigen::VectorXd Solve();
+
+    Eigen::MatrixXd GetACost() const;  // NOLINT
+
+    Eigen::VectorXd GetBCost() const;  // NOLINT
+
+    Eigen::MatrixXd GetAConst() const;  // NOLINT
+
+    Eigen::VectorXd GetLowerBound() const;  // NOLINT
+
+    Eigen::VectorXd GetUpperBound() const;  // NOLINT
+
+private:
+    std::unique_ptr<QPSolverImpl> impl_;
+};
+
+}
+
+#endif //QP_SOLVER_H

include/utils/math/se2.h

@@ -0,0 +1,42 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef SE2_H
+#define SE2_H
+
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+#include "utils/math/constants.h"
+
+namespace cmvr::math {
+
+    class SE2;
+    class se2v;
+
+    class se2v {
+    public:
+        using MatrixType = Eigen::Vector3d;
+
+    private:
+        se2v() = default;
+    };
+
+    class SE2 {
+    public:
+        using MatrixType = Eigen::Matrix3d;
+
+        static MatrixType Identity();
+
+        static MatrixType T(double angle, const Eigen::Vector<double, 2>& translation = {0, 0});
+
+        static MatrixType Exp(const se2v::MatrixType& s, double angle = 1.0);
+
+        static se2v::MatrixType Log(const MatrixType& T);
+    };
+
+}  // namespace rb::math
+
+#endif //SE2_H

include/utils/math/se3.h

@@ -0,0 +1,90 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef SE3_H
+#define SE3_H
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+#include <optional>
+
+#include "so3.h"
+
+namespace cmvr::math {
+
+class SE3;
+class se3;
+class se3v;
+
+class se3 {
+ public:
+  using MatrixType = Eigen::Matrix4d;
+
+ private:
+  se3() = default;
+};
+
+class se3v {
+ public:
+  using MatrixType = Eigen::Vector<double, 6>;
+
+ private:
+  se3v() = default;
+};
+
+class SE3 {
+ public:
+  using MatrixType = Eigen::Matrix4d;
+
+  static MatrixType Identity();
+
+  static MatrixType T(const Eigen::Vector3d& p);
+
+  static MatrixType T(const SO3::MatrixType& R, const Eigen::Vector3d& p = Eigen::Vector3d::Zero());
+
+  static MatrixType Inverse(const MatrixType& T);
+
+  static SO3::MatrixType GetRotation(const MatrixType& T);
+
+  static Eigen::Vector3d GetPosition(const MatrixType& T);
+
+  static MatrixType Exp(const se3v::MatrixType& S, double angle = 1.);
+
+  static MatrixType Exp(so3v::MatrixType w, Eigen::Vector3d v, double angle = 1.);
+
+  static se3v::MatrixType Log(const MatrixType& T);
+
+  static typename Eigen::Matrix<double, 6, 6> Ad(const MatrixType& T);
+
+  static se3v::MatrixType Ad(const MatrixType& T, const se3v::MatrixType& S);
+
+  static Eigen::Matrix<double, 6, 6> InvAd(const MatrixType& T);
+
+  static se3v::MatrixType InvAd(const MatrixType& T, const se3v::MatrixType& S);
+
+  static typename Eigen::Matrix<double, 6, 6> ad(const se3v::MatrixType& S);
+
+  static Eigen::Matrix<double, 6, 6> adTranspose(const se3v::MatrixType& S);
+
+  static se3v::MatrixType ad(const se3v::MatrixType& S1, const se3v::MatrixType& S2);
+
+  static se3v::MatrixType adTranspose(const se3v::MatrixType& S1, const se3v::MatrixType& S2);
+
+  static Eigen::Vector3d Multiply(const MatrixType& T, const Eigen::Vector3d& p);
+
+  template <typename Container, typename = std::enable_if_t<std::is_same_v<typename Container::value_type, MatrixType>>>
+  static std::optional<MatrixType> Average(const Container& matrices, double eps, int max_iter = -1);
+
+  static se3v::MatrixType Vec(const se3::MatrixType& s);
+
+  static se3::MatrixType Hat(const se3v::MatrixType& v);
+
+ private:
+  SE3() = default;
+};
+
+}  // namespace rb::math
+
+#endif //SE3_H

include/utils/math/so3.h

@@ -0,0 +1,121 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef SO3_H
+#define SO3_H
+
+#pragma once
+
+#include <eigen3/Eigen/Core>
+#include <eigen3/Eigen/Geometry>
+#include <optional>
+#include <utility>
+
+#include "constants.h"
+
+namespace cmvr::math {
+    enum class EulerAngleType { ZYX, ZYZ };
+
+    class SO3;
+    class so3;
+    class so3v;
+
+    class so3 {
+    public:
+        using MatrixType = Eigen::Matrix3d;
+
+    private:
+        so3() = default;
+    };
+
+    class so3v {
+    public:
+        using MatrixType = Eigen::Vector3d;
+
+    private:
+        so3v() = default;
+    };
+
+    class SO3 {
+    public:
+        using MatrixType = Eigen::Matrix3d;
+
+        static MatrixType Identity();
+
+        static MatrixType Inverse(const MatrixType &R);
+
+        static MatrixType Exp(so3v::MatrixType w, double angle = 1.0);
+
+        static so3v::MatrixType Log(const MatrixType &R);
+
+        static MatrixType RotX(double angle);
+
+        static MatrixType RotY(double angle);
+
+        static MatrixType RotZ(double angle);
+
+        static MatrixType FromEulerAngle(const Eigen::Vector3d &angles, EulerAngleType type);
+
+        static Eigen::Vector3d ToEulerAngle(const MatrixType &R, EulerAngleType type);
+
+        static MatrixType FromQuaternion(const Eigen::Quaterniond &q);
+
+        static Eigen::Quaterniond ToQuaternion(const MatrixType &R);
+
+        /**
+         * Calculate rotation from roll, pitch and yqw angles
+         * @param angles [0] roll [1] pitch [2] yaw
+         * @return
+         */
+        static MatrixType FromRPY(const Eigen::Vector3d &angles);
+
+        /**
+         *
+         * @param R
+         * @return [0] roll [1] pitch [2] yaw
+         */
+        static Eigen::Vector3d ToRPY(const MatrixType &R);
+
+        static MatrixType Projection(const MatrixType &m);
+
+        template<typename Container, typename = std::enable_if_t<std::is_same_v<typename Container::value_type,
+            MatrixType> > >
+        static std::optional<MatrixType> Average(const Container &matrices, double eps, int max_iter = -1) {
+            if (matrices.size() == 0) {
+                return Identity();
+            }
+
+            SO3::MatrixType avg = *matrices.begin();
+            for (int i = 0; max_iter < 0 || i < max_iter; i++) {
+                Eigen::Vector3d w = Eigen::Vector3d::Zero();
+
+                for (const auto &m: matrices) {
+                    w += Log(m * avg.inverse());
+                }
+                w /= matrices.size();
+                avg = SO3::Exp(w) * avg;
+                if (w.norm() < eps) {
+                    return avg;
+                }
+            }
+
+            return std::nullopt;
+        }
+
+        static Eigen::Vector3d GetX(const MatrixType &R);
+
+        static Eigen::Vector3d GetY(const MatrixType &R);
+
+        static Eigen::Vector3d GetZ(const MatrixType &R);
+
+        static so3v::MatrixType Vec(const so3::MatrixType &r);
+
+        static so3::MatrixType Hat(const so3v::MatrixType &w);
+
+    private:
+        SO3() = default;
+    };
+}
+
+#endif //SO3_H