feat:add eye hand calibration and pose follow

.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++)
+
+############################################################
+# 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
+        cmvr_es::httpclient
+        cmvr_es::device::canbus
+        cmvr_es::robot::c701
+        cmvr_es::device::ti5motor
+)

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

@@ -0,0 +1,138 @@
+# CMVR-ES
+
+## Overview
+
+## Installation
+
+### 1. Git submodules install 
+
+```
+git submodule update --init --recursive
+```
+
+### 2. Dependency install
+
+```shell
+# 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
+
+# json
+sudo apt install libjsoncpp-dev -y
+
+# google
+sudo apt install libgoogle-glog-dev -y
+
+# modbus
+sudo apt install libmodbus-dev -y
+
+# vision
+sudo apt install libeigen3-dev -y
+sudo apt install libopencv-dev python3-opencv -y
+
+# fcl
+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 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 -y
+sudo apt-get install librealsense2-utils -y
+sudo apt-get install librealsense2-dev -y
+sudo apt-get install librealsense2-dbg -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
+```

config/cabin_robot.xml

@@ -0,0 +1,138 @@
+<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="color" codec="H265"/>-->
+<!--                <RealsenseCamera id="cam4" serial="243122075614" w="640" h="480" fps="30" mode="video" stream_mode="rgbd" 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" codec="H265"/>-->
+            </Camera>
+
+            <DexHand>
+<!--                <RH56DFTP id="hand1" default_force="500" default_speed="500" ip_address="10.148.108.115" port="6000">-->
+<!--                    <Freedom order="01" default_force="500" default_speed="500" />-->
+<!--                </RH56DFTP>-->
+<!--                <RH56DFTP id="hand2" default_force="500" default_speed="500" ip_address="10.148.108.113" 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"
+                          bufferSize="50">
+                    <CanManger id="" devId="">
+                        <LeftArmCan id = " " devId = " " channelId ="0">
+                            <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="29" jointName="L_WRIST_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>-->
+                        </LeftArmCan>
+                        <RightArmCan id = " " devId = " " channelId ="1">
+                            <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>
+                        <WaistCan id = " " devId = " " channelId ="2">
+                            <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="2" sampleRate="44100" volume="80"/>-->
+            </Microphone>
+
+            <Speaker>
+<!--                <ffmpegSpeaker id="spk1" serial="" alas="hw:0,0" channels="2" sampleRate="44100" softResample="1" latency="500000" 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="50055">
+
+    </gRPCServer>
+
+</CMVR-ES>

config/robot_description/hc_description/dual_arm.urdf

