Merge remote-tracking branch 'origin/lgv_11_12' into lgv_11_12

This reverts commit 058ea0f505.

.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,120 @@
+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)
+
+include_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/pinocchio/3.8.0/include
+        ${CMAKE_SOURCE_DIR}/third_party/coal/3.0.2/include
+        ${CMAKE_SOURCE_DIR}/third_party/urdfdom/5.0.3/include
+        ${CMAKE_SOURCE_DIR}/third_party/urdfdom_headers/2.0.1/include
+        ${CMAKE_SOURCE_DIR}/third_party/mujoco/3.3.7/include
+)
+
+link_directories(
+        ${CMAKE_SOURCE_DIR}/third_party/pinocchio/3.8.0/lib
+        ${CMAKE_SOURCE_DIR}/third_party/coal/3.0.2/lib
+        ${CMAKE_SOURCE_DIR}/third_party/urdfdom/5.0.3/lib
+        ${CMAKE_SOURCE_DIR}/third_party/mujoco/3.3.7/lib
+)
+
+############################################################
+# PROTO
+############################################################
+
+set(PROTO_IMPORT_DIR ${PROJECT_SOURCE_DIR}/protos)
+set(PROTO_BINARY_DIR ${CMAKE_CURRENT_BINARY_DIR}/_protobuf)
+file(MAKE_DIRECTORY ${PROTO_BINARY_DIR})
+file(GLOB_RECURSE PROTO_FILES ${PROTO_IMPORT_DIR}/*.proto)
+add_library(proto-objects OBJECT ${PROTO_FILES})
+message(STATUS "proto files will be loaded in ${PROTO_IMPORT_DIR}")
+message(STATUS "proto files will be generated in ${PROTO_BINARY_DIR}")
+
+protobuf_generate(
+        TARGET proto-objects
+        IMPORT_DIRS ${PROTO_IMPORT_DIR}
+        PROTOC_OUT_DIR ${PROTO_BINARY_DIR}
+        USAGE_REQUIREMENT INTERFACE
+)
+protobuf_generate(
+        TARGET proto-objects
+        LANGUAGE grpc
+        GENERATE_EXTENSIONS .grpc.pb.h .grpc.pb.cc
+        PLUGIN "protoc-gen-grpc=$ENV{HOME}/.local/bin/grpc_cpp_plugin"
+        USAGE_REQUIREMENT INTERFACE
+        IMPORT_DIRS ${PROTO_IMPORT_DIR}
+        PROTOC_OUT_DIR ${PROTO_BINARY_DIR}
+)
+target_include_directories(proto-objects PUBLIC ${PROTO_BINARY_DIR} ${PROTO_IMPORT_DIR})
+target_link_libraries(proto-objects PUBLIC protobuf::libprotobuf gRPC::grpc++ gRPC::grpc++_reflection)
+
+############################################################
+# include
+############################################################
+include_directories(
+        /usr/include/opencv4
+        /usr/include/eigen3
+        /usr/local/include/osqp
+        /usr/local/share/osqp/codegen_files/inc/prate/
+        $ENV{HOME}/.local/include
+        /usr/local/include/
+        ${PROTO_BINARY_DIR}
+        ${PROJECT_SOURCE_DIR}/include
+        ${PROJECT_SOURCE_DIR}/src/devices
+        ${PROJECT_SOURCE_DIR}/src/utils
+        ${PROJECT_SOURCE_DIR}/third_party
+        ${PROJECT_SOURCE_DIR}/src
+        ${PROJECT_SOURCE_DIR}/third_party/toppra/0.6.2/include
+
+)
+
+############################################################
+# 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_manager
+        cmvr_es::hardware
+        cmvr_es::device::canbus
+        cmvr_es::device::ti5motor
+        cmvr_es::controller
+        cmvr_es::data_center
+        cmvr_es::ik_solver
+        cmvr_es::planner
+        cmvr_es::device::humanoid_robot
+)

LICENSE

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

README.md

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

assets/toppra

@@ -0,0 +1 @@
+Subproject commit fab1f402ef8677beac43d2226eb1cc6ce0d54735

config/cabin_robot.xml

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

config/ik_solver_config/pinocchio_dls_ik.pb.txt

@@ -0,0 +1,9 @@
+urdf_path:         "/home/lgv/cmvr/dual_arm.urdf"
+base_frame_name:   "PELVIS_S"
+flange_frame_name: "R_FLANGE"
+tcp_frame_name:    "R_TCP"
+
+max_iters: 100
+pos_eps:   1e-6
+rot_eps:   1e-6
+damping:   1e-6

config/ik_solver_config/pinocchio_qp_ik.pb.txt

@@ -0,0 +1,10 @@
+urdf_path:         "/home/lgv/cmvr/dual_arm.urdf"
+base_frame_name:   "PELVIS_S"
+flange_frame_name: "R_FLANGE"
+tcp_frame_name:    "R_TCP"
+
+lambda:        0.0001
+w_posrot:      0.5
+max_iters:     80
+tol:           1e-6
+qp_time_limit: 0.005

config/robot_description/hc_description/dual_arm.urdf

@@ -0,0 +1,660 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="dual_arm">
+   <mujoco>
+    <compiler
+    meshdir="meshes"
+    balanceinertia="true"
+    discardvisual="false" />
+   </mujoco>
+
+
+    <link name="base_link">
+      <visual>
+        <origin xyz="0 0 0.6" rpy="0 0 0"/>
+        <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>
+        <origin xyz="0 0 0.6" rpy="0 0 0"/>
+        <geometry>
+          <cylinder radius="0.05" length="1.2"/>
+        </geometry>
+      </collision>
+
+        <inertial>
+
+          <origin xyz="0 0 0" rpy="0 0 0"/>
+          <mass value="25.4469"/>
+
+          <inertia
+            ixx="3.06953"
+            ixy="0.0"
+            ixz="0.0"
+            iyy="3.06953"
+            iyz="0.0"
+            izz="0.03181"/>
+        </inertial>
+    </link>
+
+    <joint name="base_fixed" type="fixed">
+      <origin rpy="0 0 0" xyz="0 0 1.2"/>
+      <parent link="base_link"/>
+      <child link="PELVIS_S"/>
+    </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>
+
+  <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="-3.14" upper="3.14" 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="-0.78" upper="1.57" 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="-3.14" 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="3.14" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_WRIST_Y_S">
+    <inertial>
+      <origin xyz="-0.00464135887330083 -5.06426456325926E-10 -0.0341253783666614" rpy="0 0 0" />
+      <mass value="0.235738477720019" />
+      <inertia
+        ixx="5.1068694045578E-05"
+        ixy="7.45771487459288E-16"
+        ixz="6.26195689233664E-06"
+        iyy="6.00636019624515E-05"
+        iyz="1.27828046342987E-16"
+        izz="5.32182903609188E-05" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+      <material name="">
+        <color rgba="0.647058823529412 0.619607843137255 0.588235294117647 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_Y_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_Y" type="revolute">
+    <origin xyz="0 -0.1525 0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_P_S" />
+    <child link="R_WRIST_Y_S" />
+    <axis xyz="0 0 1" />
+    <limit lower="-0.78" upper="0.78" effort="50" velocity="0.79" />
+  </joint>
+
+  <link name="R_WRIST_R_S">
+    <inertial>
+      <origin xyz="-0.0201642233698132 -0.11074968386657 -0.00598955339232021" rpy="0 0 0" />
+      <mass value="0.504366058218534" />
+      <inertia
+        ixx="0.000185559065855467"
+        ixy="5.75889766751509E-06"
+        ixz="2.40683898454438E-06"
+        iyy="0.000131246007872084"
+        iyz="1.60533277040148E-06"
+        izz="0.000271800951396149" />
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+      <material
+        name="">
+        <color rgba="0.890196078431372 0.890196078431372 0.913725490196078 1" />
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0" />
+      <geometry>
+        <mesh filename="meshes/R_WRIST_R_S.STL" />
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_WRIST_R" type="revolute">
+    <origin xyz="0.03 0 -0.039" rpy="0 0 0" />
+    <parent link="R_WRIST_Y_S" />
+    <child link="R_WRIST_R_S" />
+    <axis xyz="1 0 0" />
+    <limit lower="-0.26" upper="1.57" effort="50" velocity="4.71" />
+  </joint>
+
+  <link name="R_FINGER_TIP">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.004"/>
+      </geometry>
+      <material name="">
+        <color rgba="0 1 1 1"/> <!-- 绿色 -->
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_FINGER_TIP_FIXED" type="fixed">
+    <origin xyz="0.00684256  -0.284077 0.00801525" rpy="0 0 0"/>
+    <parent link="R_WRIST_R_S"/>
+    <child link="R_FINGER_TIP"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+  <link name="R_CAM">
+    <inertial>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <mass value="0"/>
+      <inertia ixx="1e-6" ixy="0" ixz="0" iyy="1e-6" iyz="0" izz="1e-6"/>
+    </inertial>
+    <visual>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.004"/>
+      </geometry>
+      <material name="">
+        <color rgba="0 1 0 1"/> <!-- 绿色 -->
+      </material>
+    </visual>
+    <collision>
+      <origin xyz="0 0 0" rpy="0 0 0"/>
+      <geometry>
+        <sphere radius="0.005"/>
+      </geometry>
+    </collision>
+  </link>
+
+  <joint name="R_CAM_FIXED" type="fixed">
+    <origin xyz="-0.01212 -0.17655 0.07506" rpy="-1.5707963267 0 3.1415926535"/>
+    <parent link="R_WRIST_R_S"/>
+    <child link="R_CAM"/>
+    <axis xyz="0 0 1"/>
+  </joint>
+
+</robot>

config/robot_description/hc_description/dual_arm.xml

@@ -0,0 +1,206 @@
+<mujoco model="dual_arm">
+  <compiler angle="radian" meshdir="meshes/"/>
+
+  <default>
+    <joint damping="10" armature="0.001"/>
+  </default>
+
+  <asset>
+    <mesh name="PELVIS_S" file="PELVIS_S.STL"/>
+    <mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
+    <mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
+    <mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
+    <mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
+    <mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
+    <mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
+    <mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
+    <mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
+    <mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
+    <mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
+    <mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
+    <mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
+    <mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
+    <mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
+
+    <!-- 天空背景，可要可不要 -->
+    <texture name="skybox" type="skybox"
+             builtin="gradient"
+             rgb1="0.4 0.6 0.8"
+             rgb2="0 0 0"
+             width="512" height="512"/>
+
+    <!-- 蓝色方格地面纹理：builtin="checker" -->
+    <texture name="grid"
+             type="2d"
+             builtin="checker"
+             width="512" height="512"
+             rgb1="0.2 0.3 0.4"
+             rgb2="0.1 0.2 0.3"
+             mark="cross"
+             markrgb="0.8 0.8 0.8"/>
+
+    <material name="grid_floor"
+              texture="grid"
+              texrepeat="5 5"
+              rgba="1 1 1.0 1"
+              emission="0.9"
+              specular="0.5"
+              shininess="1"
+              reflectance="0.3"/>
+
+  </asset>
+
+  <worldbody>
+    <geom size="0.05 0.6" pos="0 0 0.6" type="cylinder" contype="0" conaffinity="0" group="1" density="0"/>
+    <geom size="0.05 0.6" pos="0 0 0.6" type="cylinder"/>
+    <geom pos="0 0 1.2" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+    <geom pos="0 0 1.2" quat="1 0 0 0" type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+    <body name="L_SHOULDER_P_S" pos="0 0.0945 1.242">
+      <inertial pos="-0.00982259 0.0704593 1.15262e-06" quat="0.706163 0.705933 -0.0386962 -0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0" range="-1.57 0.26" actuatorfrcrange="-120 120"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
+      <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
+      <body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
+        <inertial pos="-0.0346025 0.0917393 -1.67085e-08" quat="0.358778 0.609303 -0.358823 0.609323" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-1.57 0.78" actuatorfrcrange="-120 120"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
+        <body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
+          <inertial pos="-0.00440977 0.086362 9.50749e-09" quat="0.705001 0.704998 -0.054559 -0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-80 80"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
+          <body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
+            <inertial pos="-0.0335624 0.06032 2.99656e-07" quat="0.674756 0.674714 0.211333 0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0" range="-2.05 0" actuatorfrcrange="-50 50"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
+            <body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
+              <inertial pos="-1.39659e-10 0.0675973 0.0192006" quat="0.467537 0.530481 -0.530481 0.467537" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
+              <geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
+              <body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06427e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-0.78 0.78" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
+                <body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
+                  <inertial pos="-0.0161477 0.0955047 -0.00499392" quat="0.595488 0.369507 -0.591676 0.39847" mass="0.504894" diaginertia="0.000280921 0.000188575 0.000178055"/>
+                  <joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0" range="-1.57 0.26" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+    <body name="R_SHOULDER_P_S" pos="0 -0.0945 1.242">
+      <inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-120 120"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
+      <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
+      <body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
+        <inertial pos="-0.0346025 -0.0917393 1.86281e-08" quat="0.609323 0.358823 -0.609303 0.358778" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-0.78 1.57" actuatorfrcrange="-120 120"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
+        <body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
+          <inertial pos="-0.00440977 -0.086362 -7.58792e-09" quat="0.705001 0.704998 0.054559 0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-80 80"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
+          <body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
+            <inertial pos="-0.0335624 -0.06032 -2.97736e-07" quat="0.674756 0.674714 -0.211333 -0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0" range="0 2.05" actuatorfrcrange="-80 80"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
+            <body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
+              <inertial pos="-1.39657e-10 -0.0675973 0.0192006" quat="0.530481 0.467537 -0.467537 0.530481" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0" range="-3.14 3.14" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
+              <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
+              <body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06426e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-0.78 0.78" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
+                <body name="R_WRIST_R_S" pos="0.03 0 -0.039">
+                  <inertial pos="-0.0201642 -0.11075 -0.00598955" quat="0.483404 0.529786 -0.463203 0.520663" mass="0.504366" diaginertia="0.00027189 0.000186086 0.000130629"/>
+                  <joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0" range="-0.26 1.57" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
+                  <geom size="0.004" pos="0.00684256 -0.284077 0.00801525" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 1 1"/>
+                  <geom size="0.005" pos="0.00684256 -0.284077 0.00801525" rgba="0 1 1 1"/>
+                  <geom size="0.004" pos="-0.01212 -0.17655 0.07506" quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 0 1"/>
+                  <geom size="0.005" pos="-0.01212 -0.17655 0.07506" quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107" rgba="0 1 0 1"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+
+    <light name="top_light"
+           mode="track"
+           directional="true"
+           diffuse="0.4 0.4 0.4"
+           specular="0.3 0.3 0.3"
+           pos="0 0 3"
+           dir="0 0 -1"/>
+
+    <geom name="floor"
+          type="plane"
+          pos="0 0 0"
+          size="0 0 0.05"
+          material="grid_floor"
+          condim="3"
+          friction="1 0.005 0.0001"/>
+  </worldbody>
+
+
+  <!-- 关节空间 PD 位置控制：ctrl = 目标关节角度 (rad) -->
+  <actuator>
+    <!-- 左臂：位置控制 -->
+    <position name="L_SHOULDER_P_pos" joint="L_SHOULDER_P"
+              kp="200" ctrlrange="-1.57 0.26"/>
+    <position name="L_SHOULDER_R_pos" joint="L_SHOULDER_R"
+              kp="200" ctrlrange="-1.57 0.78"/>
+    <position name="L_SHOULDER_Y_pos" joint="L_SHOULDER_Y"
+              kp="200" ctrlrange="-3.14 3.14"/>
+
+    <position name="L_ELBOW_R_pos"    joint="L_ELBOW_R"
+              kp="150" ctrlrange="-2.05 0.00"/>
+
+    <position name="L_WRIST_P_pos"    joint="L_WRIST_P"
+              kp="80"  ctrlrange="-3.14 3.14"/>
+    <position name="L_WRIST_Y_pos"    joint="L_WRIST_Y"
+              kp="80"  ctrlrange="-0.78 0.78"/>
+    <position name="L_WRIST_R_pos"    joint="L_WRIST_R"
+              kp="80"  ctrlrange="-1.57 0.26"/>
+
+    <!-- 右臂：位置控制 -->
+    <position name="R_SHOULDER_P_pos" joint="R_SHOULDER_P"
+              kp="2000" ctrlrange="-3.14 3.14"/>
+    <position name="R_SHOULDER_R_pos" joint="R_SHOULDER_R"
+              kp="2000" ctrlrange="-0.78 1.57"/>
+    <position name="R_SHOULDER_Y_pos" joint="R_SHOULDER_Y"
+              kp="2000" ctrlrange="-3.14 3.14"/>
+
+    <position name="R_ELBOW_R_pos"    joint="R_ELBOW_R"
+              kp="1500" ctrlrange="0 2.05"/>
+
+    <position name="R_WRIST_P_pos"    joint="R_WRIST_P"
+              kp="800"  ctrlrange="-3.14 3.14"/>
+    <position name="R_WRIST_Y_pos"    joint="R_WRIST_Y"
+              kp="800"  ctrlrange="-0.78 0.78"/>
+    <position name="R_WRIST_R_pos"    joint="R_WRIST_R"
+              kp="800"  ctrlrange="-0.26 1.57"/>
+  </actuator>
+
+
+
+</mujoco>

config/robot_description/hc_description/dual_arm_temp.xml

@@ -0,0 +1,347 @@
+<mujoco model="dual_arm">
+  <compiler angle="radian" meshdir="meshes/"/>
+
+  <!-- 全局默认：给所有关节一点阻尼和等效转子惯量 -->
+  <default>
+    <joint damping="10" armature="0.001"/>
+  </default>
+
+
+
+  <!-- 稳定一点的时间步和积分器 -->
+<!--  <option timestep="0.001" integrator="RK4"/>-->
+
+  <asset>
+    <mesh name="PELVIS_S" file="PELVIS_S.STL"/>
+    <mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
+    <mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
+    <mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
+    <mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
+    <mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
+    <mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
+    <mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
+    <mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
+    <mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
+    <mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
+    <mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
+    <mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
+    <mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
+    <mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
+
+
+    <!-- 天空背景，可要可不要 -->
+    <texture name="skybox" type="skybox"
+             builtin="gradient"
+             rgb1="0.4 0.6 0.8"
+             rgb2="0 0 0"
+             width="512" height="512"/>
+
+    <!-- 蓝色方格地面纹理：builtin="checker" -->
+    <texture name="grid"
+             type="2d"
+             builtin="checker"
+             width="512" height="512"
+             rgb1="0.2 0.3 0.4"
+             rgb2="0.1 0.2 0.3"
+             mark="cross"
+             markrgb="0.8 0.8 0.8"/>
+
+    <!-- 地面材质，绑定上面的纹理 -->
+    <material name="grid_floor"
+              texture="grid"
+              texrepeat="5 5"
+              reflectance="0.2"/>
+  </asset>
+
+  <worldbody>
+    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+          rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+    <geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+
+    <body name="L_SHOULDER_P_S" pos="0 0.0945 0.042">
+      <inertial pos="-0.00982259 0.0704593 1.15262e-06"
+                quat="0.706163 0.705933 -0.0386962 -0.0386741"
+                mass="0.880738"
+                diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0"
+             range="-1.57 1.57" actuatorfrcrange="-120 120"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+            rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
+      <geom type="mesh" rgba="0.898039 0.917647 0.929412 1"
+            mesh="L_SHOULDER_P_S"/>
+
+      <body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
+        <inertial pos="-0.0346025 0.0917393 -1.67085e-08"
+                  quat="0.358778 0.609303 -0.358823 0.609323"
+                  mass="0.594788"
+                  diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0"
+               range="-2 2" actuatorfrcrange="-120 120"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+              rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+              mesh="L_SHOULDER_R_S"/>
+
+        <body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
+          <inertial pos="-0.00440977 0.086362 9.50749e-09"
+                    quat="0.705001 0.704998 -0.054559 -0.0545441"
+                    mass="0.563406"
+                    diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0"
+                 range="-2.18 0" actuatorfrcrange="-80 80"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+                mesh="L_SHOULDER_Y_S"/>
+
+          <body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
+            <inertial pos="-0.0335624 0.06032 2.99656e-07"
+                      quat="0.674756 0.674714 0.211333 0.211667"
+                      mass="0.393572"
+                      diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0"
+                   range="-2.05 0" actuatorfrcrange="-50 50"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+                  mesh="L_ELBOW_R_S"/>
+
+            <body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
+              <inertial pos="-1.39659e-10 0.0675973 0.0192006"
+                        quat="0.467537 0.530481 -0.530481 0.467537"
+                        mass="0.442332"
+                        diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0"
+                     range="0 3.14" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
+              <geom type="mesh" rgba="0.698039 0.698039 0.698039 1"
+                    mesh="L_WRIST_P_S"/>
+
+              <body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06427e-10 -0.0341254"
+                          quat="0.298107 0.641196 0.641196 0.298107"
+                          mass="0.235738"
+                          diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1"
+                       range="-0.78 0.78" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
+                      mesh="L_WRIST_Y_S"/>
+
+                <body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
+                  <inertial pos="-0.0161477 0.0955047 -0.00499392"
+                            quat="0.595488 0.369507 -0.591676 0.39847"
+                            mass="0.504894"
+                            diaginertia="0.000280921 0.000188575 0.000178055"/>
+                  <joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0"
+                         range="-1.57 0.26" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+                        mesh="L_WRIST_R_S"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+
+    <body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
+      <inertial pos="-0.00982259 -0.0704593 -1.1507e-06"
+                quat="0.706163 0.705933 0.0386962 0.0386741"
+                mass="0.880738"
+                diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0"
+             range="-1.57 1.57" actuatorfrcrange="-120 120"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+            rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
+      <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+            mesh="R_SHOULDER_P_S"/>
+
+      <body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
+        <inertial pos="-0.0346025 -0.0917393 1.86281e-08"
+                  quat="0.609323 0.358823 -0.609303 0.358778"
+                  mass="0.594788"
+                  diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0"
+               range="-2 2" actuatorfrcrange="-120 120"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+              rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+              mesh="R_SHOULDER_R_S"/>
+
+        <body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
+          <inertial pos="-0.00440977 -0.086362 -7.58792e-09"
+                    quat="0.705001 0.704998 0.054559 0.0545441"
+                    mass="0.563406"
+                    diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0"
+                 range="0 3.14" actuatorfrcrange="-80 80"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+                mesh="R_SHOULDER_Y_S"/>
+
+          <body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
+            <inertial pos="-0.0335624 -0.06032 -2.97736e-07"
+                      quat="0.674756 0.674714 -0.211333 -0.211667"
+                      mass="0.393572"
+                      diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0"
+                   range="0 2.05" actuatorfrcrange="-80 80"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                  rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+                  mesh="R_ELBOW_R_S"/>
+
+            <body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
+              <inertial pos="-1.39657e-10 -0.0675973 0.0192006"
+                        quat="0.530481 0.467537 -0.467537 0.530481"
+                        mass="0.442332"
+                        diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0"
+                     range="-3.14 0" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                    rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
+              <geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
+                    mesh="R_WRIST_P_S"/>
+
+              <body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06426e-10 -0.0341254"
+                          quat="0.298107 0.641196 0.641196 0.298107"
+                          mass="0.235738"
+                          diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1"
+                       range="-0.78 0.78" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                      rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1"
+                      mesh="R_WRIST_Y_S"/>
+
+                <body name="R_WRIST_R_S" pos="0.03 0 -0.039">
+                  <inertial pos="-0.0201642 -0.11075 -0.00598955"
+                            quat="0.483404 0.529786 -0.463203 0.520663"
+                            mass="0.504366"
+                            diaginertia="0.00027189 0.000186086 0.000130629"/>
+                  <joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0"
+                         range="-0.26 1.57" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0"
+                        rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1"
+                        mesh="R_WRIST_R_S"/>
+
+                  <!-- 小标记点 -->
+                  <geom size="0.004"
+                        pos="0.00684256 -0.284077 0.00801525"
+                        contype="0" conaffinity="0" group="1"
+                        density="0" rgba="0 1 1 1"/>
+                  <geom size="0.005"
+                        pos="0.00684256 -0.284077 0.00801525"
+                        rgba="0 1 1 1"/>
+                  <geom size="0.004"
+                        pos="-0.01212 -0.17655 0.07506"
+                        quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
+                        contype="0" conaffinity="0" group="1"
+                        density="0" rgba="0 1 0 1"/>
+                  <geom size="0.005"
+                        pos="-0.01212 -0.17655 0.07506"
+                        quat="3.17467e-11 -3.17467e-11 -0.707107 0.707107"
+                        rgba="0 1 0 1"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+
+    <light name="top_light"
+           mode="track"
+           directional="true"
+           diffuse="1 1 1"
+           specular="0.3 0.3 0.3"
+           pos="0 0 3"
+           dir="0 0 -1"/>
+
+    <geom name="floor"
+          type="plane"
+          pos="0 0 0"
+          size="0 0 0.05"
+          material="grid_floor"
+          condim="3"
+          friction="1 0.005 0.0001"/>
+  </worldbody>
+
+<!--  <actuator>-->
+<!--    &lt;!&ndash; 左臂：力矩控制，ctrl = 关节力矩 (Nm) &ndash;&gt;-->
+<!--    <motor name="L_SHOULDER_P_torque" joint="L_SHOULDER_P"-->
+<!--           gear="1" ctrlrange="-200 200"/>-->
+<!--    <motor name="L_SHOULDER_R_torque" joint="L_SHOULDER_R"-->
+<!--           gear="1" ctrlrange="-200 200"/>-->
+<!--    <motor name="L_SHOULDER_Y_torque" joint="L_SHOULDER_Y"-->
+<!--           gear="1" ctrlrange="-150 150"/>-->
+<!--    <motor name="L_ELBOW_R_torque"    joint="L_ELBOW_R"-->
+<!--           gear="1" ctrlrange="-120 120"/>-->
+<!--    <motor name="L_WRIST_P_torque"    joint="L_WRIST_P"-->
+<!--           gear="1" ctrlrange="-80 80"/>-->
+<!--    <motor name="L_WRIST_Y_torque"    joint="L_WRIST_Y"-->
+<!--           gear="1" ctrlrange="-80 80"/>-->
+<!--    <motor name="L_WRIST_R_torque"    joint="L_WRIST_R"-->
+<!--           gear="1" ctrlrange="-80 80"/>-->
+
+<!--    &lt;!&ndash; 右臂：力矩控制 &ndash;&gt;-->
+<!--    <motor name="R_SHOULDER_P_torque" joint="R_SHOULDER_P"-->
+<!--           gear="1" ctrlrange="-200 200"/>-->
+<!--    <motor name="R_SHOULDER_R_torque" joint="R_SHOULDER_R"-->
+<!--           gear="1" ctrlrange="-200 200"/>-->
+<!--    <motor name="R_SHOULDER_Y_torque" joint="R_SHOULDER_Y"-->
+<!--           gear="1" ctrlrange="-150 150"/>-->
+<!--    <motor name="R_ELBOW_R_torque"    joint="R_ELBOW_R"-->
+<!--           gear="1" ctrlrange="-120 120"/>-->
+<!--    <motor name="R_WRIST_P_torque"    joint="R_WRIST_P"-->
+<!--           gear="1" ctrlrange="-80 80"/>-->
+<!--    <motor name="R_WRIST_Y_torque"    joint="R_WRIST_Y"-->
+<!--           gear="1" ctrlrange="-80 80"/>-->
+<!--    <motor name="R_WRIST_R_torque"    joint="R_WRIST_R"-->
+<!--           gear="1" ctrlrange="-80 80"/>-->
+<!--  </actuator>-->
+
+  <actuator>
+    <!-- 左臂：位置控制，ctrl = 关节目标角度 (rad) -->
+    <position name="L_SHOULDER_P_pos" joint="L_SHOULDER_P"
+              kp="40" ctrlrange="-1.57 1.57"/>
+    <position name="L_SHOULDER_R_pos" joint="L_SHOULDER_R"
+              kp="40" ctrlrange="-1.57 1.57"/>
+    <position name="L_SHOULDER_Y_pos" joint="L_SHOULDER_Y"
+              kp="30" ctrlrange="-1.57 1.57"/>
+    <position name="L_ELBOW_R_pos"    joint="L_ELBOW_R"
+              kp="30" ctrlrange="-2.09 0.0"/>
+    <position name="L_WRIST_P_pos"    joint="L_WRIST_P"
+              kp="20" ctrlrange="-1.57 1.57"/>
+    <position name="L_WRIST_Y_pos"    joint="L_WRIST_Y"
+              kp="20" ctrlrange="-1.57 1.57"/>
+    <position name="L_WRIST_R_pos"    joint="L_WRIST_R"
+              kp="20" ctrlrange="-1.57 1.57"/>
+
+    <!-- 右臂：位置控制 -->
+    <position name="R_SHOULDER_P_pos" joint="R_SHOULDER_P"
+              kp="40" ctrlrange="-1.57 1.57"/>
+    <position name="R_SHOULDER_R_pos" joint="R_SHOULDER_R"
+              kp="40" ctrlrange="-1.57 1.57"/>
+    <position name="R_SHOULDER_Y_pos" joint="R_SHOULDER_Y"
+              kp="30" ctrlrange="-1.57 1.57"/>
+    <position name="R_ELBOW_R_pos"    joint="R_ELBOW_R"
+              kp="40" ctrlrange="0 2.05"/>
+    <position name="R_WRIST_P_pos"    joint="R_WRIST_P"
+              kp="20" ctrlrange="-1.57 1.57"/>
+    <position name="R_WRIST_Y_pos"    joint="R_WRIST_Y"
+              kp="20" ctrlrange="-1.57 1.57"/>
+    <position name="R_WRIST_R_pos"    joint="R_WRIST_R"
+              kp="20" ctrlrange="-1.57 1.57"/>
+  </actuator>
+
+
+</mujoco>

config/robot_description/hc_description/left_arm.urdf

@@ -0,0 +1,281 @@
+<?xml version="1.0" encoding="utf-8"?>
+<robot name="left_arm">
+   <mujoco>
+    <compiler
+    meshdir="meshes"
+    balanceinertia="true"
+    discardvisual="false" />
+   </mujoco>
+  <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.05" 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/left_arm.xml

@@ -0,0 +1,63 @@
+<mujoco model="left_arm">
+  <compiler angle="radian" meshdir="meshes/"/>
+
+  <asset>
+    <mesh name="PELVIS_S" file="PELVIS_S.STL"/>
+    <mesh name="L_SHOULDER_P_S" file="L_SHOULDER_P_S.STL"/>
+    <mesh name="L_SHOULDER_R_S" file="L_SHOULDER_R_S.STL"/>
+    <mesh name="L_SHOULDER_Y_S" file="L_SHOULDER_Y_S.STL"/>
+    <mesh name="L_ELBOW_R_S" file="L_ELBOW_R_S.STL"/>
+    <mesh name="L_WRIST_P_S" file="L_WRIST_P_S.STL"/>
+    <mesh name="L_WRIST_Y_S" file="L_WRIST_Y_S.STL"/>
+    <mesh name="L_WRIST_R_S" file="L_WRIST_R_S.STL"/>
+  </asset>
+
+  <worldbody>
+    <geom size="0.05 0.6" type="cylinder" contype="0" conaffinity="0" group="1" density="0"/>
+    <geom size="0.05 0.6" type="cylinder"/>
+    <geom pos="0 0 0.6" quat="1 0 0 0" type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+    <geom pos="0 0 0.6" quat="1 0 0 0" type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="PELVIS_S"/>
+    <body name="L_SHOULDER_P_S" pos="0 0.0945 0.642">
+      <inertial pos="-0.00982259 0.0704593 1.15262e-06" quat="0.706163 0.705933 -0.0386962 -0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="L_SHOULDER_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-100 100"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
+      <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="L_SHOULDER_P_S"/>
+      <body name="L_SHOULDER_R_S" pos="0.035 0.0765 0">
+        <inertial pos="-0.0346025 0.0917393 -1.67085e-08" quat="0.358778 0.609303 -0.358823 0.609323" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="L_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-100 100"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_R_S"/>
+        <body name="L_SHOULDER_Y_S" pos="-0.035 0.1475 0">
+          <inertial pos="-0.00440977 0.086362 9.50749e-09" quat="0.705001 0.704998 -0.054559 -0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="L_SHOULDER_Y" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-49 49"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_SHOULDER_Y_S"/>
+          <body name="L_ELBOW_R_S" pos="0.034 0.1025 0">
+            <inertial pos="-0.0335624 0.06032 2.99656e-07" quat="0.674756 0.674714 0.211333 0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="L_ELBOW_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-49 49"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_ELBOW_R_S"/>
+            <body name="L_WRIST_P_S" pos="-0.034 0.0965 0">
+              <inertial pos="-1.39659e-10 0.0675973 0.0192006" quat="0.467537 0.530481 -0.530481 0.467537" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="L_WRIST_P" pos="0 0 0" axis="0 1 0" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
+              <geom type="mesh" rgba="0.698039 0.698039 0.698039 1" mesh="L_WRIST_P_S"/>
+              <body name="L_WRIST_Y_S" pos="0 0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06427e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="L_WRIST_Y" pos="0 0 0" axis="0 0 1" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="L_WRIST_Y_S"/>
+                <body name="L_WRIST_R_S" pos="0.0258 0 -0.039">
+                  <inertial pos="-0.0161477 0.0955047 -0.00499392" quat="0.595488 0.369507 -0.591676 0.39847" mass="0.504894" diaginertia="0.000280921 0.000188575 0.000178055"/>
+                  <joint name="L_WRIST_R" pos="0 0 0" axis="1 0 0" range="-3.14 3.14" actuatorfrcrange="-26 26"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="L_WRIST_R_S"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>

config/robot_description/hc_description/right_arm.urdf

@@ -0,0 +1,322 @@
+<?xml version="1.0" encoding="utf-8"?>
+
+<robot name="right_arm">
+   <mujoco>
+    <compiler
+    meshdir="meshes"
+    balanceinertia="true"
+    discardvisual="false" />
+   </mujoco>
+  <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>

config/robot_description/hc_description/right_arm.xml

@@ -0,0 +1,63 @@
+<mujoco model="right_arm">
+  <compiler angle="radian" meshdir="meshes/"/>
+
+  <asset>
+    <mesh name="PELVIS_S" file="PELVIS_S.STL"/>
+    <mesh name="R_SHOULDER_P_S" file="R_SHOULDER_P_S.STL"/>
+    <mesh name="R_SHOULDER_R_S" file="R_SHOULDER_R_S.STL"/>
+    <mesh name="R_SHOULDER_Y_S" file="R_SHOULDER_Y_S.STL"/>
+    <mesh name="R_ELBOW_R_S" file="R_ELBOW_R_S.STL"/>
+    <mesh name="R_WRIST_P_S" file="R_WRIST_P_S.STL"/>
+    <mesh name="R_WRIST_Y_S" file="R_WRIST_Y_S.STL"/>
+    <mesh name="R_WRIST_R_S" file="R_WRIST_R_S.STL"/>
+  </asset>
+
+  <worldbody>
+    <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.698 0.698 0.698 1" mesh="PELVIS_S"/>
+    <geom type="mesh" rgba="0.698 0.698 0.698 1" mesh="PELVIS_S"/>
+    <body name="R_SHOULDER_P_S" pos="0 -0.0945 0.042">
+      <inertial pos="-0.00982259 -0.0704593 -1.1507e-06" quat="0.706163 0.705933 0.0386962 0.0386741" mass="0.880738" diaginertia="0.000584874 0.000465648 0.000443849"/>
+      <joint name="R_SHOULDER_P" pos="0 0 0" axis="0 -1 0" range="-1.57 1.57" actuatorfrcrange="-120 120"/>
+      <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
+      <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_P_S"/>
+      <body name="R_SHOULDER_R_S" pos="0.035 -0.0765 0">
+        <inertial pos="-0.0346025 -0.0917393 1.86281e-08" quat="0.609323 0.358823 -0.609303 0.358778" mass="0.594788" diaginertia="0.000414771 0.000407636 0.000294296"/>
+        <joint name="R_SHOULDER_R" pos="0 0 0" axis="1 0 0" range="-2 2" actuatorfrcrange="-120 120"/>
+        <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
+        <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_R_S"/>
+        <body name="R_SHOULDER_Y_S" pos="-0.035 -0.1475 0">
+          <inertial pos="-0.00440977 -0.086362 -7.58792e-09" quat="0.705001 0.704998 0.054559 0.0545441" mass="0.563406" diaginertia="0.000329815 0.000297341 0.000211019"/>
+          <joint name="R_SHOULDER_Y" pos="0 0 0" axis="0 -1 0" range="0 3.14" actuatorfrcrange="-80 80"/>
+          <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
+          <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_SHOULDER_Y_S"/>
+          <body name="R_ELBOW_R_S" pos="0.034 -0.1025 0">
+            <inertial pos="-0.0335624 -0.06032 -2.97736e-07" quat="0.674756 0.674714 -0.211333 -0.211667" mass="0.393572" diaginertia="0.000189078 0.000181042 0.000139034"/>
+            <joint name="R_ELBOW_R" pos="0 0 0" axis="1 0 0" range="0 2.18" actuatorfrcrange="-80 80"/>
+            <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
+            <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_ELBOW_R_S"/>
+            <body name="R_WRIST_P_S" pos="-0.034 -0.0965 0">
+              <inertial pos="-1.39657e-10 -0.0675973 0.0192006" quat="0.530481 0.467537 -0.467537 0.530481" mass="0.442332" diaginertia="0.000489142 0.000476754 9.72811e-05"/>
+              <joint name="R_WRIST_P" pos="0 0 0" axis="0 -1 0" range="-3.14 0" actuatorfrcrange="-50 50"/>
+              <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
+              <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_P_S"/>
+              <body name="R_WRIST_Y_S" pos="0 -0.1525 0.039">
+                <inertial pos="-0.00464136 -5.06426e-10 -0.0341254" quat="0.298107 0.641196 0.641196 0.298107" mass="0.235738" diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
+                <joint name="R_WRIST_Y" pos="0 0 0" axis="0 0 1" range="0 0.78" actuatorfrcrange="-50 50"/>
+                <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
+                <geom type="mesh" rgba="0.647059 0.619608 0.588235 1" mesh="R_WRIST_Y_S"/>
+                <body name="R_WRIST_R_S" pos="0.03 0 -0.039">
+                  <inertial pos="-0.0201642 -0.11075 -0.00598955" quat="0.483404 0.529786 -0.463203 0.520663" mass="0.204366" diaginertia="0.00027189 0.000186086 0.000130629"/>
+                  <joint name="R_WRIST_R" pos="0 0 0" axis="1 0 0" range="-0.26 1.57" actuatorfrcrange="-50 50"/>
+                  <geom type="mesh" contype="0" conaffinity="0" group="1" density="0" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
+                  <geom type="mesh" rgba="0.890196 0.890196 0.913725 1" mesh="R_WRIST_R_S"/>
+                  <geom size="0.002" pos="-0.01212 -0.17655 0.07506" quat="9.38184e-07 0.707105 -0.707108 -9.38187e-07" contype="0" conaffinity="0" group="1" density="0" rgba="0 1 0 1"/>
+                  <geom size="0.005" pos="-0.01212 -0.17655 0.07506" quat="9.38184e-07 0.707105 -0.707108 -9.38187e-07" rgba="0 1 0 1"/>
+                </body>
+              </body>
+            </body>
+          </body>
+        </body>
+      </body>
+    </body>
+  </worldbody>
+</mujoco>

data/ik/plot_joint_data.py

@@ -0,0 +1,73 @@
+import pandas as pd
+import numpy as np
+import matplotlib.pyplot as plt
+
+CSV = '/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv'
+df = pd.read_csv(CSV)
+
+# ---- 曲线图：q1~q7 (+ 可选 psi) vs index ----
+joint_cols_expect = ['q1','q2','q3','q4','q5','q6','q7']
+joint_cols = [c for c in joint_cols_expect if c in df.columns]
+assert len(joint_cols) == 7, f"CSV 缺少关节列，期望 {joint_cols_expect}，实际 {list(df.columns)}"
+
+x = np.arange(len(df))
+plt.figure(figsize=(12, 5))
+for c in joint_cols:
+    plt.plot(x, df[c].to_numpy(), label=c, linewidth=1.2)
+
+if 'psi' in df.columns:
+    plt.plot(x, df['psi'].to_numpy(), '--', label='psi', linewidth=1.2)
+
+plt.xlabel('index')
+plt.ylabel('angle (rad)')
+plt.title('q1..q7 (and psi) vs index')
+plt.grid(True, alpha=0.35)
+plt.legend(ncol=4, fontsize=9)
+plt.tight_layout()
+plt.show()
+
+# ---- 柱状图：最近限位距离（选“最危险帧”） ----
+limits = np.array([
+    [-0.26,  1.57],
+    [-0.78,  1.57],
+    [-np.pi,  np.pi],
+    [ 0.00,  2.05],
+    [-3.00,  3.00],
+    [-2.00,  2.00],
+    [-0.57,  1.57],
+])
+
+Q = df[joint_cols].to_numpy()          # [N,7]
+lo = limits[:, 0][None, :]             # [1,7]
+hi = limits[:, 1][None, :]
+
+dist_low  = Q - lo                     # 到下限的距离
+dist_high = hi - Q                     # 到上限的距离
+nearest   = np.minimum(dist_low, dist_high)  # 最近限位（可为负，负值=超限）
+
+# 找“最危险”的 index（全关节最小裕度最小）
+min_margin_per_row = nearest.min(axis=1)     # 每帧的最小关节裕度
+worst_idx = int(np.argmin(min_margin_per_row))
+vals = nearest[worst_idx, :]
+
+psi_text = f", psi={df['psi'].iloc[worst_idx]:.4f}" if 'psi' in df.columns else ""
+
+plt.figure(figsize=(9, 4.5))
+plt.bar(joint_cols, vals)
+plt.axhline(0.0, linewidth=1, color='k')
+plt.ylabel('Nearest distance to limit (rad)')
+plt.title(f'Per-joint margin at worst frame (index={worst_idx}{psi_text})')
+plt.grid(True, axis='y', alpha=0.35)
+plt.tight_layout()
+plt.show()
+
+# ---- 可选：整段最小裕度曲线（帮助定位危险段）----
+plt.figure(figsize=(12, 3.2))
+plt.plot(min_margin_per_row, linewidth=1.2)
+plt.axhline(0.0, linewidth=1, color='k')
+plt.xlabel('index')
+plt.ylabel('min margin (rad)')
+plt.title('Minimum per-frame joint margin over sweep (rad)')
+plt.grid(True, alpha=0.35)
+plt.tight_layout()
+plt.show()

data/ik/plot_joint_traj.py

@@ -0,0 +1,59 @@
+# pip install toppra numpy scipy pandas matplotlib
+import numpy as np
+import pandas as pd
+import matplotlib.pyplot as plt
+import toppra as ta
+import toppra.algorithm as algo
+import toppra.constraint as constraint
+import toppra.interpolator as interp
+
+# === 1) 读取 CSV（只取 q1..q7）===
+CSV_PATH = "/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv"   #
+joint_cols = ["q1","q2","q3","q4","q5","q6","q7"]
+df = pd.read_csv(CSV_PATH)
+assert all(c in df.columns for c in joint_cols), "CSV 缺少关节列 q1..q7"
+waypoints = df[joint_cols].to_numpy()
+N, dof = waypoints.shape
+assert N >= 2, "至少需要两行关节路点"
+
+# === 2) 构造样条路径（路径参数 s∈[0,1]）===
+s_grid = np.linspace(0, 1, N)
+path = interp.SplineInterpolator(s_grid, waypoints)
+
+# === 3) 约束（示例值：请改成你的真实上限）===
+vmax = np.array([1.5, 1.5, 1.5, 1.5, 1.5, 1.5, 1.5])  # rad/s
+amax = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0])  # rad/s^2
+vlim = np.vstack([-vmax, vmax]).T
+alim = np.vstack([-amax, amax]).T
+pc_vel = constraint.JointVelocityConstraint(vlim)
+pc_acc = constraint.JointAccelerationConstraint(alim)
+
+# === 4) toppra 时间最优轨迹（起止速度=0）===
+topp = algo.TOPPRA([pc_vel, pc_acc], path, solver_wrapper="seidel")
+traj = topp.compute_trajectory(0.0, 0.0)
+T = traj.get_duration()
+print(f"Total duration: {T:.4f} s")
+
+# === 5) 采样并绘图 ===
+ts = np.linspace(0, T, 300)
+q   = traj.eval(ts)    # (300,7)
+qd  = traj.evald(ts)
+qdd = traj.evaldd(ts)
+
+# 位置
+plt.figure()
+for i in range(dof):
+    plt.plot(ts, q[:, i], label=f"q{i+1}")
+plt.xlabel("Time (s)"); plt.ylabel("Position (rad)"); plt.title("Joint Positions"); plt.legend(); plt.tight_layout(); plt.show()
+
+# 速度
+plt.figure()
+for i in range(dof):
+    plt.plot(ts, qd[:, i], label=f"qd{i+1}")
+plt.xlabel("Time (s)"); plt.ylabel("Velocity (rad/s)"); plt.title("Joint Velocities"); plt.legend(); plt.tight_layout(); plt.show()
+
+# 加速度
+plt.figure()
+for i in range(dof):
+    plt.plot(ts, qdd[:, i], label=f"qdd{i+1}")
+plt.xlabel("Time (s)"); plt.ylabel("Acceleration (rad/s²)"); plt.title("Joint Accelerations"); plt.legend(); plt.tight_layout(); plt.show()

data/ik/plot_psi_data.py

@@ -0,0 +1,59 @@
+import pandas as pd
+import numpy as np
+import matplotlib.pyplot as plt
+
+df = pd.read_csv('/home/lgv/cmvr/cmvr-es/data/ik_psi_sweep.csv')
+
+joints = ['q1','q2','q3','q4','q5','q6','q7']
+fig, axes = plt.subplots(len(joints), 1, figsize=(10, 2.6*len(joints)), sharex=True)
+x = df['psi'].to_numpy()
+
+for ax, col in zip(axes, joints):
+    y = df[col].to_numpy()
+
+    # 过滤 NaN
+    mask = ~np.isnan(x) & ~np.isnan(y)
+    xv, yv = x[mask], y[mask]
+    if len(xv) == 0:
+        ax.set_title(f'{col}: no data')
+        continue
+
+    # 散点图（不画连线）
+    ax.scatter(xv, yv, s=10, alpha=0.8)  # s 调整点大小
+
+    # 极值与对应 psi
+    i_min = int(np.argmin(yv))
+    i_max = int(np.argmax(yv))
+    y_min, psi_min = float(yv[i_min]), float(xv[i_min])
+    y_max, psi_max = float(yv[i_max]), float(xv[i_max])
+
+    # 水平红虚线（值）
+    ax.axhline(y_min, color='red', linestyle='--', linewidth=1)
+    ax.axhline(y_max, color='red', linestyle='--', linewidth=1)
+    # 垂直红虚线（psi）
+    ax.axvline(psi_min, color='red', linestyle='--', linewidth=1, alpha=0.7)
+    ax.axvline(psi_max, color='red', linestyle='--', linewidth=1, alpha=0.7)
+
+    # 极值点标记
+    ax.plot(psi_min, y_min, 'ro', markersize=4)
+    ax.plot(psi_max, y_max, 'ro', markersize=4)
+
+    # 标注：值 + psi
+    ax.annotate(f"min={y_min:.4f}\nψ={psi_min:.4f}",
+                xy=(psi_min, y_min), xytext=(8, -10),
+                textcoords='offset points', color='red',
+                ha='left', va='top', fontsize=9,
+                bbox=dict(boxstyle="round,pad=0.2", fc="white", ec="none"))
+    ax.annotate(f"max={y_max:.4f}\nψ={psi_max:.4f}",
+                xy=(psi_max, y_max), xytext=(8, 10),
+                textcoords='offset points', color='red',
+                ha='left', va='bottom', fontsize=9,
+                bbox=dict(boxstyle="round,pad=0.2", fc="white", ec="none"))
+
+    ax.set_ylabel(f"{col} (rad)")
+    ax.grid(True, alpha=0.35)
+
+axes[-1].set_xlabel('psi (rad)')
+fig.suptitle('IK joints vs arm-angle psi', y=0.995)
+plt.tight_layout()
+plt.show()

data/ik_psi_sweep.csv

@@ -0,0 +1,128 @@
+psi,q1,q2,q3,q4,q5,q6,q7
+0.000000000,0.002038980,1.340620000,0.000000000,0.522261000,0.000000000,-0.000210733,-0.094236400
+0.000000000,0.578202939,0.486316887,0.740005579,1.570796327,0.625727922,0.531578664,0.175673351
+0.000000000,0.440761487,0.565335165,0.964336466,1.570796327,0.491292823,0.442355133,0.112628007
+0.000000000,0.371238784,0.622365950,1.090264075,1.575500039,0.409159634,0.381149325,0.079289637
+0.000000000,0.330463834,0.665306397,1.171413265,1.580187790,0.353118626,0.336100996,0.059064268
+0.000000000,0.303629491,0.698061413,1.228156884,1.584859682,0.312714061,0.302027645,0.045779819
+0.000000000,0.284432039,0.723308206,1.269901344,1.589515814,0.282624592,0.275821335,0.036565409
+0.000000000,0.269759165,0.742935173,1.301736480,1.594156283,0.259722475,0.255419601,0.029896996
+0.000000000,0.257917625,0.758312994,1.326716496,1.598781185,0.241997632,0.239372235,0.024895220
+0.000000000,0.247921267,0.770456663,1.346806717,1.603390613,0.228082340,0.226624470,0.021022719
+0.000000000,0.239166095,0.780127414,1.363333864,1.607984658,0.217010934,0.216393752,0.017936958
+0.000000000,0.231267578,0.787900642,1.377223781,1.612563410,0.208084987,0.208092599,0.015412337
+0.000000000,0.223973270,0.794213370,1.389137788,1.617126957,0.200791220,0.201276831,0.013296323
+0.000000000,0.217113255,0.799398540,1.399556180,1.621675385,0.194748299,0.195609113,0.011483536
+0.000000000,0.210570785,0.803710343,1.408832069,1.626208778,0.189670728,0.190832474,0.009899847
+0.000000000,0.204264237,0.807343170,1.417227480,1.630727217,0.185343528,0.186750724,0.008492307
+0.000000000,0.198135690,0.810445914,1.424938288,1.635230785,0.181604070,0.183213775,0.007222605
+0.000000000,0.192143504,0.813132804,1.432111826,1.639719559,0.178328830,0.180106596,0.006062701
+0.000000000,0.186257388,0.815491634,1.438859529,1.644193617,0.175423635,0.177340836,0.004991872
+0.000000000,0.180455066,0.817590024,1.445266121,1.648653034,0.172816415,0.174848488,0.003994663
+0.000000000,0.174719980,0.819480182,1.451396347,1.653097884,0.170451797,0.172577060,0.003059449
+0.000000000,0.169039695,0.821202537,1.457299956,1.657528240,0.168287051,0.170485916,0.002177423
+0.000000000,0.163404770,0.822788501,1.463015392,1.661944171,0.166289047,0.168543487,0.001341855
+0.000000000,0.157807954,0.824262581,1.468572550,1.666345748,0.164431955,0.166725144,0.000547564
+0.000000000,0.152243613,0.825643979,1.473994836,1.670733038,0.162695519,0.165011583,-0.000209470
+0.000000000,0.146707313,0.826947825,1.479300719,1.675106107,0.161063745,0.163387594,-0.000932394
+0.000000000,0.141195517,0.828186109,1.484504889,1.679465019,0.159523905,0.161841116,-0.001623691
+0.000000000,0.135705364,0.829368399,1.489619141,1.683809837,0.158065790,0.160362521,-0.002285340
+0.000000000,0.130234505,0.830502387,1.494653037,1.688140624,0.156681132,0.158944065,-0.002918932
+0.000000000,0.124780985,0.831594308,1.499614410,1.692457439,0.155363175,0.157579468,-0.003525759
+0.000000000,0.119343153,0.832649258,1.504509746,1.696760341,0.154106335,0.156263589,-0.004106883
+0.000000000,0.113919592,0.833671449,1.509344478,1.701049387,0.152905953,0.154992182,-0.004663182
+0.000000000,0.108509076,0.834664389,1.514123204,1.705324634,0.151758094,0.153761706,-0.005195391
+0.000000000,0.103110526,0.835631035,1.518849866,1.709586135,0.150659401,0.152569174,-0.005704133
+0.000000000,0.097722982,0.836573903,1.523527872,1.713833945,0.149606979,0.151412044,-0.006189938
+0.000000000,0.092345584,0.837495157,1.528160207,1.718068116,0.148598304,0.150288131,-0.006653263
+0.000000000,0.086977554,0.838396676,1.532749505,1.722288697,0.147631153,0.149195535,-0.007094505
+0.000000000,0.081618181,0.839280108,1.537298113,1.726495738,0.146703554,0.148132591,-0.007514010
+0.000000000,0.076266813,0.840146913,1.541808142,1.730689288,0.145813738,0.147097827,-0.007912084
+0.000000000,0.070922846,0.840998392,1.546281501,1.734869392,0.144960109,0.146089928,-0.008288996
+0.000000000,0.065585720,0.841835716,1.550719930,1.739036098,0.144141215,0.145107715,-0.008644986
+0.000000000,0.060254914,0.842659947,1.555125023,1.743189448,0.143355729,0.144150120,-0.008980270
+0.000000000,0.054929937,0.843472051,1.559498249,1.747329486,0.142602429,0.143216170,-0.009295039
+0.000000000,0.049610331,0.844272918,1.563840968,1.751456254,0.141880187,0.142304975,-0.009589466
+0.000000000,0.044295663,0.845063363,1.568154441,1.755569792,0.141187955,0.141415716,-0.009863707
+0.000000000,0.038985523,0.845844147,1.572439847,1.759670141,0.140524756,0.140547637,-0.010117902
+0.000000000,0.033679525,0.846615971,1.576698286,1.763757337,0.139889677,0.139700037,-0.010352177
+0.000000000,0.028377300,0.847379495,1.580930790,1.767831419,0.139281864,0.138872263,-0.010566647
+0.000000000,0.023078501,0.848135333,1.585138328,1.771892423,0.138700512,0.138063706,-0.010761414
+0.000000000,0.017782795,0.848884063,1.589321815,1.775940383,0.138144864,0.137273797,-0.010936570
+0.000000000,0.012489864,0.849626229,1.593482110,1.779975333,0.137614205,0.136502000,-0.011092198
+0.000000000,0.007199406,0.850362343,1.597620030,1.783997306,0.137107858,0.135747816,-0.011228373
+0.000000000,0.001911132,0.851092890,1.601736342,1.788006333,0.136625184,0.135010771,-0.011345161
+0.000000000,-0.003375237,0.851818328,1.605831779,1.792002444,0.136165576,0.134290419,-0.011442619
+0.000000000,-0.008659966,0.852539094,1.609907031,1.795985670,0.135728457,0.133586342,-0.011520800
+0.000000000,-0.013943309,0.853255600,1.613962757,1.799956038,0.135313278,0.132898140,-0.011579750
+0.000000000,-0.019225512,0.853968239,1.617999581,1.803913576,0.134919519,0.132225440,-0.011619505
+0.000000000,-0.024506812,0.854677386,1.622018097,1.807858309,0.134546681,0.131567884,-0.011640101
+0.000000000,-0.029787434,0.855383399,1.626018873,1.811790264,0.134194290,0.130925134,-0.011641565
+0.000000000,-0.035067598,0.856086617,1.630002447,1.815709463,0.133861895,0.130296872,-0.011623919
+0.000000000,-0.040347515,0.856787368,1.633969333,1.819615930,0.133549062,0.129682792,-0.011587182
+0.000000000,-0.045627388,0.857485962,1.637920022,1.823509687,0.133255378,0.129082605,-0.011531367
+0.000000000,-0.050907414,0.858182698,1.641854983,1.827390756,0.132980448,0.128496037,-0.011456482
+0.000000000,-0.056187785,0.858877862,1.645774662,1.831259155,0.132723892,0.127922826,-0.011362533
+0.000000000,-0.061468683,0.859571728,1.649679488,1.835114905,0.132485349,0.127362723,-0.011249521
+0.000000000,-0.066750288,0.860264561,1.653569869,1.838958023,0.132264471,0.126815490,-0.011117442
+0.000000000,-0.072032772,0.860956612,1.657446195,1.842788526,0.132060923,0.126280903,-0.010966291
+0.000000000,-0.077316303,0.861648126,1.661308841,1.846606431,0.131874386,0.125758746,-0.010796058
+0.000000000,-0.082601044,0.862339336,1.665158164,1.850411752,0.131704553,0.125248813,-0.010606730
+0.000000000,-0.087887153,0.863030468,1.668994507,1.854204504,0.131551128,0.124750909,-0.010398291
+0.000000000,-0.093174784,0.863721739,1.672818198,1.857984700,0.131413827,0.124264846,-0.010170721
+0.000000000,-0.098464085,0.864413358,1.676629550,1.861752352,0.131292378,0.123790447,-0.009923998
+0.000000000,-0.103755203,0.865105528,1.680428865,1.865507472,0.131186518,0.123327542,-0.009658099
+0.000000000,-0.109048278,0.865798443,1.684216430,1.869250069,0.131095995,0.122875967,-0.009372995
+0.000000000,-0.114343449,0.866492291,1.687992522,1.872980154,0.131020565,0.122435568,-0.009068657
+0.000000000,-0.119640850,0.867187256,1.691757406,1.876697735,0.130959994,0.122006196,-0.008745051
+0.000000000,-0.124940612,0.867883512,1.695511334,1.880402820,0.130914057,0.121587711,-0.008402144
+0.000000000,-0.130242862,0.868581230,1.699254550,1.884095415,0.130882535,0.121179977,-0.008039898
+0.000000000,-0.135547726,0.869280574,1.702987286,1.887775526,0.130865219,0.120782865,-0.007658274
+0.000000000,-0.140855324,0.869981705,1.706709765,1.891443158,0.130861907,0.120396252,-0.007257229
+0.000000000,-0.146165776,0.870684776,1.710422202,1.895098315,0.130872404,0.120020021,-0.006836721
+0.000000000,-0.151479199,0.871389938,1.714124799,1.898741001,0.130896520,0.119654058,-0.006396704
+0.000000000,-0.156795705,0.872097336,1.717817754,1.902371216,0.130934076,0.119298259,-0.005937130
+0.000000000,-0.162115406,0.872807111,1.721501253,1.905988964,0.130984894,0.118952519,-0.005457950
+0.000000000,-0.167438410,0.873519398,1.725175477,1.909594244,0.131048805,0.118616741,-0.004959112
+0.000000000,-0.172764825,0.874234332,1.728840596,1.913187056,0.131125646,0.118290833,-0.004440562
+0.000000000,-0.178094754,0.874952041,1.732496776,1.916767399,0.131215259,0.117974706,-0.003902247
+0.000000000,-0.183428299,0.875672650,1.736144172,1.920335270,0.131317489,0.117668276,-0.003344109
+0.000000000,-0.188765561,0.876396281,1.739782936,1.923890667,0.131432190,0.117371461,-0.002766090
+0.000000000,-0.194106637,0.877123052,1.743413209,1.927433586,0.131559218,0.117084186,-0.002168129
+0.000000000,-0.199451624,0.877853078,1.747035130,1.930964022,0.131698435,0.116806378,-0.001550166
+0.000000000,-0.204800616,0.878586471,1.750648827,1.934481969,0.131849707,0.116537967,-0.000912137
+0.000000000,-0.210153705,0.879323339,1.754254425,1.937987422,0.132012905,0.116278888,-0.000253978
+0.000000000,-0.215510983,0.880063789,1.757852042,1.941480372,0.132187903,0.116029078,0.000424378
+0.000000000,-0.220872538,0.880807923,1.761441791,1.944960813,0.132374581,0.115788479,0.001122999
+0.000000000,-0.226238458,0.881555841,1.765023777,1.948428734,0.132572820,0.115557034,0.001841952
+0.000000000,-0.231608829,0.882307642,1.768598103,1.951884127,0.132782508,0.115334690,0.002581309
+0.000000000,-0.236983736,0.883063421,1.772164864,1.955326981,0.133003534,0.115121397,0.003341141
+0.000000000,-0.242363261,0.883823269,1.775724150,1.958757284,0.133235792,0.114917109,0.004121521
+0.000000000,-0.247747485,0.884587277,1.779276047,1.962175024,0.133479179,0.114721780,0.004922524
+0.000000000,-0.253136490,0.885355532,1.782820636,1.965580188,0.133733595,0.114535369,0.005744225
+0.000000000,-0.258530353,0.886128121,1.786357992,1.968972763,0.133998943,0.114357837,0.006586701
+0.000000000,-0.263929153,0.886905126,1.789888186,1.972352734,0.134275129,0.114189147,0.007450029
+0.000000000,-0.269332966,0.887686629,1.793411286,1.975720085,0.134562062,0.114029264,0.008334290
+0.000000000,-0.274741865,0.888472709,1.796927352,1.979074800,0.134859656,0.113878158,0.009239563
+0.000000000,-0.280155926,0.889263443,1.800436443,1.982416862,0.135167823,0.113735798,0.010165930
+0.000000000,-0.285575221,0.890058905,1.803938611,1.985746253,0.135486482,0.113602158,0.011113473
+0.000000000,-0.290999820,0.890859169,1.807433906,1.989062955,0.135815553,0.113477212,0.012082277
+0.000000000,-0.296429795,0.891664305,1.810922373,1.992366949,0.136154957,0.113360936,0.013072425
+0.000000000,-0.301865215,0.892474384,1.814404052,1.995658213,0.136504620,0.113253312,0.014084004
+0.000000000,-0.307306147,0.893289473,1.817878980,1.998936727,0.136864469,0.113154318,0.015117101
+0.000000000,-0.312752658,0.894109637,1.821347191,2.002202469,0.137234433,0.113063939,0.016171803
+0.000000000,-0.318204815,0.894934939,1.824808713,2.005455417,0.137614443,0.112982160,0.017248199
+0.000000000,-0.323662683,0.895765444,1.828263571,2.008695547,0.138004434,0.112908967,0.018346379
+0.000000000,-0.329126324,0.896601210,1.831711788,2.011922835,0.138404340,0.112844349,0.019466434
+0.000000000,-0.334595804,0.897442297,1.835153381,2.015137257,0.138814099,0.112788297,0.020608455
+0.000000000,-0.340071182,0.898288761,1.838588365,2.018338785,0.139233651,0.112740803,0.021772535
+0.000000000,-0.345552521,0.899140659,1.842016750,2.021527395,0.139662937,0.112701861,0.022958766
+0.000000000,-0.351039881,0.899998044,1.845438544,2.024703059,0.140101899,0.112671467,0.024167244
+0.000000000,-0.356533320,0.900860968,1.848853751,2.027865749,0.140550484,0.112649617,0.025398064
+0.000000000,-0.362032897,0.901729483,1.852262372,2.031015436,0.141008637,0.112636311,0.026651320
+0.000000000,-0.367538669,0.902603637,1.855664402,2.034152091,0.141476307,0.112631550,0.027927111
+0.000000000,-0.373050693,0.903483479,1.859059837,2.037275683,0.141953442,0.112635335,0.029225533
+0.000000000,-0.378569025,0.904369054,1.862448667,2.040386181,0.142439995,0.112647669,0.030546685
+0.000000000,-0.384093719,0.905260407,1.865830879,2.043483554,0.142935918,0.112668558,0.031890665
+0.000000000,-0.389624828,0.906157581,1.869206457,2.046567770,0.143441165,0.112698008,0.033257574
+0.000000000,-0.395162407,0.907060618,1.872575383,2.049638794,0.143955692,0.112736027,0.034647511

data/planner/plot_traj.py

@@ -0,0 +1,63 @@
+import os
+import re
+import sys
+import pandas as pd
+import matplotlib.pyplot as plt
+
+def main(csv_path: str):
+    # 更健壮的读取
+    df = pd.read_csv(csv_path, sep=None, engine="python", comment="#")
+    if 't' not in df.columns:
+        raise RuntimeError("CSV missing 't' column")
+
+    # ——严格区分列名——
+    # 允许列名形如：q0, q1, ... | qd0, qd1, ... | qdd0, qdd1, ...
+    def grep(pattern):
+        r = re.compile(pattern)
+        return [c for c in df.columns if r.fullmatch(c)]
+
+    q_cols   = grep(r"q\d+")
+    qd_cols  = grep(r"qd\d+")
+    qdd_cols = grep(r"qdd\d+")
+
+    # 如果你的列名不是纯数字后缀（比如 q_joint1），把上面的正则改成更宽松的：
+    # q_cols   = [c for c in df.columns if c.startswith("q")   and not c.startswith(("qd","qdd"))]
+    # qd_cols  = [c for c in df.columns if c.startswith("qd")  and not c.startswith("qdd")]
+    # qdd_cols = [c for c in df.columns if c.startswith("qdd")]
+
+    t = df['t'].values
+
+    # 位置
+    plt.figure()
+    for c in q_cols:
+        plt.plot(t, df[c].values, label=c)
+    plt.xlabel('time [s]')
+    plt.ylabel('position [rad]')
+    plt.title('Joint Positions')
+    plt.legend()
+    plt.grid(True)
+
+    # 速度（只画 qd*）
+    plt.figure()
+    for c in qd_cols:
+        plt.plot(t, df[c].values, label=c)
+    plt.xlabel('time [s]')
+    plt.ylabel('velocity [rad/s]')
+    plt.title('Joint Velocities')
+    plt.legend()
+    plt.grid(True)
+
+    # 加速度（只画 qdd*）
+    plt.figure()
+    for c in qdd_cols:
+        plt.plot(t, df[c].values, label=c)
+    plt.xlabel('time [s]')
+    plt.ylabel('acceleration [rad/s^2]')
+    plt.title('Joint Accelerations')
+    plt.legend()
+    plt.grid(True)
+
+    plt.show()
+
+if __name__ == '__main__':
+    main("/home/lgv/cmvr/cmvr-es/data/planner/traj.csv")

data/planner/traj.csv

@@ -0,0 +1,110 @@
+t,q1,qd1,qdd1
+0.0000000000,0.0000000000,0.0000000000,0.0000000000
+0.0100000000,-0.0000159557,-0.0063777738,-1.9106392001
+0.0200000000,-0.0002496495,-0.0461350389,-4.8925174498
+0.0300000000,-0.0009799863,-0.1009783500,-5.7006022582
+0.0400000000,-0.0022807296,-0.1593365091,-5.9446327575
+0.0500000000,-0.0041699534,-0.2185817120,-5.9159701967
+0.0600000000,-0.0066533315,-0.2781414650,-5.9705725375
+0.0700000000,-0.0097336379,-0.3379378160,-5.9974845155
+0.0800000000,-0.0134121852,-0.3977883838,-5.9866809788
+0.0900000000,-0.0176896602,-0.4577119179,-5.9970097232
+0.1000000000,-0.0225665586,-0.5176710831,-5.9958021374
+0.1100000000,-0.0280431623,-0.5776536328,-5.9994155793
+0.1200000000,-0.0341196983,-0.6376542617,-5.9998355059
+0.1300000000,-0.0407961934,-0.6976428031,-5.9975948022
+0.1400000000,-0.0480724867,-0.7576129914,-5.9994877401
+0.1500000000,-0.0559484540,-0.8175781906,-5.9948813054
+0.1600000000,-0.0644239673,-0.8775223553,-5.9991411360
+0.1700000000,-0.0734989367,-0.9374685960,-5.9921968215
+0.1800000000,-0.0831732416,-0.9973926934,-5.9980487477
+0.1900000000,-0.0934468113,-1.0573180377,-5.9884990499
+0.2000000000,-0.1043195108,-1.1172260364,-5.9955305424
+0.2100000000,-0.1157912824,-1.1771239542,-5.9828859374
+0.2200000000,-0.1278619739,-1.2370190713,-5.9907608055
+0.2300000000,-0.1405315123,-1.2968780749,-5.9743209965
+0.2400000000,-0.1537997689,-1.3567629311,-5.9826547281
+0.2500000000,-0.1676665996,-1.4166118373,-5.9923074269
+0.2600000000,-0.1821319218,-1.4764412133,-5.9697616364
+0.2700000000,-0.1970596457,-1.5002473361,0.0171102264
+0.2800000000,-0.2120611169,-1.5001229872,-0.0936585266
+0.2900000000,-0.2270627160,-1.5001591883,0.0626046461
+0.3000000000,-0.2420638680,-1.5001494874,-0.0493634487
+0.3100000000,-0.2570652126,-1.5001168351,0.0601054870
+0.3200000000,-0.2720661740,-1.5001226497,-0.0429343553
+0.3300000000,-0.2870673191,-1.5001201883,0.0376000906
+0.3400000000,-0.3020681493,-1.5000681224,-0.0536751182
+0.3500000000,-0.3170691028,-1.5001264472,0.0082265532
+0.3600000000,-0.3320699393,-1.5000247482,0.0601852966
+0.3700000000,-0.3470706417,-1.5000970260,-0.0220756027
+0.3800000000,-0.3620714540,-1.5000937922,0.0202436733
+0.3900000000,-0.3770720959,-1.5000067263,-0.0509307278
+0.4000000000,-0.3920727217,-1.5000860034,-0.0153620262
+0.4100000000,-0.4070734099,-1.5000814991,0.0158196206
+0.4200000000,-0.4220739813,-1.5000096953,0.0433168602
+0.4300000000,-0.4370744971,-1.5000652115,-0.0197234398
+0.4400000000,-0.4520750926,-1.5000792039,0.0048924020
+0.4500000000,-0.4670756435,-1.5000418289,0.0272059475
+0.4600000000,-0.4820760839,-1.5000304960,-0.0291077061
+0.4700000000,-0.4970765865,-1.5000689907,-0.0084527266
+0.4800000000,-0.5120771137,-1.5000650107,0.0108023623
+0.4900000000,-0.5270775833,-1.5000230438,0.0289463322
+0.5000000000,-0.5420779858,-1.5000379628,-0.0229003696
+0.5100000000,-0.5570784597,-1.5000659848,-0.0053852033
+0.5200000000,-0.5720789504,-1.5000591832,0.0116064517
+0.5300000000,-0.5870793908,-1.5000189548,0.0282953124
+0.5400000000,-0.6020797808,-1.5000382186,-0.0217869758
+0.5500000000,-0.6170802457,-1.5000652898,-0.0051190608
+0.5600000000,-0.6320807334,-1.5000590120,0.0117371194
+0.5700000000,-0.6470811772,-1.5000183275,0.0289971921
+0.5800000000,-0.6620815779,-1.5000411362,-0.0221300068
+0.5900000000,-0.6770820652,-1.5000691816,-0.0043107723
+0.6000000000,-0.6920825806,-1.5000599565,0.0144703189
+0.6100000000,-0.7070830438,-1.5000094950,0.0344841461
+0.6200000000,-0.7220834921,-1.5000550755,-0.0204007575
+0.6300000000,-0.7370840458,-1.5000780803,0.0010977386
+0.6400000000,-0.7520846130,-1.5000518625,0.0246543532
+0.6500000000,-0.7670850940,-1.5000277946,-0.0362960678
+0.6600000000,-0.7820856746,-1.5000839673,-0.0101938888
+0.6700000000,-0.7970863370,-1.5000768918,0.0192059116
+0.6800000000,-0.8120869163,-1.5000079810,-0.0500617756
+0.6900000000,-0.8270875558,-1.5000937560,-0.0161395518
+0.7000000000,-0.8420883334,-1.5000899803,0.0230861013
+0.7100000000,-0.8570890000,-1.5000264122,-0.0561275847
+0.7200000000,-0.8720897997,-1.5001202617,-0.0085789842
+0.7300000000,-0.8870907129,-1.5000718602,0.0477346007
+0.7400000000,-0.9020914935,-1.5001047330,-0.0409993467
+0.7500000000,-0.9170925757,-1.5001292996,0.0312149063
+0.7600000000,-0.9320934962,-1.5000800883,-0.0699810062
+0.7700000000,-0.9470947367,-1.5001636089,0.0266269093
+0.7800000000,-0.9620958396,-1.5000957794,-0.0858124395
+0.7900000000,-0.9770973277,-1.5001775357,0.0497342643
+0.8000000000,-0.9920986393,-1.5001878235,-0.0674476188
+0.8100000000,-1.0071004195,-1.5000874072,0.1329867627
+0.8200000000,-1.0219392817,-1.4598649714,5.9855481837
+0.8300000000,-1.0362386361,-1.4000134628,5.9761092719
+0.8400000000,-1.0499395206,-1.3401551196,5.9954386163
+0.8500000000,-1.0630417494,-1.2802946681,5.9861781069
+0.8600000000,-1.0755452385,-1.2203987456,5.9782697343
+0.8700000000,-1.0874498709,-1.1605200735,5.9923908792
+0.8800000000,-1.0987555135,-1.1006126206,5.9851787314
+0.8900000000,-1.1094621009,-1.0407014354,5.9959626652
+0.9000000000,-1.1195694896,-0.9807796092,5.9898010161
+0.9100000000,-1.1290776208,-0.9208443071,5.9978168972
+0.9200000000,-1.1379863614,-0.8609046692,5.9930906392
+0.9300000000,-1.1462956389,-0.8009489668,5.9986985104
+0.9400000000,-1.1540053226,-0.7409868136,5.9960412019
+0.9500000000,-1.1611153106,-0.6810089169,5.9993530710
+0.9600000000,-1.1676254504,-0.6210160321,6.0002469001
+0.9700000000,-1.1735356080,-0.5610186206,5.9983480103
+0.9800000000,-1.1788458912,-0.5010412674,5.9984313020
+0.9900000000,-1.1835565264,-0.4410898752,5.9914009231
+1.0000000000,-1.1876678759,-0.3811931772,5.9988240228
+1.0100000000,-1.1911804273,-0.3213300729,5.9806076203
+1.0200000000,-1.1940950327,-0.2616201787,5.9451605548
+1.0300000000,-1.1964133597,-0.2020822778,5.9652439958
+1.0400000000,-1.1981383931,-0.1430012424,5.8369634669
+1.0500000000,-1.1992783791,-0.0854621410,5.4905248998
+1.0600000000,-1.1998598641,-0.0324705716,5.6308791939
+1.0700000000,-1.1999956445,-0.0024097970,0.9995966965
+1.0772269967,-1.2000000000,0.0000000000,0.0000000000

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

example/solve_fk.cpp

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

example/solve_ik.cpp

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

example/torqueOff.cpp

@@ -0,0 +1,34 @@
+//
+// Created by lgv on 2025/8/13.
+//
+
+#include <iostream>
+#include <vector>
+#include <string>
+#include <cstdlib> // for std::atof
+
+#include <glog/logging.h>
+#include "../src/device_manager/include/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 "../src/device_manager/include/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 "../src/utils/dynamics/include/robot.h"
+#include "../src/utils/controller/include/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;
+}
+

protos/cmvr/api/biohead_command.proto

@@ -0,0 +1,270 @@
+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;      // 已停止的进程列表
+  }
+}
+
+message SpeakStart {
+  message Request {
+    CommandHeader.Request header = 1;  // 通用命令头
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1; // 通用反馈头
+  }
+}
+
+message SpeakStop {
+  message Request {
+    CommandHeader.Request header = 1;  // 通用命令头
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1; // 通用反馈头
+  }
+}
+
+
+
+// 表情控制 - 微笑
+message Happy {
+  message Request {
+    CommandHeader.Request header = 1;  // 通用命令头
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1; // 通用反馈头
+  }
+}
+
+
+// 表情控制 - 惊讶
+message Surprise {
+  message Request {
+    CommandHeader.Request header = 1;  // 通用命令头
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1; // 通用反馈头
+  }
+}
+
+
+//表情控制-疲惫
+message ExpressionTired {
+  message Request {
+    CommandHeader.Request header = 1;
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1;
+  }
+}
+
+//表情控制-愤怒
+message ExpressionAngry {
+  message Request {
+    CommandHeader.Request header = 1;
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1;
+  }
+}
+
+
+//表情控制-悲伤
+message ExpressionSadness {
+  message Request {
+    CommandHeader.Request header = 1;
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1;
+  }
+}
+
+//表情控制-打哈欠
+message ExpressionYawn {
+  message Request {
+    CommandHeader.Request header = 1;
+  }
+
+  message Feedback {
+    CommandHeader.Feedback header = 1;
+  }
+}
+
+
+
+

protos/cmvr/api/biohead_service.proto

@@ -0,0 +1,47 @@
+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){};
+
+    // 开始说话
+    rpc SpeakStart(SpeakStart.Request) returns (SpeakStart.Feedback){};
+    // 停止说话
+    rpc SpeakStop(SpeakStop.Request) returns (SpeakStop.Feedback){};
+
+    rpc Happy(Happy.Request) returns (Happy.Feedback){};
+
+    rpc Surprise(Surprise.Request) returns (Surprise.Feedback){};
+
+    rpc ExpressionTired(ExpressionTired.Request) returns (ExpressionTired.Feedback){};
+
+    rpc ExpressionAngry(ExpressionAngry.Request) returns (ExpressionAngry.Feedback){};
+
+    rpc ExpressionSadness(ExpressionSadness.Request) returns (ExpressionSadness.Feedback){};
+
+    rpc ExpressionYawn(ExpressionYawn.Request) returns (ExpressionYawn.Feedback){};
+
+
+
+
+
+
+
+
+}

protos/cmvr/api/camera_command.proto

@@ -0,0 +1,175 @@
+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 CameraIntrinsics {
+  float cx = 1;                           // 主点水平坐标（从左边缘的像素偏移）
+  float cy = 2;                           // 主点垂直坐标（从上边缘的像素偏移）
+  float fx = 3;                            // x方向焦距（像素宽度的倍数）
+  float fy = 4;                            // y方向焦距（像素高度的倍数）
+  repeated float coeffs = 5 [packed = true]; // 畸变系数（固定5个元素）
+}
+
+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;
+        CameraIntrinsics intrinsics = 3;
+    }
+}
+
+message GetDepthImageCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData depth_frame = 2;
+        CameraIntrinsics intrinsics = 3;
+    }
+}
+
+message GetRGBDImagesCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData color_frame = 2;
+        FrameData depth_frame = 3;
+        CameraIntrinsics intrinsics = 4;
+    }
+}
+
+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;
+        CameraIntrinsics intrinsics = 3;
+        int32 seq_no = 4;
+    }
+}
+
+message GetDepthImageStreamCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        bool eof = 2;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        FrameData depth_frame = 2;
+        CameraIntrinsics intrinsics = 3;
+        int32 seq_no = 4;
+    }
+}
+
+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;
+        CameraIntrinsics intrinsics = 4;
+        int32 seq_no = 5;
+    }
+}
+
+
+

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,43 @@
+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;      // 回复时间
+    }
+}
+
+message ConfigParam {
+
+    string param_name = 1;
+
+    oneof param_value {
+        int32 int_value = 2;     // 整数类型
+        double double_value = 3; // 浮点数类型
+        string string_value = 4; // 字符串类型
+        bool bool_value = 5;     // 布尔类型
+        bytes bytes_value = 6;   // 二进制数据类型
+    }
+}

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 FreedomState {
+    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 FreedomState 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/hlc_command.proto

@@ -0,0 +1,24 @@
+syntax = "proto3";
+
+import "cmvr/api/common.proto";
+
+package cmvr.api;
+
+
+message Touch{
+  message Request{
+    CommandHeader.Request header = 1;
+    // 触点坐标，unit: ms
+    int32 u = 2;   // 水平坐标
+    int32 v = 3;   // 垂直坐标
+
+    // 触控时允许的最大作用力，unit: N
+    double max_force = 4;
+  }
+
+  message Response{
+    CommandHeader.Feedback header= 1;
+  }
+
+}
+

protos/cmvr/api/hlc_service.proto

@@ -0,0 +1,9 @@
+syntax = "proto3";
+
+import "cmvr/api/hlc_command.proto";
+
+package cmvr.api;
+
+service HlcService{
+  rpc touch(Touch.Request) returns (Touch.Response);
+}

protos/cmvr/api/humanoid_robot_command.proto

@@ -0,0 +1,135 @@
+
+
+syntax = "proto3";
+package cmvr.api;
+import "cmvr/api/common.proto";
+
+message JointCmd {
+  string joint_name = 1;  // 关节名称
+  double rad = 2;         // 弧度
+  double vel = 3;         // 角速度，rad/s
+}
+
+message Pose3D{
+  double x = 1;
+  double y = 2;
+  double z = 3;
+  double rx = 4;
+  double ry = 5;
+  double rz = 6;
+}
+
+enum RobotCartesian{
+  X = 0;
+  Y = 1;
+  Z = 2;
+  RX = 3;
+  RY = 4;
+  RZ = 5;
+} ;
+enum RobotJointIndexDirection{
+  FORWARD = 0;
+  BACKWARD = 1;
+  X_POSITIVE = 2;   // X轴正向
+  X_NEGATIVE = 3;  // X轴负向
+  Y_POSITIVE = 4;  // Y轴正向
+  Y_NEGATIVE = 5;   // Y轴负向
+  Z_POSITIVE = 6;   // Z轴正向
+  Z_NEGATIVE = 7;   // Z轴负向
+  ROTATE_X = 8;   // 绕X轴旋转
+  ROTATE_Y = 9;    // 绕Y轴旋转
+  ROTATE_Z = 10;     // 绕Z轴旋转
+}
+message MoveJ{
+  message Request{
+    CommandHeader.Request header = 1;
+    repeated JointCmd cmds = 2;
+    double vel = 3;
+    double acc = 4;
+  }
+
+  message Response{
+    CommandHeader.Feedback header= 1;
+  }
+
+}
+
+message GetPose{
+  message Request{
+    CommandHeader.Request header = 1;
+    string base_link = 2;
+    string ee_link = 3;
+  }
+
+  message Response{
+    CommandHeader.Feedback header= 1;
+    Pose3D pose = 2;
+  }
+}
+
+message MoveL{
+
+  message Request{
+    CommandHeader.Request header = 1;
+    string ee_link = 2; //连杆名称
+    Pose3D targetPose = 3;
+    double vel = 4;
+    double acc = 5;
+  }
+
+  message Response{
+    CommandHeader.Feedback header= 1;
+  }
+
+}
+
+message SpeedJ{
+
+  message Request{
+    CommandHeader.Request header = 1;
+    string joint_name = 2;
+    double vel = 3;
+    double acc = 4;
+    RobotJointIndexDirection dir = 5;
+  }
+
+  message Response{
+    CommandHeader.Feedback header= 1;
+  }
+}
+
+message SpeedL{
+  message Request{
+    CommandHeader.Request header = 1;
+    string ee_link = 2;
+    double vel = 3;
+    double acc = 4;
+    RobotJointIndexDirection dir = 5;
+    RobotCartesian cart = 6;
+  }
+
+  message Response{
+    CommandHeader.Feedback header= 1;
+  }
+}
+
+// 关节状态消息
+message JointState {
+  repeated string name = 1;       // 关节名称
+  repeated double position = 2;   // 关节位置
+  repeated double velocity = 3;   // 可选：关节速度
+  repeated double effort = 4;     // 可选：关节力矩
+  double timestamp = 5;           // 可选：时间戳
+}
+
+// 返回消息
+message JointResponse {
+  CommandHeader.Feedback header= 1;
+  repeated JointState state = 2;
+}
+
+// 请求消息（可选）
+message JointRequest {
+  CommandHeader.Request header = 1;
+}
+

protos/cmvr/api/humanoid_robot_service.proto

@@ -0,0 +1,17 @@
+
+
+syntax = "proto3";
+package cmvr.api;
+import "cmvr/api/common.proto";
+import "cmvr/api/humanoid_robot_command.proto";
+
+service HumanoidRobotService{
+  rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback);
+  rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback);
+  rpc moveJ(MoveJ.Request) returns (MoveJ.Response);
+  rpc moveL(MoveL.Request) returns (MoveL.Response);
+  rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response);
+  rpc speedL(SpeedL.Request) returns (SpeedL.Response);
+  rpc getJointState(JointRequest) returns (JointResponse);
+  rpc getPose(GetPose.Request) returns (GetPose.Response);
+}

protos/cmvr/api/microphone_command.proto

@@ -0,0 +1,81 @@
+syntax = "proto3";
+
+import "cmvr/api/common.proto";
+
+package cmvr.api;
+
+message MicState  {
+    bool is_initialized = 1;
+    bool is_running = 2;
+    bool is_recording = 3;
+    int32 volume = 4;
+    string error_message = 5;
+}
+message GetMicStateCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        MicState  state = 2;
+    }
+}
+message StartMicRecordingCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        string file_path = 2;
+    }
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message StopMicRecordingCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message PauseMicRecordingCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message ResumeMicRecordingCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message SetMicPhoneVolumeCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+        int32 volume = 2;  // 音量值（0 ~ 100）
+    }
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+    }
+}
+
+message GetMicPhoneVolumeCommand {
+    message Request {
+        CommandHeader.Request header = 1;
+    }
+    message Feedback {
+        CommandHeader.Feedback header = 1;
+        int32 volume = 2;  // 当前音量值
+    }
+}
+

protos/cmvr/api/microphone_service.proto

@@ -0,0 +1,20 @@
+syntax = "proto3";
+
+import "cmvr/api/microphone_command.proto";
+
+package cmvr.api;
+
+
+service MicPhoneService {
+    // 基本控制
+    rpc GetStatus(GetMicStateCommand.Request) returns (GetMicStateCommand.Feedback);
+    rpc StartRecord(StartMicRecordingCommand.Request) returns (StartMicRecordingCommand.Feedback);
+    rpc StopRecord(StopMicRecordingCommand.Request) returns (StopMicRecordingCommand.Feedback);
+    rpc PauseRecord(PauseMicRecordingCommand.Request) returns (PauseMicRecordingCommand.Feedback);
+    rpc ResumeRecord(ResumeMicRecordingCommand.Request) returns (ResumeMicRecordingCommand.Feedback);
+
+    // 音量控制
+    rpc SetVolume(SetMicPhoneVolumeCommand.Request) returns (SetMicPhoneVolumeCommand.Feedback);
+    rpc GetVolume(GetMicPhoneVolumeCommand.Request) returns (GetMicPhoneVolumeCommand.Feedback);
+
+}