SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

copy code file

Browse Source
This commit is contained in:
linbo 2025-08-21 13:56:39 +08:00
parent 2732f9e7ac
commit cd6ad32bce
1426 changed files with 587350 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.gitignore vendored Normal file
View File

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

9
.gitmodules vendored Normal file
View File

@ -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

98
CMakeLists.txt Normal file
View File

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

21
LICENSE Normal file
View File

@ -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.

206
README.md
View File

@ -1,92 +1,138 @@
# 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
```
### 3. gRPC install (build from source)
version: https://grpc.io/docs/languages/cpp/quickstart/（grpc官网地址 v1.73.0）
#### 安装完成之后要配置环境变量
vim ~/.bashrc
在末尾添加
### 4. ffmpeg install (build from source)
#### 4.1 install dependencies
```shell
sudo apt-get update
sudo apt install build-essential yasm nasm git -y
sudo apt install libx264-dev libx265-dev libvpx-dev libfdk-aac-dev libmp3lame-dev libopus-dev -y
```
#### 4.2 build from source
```shell
cd assets
tar xjvf ffmpeg-4.4.tar.bz2
cd ffmpeg-4.4
# set configurations
./configure \
--extra-libs="-lpthread -lm" \
--enable-gpl \
--enable-libass \
--enable-libfdk-aac \
--enable-libfreetype \
--enable-libmp3lame \
--enable-libopus \
--enable-libvorbis \
--enable-libvpx \
--enable-libx264 \
--enable-libx265 \
--enable-pic \
--enable-shared \
--enable-nonfree \
# build and install
make -j4
sudo make install
```
### 5. librealsense2 install
```shell
sudo apt-key adv --keyserver keyserver.ubuntu.com --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE || sudo apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key F6E65AC044F831AC80A06380C8B3A55A6F3EFCDE
sudo add-apt-repository "deb https://librealsense.intel.com/Debian/apt-repo $(lsb_release -cs) main" -u
sudo apt-get install librealsense2-dkms -y
sudo apt-get install librealsense2-utils -y
sudo apt-get install librealsense2-dev -y
sudo apt-get install librealsense2-dbg -y
```
### 5. OSQPEigen install(build from source)
#### 5.1 install osqp (v0.6.3)
```shell
git clone --branch v0.6.3 https://github.com/osqp/osqp.git
cd osqp
git submodule init
git submodule update
mkdir build
cd build
cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..
make -j
sudo make install
```
#### 5.2 install OsqpEigen (v0.10.1)
```shell
git clone --branch v0.10.1 https://github.com/robotology/osqp-eigen.git
cd osqp-eigen
git submodule init
git submodule update
mkdir build
cd build
cmake -DCMAKE_INSTALL_PREFIX=/usr/local ..
make -j
sudo make install
```
#### 6 mech-eye-sdk
```shell
cd assets
sudo dkpg -i Mech-Eye_API_2.5.1_amd64.deb
```

BIN
assets/Mech-Eye_API_2.5.1_amd64.deb Normal file
View File

Binary file not shown.

BIN
assets/Mech-Eye_API_2.5.1_arm64.deb Normal file
View File

Binary file not shown.

194840
assets/cmake-3.30.3-linux-x86_64.sh Normal file
View File

File diff suppressed because one or more lines are too long

BIN
assets/ffmpeg-4.4.tar.bz2 Normal file
View File

Binary file not shown.

BIN
assets/grpcurl_1.8.7_linux_x86_64.tar.gz Normal file
View File

Binary file not shown.

138
config/cabin_robot.xml Normal file
View File

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

582
config/robot_description/hc_description/dual_arm.urdf Normal file
View File

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

275
config/robot_description/hc_description/left_arm.urdf Normal file
View File

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

BIN
config/robot_description/hc_description/meshes/L_ELBOW_R_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/L_SHOULDER_P_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/L_SHOULDER_R_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/L_SHOULDER_Y_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/L_WRIST_P_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/L_WRIST_R_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/L_WRIST_Y_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/NECK_P_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/NECK_R_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/NECK_Y_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/PELVIS_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/R_ELBOW_R_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/R_SHOULDER_P_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/R_SHOULDER_R_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/R_SHOULDER_Y_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/R_WRIST_P_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/R_WRIST_R_S.STL Normal file
View File

Binary file not shown.

BIN
config/robot_description/hc_description/meshes/R_WRIST_Y_S.STL Normal file
View File

Binary file not shown.

316
config/robot_description/hc_description/right_arm.urdf Normal file
View File

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

80
example/CMakeLists.txt Normal file
View File

@ -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})

234
example/follow_traj_example.cpp Normal file
View File

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

73
example/moveJ.cpp Normal file
View File

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

84
example/robot_wrapper.cpp Normal file
View File

@ -0,0 +1,84 @@
//
// Created by lgv on 2025/8/15.
//
#ifdef MAX_ITER
#undef MAX_ITER
#endif
#include <opencv2/opencv.hpp>
#include <pybind11/pybind11.h>
#include <pybind11/stl.h>
#include "devices/abstract_robot.h"
#include "device_manager/device_manager.h"
namespace py = pybind11;
using namespace cmvr::device;
class PyRobotWrapper {
public:
// 构造时只需要传入 config_path 和 robot_name（只在第一次初始化有效）
PyRobotWrapper(const std::string& config_path, const std::string& robot_name) {
// 如果已经初始化过，则直接返回
if (!robot_) {
const XmlNode config(config_path);
if (!config.hasChild("DeviceManager")) {
throw std::runtime_error("Device Manager node not found");
}
auto dmgr_cfg = config.getChild("DeviceManager");
dmgr_ = &DeviceManager::getInstance(dmgr_cfg);
robot_ = dmgr_->getDevice<AbstractRobot>(robot_name);
}
}
void moveJ(const std::string &side, const std::vector<double> &q) {
if (q.size() != 7)
throw std::runtime_error("Expected 7 joint values");
std::vector<std::string> joint_names;
if (side == "left") {
joint_names = {"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y",
"L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R"};
} else if (side == "right") {
joint_names = {"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
} else {
throw std::runtime_error("Side must be 'left' or 'right'");
}
std::vector<JointPositionCmd> cmd;
for (size_t i = 0; i < 7; ++i)
cmd.push_back({joint_names[i], q[i]});
robot_->moveJ(cmd, 0.8);
}
void torqueOn() {
if (!robot_)
throw std::runtime_error("Robot not initialized");
robot_->torqueOn();
}
void torqueOff() {
if (!robot_)
throw std::runtime_error("Robot not initialized");
robot_->torqueOff();
}
private:
static DeviceManager* dmgr_;
static std::shared_ptr<AbstractRobot> robot_;
};
// 静态成员初始化
DeviceManager* PyRobotWrapper::dmgr_ = nullptr;
std::shared_ptr<AbstractRobot> PyRobotWrapper::robot_ = nullptr;
PYBIND11_MODULE(robot_wrapper, m) {
py::class_<PyRobotWrapper>(m, "Robot")
.def(py::init<const std::string&, const std::string&>())
.def("moveJ", &PyRobotWrapper::moveJ, py::arg("side"), py::arg("q"))
.def("torqueOn", &PyRobotWrapper::torqueOn) // 直接绑定 wrapper 的方法
.def("torqueOff", &PyRobotWrapper::torqueOff);
}

101
example/solve_fk.cpp Normal file
View File

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

129
example/solve_ik.cpp Normal file
View File

@ -0,0 +1,129 @@
//
// Created by xtkuang on 2025/7/31.
//
#include "utils/dynamics/robot.h"
#include "utils/controller/cartesian_controller.h"
#include <Eigen/Dense>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <cmath>
constexpr int DOF = 14;
using RobotT = cmvr::dyn::Robot<DOF>;
using ControllerT = cmvr::ctrl::CartesianController<DOF>;
// 将欧拉角(rx, ry, rz)转为旋转矩阵，旋转顺序 Z→Y→X
Eigen::Matrix3d eulerToRotationMatrix(double rx, double ry, double rz)
{
Eigen::Matrix3d R_x;
R_x << 1, 0, 0,
0, cos(rx), -sin(rx),
0, sin(rx), cos(rx);
Eigen::Matrix3d R_y;
R_y << cos(ry), 0, sin(ry),
0, 1, 0,
-sin(ry), 0, cos(ry);
Eigen::Matrix3d R_z;
R_z << cos(rz), -sin(rz), 0,
sin(rz), cos(rz), 0,
0, 0, 1;
// ✅ 按照输入顺序 X → Y → Z 旋转
return R_x * R_y * R_z;
}
int main(int argc, char **argv)
{
if (argc != 9)
{
std::cerr << "用法: " << argv[0] << " <base link> <target link> <x> <y> <z> <rx> <ry> <rz>\n";
return -1;
}
std::vector<std::string> link_names = {
"PELVIS_S",
"L_SHOULDER_P_S", "L_SHOULDER_R_S", "L_SHOULDER_Y_S", "L_ELBOW_R_S", "L_WRIST_P_S", "L_WRIST_Y_S", "L_WRIST_R_S",
"R_SHOULDER_P_S", "R_SHOULDER_R_S", "R_SHOULDER_Y_S", "R_ELBOW_R_S", "R_WRIST_P_S", "R_WRIST_Y_S", "R_WRIST_R_S", "R_FINGER_TIP"
};
std::vector<std::string> joint_names = {
"L_SHOULDER_P", "L_SHOULDER_R", "L_SHOULDER_Y", "L_ELBOW_R", "L_WRIST_P", "L_WRIST_Y", "L_WRIST_R",
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R",
};
std::string BASE_LINK = argv[1];
std::string target_link = argv[2];
bool find_base_link = false;
bool find_target_link = false;
for (auto link_name : link_names) {
if (BASE_LINK == link_name) {
find_base_link = true;
}
if (target_link == link_name) {
find_target_link = true;
}
}
if (!find_base_link) {
std::cerr << "base link 无效" << std::endl;
return -1;
}
if (!find_target_link) {
std::cerr << "target link 无效" << std::endl;
return -1;
}
double x = std::stod(argv[3]);
double y = std::stod(argv[4]);
double z = std::stod(argv[5]);
double rx = std::stod(argv[6]) * M_PI / 180.0;
double ry = std::stod(argv[7]) * M_PI / 180.0;
double rz = std::stod(argv[8]) * M_PI / 180.0;
// 1. 加载机器人模型
auto rcfg = LoadRobotFromURDF(
"/home/xtkuang/projects/cmvr-es/config/robot_description/hc_description/dual_arm.urdf",
/*root*/ "PELVIS_S");
auto robot = std::make_shared<RobotT>(rcfg);
auto state = robot->MakeState(link_names, joint_names);
// 初始关节角度
Eigen::Vector<double, DOF> q_init;
q_init << -0.3647738137, -1.4556045962, 1.6057029118, -1.8483036779, 2.9024825461, 0.1850049007, -0.5602506899,
-0.02679378, 0.9625459, 2.0013871, 1.4209224, -2.552366, 0.34969908, 0.10842471;
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, 100, 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;
}

34
example/torqueOff.cpp Normal file
View File

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

36
example/torqueOn.cpp Normal file
View File

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

94
example/wbc_example.cpp Normal file
View File

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

42
include/device_manager/device_factory.h Normal file
View File

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

77
include/device_manager/device_manager.h Normal file
View File

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

45
include/device_manager/system_monitor.h Normal file
View File

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

36
include/devices/abstract_agv.h Normal file
View File

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

25
include/devices/abstract_battery.h Normal file
View File

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

83
include/devices/abstract_biohead.h Normal file
View File

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

63
include/devices/abstract_camera.h Normal file
View File

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

104
include/devices/abstract_canbus.h Normal file
View File

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

42
include/devices/abstract_device.h Normal file
View File

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

227
include/devices/abstract_dexhand.h Normal file
View File

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

30
include/devices/abstract_gripper.h Normal file
View File

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

30
include/devices/abstract_microphone.h Normal file
View File

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

164
include/devices/abstract_motor.h Normal file
View File

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

99
include/devices/abstract_robot.h Normal file
View File

@ -0,0 +1,99 @@
//
// Created by xtkuang on 2025/5/8.
//
#ifndef CMVR_ES_ABSTRACT_ROBOT_H
#define CMVR_ES_ABSTRACT_ROBOT_H
#pragma once
#include "jsoncpp/json/json.h"
#include "abstract_device.h"
#include "utils/controller/cartesian_controller.h"
namespace cmvr::device{
struct JointPositionCmd{
std::string joint_name;
double rad; // rad
double vel; // rad / s
JointPositionCmd(const std::string& name, double r, double v)
: joint_name(name), rad(r), vel(v) {}
JointPositionCmd(const std::string& name, double r)
: joint_name(name), rad(r), vel(0) {}
JointPositionCmd()
: joint_name(""), rad(0), vel(0) {}
} ;
typedef struct {
std::string joint_name;
double vel; // rad / s
} JointVelocityCommand;
typedef struct {
std::string joint_name;
double current; //
} JointCurrentCommand;
class AbstractRobot: public AbstractDevice {
public:
explicit AbstractRobot(const XmlNode &config): AbstractDevice(config) {}
~AbstractRobot() override=default;
virtual int getDOF() { throw std::runtime_error("Not implemented"); }
virtual std::vector<std::string> getJointNames() { throw std::runtime_error("Not implemented"); }
virtual std::vector<std::string> getLinkNames() { throw std::runtime_error("Not implemented"); }
virtual void getState(RobotState &state) { throw std::runtime_error("Not implemented"); }
virtual math::Pose3d getTransform(std::string &bask_link, std::string &target_link) {throw std::runtime_error("Not implemented");}
virtual void torqueOn() { throw std::runtime_error("Not implemented"); }
virtual void torqueOff() { throw std::runtime_error("Not implemented"); }
virtual void eStop() { throw std::runtime_error("Not implemented"); }
virtual void moveJ(std::vector<double> &joints, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveJ(std::vector<JointPositionCmd> &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveJ_IK(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveJ_IK(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); }
virtual void followJointTrajectory(std::vector<std::vector<double>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
virtual void followJointTrajectory(std::vector<std::vector<JointPositionCmd>> &traj, double dt) { throw std::runtime_error("Not implemented"); }
virtual void followPoseTrajectory(std::vector<math::Pose3d> &traj, double dt) { throw std::runtime_error("Not implemented"); }
virtual void followPoseTrajectory(std::string &base_link, std::vector<std::vector<cmvr::ctrl::PoseTarget>> &targets, double dt) { throw std::runtime_error("Not implemented"); }
virtual void servoJ(std::vector<double> &joints, double dt) { throw std::runtime_error("Not implemented"); }
virtual void servoJ(std::vector<JointPositionCmd> &joints, double dt) { throw std::runtime_error("Not implemented"); }
virtual void servoJ(std::vector<JointPositionCmd> &joints, double vel, double dt) { throw std::runtime_error("Not implemented"); }
virtual void servoL(math::Pose3d &pose, double dt) { throw std::runtime_error("Not implemented"); }
virtual void servoL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double dt) { throw std::runtime_error("Not implemented"); }
virtual void calibrateZeroQ(const std::string &joint_name) = 0;
protected:
int dof_{};
RobotState state_{};
};
}
#endif //CMVR_ES_ABSTRACT_ROBOT_H

30
include/devices/abstract_speaker.h Normal file
View File

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

143
include/devices/state_define.h Normal file
View File

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

16
include/hardware/can_interface.h Normal file
View File

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

33
include/hardware/esp32_serial_port.h Normal file
View File

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

120
include/hardware/serial_interface.h Normal file
View File

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

88
include/http/httpclient.h Normal file
View File

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

42
include/monitor/abstract_monitor.h Normal file
View File

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

27
include/monitor/monitor_factory.h Normal file
View File

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

37
include/monitor/monitor_manager.h Normal file
View File

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

46
include/service/grpc_camera_service.h Normal file
View File

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

30
include/service/grpc_dexhand_service.h Normal file
View File

@ -0,0 +1,30 @@
//
// Created by linbo on 2025/7/3.
//
#ifndef GRPC_DEXHAND_SERVICE_H
#define GRPC_DEXHAND_SERVICE_H
#include "cmvr/api/dexhand_service.grpc.pb.h"
#include "../utils/base/grpc_utils.h"
#include "device_manager/device_manager.h"
namespace cmvr::service {
class gRPCDexHandServiceImpl final: public api::DexHandService::Service {
public:
gRPCDexHandServiceImpl();
~gRPCDexHandServiceImpl() override = default;
grpc::Status GetStatus(grpc::ServerContext* context, const api::GetDexHandStateCommand_Request* request,api::GetDexHandStateCommand_Feedback* response) override;
grpc::Status SetDexHandPos(grpc::ServerContext* context, const cmvr::api::SetDexHandPositionsCommand_Request* request, cmvr::api::SetDexHandPositionsCommand_Feedback* response) override;
grpc::Status SetDexHandAngle(grpc::ServerContext* context, const cmvr::api::SetDexHandAnglesCommand_Request* request, cmvr::api::SetDexHandAnglesCommand_Feedback* response) override;
grpc::Status SetDexHandForce(grpc::ServerContext* context, const cmvr::api::SetDexHandForceCommand_Request* request, cmvr::api::SetDexHandForceCommand_Feedback* response) override;
grpc::Status SetDexHandSpeed(grpc::ServerContext* context, const cmvr::api::SetDexHandSpeedCommand_Request* request, cmvr::api::SetDexHandSpeedCommand_Feedback* response) override;
grpc::Status SetDexHandPresetAct(grpc::ServerContext* context, const cmvr::api::SetDexHandPresetActCommand_Request* request, cmvr::api::SetDexHandPresetActCommand_Feedback* response) override;
grpc::Status GetSensorData(grpc::ServerContext* context, const cmvr::api::GetSensorDataCommand_Request* request, cmvr::api::GetSensorDataCommand_Feedback* response) override;
grpc::Status GetSensorDataStream(grpc::ServerContext* context, grpc::ServerReaderWriter<cmvr::api::GetSensorDataStreamCommand_Feedback, cmvr::api::GetSensorDataStreamCommand_Request>* stream) override;
private:
device::DeviceManager& dmgr_;
};
}
#endif //GRPC_DEXHAND_SERVICE_H

35
include/service/grpc_head_service.h Normal file
View File

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

30
include/service/grpc_humanoid_robot_service.h Normal file
View File

@ -0,0 +1,30 @@
//
// Created by lgv on 2025/8/15.
//
#pragma once
#include "device_manager/device_manager.h"
#include "cmvr/api/humanoid_robot.grpc.pb.h"
namespace cmvr {
namespace service {
class gRPCHumanoidRobotServiceImpl final : public api::HumanoidRobotService::Service {
public:
gRPCHumanoidRobotServiceImpl();
~gRPCHumanoidRobotServiceImpl() = default;
grpc::Status torqueOff(grpc::ServerContext *context,
const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override;
grpc::Status torqueOn(grpc::ServerContext *context,
const cmvr::api::CommandHeader_Request *request, cmvr::api::CommandHeader_Feedback *response) override;
grpc::Status moveJ(grpc::ServerContext *context, const cmvr::api::MoveJ_Request *request, cmvr::api::MoveJ_Response *response) override;
private:
device::DeviceManager& dmgr_;
};
}
}

30
include/service/grpc_microphone_service.h Normal file
View File

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

30
include/service/grpc_speaker_service.h Normal file
View File

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

25
include/service/grpc_system_service.h Normal file
View File

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

17
include/utils/base/grpc_utils.h Normal file
View File

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

53
include/utils/base/logger.h Normal file
View File

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

33
include/utils/base/os.h Normal file
View File

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

136
include/utils/base/ring_buffer.h Normal file
View File

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

81
include/utils/base/thread_pool.h Normal file
View File

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

37
include/utils/base/timer.h Normal file
View File

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

118
include/utils/controller/cartesian_controller.h Normal file
View File

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

54
include/utils/dynamics/inertial.h Normal file
View File

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

108
include/utils/dynamics/joint.h Normal file
View File

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

143
include/utils/dynamics/link.h Normal file
View File

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

288
include/utils/dynamics/robot.h Normal file
View File

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

828
include/utils/dynamics/robot.tpp Normal file
View File

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

141
include/utils/dynamics/state.h Normal file
View File

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

52
include/utils/math/constants.h Normal file
View File

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

57
include/utils/math/geometry.h Normal file
View File

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

15
include/utils/math/liegroup.h Normal file
View File

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

126
include/utils/math/qp_solver.h Normal file
View File

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

42
include/utils/math/se2.h Normal file
View File

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

90
include/utils/math/se3.h Normal file
View File

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

121
include/utils/math/so3.h Normal file
View File

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

124
include/utils/math/velocity_estimator.h Normal file
View File

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

177
protos/cmvr/api/biohead_command.proto Normal file
View File

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

22
protos/cmvr/api/biohead_service.proto Normal file
View File

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

161
protos/cmvr/api/camera_command.proto Normal file
View File

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

21
protos/cmvr/api/camera_service.proto Normal file
View File

@ -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) {}
}

30
protos/cmvr/api/common.proto Normal file
View File

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

149
protos/cmvr/api/dexhand_command.proto Normal file
View File

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

18
protos/cmvr/api/dexhand_service.proto Normal file
View File

@ -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);
}

67
protos/cmvr/api/geometry.proto Normal file
View File

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

Some files were not shown because too many files have changed in this diff Show More