@@ -0,0 +1,582 @@
+<?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.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="-1.5707963267948966 0 3.141592653589793"/>
+    <parent link="R_WRIST_R_S"/>
+    <child link="R_FINGER_TIP"/>
+    <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/controller/cartesian_controller.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,114 @@
+//
+// 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 "devices/abstract_robot.h"
+#include "device_manager/device_manager.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) {
+        // 如果已经初始化过，则直接返回
+        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) {
+        if (q.size() != 7)
+            throw std::runtime_error("Expected 7 joint values");
+
+        std::vector<std::string> joint_names;
+        if (side == "left") {
+            joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
+                           "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
+        } else if (side == "right") {
+            joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
+                           "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
+        } else {
+            throw std::runtime_error("Side must be 'left' or 'right'");
+        }
+
+        std::vector<JointPoint> cmd;
+        cmd.push_back({"WAIST_Y", 0});
+        cmd.push_back({"WAIST_P", 0});
+        for (size_t i = 0; i < 7; ++i)
+            cmd.push_back({joint_names[i], q[i]});
+
+        robot_->moveJ(cmd, 0.8);
+    }
+
+
+    void torqueOn() {
+        if (!robot_)
+            throw std::runtime_error("Robot not initialized");
+        robot_->torqueOn();
+    }
+
+    void torqueOff() {
+        if (!robot_)
+            throw std::runtime_error("Robot not initialized");
+        robot_->torqueOff();
+    }
+
+    std::vector<double> getJointQ(const std::string& side) const {
+        if (!robot_)
+            throw std::runtime_error("Robot not initialized");
+
+        auto joint_qs = robot_->getJointQ();  // unordered_map<std::string,double>
+
+        std::vector<std::string> joint_names;
+        if (side == "left") {
+            joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
+                           "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
+        } else if (side == "right") {
+            joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
+                           "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
+        } else {
+            throw std::runtime_error("Side must be 'left' or 'right'");
+        }
+
+        std::vector<double> values;
+        for (const auto& name : joint_names) {
+            values.push_back(joint_qs[name]);
+        }
+
+        return values;
+    }
+
+
+
+private:
+    static DeviceManager* dmgr_;
+    static std::shared_ptr<AbstractRobot> robot_;
+};
+
+// 静态成员初始化
+DeviceManager* PyRobotWrapper::dmgr_ = nullptr;
+std::shared_ptr<AbstractRobot> PyRobotWrapper::robot_ = nullptr;
+
+PYBIND11_MODULE(robot_wrapper, m) {
+    py::class_<PyRobotWrapper>(m, "Robot")
+        .def(py::init<const std::string&, const std::string&>())
+        .def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"))
+        .def("torqueOn", &PyRobotWrapper::torqueOn)
+        .def("torqueOff", &PyRobotWrapper::torqueOff)
+        .def("getJointQ", &PyRobotWrapper::getJointQ, py::arg("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/xtkuang/projects/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_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,129 @@
+//
+// Created by xtkuang on 2025/7/31.
+//
+
+#include "utils/dynamics/robot.h"
+#include "utils/controller/cartesian_controller.h"
+#include <Eigen/Dense>
+#include <iostream>
+#include <memory>
+#include <string>
+#include <vector>
+#include <cmath>
+
+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_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/xtkuang/projects/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.3448998226801712, 0.9351815151808957, 2.2703399775931388, 1.6895896595300133, -2.3279553847104077, 0.46019613485676647, 0.3009592467697589;
+    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/controller/cartesian_controller.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,63 @@
+#ifndef CMVR_ES_ABSTRACT_CAMERA_H
+#define CMVR_ES_ABSTRACT_CAMERA_H
+#pragma once
+
+#include <opencv2/opencv.hpp>
+#include "abstract_device.h"
+
+namespace cmvr::device {
+    struct StreamFrameData
+    {
+        //原始图像数据
+        cv::Mat rgbImage;
+        //深度图原始数据
+        cv::Mat depthImage;
+        //彩色图像帧
+        std::vector<uint8_t> rgbFrame;
+        //深度图像帧
+        std::vector<uint8_t> depthFrame;
+        //编码格式
+        std::string codec = ".h264";
+
+        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) {}
+        virtual void getDepthImage(cv::Mat &depth) {}
+        virtual void getRGBDImages(cv::Mat &color, cv::Mat &depth) {}
+        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() {}
+    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,104 @@
+//
+// 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_;
+
+    };
+    }

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,171 @@
+//
+// 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 "motor/motor_protocol_interface.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;
+        };
+
+        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 void setProtocol(std::shared_ptr<MotorProtocolInterface> 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<MotorProtocolInterface> protocol_;
+    };
+}
+#endif //CMVR_ES_ABSTRACT_MOTOR_H

include/devices/abstract_robot.h

@@ -0,0 +1,106 @@
+//
+// 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/controller/cartesian_controller.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 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 torqueOff() { 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_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
+
+        virtual void moveJ_IK(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(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 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 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;
+    protected:
+        int dof_{};
+        RobotState state_{};
+    };
+}
+
+#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,143 @@
+//
+// 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
+    } RobotJointIndexDirection;
+
+    /// robot cartesian index
+    typedef enum {
+        X, Y, Z, RX, RY, RZ
+    } RobotCartesian;
+
+    typedef struct {
+        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_interface.h

@@ -0,0 +1,16 @@
+//
+// Created by xtkuang on 2025/5/6.
+//
+
+#ifndef CMVR_ES_CAN_INTERFACE_H
+#define CMVR_ES_CAN_INTERFACE_H
+
+namespace cmvr {
+
+    class can_interface {
+
+    };
+
+} // cmvr
+
+#endif //CMVR_ES_CAN_INTERFACE_H

include/hardware/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_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/http/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/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_camera_service.h

@@ -0,0 +1,46 @@
+//
+// 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;
+    public:
+        void read_message_(grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback
+            , cmvr::api::GetRGBImageStreamCommand_Request>* stream);
+        void write_message_(grpc::ServerReaderWriter<cmvr::api::GetRGBImageStreamCommand_Feedback
+            , cmvr::api::GetRGBImageStreamCommand_Request>* stream,std::shared_ptr<cmvr::device::AbstractCamera> dev);
+    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_dexhand_service.h

@@ -0,0 +1,30 @@
+//
+// 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_;
+    };
+
+}
+
+#endif //GRPC_DEXHAND_SERVICE_H

include/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_humanoid_robot_service.h

@@ -0,0 +1,30 @@
+//
+// Created by lgv on 2025/8/15.
+//
+
+#pragma once
+
+#include "device_manager/device_manager.h"
+#include "cmvr/api/humanoid_robot.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;
+        private:
+            device::DeviceManager& dmgr_;
+        };
+
+
+
+    }
+
+}

include/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_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_system_service.h

@@ -0,0 +1,25 @@
+//
+// 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;
+    private:
+        device::DeviceManager& dmgr_;
+    };
+}
+
+#endif //GRPC_SYSTEM_SERVICE_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/controller/cartesian_controller.h

@@ -0,0 +1,118 @@
+//
+// Created by xtkuang on 2025/7/21.
+//
+
+#ifndef CMVR_ES_CARTESIAN_CONTROLLER_H
+#define CMVR_ES_CARTESIAN_CONTROLLER_H
+
+#pragma once
+
+#include <Eigen/Core>
+#include <Eigen/Dense>
+#include <vector>
+#include <string>
+#include <memory>
+#include <cmath>
+#include <fcl/fcl.h>
+#include <optional>
+#include <unordered_map>
+
+#include "utils/math/se3.h"
+#include "utils/math/qp_solver.h"
+#include "utils/dynamics/robot.h"
+
+namespace cmvr::ctrl{
+    using cmvr::math::SE3;
+
+    // -----------------------------------------------------------------------------
+    // Task & collision descriptions
+    // -----------------------------------------------------------------------------
+    struct PoseTarget {
+        std::string     link_name;    //!< link to be controlled
+        Eigen::Matrix4d T_target;     //!< desired pose w.r.t *base_link*
+        double          w_posrot = 0.5; // [0→pure pos, 1→pure rot]
+        double          weight   = 1.0; // task weight (0‑1 soft, 1 hard)
+    };
+
+    struct CollisionPair { std::string link_A, link_B; };
+
+    // -----------------------------------------------------------------------------
+    // CartesianController declaration (implementation in .cpp)
+    // -----------------------------------------------------------------------------
+    template<int DOF>
+    class CartesianController {
+    public:
+        using RobotT = cmvr::dyn::Robot<DOF>;
+        using VecD   = Eigen::Vector<double, DOF>;
+        using Mat6D  = Eigen::Matrix<double, 6, DOF>;
+
+        enum class Mode { Position, Velocity, Torque };
+
+        CartesianController(std::shared_ptr<RobotT> robot, double dsafe = 0.05, double lambda = 1e-2);
+        /* ------------------------- configuration ----------------------------- */
+        void setCollisionPairs(const std::vector<CollisionPair>& pairs);
+
+        // Joint limits (size = DOF).  Any value = std::nullopt → no limit.
+        void setPositionLimits(const std::vector<std::optional<double>>& q_min, const std::vector<std::optional<double>>& q_max);
+        void setVelocityLimits(const std::vector<std::optional<double>>& qd_max);
+        void setAccelerationLimits(const std::vector<std::optional<double>>& qdd_max);
+
+        /* --------------------------- main API -------------------------------- */
+        /**
+         * Compute joint command given Cartesian objectives.
+         * @param state   current (mutable) robot state
+         * @param base    reference frame (string)
+         * @param targets list of PoseTarget
+         * @param dt      controller step [s]
+         * @param mode    desired output type
+         * @param out_cmd filled with q / q̇ / τ
+         * @return        true if IK converged (position) or soft OK (others)
+         */
+        bool compute(
+                std::shared_ptr<cmvr::dyn::State<DOF>>& state,
+                const std::string& base_link,
+                const std::vector<PoseTarget>& targets,
+                double dt,
+                Mode mode,
+                VecD& out_cmd,
+                int max_iters = 60,
+                double tol = 1e-3,
+                double alpha_h = 10.0);
+
+    private:
+        /* -------- internal helpers (implemented in .cpp) --------------------- */
+        bool solveIK(std::shared_ptr<cmvr::dyn::State<DOF>>& state, const std::string& base, const std::vector<PoseTarget>& targets,
+                     const VecD& q_init, VecD& q_out, int max_iters, double tol, double alpha_h) const;
+
+        bool distanceConstraint(const CollisionPair& cp, std::shared_ptr<cmvr::dyn::State<DOF>>& state,
+                                const std::vector<std::shared_ptr<fcl::CollisionObjectd>>& objs,
+                                double& h_out, Eigen::RowVectorXd& J_out) const;
+
+        inline void clampVec(VecD& v, const std::vector<std::optional<double>>& lo, const std::vector<std::optional<double>>& hi) const;
+
+        /* -------------------------- data ------------------------------------ */
+        std::shared_ptr<RobotT> robot_;
+        std::vector<CollisionPair> pairs_;
+        double d_safe_  = 0.05;
+        double lambda_  = 1e-2;
+        mutable math::QPSolver solver_;
+
+        // joint limits (optional)
+        std::vector<std::optional<double>> q_min_  = std::vector<std::optional<double>>(DOF);
+        std::vector<std::optional<double>> q_max_  = std::vector<std::optional<double>>(DOF);
+        std::vector<std::optional<double>> qd_max_ = std::vector<std::optional<double>>(DOF);
+        std::vector<std::optional<double>> qdd_max_= std::vector<std::optional<double>>(DOF);
+
+        // FCL reusable buffers
+        mutable fcl::DistanceRequestd dist_req_{};
+        mutable fcl::DistanceResultd dist_res_{};
+    };
+}
+
+// ------------------------------ explicit instantiation -----------------------
+extern template class cmvr::ctrl::CartesianController<7>;
+extern template class cmvr::ctrl::CartesianController<14>;
+extern template class cmvr::ctrl::CartesianController<20>;
+
+
+#endif //CMVR_ES_CARTESIAN_CONTROLLER_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

include/utils/math/velocity_estimator.h

@@ -0,0 +1,124 @@
+//
+// Created by xtkuang on 2025/7/7.
+//
+
+#ifndef VELOCITY_ESTIMATOR_H
+#define VELOCITY_ESTIMATOR_H
+
+#pragma once
+
+#include <cstddef>
+#include <queue>
+
+#include "Eigen/Dense"
+
+namespace cmvr {
+    class VelocityFilterEstimator {
+    public:
+        explicit VelocityFilterEstimator(double dt, int avg_count = 5)
+            : avg_count_(avg_count), valid_(false), velocity_(0), dt_(dt), sum_velocity_(0) {
+        }
+
+        void Update(double position, bool valid) {
+            if (valid && valid_) {
+                double v = (position - prev_position_) / dt_;
+                velocities_.push(v);
+                sum_velocity_ += v;
+                while (velocities_.size() > avg_count_) {
+                    sum_velocity_ -= velocities_.front();
+                    velocities_.pop();
+                }
+            } else {
+                while (!velocities_.empty()) {
+                    velocities_.pop();
+                }
+                sum_velocity_ = 0;
+            }
+            prev_position_ = position;
+            valid_ = valid;
+        }
+
+        double GetVelocity() {
+            if (velocities_.empty()) {
+                return 0.;
+            }
+            return sum_velocity_ / (double) velocities_.size();
+        }
+
+    private:
+        unsigned int avg_count_;
+        std::queue<double> velocities_;
+        double sum_velocity_;
+
+        bool valid_;
+        double prev_position_{0.};
+
+        double velocity_;
+        double dt_;
+    };
+
+    class VelocityEstimator {
+    public:
+        VelocityEstimator() {
+            x_.setZero();
+            P_.setIdentity();
+            F_.setZero();
+            Q_.setZero();
+            H_ << 1, 0;
+            R_.setZero();
+        }
+
+        VelocityEstimator(double initial_position, double initial_velocity, double position_variance,
+                          double velocity_variance, double measurement_variance)
+            : VelocityEstimator() {
+            SetInitialPosition(initial_position);
+            SetInitialVelocity(initial_velocity);
+            SetPositionVariance(position_variance);
+            SetVelocityVariance(velocity_variance);
+            SetMeasurementVariance(measurement_variance);
+        }
+
+        void SetInitialPosition(double initial_position) { x_(0) = initial_position; }
+
+        void SetInitialVelocity(double initial_velocity) { x_(1) = initial_velocity; }
+
+        void SetPositionVariance(double position_variance) { Q_(0, 0) = position_variance; }
+
+        void SetVelocityVariance(double velocity_variance) { Q_(1, 1) = velocity_variance; }
+
+        void SetMeasurementVariance(double measurement_variance) { R_(0, 0) = measurement_variance; }
+
+        void Predict(double dt) {
+            F_(0, 0) = 1;
+            F_(0, 1) = dt;
+            F_(1, 0) = 0;
+            F_(1, 1) = 1;
+
+            x_ = F_ * x_;
+            P_ = F_ * P_ * F_.transpose() + Q_;
+        }
+
+        void Update(double measured_position) {
+            Eigen::Vector<double, 1> y;
+            y(0) = measured_position - H_ * x_;
+
+            Eigen::Matrix<double, 1, 1> S = H_ * P_ * H_.transpose() + R_;
+            Eigen::Matrix<double, 2, 1> K = P_ * H_.transpose() * S.inverse();
+
+            x_ = x_ + K * y;
+            P_ = (Eigen::Matrix2d::Identity() - K * H_) * P_;
+        }
+
+        double GetVelocity() { return x_(1); }
+
+    private:
+        Eigen::Vector2d x_;
+        Eigen::Matrix2d P_;
+        Eigen::Matrix2d F_;
+        Eigen::Matrix2d Q_;
+        Eigen::Matrix<double, 1, 2> H_;
+        Eigen::Matrix<double, 1, 1> R_;
+    };
+}
+
+#endif //VELOCITY_ESTIMATOR_H

protos/cmvr/api/biohead_command.proto

@@ -0,0 +1,177 @@
+syntax = "proto3";
+
+import "cmvr/api/common.proto";  // 导入通用命令头定义，确保与其他服务接口一致
+
+package cmvr.api;
+
+/**
+ * 面部表情控制参数定义
+ * 包含生物头部机器人面部各部位的运动参数，用于精确控制面部表情
+ */
+message FacialExpression {
+  /**
+   * 眉毛控制参数
+   * 左右眉毛的内外侧垂直位置（0.0-1.0标准化值）
+   */
+  message Eyebrow {
+    float left_outside_y = 1;  // 左眉外侧垂直位置
+    float left_inside_y = 2;   // 左眉内侧垂直位置
+    float right_outside_y = 3; // 右眉外侧垂直位置
+    float right_inside_y = 4;  // 右眉内侧垂直位置
+  }
+  Eyebrow eyebrow = 3;        // 眉毛参数封装
+
+  /**
+   * 眼睑控制参数
+   * 左右眼睑的上下位置（0.0-1.0标准化值）
+   */
+  message Eyelid {
+    float left_upper_y = 1;    // 左上眼睑垂直位置
+    float left_lower_y = 2;    // 左下眼睑垂直位置
+    float right_upper_y = 3;   // 右上眼睑垂直位置
+    float right_lower_y = 4;   // 右下眼睑垂直位置
+  }
+  Eyelid eyelid = 4;          // 眼睑参数封装
+
+  /**
+   * 眼球控制参数
+   * 左右眼球的二维坐标（-1.0到1.0标准化值，中心为0）
+   */
+  message Eyeball {
+    float left_x = 1;          // 左眼球水平位置
+    float left_y = 2;          // 左眼球垂直位置
+    float right_x = 3;         // 右眼球水平位置
+    float right_y = 4;         // 右眼球垂直位置
+  }
+  Eyeball eyeball = 5;        // 眼球参数封装
+
+  /**
+   * 鼻子控制参数
+   * 鼻子左右侧的垂直位置（0.0-1.0标准化值）
+   */
+  message Nose {
+    float left_y = 1;          // 鼻子左侧垂直位置
+    float right_y = 2;         // 鼻子右侧垂直位置
+  }
+  Nose nose = 6;              // 鼻子参数封装
+
+  /**
+   * 嘴巴控制参数
+   * 包含嘴唇整体位置和左右唇角细节
+   */
+  message Mouth {
+    float upper_lip_y = 1;     // 上唇垂直位置
+    float upper_lip_z = 2;     // 上唇前后位置（Z轴）
+    float lower_lip_y = 3;     // 下唇垂直位置
+    float lower_lip_z = 4;     // 下唇前后位置（Z轴）
+
+    /**
+     * 左唇角控制参数
+     * 包含上唇、唇角、下唇的坐标点
+     */
+    message LeftLip {
+      float upper_x = 1;       // 左上唇水平位置
+      float upper_y = 2;       // 左上唇垂直位置
+      float corner_x = 3;      // 左唇角水平位置
+      float corner_y = 4;      // 左唇角垂直位置
+      float lower_x = 5;       // 左下唇水平位置
+      float lower_y = 6;       // 左下唇垂直位置
+    }
+    LeftLip left_lip = 5;      // 左唇角参数封装
+
+    /**
+     * 右唇角控制参数
+     * 结构与左唇角对称
+     */
+    message RightLip {
+      float upper_x = 1;       // 右上唇水平位置
+      float upper_y = 2;       // 右上唇垂直位置
+      float corner_x = 3;      // 右唇角水平位置
+      float corner_y = 4;      // 右唇角垂直位置
+      float lower_x = 5;       // 右下唇水平位置
+      float lower_y = 6;       // 右下唇垂直位置
+    }
+    RightLip right_lip = 6;    // 右唇角参数封装
+  }
+  Mouth mouth = 7;            // 嘴巴参数封装
+
+  /**
+   * 下巴控制参数
+   * 下巴的二维移动坐标（X/Y轴）
+   */
+  message Jaw {
+    float x = 1;               // 下巴水平位置
+    float y = 2;               // 下巴垂直位置
+  }
+  Jaw jaw = 8;                // 下巴参数封装
+}
+
+/**
+ * 设置面部表情命令（集成通用命令头）
+ * 用于单次设置生物头部的面部表情
+ */
+message SetFacialExpression {
+  message Request {
+    CommandHeader.Request header = 1;  // 通用命令头，包含设备ID和时间戳
+    FacialExpression expression = 2;   // 具体面部表情参数
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1; // 通用反馈头，包含操作状态和时间戳
+    string execution_id = 2;           // 表情执行任务ID
+    float execution_time_ms = 3;        // 表情执行耗时（毫秒）
+  }
+}
+
+/**
+ * 流式面部表情命令（集成通用命令头）
+ * 用于连续发送多个面部表情，实现表情动画效果
+ */
+message StreamFacialExpression {
+    message Request {
+        CommandHeader.Request header = 1;   // 通用命令头
+        FacialExpression expr =  2;         // 执行
+        bool eof = 3;                       // 标记是否为流结束
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;  // 通用反馈头
+        FacialExpression expr_diff = 2;     // 执行误差
+    }
+}
+
+/**
+ * 获取生物头部状态命令（集成通用命令头）
+ * 用于查询当前头部各部位的状态和位置
+ */
+message GetStatus {
+  message Request {
+    CommandHeader.Request header = 1;  // 通用命令头
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1; // 通用反馈头
+    bool is_moving = 2;                // 是否有部位在移动
+    string last_request_id = 3;        // 上一次请求的ID
+    repeated float current_positions = 4; // 当前各部位位置参数
+    bool camera_recording = 5;         // 相机是否在录制
+    string active_recording_id = 6;    // 活跃录制任务的ID
+  }
+}
+
+/**
+ * 紧急停止命令（集成通用命令头）
+ * 用于立即停止所有面部动作
+ */
+message EmergencyStop {
+  message Request {
+    CommandHeader.Request header = 1;  // 通用命令头
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1; // 通用反馈头
+    string stopped_processes = 2;      // 已停止的进程列表
+  }
+}
+
+

protos/cmvr/api/biohead_service.proto

@@ -0,0 +1,22 @@
+syntax = "proto3";
+
+package cmvr.api;
+import "cmvr/api/biohead_command.proto";
+
+// 生物头部机器人服务接口
+service BioHeadService {
+    // 设置面部表情
+    rpc SetExpression(SetFacialExpression.Request) returns (SetFacialExpression.Feedback){};
+
+    // 流式表情控制
+    //rpc StreamExpression(StreamFacialExpression.Request) returns (StreamFacialExpression.Feedback){};
+
+    rpc StreamExpression (stream StreamFacialExpression.Request) returns (stream StreamFacialExpression.Feedback);
+
+
+    // 获取状态
+    rpc GetSystemStatus(GetStatus.Request) returns (GetStatus.Feedback){};
+
+    // 紧急停止
+    rpc EmergencyStop(EmergencyStop.Request) returns (EmergencyStop.Feedback){};
+}

protos/cmvr/api/camera_command.proto

@@ -0,0 +1,161 @@
+syntax = "proto3";
+
+import "cmvr/api/common.proto";
+
+package cmvr.api;
+
+message FrameData {
+    enum FrameType {
+        U8C1 = 0;
+        U16C1 = 1;
+        U8C3 = 2;
+        U16C3 = 3;
+        F16C1 = 4;
+        F32C1 = 5;
+    }
+    bytes data = 1;
+    int32 width = 2;
+    int32 height = 3;
+    FrameType type = 4;
+    string codec = 5;
+    bool is_key_frame = 6;
+}
+
+message CameraState {
+    bool is_initialized = 1;
+    bool is_opened = 2;
+    bool is_streaming = 3;
+    bool is_recording = 4;
+    bool is_error = 5;
+    string error_message = 6;
+    int32 fps = 7;
+    int32 width = 8;
+    int32 height = 9;
+}
+
+message GetCameraStateCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        CameraState state = 2;
+    }
+}
+
+message StartCameraCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message StopCameraCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message GetRGBImageCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData color_frame = 2;
+    }
+}
+
+message GetDepthImageCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData depth_frame = 2;
+    }
+}
+
+message GetRGBDImagesCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData color_frame = 2;
+        FrameData depth_frame = 3;
+    }
+}
+
+message StartCameraRecordingCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        string video_path = 2;
+    }
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message StopCameraRecordingCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message GetRGBImageStreamCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        bool eof = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData color_frame = 2;
+        int32 seq_no = 3;
+    }
+}
+
+message GetDepthImageStreamCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        bool eof = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData depth_frame = 2;
+        int32 seq_no = 3;
+    }
+}
+
+message GetRGBDImagesStreamCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        bool eof = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData color_frame = 2;
+        FrameData depth_frame = 3;
+        int32 seq_no = 4;
+    }
+}
+
+
+

protos/cmvr/api/camera_service.proto

@@ -0,0 +1,21 @@
+
+syntax = "proto3";
+
+import "cmvr/api/camera_command.proto";
+
+package cmvr.api;
+
+service CameraService {
+    rpc GetStatus(GetCameraStateCommand.Request) returns (GetCameraStateCommand.Feedback) {}
+    rpc StartCamera(StartCameraCommand.Request) returns (StartCameraCommand.Feedback) {}
+    rpc StopCamera(StopCameraCommand.Request) returns (StopCameraCommand.Feedback) {}
+    rpc GetRGBImage(GetRGBImageCommand.Request) returns (GetRGBImageCommand.Feedback) {}
+    rpc GetDepthImage(GetDepthImageCommand.Request) returns (GetDepthImageCommand.Feedback) {}
+    rpc GetRGBDImages(GetRGBDImagesCommand.Request) returns (GetRGBDImagesCommand.Feedback) {}
+    rpc StartRecording(StartCameraRecordingCommand.Request) returns (StartCameraRecordingCommand.Feedback) {}
+    rpc StopRecording(StopCameraRecordingCommand.Request) returns (StopCameraRecordingCommand.Feedback) {}
+
+    rpc GetRGBImageStream(stream GetRGBImageStreamCommand.Request) returns (stream GetRGBImageStreamCommand.Feedback) {}
+    rpc GetDepthImageStream(stream GetDepthImageStreamCommand.Request) returns (stream GetDepthImageStreamCommand.Feedback) {}
+    rpc GetRGBDImagesStream(stream GetRGBDImagesStreamCommand.Request) returns (stream GetRGBDImagesStreamCommand.Feedback) {}
+}

protos/cmvr/api/common.proto

@@ -0,0 +1,30 @@
+syntax = "proto3";
+
+package cmvr.api;
+
+import "google/protobuf/timestamp.proto";
+
+message DeviceLifecycle {
+    enum Lifecycle {
+        STATE_INIT = 0;
+        STATE_READY = 1;
+        STATE_RUNNING = 2;
+        STATE_ERROR = 3;
+        STATE_ESTOP = 4;
+        STATE_STOP = 5;
+    }
+    Lifecycle state = 1;
+}
+
+message CommandHeader {
+    message Request {
+        string device_id = 1;                      // 目标设备名称
+        google.protobuf.Timestamp timestamp = 2;     // 请求时间
+    }
+
+    message Feedback {
+        bool success = 1;                             // 是否成功
+        string error_message = 2;                     // 错误信息（成功时为空）
+        google.protobuf.Timestamp timestamp = 3;      // 回复时间
+    }
+}

protos/cmvr/api/dexhand_command.proto

@@ -0,0 +1,149 @@
+syntax = "proto3";
+
+import "cmvr/api/common.proto";
+
+package cmvr.api;
+
+message FreedomValue {
+    int32 id = 1;                 // 自由度id 0-6 分别对应六个自由度 小拇指 无名指 中指 食指 大拇指弯曲 大拇指旋转
+    float value = 2;              // 值 0-1百分比
+}
+
+message RH56DFTPDexHand {
+    int32 dof_id = 1;             // 自由度ID，唯一标识不同的关节或自由度
+    int32 angle = 2;              // 自由度角度
+    int32 speed = 3;              // 自由度速度
+    int32 force = 4;              // 实际受力
+    int32 position = 5;           // 执行器位置
+    int32 current = 6;            // 执行器实际电流
+    int32 temperature = 7;        // 执行器温度
+    int32 error = 8;              // 执行器故障信息
+    repeated string error_message = 9;  // 错误消息列表
+}
+// 传感器数据
+// 传感器数据
+message SensorData {
+    // 手指类型（掌心作为特殊类型）
+    enum FingerType {
+        PINKY = 0;    // 小拇指
+        RING = 1;     // 无名指
+        MIDDLE_FINGER = 2;   // 中指（避免与PartType.MIDDLE冲突）
+        INDEX = 3;    // 食指
+        THUMB = 4;    // 大拇指
+        PALM = 5;     // 掌心
+    }
+
+    // 部位类型（指中改为THUMB_MIDDLE以避免冲突）
+    enum PartType {
+        TIP = 0;      // 指端
+        FINGER = 1;   // 指尖
+        PAD = 2;      // 指腹
+        THUMB_MIDDLE = 3;  // 大拇指指中（仅大拇指有）
+        PALM_PAD = 4;      // 掌心部位（对应PalmTactileData）
+    }
+
+    FingerType finger_type = 4;  // 手指类型（掌心使用PALM）
+    PartType part_type = 5;      // 部位类型
+    string sensor_name = 6;      // 传感器名称（如"小拇指指端"）
+
+    message RowData {
+        repeated int32 values = 1 [packed = true];  // 一维数组，存放每行的浮点数值
+    }
+    repeated RowData data = 1;  // 二维数组，每行数据是 RowData 类型
+    int32 rows = 2;             // 行
+    int32 cols = 3;             // 列
+}
+
+
+message DexHandState {
+    bool is_initialized = 1;      // 是否初始化
+    repeated RH56DFTPDexHand hands = 2;  // 包含多个自由度状态
+}
+
+message GetDexHandStateCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        DexHandState  state = 2;
+    }
+}
+
+message SetDexHandPositionsCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        repeated FreedomValue values = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message SetDexHandAnglesCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        repeated FreedomValue values = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message SetDexHandForceCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        repeated FreedomValue values = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message SetDexHandSpeedCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        repeated FreedomValue values = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message SetDexHandPresetActCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        int32 presetActId = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message GetSensorDataCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        repeated SensorData sensor = 2;//所有传感器的数据
+    }
+}
+
+message GetSensorDataStreamCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        repeated SensorData sensor = 2;//所有传感器的数据
+    }
+}

protos/cmvr/api/dexhand_service.proto

@@ -0,0 +1,18 @@
+syntax = "proto3";
+
+import "cmvr/api/dexhand_command.proto";
+
+package cmvr.api;
+
+
+service DexHandService {
+    // 基本控制
+    rpc GetStatus(GetDexHandStateCommand.Request) returns (GetDexHandStateCommand.Feedback);
+    rpc SetDexHandPos(SetDexHandPositionsCommand.Request) returns (SetDexHandPositionsCommand.Feedback);
+    rpc SetDexHandAngle(SetDexHandAnglesCommand.Request) returns (SetDexHandAnglesCommand.Feedback);
+    rpc SetDexHandForce(SetDexHandForceCommand.Request) returns (SetDexHandForceCommand.Feedback);
+    rpc SetDexHandSpeed(SetDexHandSpeedCommand.Request) returns (SetDexHandSpeedCommand.Feedback);
+    rpc SetDexHandPresetAct(SetDexHandPresetActCommand.Request) returns (SetDexHandPresetActCommand.Feedback);
+    rpc GetSensorData(GetSensorDataCommand.Request) returns (GetSensorDataCommand.Feedback);
+    rpc GetSensorDataStream(stream GetSensorDataStreamCommand.Request) returns (stream GetSensorDataStreamCommand.Feedback);
+}

protos/cmvr/api/geometry.proto

@@ -0,0 +1,67 @@
+syntax = "proto3";
+
+package cmvr.api;
+
+
+message Vec2 {
+    double x = 1;
+    double y = 2;
+}
+
+message Vec3 {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+}
+
+message SE2Pose {
+    Vec2 position = 1;    // (m)
+    double angle = 2;     // (rad)
+}
+
+message SE2Velocity {
+    Vec2 linear = 1;    // (m/s)
+    double angular = 2; // (rad/s)
+}
+
+message Quaternion {
+    double x = 1;
+    double y = 2;
+    double z = 3;
+    double w = 4;
+}
+
+message EulerAngleZYX {
+    double z = 1;
+    double y = 2;
+    double x = 3;
+}
+
+message SE3Pose {
+    Vec3 position = 1; // (m)
+    oneof rotation {
+        Quaternion quaternion = 2;
+        EulerAngleZYX euler = 3;
+    }
+}
+
+message Inertial {
+    // Mass (kg)
+    double mass = 1;
+
+    // Center of mass (m)
+    Vec3 center_of_mass = 2;
+
+    // Inertia tensor
+    Inertia inertia = 3;
+}
+
+// Inertia tensor components (kg*m^2)
+message Inertia {
+    double ixx = 1;
+    double iyy = 2;
+    double izz = 3;
+    double ixy = 4;
+    double ixz = 5;
+    double iyz = 6;
+}