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

merge lgv device changes into linbo_dev_new

Browse Source
This commit is contained in:
linbo 2026-09-16 15:55:24 +08:00
commit 8415bdd1c3
496 changed files with 43027 additions and 2131 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -18,7 +18,6 @@ set(CMAKE_INSTALL_PREFIX "${CMAKE_SOURCE_DIR}/output" CACHE PATH "" FORCE)
set(CMAKE_BUILD_RPATH "\$ORIGIN:\$ORIGIN/../lib")
set(CMAKE_INSTALL_RPATH "\$ORIGIN:\$ORIGIN/../lib")
# Use RUNPATH (new dtags) generally preferable
set(CMAKE_BUILD_WITH_INSTALL_RPATH OFF)
set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)

12
MUJOCO_LOG.TXT Normal file
View File

@ -0,0 +1,12 @@
Fri Jul 24 15:39:05 2026
ERROR: could not create window
Fri Jul 24 15:40:37 2026
ERROR: could not create window
Fri Sep 11 13:10:38 2026
ERROR: could not initialize GLFW
Fri Sep 11 14:13:09 2026
ERROR: could not initialize GLFW

103
README.md
View File

@ -1,23 +1,25 @@
# CMVR-ES
## Overview
## 简介
## Installation
CMVR-ES 工程。
### 1. Git submodules install
## 安装
### 1. 拉取 Git 子模块
```
git submodule update --init --recursive
```
### 2. Dependency install
### 2. 安装系统依赖
```shell
# basic
# 基础工具
sudo apt-get update
sudo apt install -y build-essential cmake git pkg-config patchelf libboost-all-dev libssl-dev
# opencv
# OpenCV
sudo apt install -y \
libjpeg-dev libpng-dev libtiff-dev \
libavcodec-dev libavformat-dev libswscale-dev \
@ -39,7 +41,7 @@ sudo apt-get install libassimp-dev
# visp
sudo apt-get install -y libx11-dev liblapack-dev libzbar-dev libpthread-stubs0-dev libdc1394-dev nlohmann-json3-dev
# realsense
# RealSense
sudo apt-get install -y \
libusb-1.0-0-dev libudev-dev \
libglu1-mesa-dev
@ -48,4 +50,91 @@ sudo apt-get install -y \
sudo apt install gnuplot-qt
```
### 3. 配置 IgH EtherCAT
工程已内置 IgH EtherCAT 1.7.0 的 userspace 文件：
```text
dependency/x86/third_party/ethercat/v1.7.0
```
先设置本机路径：
```shell
export CMVR_ES_ROOT=/path/to/cmvr-es
export IGH_ETHERCAT_ROOT=$CMVR_ES_ROOT/dependency/x86/third_party/ethercat/v1.7.0
```
安装当前内核的 header：
```shell
sudo apt-get update
sudo apt-get install -y linux-headers-$(uname -r)
```
如果内置目录里已经有当前内核版本的 EtherCAT 内核模块，直接安装到系统：
```shell
sudo mkdir -p /lib/modules/$(uname -r)/ethercat
sudo cp -r $IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)/ethercat/* /lib/modules/$(uname -r)/ethercat/
sudo depmod
```
内置内核模块只适用于相同内核版本。如果 `$IGH_ETHERCAT_ROOT/lib/modules/$(uname -r)` 不存在，说明这台机器的内核版本不匹配，需要在这台机器上重新编译安装 IgH EtherCAT：
```shell
sudo apt-get update
sudo apt-get install -y \
build-essential autoconf automake libtool pkg-config git \
linux-headers-$(uname -r)
cd /tmp
git clone --branch stable-1.7 --depth 1 https://gitlab.com/etherlab.org/ethercat.git ethercat-stable-1.7
cd ethercat-stable-1.7
./bootstrap
./configure \
--prefix=$IGH_ETHERCAT_ROOT \
--libdir=$IGH_ETHERCAT_ROOT/lib \
--includedir=$IGH_ETHERCAT_ROOT/include \
--sysconfdir=$IGH_ETHERCAT_ROOT/etc \
--with-systemdsystemunitdir=$IGH_ETHERCAT_ROOT/lib/systemd/system \
--enable-generic \
--with-linux-dir=/lib/modules/$(uname -r)/build
make -j$(nproc) all modules
make install
sudo make modules_install
sudo depmod
```
启动 EtherCAT。`eno1` 换成实际连接 EtherCAT 从站的网卡：
```shell
sudo script/ethercat/start_ethercat.sh eno1
```
脚本会写入内置 IgH 配置文件：
```text
$IGH_ETHERCAT_ROOT/etc/ethercat.conf
```
脚本会把该网卡的 MAC 写到 `MASTER0_DEVICE`，使用 `DEVICE_MODULES="generic"`，把网卡从 NetworkManager 断开，并通过 `ethercatctl -c` 启动 IgH master。
查看状态：
```shell
script/ethercat/status_ethercat.sh
$IGH_ETHERCAT_ROOT/bin/ethercat master
$IGH_ETHERCAT_ROOT/bin/ethercat slaves
$IGH_ETHERCAT_ROOT/bin/ethercat pdos
```
停止 EtherCAT：
```shell
sudo script/ethercat/stop_ethercat.sh eno1
sudo script/ethercat/stop_ethercat.sh eno1 --restore-network
```

12
cmake/FindExternalLib.cmake
View File

@ -55,7 +55,17 @@ function(setup_external_libs ARCH)
# ---- library dirs ----
if(EXISTS "${FULL_PATH}/lib")
list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib")
file(GLOB _BUNDLED_LIBSTDCXX_FILES
"${FULL_PATH}/lib/libstdc++.so"
"${FULL_PATH}/lib/libstdc++.so.*"
)
if(_BUNDLED_LIBSTDCXX_FILES)
message(STATUS
"${LIB_NAME}: excluding vendor lib directory from global "
"link paths because it contains a private libstdc++")
else()
list(APPEND LIBRARY_DIRS "${FULL_PATH}/lib")
endif()
set(HAS_LIB TRUE)
# Collect shared libs for install: *.so and *.so.*

6
cmvr-es/common/CMakeLists.txt
View File

@ -29,6 +29,12 @@ target_link_libraries(common PUBLIC
add_library(cmvr_es::common ALIAS common)
install(TARGETS common LIBRARY DESTINATION lib)
add_executable(support_functions_test
math/support_functions_test.cpp
)
target_include_directories(support_functions_test PRIVATE ${CMAKE_SOURCE_DIR}/cmvr-es)
target_link_libraries(support_functions_test PRIVATE gtest gtest_main glog)
#add_executable(image_display_test
# utils/visualization/image_display_test.cpp
#)

24
cmvr-es/common/math/proto_geometry.h
View File

@ -27,6 +27,26 @@ inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src)
return toEigenVec3(src, Eigen::Vector3d::Zero());
}
inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src,
Eigen::Vector3d defaults)
{
if (src.has_rx()) {
defaults.x() = src.rx();
}
if (src.has_ry()) {
defaults.y() = src.ry();
}
if (src.has_rz()) {
defaults.z() = src.rz();
}
return defaults;
}
inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src)
{
return toEigenEuler(src, Eigen::Vector3d::Zero());
}
inline Eigen::Matrix<double, 6, 1> toEigenVec6(
const cmvr::common::Vec6& src,
Eigen::Matrix<double, 6, 1> defaults)
@ -102,6 +122,10 @@ inline bool hasVec3(const cmvr::common::Vec3& value) {
return value.has_x() && value.has_y() && value.has_z();
}
inline bool hasEuler(const cmvr::common::Euler& value) {
return value.has_rx() && value.has_ry() && value.has_rz();
}
inline bool hasVec6(const cmvr::common::Vec6& value) {
return value.has_x() && value.has_y() && value.has_z() &&
value.has_rx() && value.has_ry() && value.has_rz();

20
cmvr-es/common/math/support_functions.h
View File

@ -3,6 +3,7 @@
//
#pragma once
#include <cstdint>
#include <cmath>
#include <vector>
#include <algorithm>
@ -14,6 +15,25 @@ class SupportFunctions {
private:
static constexpr double EPS = 1e-9;
public:
static constexpr std::int64_t absoluteDifference(const std::int32_t lhs,
const std::int32_t rhs) noexcept {
return lhs >= rhs
? static_cast<std::int64_t>(lhs) - static_cast<std::int64_t>(rhs)
: static_cast<std::int64_t>(rhs) - static_cast<std::int64_t>(lhs);
}
static constexpr std::int64_t cyclicAbsoluteDifference(
const std::int32_t lhs,
const std::int32_t rhs,
const std::int64_t period) noexcept {
const auto linear_distance = absoluteDifference(lhs, rhs);
if (period <= 0) {
return linear_distance;
}
const auto wrapped_distance = linear_distance % period;
return std::min(wrapped_distance, period - wrapped_distance);
}
static std::vector<double> eigen_to_vector(const Eigen::VectorXd &v) {
return std::vector<double>(v.data(), v.data() + v.size());
}

34
cmvr-es/common/math/support_functions_test.cpp Normal file
View File

@ -0,0 +1,34 @@
#include <cstdint>
#include <limits>
#include <gtest/gtest.h>
#include "common/math/support_functions.h"
TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceTreatsFullTurnsAsEquivalent)
{
constexpr std::int64_t period = 65536LL * 101LL;
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(5254257, -1364879, period), 0);
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(-10883488, -17502624, period), 0);
}
TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceUsesShortestWrappedDistance)
{
constexpr std::int64_t period = 100;
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(3, 97, period), 6);
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(97, 3, period), 6);
EXPECT_EQ(SupportFunctions::cyclicAbsoluteDifference(10, 40, period), 30);
}
TEST(SupportFunctionsTest, CyclicAbsoluteDifferenceHandlesInt32Range)
{
constexpr std::int64_t period = 65536LL * 101LL;
const auto distance = SupportFunctions::cyclicAbsoluteDifference(
std::numeric_limits<std::int32_t>::min(),
std::numeric_limits<std::int32_t>::max(), period);
EXPECT_GE(distance, 0);
EXPECT_LE(distance, period / 2);
}

8
cmvr-es/common/types/arm/arm_types.h
View File

@ -112,6 +112,14 @@ struct JointGroupState {
}
};
struct JointTrajectoryPoint {
double time_s{0.0};
std::vector<double> position;
std::vector<double> velocity;
};
using JointTrajectory = std::vector<JointTrajectoryPoint>;
struct JointPositionCommand {
std::vector<double> position;

166
cmvr-es/config/devices/arm/arm.pb.txt
View File

@ -3,8 +3,8 @@ arm {
id: "right_arm"
motor {
motor_system_id: "ti5_motors"
motor_group_ids: "right_arm_can"
motor_system_id: "right_arm_can_motors"
motor_group_ids: "right_arm_can_motors"
dof: 7
joint_names: "R_SHOULDER_P"
joint_names: "R_SHOULDER_R"
@ -51,7 +51,6 @@ arm {
gain: 0.2
margin_ratio: 0.15
max_push: 0.25
weight: 0.05
}
}
}
@ -59,6 +58,14 @@ arm {
motion {
move_j {
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
settle_timeout_s: 2.0
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
settle_position_tolerance_rad: 0.002
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
settle_velocity_tolerance_rad_s: 0.02
# 位置和速度连续满足条件的采样次数。
settle_stable_sample_count: 3
toppra_joint_motion_planner {
path_type: TOPPRA_PATH_TYPE_QUINTIC
sample_period_s: 0.001
@ -144,9 +151,162 @@ arm {
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 10
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}
}
}
robot_arms {
id: "left_arm"
motor {
motor_system_id: "left_arm_can_motors"
motor_group_ids: "left_arm_can_motors"
dof: 7
joint_names: "L_SHOULDER_P"
joint_names: "L_SHOULDER_R"
joint_names: "L_SHOULDER_Y"
joint_names: "L_ELBOW_R"
joint_names: "L_WRIST_P"
joint_names: "L_WRIST_Y"
joint_names: "L_WRIST_R"
upd_freq: 1000
buffer_size: 50
default_vel: 1.0
default_acc: 2.0
}
kinematics {
pinocchio_dls_ik_solver {
urdf_path: "model/xiaoyan_description/dual_arm.urdf"
base_frame_name: "PELVIS_S"
flange_frame_name: "L_WRIST_R_S"
tcp_frame_name: "L_FINGER_TIP_FIXED"
max_iters: 100
pos_eps: 1e-6
rot_eps: 1e-6
damping: 1e-6
joint_limit_policy {
limits {
enable: true
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "L_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
joints { joint_name: "L_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
soft_limit {
enable: true
margin_ratio: 0.01
min_margin_rad: 0.01
}
avoidance {
enable: false
gain: 0.2
margin_ratio: 0.15
max_push: 0.25
weight: 0.05
}
}
}
}
motion {
move_j {
toppra_joint_motion_planner {
path_type: TOPPRA_PATH_TYPE_QUINTIC
sample_period_s: 0.001
grid_size: 150
high_grid_size: 300
}
}
move_l {
pinocchio_cartesian_motion_planner {
sample_period_s: 0.001
position_gain: 4.0
rotation_gain: 4.0
line_deviation_check {
enable: true
line_deviation_warn_m: 0.01
line_deviation_stop_m: 0.03
line_direction_warn_deg: 20.0
line_direction_stop_deg: 45.0
line_direction_reset_deg: 10.0
line_check_min_distance_m: 0.01
}
joint_continuity_check {
enable: true
max_joint_delta_rad: 0.05
max_joint_velocity_rad_s: 10.0
max_joint_acceleration_rad_s2: 5000.0
}
cartesian_step_feasibility_check {
enable: true
min_linear_speed_ratio: 0.2
max_linear_direction_deviation_deg: 45.0
min_angular_speed_ratio: 0.2
max_angular_direction_deviation_deg: 45.0
min_desired_linear_speed: 1e-4
min_desired_angular_speed: 1e-4
}
}
}
speed_l {
pinocchio_cartesian_motion_planner {
linear_velocity_max: 0.55
linear_acceleration_max: 5.0
linear_jerk_max: 10.0
angular_velocity_max: 1.0
angular_acceleration_max: 5.0
angular_jerk_max: 12.0
linear_target_replan_threshold: 1e-4
angular_target_replan_threshold: 1e-4
linear_reverse_cos_threshold: -0.8660254037844386
linear_reverse_switch_speed_threshold: 1e-3
enforce_joint_acceleration_limits: true
line_deviation_check {
enable: true
line_deviation_warn_m: 0.01
line_deviation_stop_m: 0.03
line_direction_warn_deg: 20.0
line_direction_stop_deg: 45.0
line_direction_reset_deg: 10.0
line_check_min_distance_m: 0.01
}
joint_velocity_check {
enable: true
max_joint_velocity_rad_s: 30.0
max_joint_acceleration_rad_s2: 10000.0
}
cartesian_velocity_feasibility_check {
enable: true
min_linear_speed_ratio: 0.2
max_linear_direction_deviation_deg: 5.0
min_angular_speed_ratio: 0.2
max_angular_direction_deviation_deg: 5.0
min_desired_linear_speed: 1e-4
min_desired_angular_speed: 1e-4
}
}
speed_l_controller {
cartesian_velocity_controller {
control_period_s: 0.001
stop_twist_norm: 1e-9
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 10
}
}
}
}
}
}

160
cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt Normal file
View File

@ -0,0 +1,160 @@
arm {
robot_arms {
id: "mujoco_right_arm"
motor {
motor_system_id: "right_arm_mujoco_motors"
motor_group_ids: "right_arm_mujoco_motors"
dof: 7
joint_names: "right_arm_J1"
joint_names: "right_arm_J2"
joint_names: "right_arm_J3"
joint_names: "right_arm_J4"
joint_names: "right_arm_J5"
joint_names: "right_arm_J6"
joint_names: "right_arm_J7"
upd_freq: 1000
buffer_size: 50
default_vel: 0.6
default_acc: 2.0
}
kinematics {
pinocchio_dls_ik_solver {
urdf_path: "model/gen2/robot.urdf"
base_frame_name: "body_link"
flange_frame_name: "arm_link_7_2"
max_iters: 200
pos_eps: 1e-6
rot_eps: 1e-6
damping: 1e-5
joint_limit_policy {
limits {
enable: true
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
}
soft_limit {
enable: true
margin_ratio: 0.01
min_margin_rad: 0.01
}
avoidance {
enable: false
gain: 0.2
margin_ratio: 0.15
max_push: 0.25
}
}
}
}
motion {
move_j {
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
settle_timeout_s: 2.0
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
settle_position_tolerance_rad: 0.002
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
settle_velocity_tolerance_rad_s: 0.02
# 位置和速度连续满足条件的采样次数。
settle_stable_sample_count: 3
toppra_joint_motion_planner {
path_type: TOPPRA_PATH_TYPE_QUINTIC
sample_period_s: 0.001
grid_size: 150
high_grid_size: 300
}
}
move_l {
pinocchio_cartesian_motion_planner {
sample_period_s: 0.001
position_gain: 4.0
rotation_gain: 4.0
line_deviation_check {
enable: true
line_deviation_warn_m: 0.01
line_deviation_stop_m: 0.03
line_direction_warn_deg: 20.0
line_direction_stop_deg: 45.0
line_direction_reset_deg: 10.0
line_check_min_distance_m: 0.005
}
joint_continuity_check {
enable: true
max_joint_delta_rad: 0.05
max_joint_velocity_rad_s: 4.0
max_joint_acceleration_rad_s2: 100.0
}
cartesian_step_feasibility_check {
enable: true
min_linear_speed_ratio: 0.2
max_linear_direction_deviation_deg: 10.0
min_angular_speed_ratio: 0.2
max_angular_direction_deviation_deg: 10.0
min_desired_linear_speed: 1e-4
min_desired_angular_speed: 1e-4
}
}
}
speed_l {
pinocchio_cartesian_motion_planner {
linear_velocity_max: 0.5
linear_acceleration_max: 2.0
linear_jerk_max: 10.0
angular_velocity_max: 1.0
angular_acceleration_max: 5.0
angular_jerk_max: 12.0
linear_target_replan_threshold: 1e-4
angular_target_replan_threshold: 1e-4
linear_reverse_cos_threshold: -0.8660254037844386
linear_reverse_switch_speed_threshold: 1e-3
enforce_joint_acceleration_limits: true
line_deviation_check {
enable: true
line_deviation_warn_m: 0.01
line_deviation_stop_m: 0.03
line_direction_warn_deg: 20.0
line_direction_stop_deg: 45.0
line_direction_reset_deg: 10.0
line_check_min_distance_m: 0.005
}
joint_velocity_check {
enable: true
max_joint_velocity_rad_s: 4.0
max_joint_acceleration_rad_s2: 100.0
}
cartesian_velocity_feasibility_check {
enable: true
min_linear_speed_ratio: 0.2
max_linear_direction_deviation_deg: 10.0
min_angular_speed_ratio: 0.2
max_angular_direction_deviation_deg: 10.0
min_desired_linear_speed: 0.01
min_desired_angular_speed: 1e-4
}
}
speed_l_controller {
cartesian_velocity_controller {
control_period_s: 0.001
stop_twist_norm: 1e-9
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 2.0
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}
}
}
}

15
cmvr-es/config/devices/arm/arm_mujoco.pb.txt
View File

@ -3,8 +3,8 @@ arm {
id: "mujoco_right_arm"
motor {
motor_system_id: "mujoco_motors"
motor_group_ids: "mujoco_right_arm"
motor_system_id: "right_arm_mujoco_motors"
motor_group_ids: "right_arm_mujoco_motors"
dof: 7
joint_names: "R_SHOULDER_P"
joint_names: "R_SHOULDER_R"
@ -51,7 +51,6 @@ arm {
gain: 0.2
margin_ratio: 0.15
max_push: 0.25
weight: 2.0
}
}
}
@ -59,6 +58,14 @@ arm {
motion {
move_j {
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
settle_timeout_s: 2.0
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
settle_position_tolerance_rad: 0.002
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
settle_velocity_tolerance_rad_s: 0.02
# 位置和速度连续满足条件的采样次数。
settle_stable_sample_count: 3
toppra_joint_motion_planner {
path_type: TOPPRA_PATH_TYPE_QUINTIC
sample_period_s: 0.001
@ -144,6 +151,8 @@ arm {
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 10
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}

15
cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt
View File

@ -3,8 +3,8 @@ arm {
id: "mujoco_right_arm"
motor {
motor_system_id: "mujoco_motors"
motor_group_ids: "mujoco_right_arm"
motor_system_id: "right_arm_mujoco_motors"
motor_group_ids: "right_arm_mujoco_motors"
dof: 7
joint_names: "R_SHOULDER_P"
joint_names: "R_SHOULDER_R"
@ -52,7 +52,6 @@ arm {
gain: 0.2
margin_ratio: 0.01
max_push: 0.02
weight: 0.05
}
}
}
@ -60,6 +59,14 @@ arm {
motion {
move_j {
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
settle_timeout_s: 2.0
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
settle_position_tolerance_rad: 0.002
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
settle_velocity_tolerance_rad_s: 0.02
# 位置和速度连续满足条件的采样次数。
settle_stable_sample_count: 3
toppra_joint_motion_planner {
path_type: TOPPRA_PATH_TYPE_QUINTIC
sample_period_s: 0.001
@ -145,6 +152,8 @@ arm {
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 5
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}

21
cmvr-es/config/devices/arm/arm_qp.pb.txt
View File

@ -3,8 +3,8 @@ arm {
id: "right_arm"
motor {
motor_system_id: "ti5_motors"
motor_group_ids: "right_arm_can"
motor_system_id: "right_arm_can_motors"
motor_group_ids: "right_arm_can_motors"
dof: 7
joint_names: "R_SHOULDER_P"
joint_names: "R_SHOULDER_R"
@ -52,7 +52,6 @@ arm {
gain: 0.2
margin_ratio: 0.01
max_push: 0.02
weight: 0.05
}
}
}
@ -60,6 +59,14 @@ arm {
motion {
move_j {
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
settle_timeout_s: 2.0
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
settle_position_tolerance_rad: 0.002
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
settle_velocity_tolerance_rad_s: 0.02
# 位置和速度连续满足条件的采样次数。
settle_stable_sample_count: 3
toppra_joint_motion_planner {
path_type: TOPPRA_PATH_TYPE_QUINTIC
sample_period_s: 0.001
@ -130,9 +137,9 @@ arm {
cartesian_velocity_feasibility_check {
enable: true
min_linear_speed_ratio: 0.2
max_linear_direction_deviation_deg: 5.0
max_linear_direction_deviation_deg: 70
min_angular_speed_ratio: 0.2
max_angular_direction_deviation_deg: 5.0
max_angular_direction_deviation_deg: 70
min_desired_linear_speed: 1e-4
min_desired_angular_speed: 1e-4
}
@ -144,7 +151,9 @@ arm {
stop_twist_norm: 1e-9
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 0.5
stop_acceleration: 5
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}

36
cmvr-es/config/devices/camera/camera.pb.txt
View File

@ -83,8 +83,8 @@ camera {
stream_mode: STREAM_MODE_RGBD
}
encoder {
width: 1280
height: 720
width: 480
height: 320
fps: 30
codec: "H264"
enable_stream_timestamp: true
@ -92,7 +92,7 @@ camera {
}
consume_new_frame_only: false
viewer_pip {
enable: true
enable: false
left: -10
bottom: 10
width: 320
@ -101,6 +101,36 @@ camera {
}
}
cameras {
id: "mujoco_external_touch_cam"
mujoco {
world_id: "mujoco_world"
camera_name: "external_touch_cam"
render {
width: 1280
height: 720
fps: 30
stream_mode: STREAM_MODE_RGBD
}
encoder {
width: 480
height: 320
fps: 30
codec: "H264"
enable_stream_timestamp: true
buffer_size: 30
}
consume_new_frame_only: false
viewer_pip {
enable: false
left: 10
bottom: 10
width: 320
height: 180
}
}
}
cameras {
id: "left_eye_cam"
uvc {

7
cmvr-es/config/devices/dexhand/dexhand.pb.txt
View File

@ -2,7 +2,7 @@ dexhand {
dexhands {
id: "hand1"
rh56dftp {
ip: "192.168.1.213"
ip: "192.168.1.223"
port: 6000
poll_interval_ms: 10
}
@ -38,4 +38,9 @@ dexhand {
auto_calibrate: false
}
}
dexhands {
id: "mujoco_zero_touch_dexhand"
zero_sim_touch {}
}
}

72
cmvr-es/config/devices/motor/ethercat_motors.pb.txt Normal file
View File

@ -0,0 +1,72 @@
motor {
id: "ethercat_motors"
motor_groups {
id: "right_arm_ethercat_motors"
bus_type: MOTOR_BUS_ETHERCAT
vendor: MOTOR_VENDOR_EYOU
protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
ethercat {
master_index: 0
cycle_us: 1000
slave_op_timeout_ms: 12000
slave_state_poll_period_ms: 10
cia402 {
state_transition_timeout_ms: 1200
velocity_stop_timeout_ms: 2000
status_poll_period_ms: 10
stopped_velocity_tolerance_rad_s: 0.001
}
zero_calibration {
timeout_ms: 2000
poll_period_ms: 10
stable_sample_count: 5
position_tolerance_counts: 10000
stable_delta_counts: 1000
}
dc {
enable: true
reference_motor_id: 1
sync0_cycle_us: 1000
sync0_shift_us: 0
sync_reference_clock_period: 1
assign_activate: 768
sync_monitor_period_ms: 1000
}
slaves { motor_id: 1 alias: 0 position: 0 }
slaves { motor_id: 2 alias: 0 position: 1 }
slaves { motor_id: 3 alias: 0 position: 2 }
slaves { motor_id: 4 alias: 0 position: 3 }
slaves { motor_id: 5 alias: 0 position: 4 }
slaves { motor_id: 6 alias: 0 position: 5 }
slaves { motor_id: 7 alias: 0 position: 6 }
}
joint_limits {
enable: true
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_Y" q_lb: -0.78 q_ub: 0.78 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
motors {
motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 5 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 6 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 7 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}

63
cmvr-es/config/devices/motor/ethercat_motors_four_real_test.pb.txt Normal file
View File

@ -0,0 +1,63 @@
motor {
id: "ethercat_motors"
motor_groups {
id: "right_arm_ethercat_motors"
bus_type: MOTOR_BUS_ETHERCAT
vendor: MOTOR_VENDOR_EYOU
protocol: MOTOR_PROTOCOL_ETHERCAT_CIA402
ethercat {
master_index: 0
cycle_us: 1000
slave_op_timeout_ms: 15000
slave_state_poll_period_ms: 10
cia402 {
state_transition_timeout_ms: 1200
velocity_stop_timeout_ms: 2000
status_poll_period_ms: 10
stopped_velocity_tolerance_rad_s: 0.001
}
zero_calibration {
timeout_ms: 2000
poll_period_ms: 10
stable_sample_count: 5
position_tolerance_counts: 10000
stable_delta_counts: 1000
}
dc {
enable: false
reference_motor_id: 1
sync0_cycle_us: 1000
sync0_shift_us: 0
sync_reference_clock_period: 1
assign_activate: 768
sync_monitor_period_ms: 1000
}
slaves { motor_id: 1 alias: 0 position: 0 }
slaves { motor_id: 2 alias: 0 position: 1 }
slaves { motor_id: 3 alias: 0 position: 2 }
slaves { motor_id: 4 alias: 0 position: 3 }
}
joint_limits {
enable: true
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "R_SHOULDER_P" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_R" q_lb: -0.78 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_SHOULDER_Y" q_lb: -3.14 q_ub: 3.14 qd: 5.0 qdd: 10.0 }
joints { joint_name: "R_ELBOW_R" q_lb: 0 q_ub: 2.05 qd: 5.0 qdd: 10.0 }
}
motors {
motors { id: 1 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 2 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 3 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 4 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}

3
cmvr-es/config/devices/motor/mujoco_motors.pb.txt
View File

@ -2,11 +2,10 @@ motor {
id: "mujoco_motors"
motor_groups {
id: "mujoco_right_arm"
id: "right_arm_mujoco_motors"
bus_type: MOTOR_BUS_MUJOCO
vendor: MOTOR_VENDOR_MUJOCO
protocol: MOTOR_PROTOCOL_MUJOCO
tool_frame: "R_FINGER_TIP"
mujoco {
world_id: "mujoco_world"
}

35
cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt Normal file
View File

@ -0,0 +1,35 @@
motor {
id: "mujoco_motors"
motor_groups {
id: "right_arm_mujoco_motors"
bus_type: MOTOR_BUS_MUJOCO
vendor: MOTOR_VENDOR_MUJOCO
protocol: MOTOR_PROTOCOL_MUJOCO
mujoco {
world_id: "mujoco_world"
}
joint_limits {
enable: true
source: JOINT_LIMIT_SOURCE_CUSTOM
joints { joint_name: "right_arm_J1" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J2" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J3" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J4" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J5" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J6" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
joints { joint_name: "right_arm_J7" q_lb: -3.14159 q_ub: 3.14159 qd: 2.0 qdd: 10.0 }
}
motors {
motors { id: 1 joint_name: "right_arm_J1" }
motors { id: 2 joint_name: "right_arm_J2" }
motors { id: 3 joint_name: "right_arm_J3" }
motors { id: 4 joint_name: "right_arm_J4" }
motors { id: 5 joint_name: "right_arm_J5" }
motors { id: 6 joint_name: "right_arm_J6" }
motors { id: 7 joint_name: "right_arm_J7" }
}
}
}

48
cmvr-es/config/devices/motor/ti5_motors.pb.txt
View File

@ -2,11 +2,10 @@ motor {
id: "ti5_motors"
motor_groups {
id: "left_arm_can"
id: "left_arm_can_motors"
bus_type: MOTOR_BUS_CAN
vendor: MOTOR_VENDOR_TI5
protocol: MOTOR_PROTOCOL_CANOPEN
tool_frame: "L_FINGER_TIP"
can {
channel_id: 0
}
@ -16,22 +15,21 @@ motor {
urdf_path: "model/xiaoyan_description/dual_arm.urdf"
}
motors {
motors { id: 23 joint_name: "L_SHOULDER_P" }
motors { id: 24 joint_name: "L_SHOULDER_R" }
motors { id: 25 joint_name: "L_SHOULDER_Y" }
motors { id: 26 joint_name: "L_ELBOW_R" }
motors { id: 27 joint_name: "L_WRIST_P" }
motors { id: 28 joint_name: "L_WRIST_Y" }
motors { id: 29 joint_name: "L_WRIST_R" }
motors { id: 23 joint_name: "L_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 24 joint_name: "L_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 25 joint_name: "L_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 26 joint_name: "L_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 27 joint_name: "L_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 28 joint_name: "L_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 29 joint_name: "L_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
motor_groups {
id: "right_arm_can"
id: "right_arm_can_motors"
bus_type: MOTOR_BUS_CAN
vendor: MOTOR_VENDOR_TI5
protocol: MOTOR_PROTOCOL_CANOPEN
tool_frame: "R_FINGER_TIP"
can {
channel_id: 1
}
@ -47,18 +45,18 @@ motor {
joints { joint_name: "R_WRIST_R" q_lb: -0.57 q_ub: 1.57 qd: 5.0 qdd: 10.0 }
}
motors {
motors { id: 16 joint_name: "R_SHOULDER_P" }
motors { id: 17 joint_name: "R_SHOULDER_R" }
motors { id: 18 joint_name: "R_SHOULDER_Y" }
motors { id: 19 joint_name: "R_ELBOW_R" }
motors { id: 20 joint_name: "R_WRIST_P" }
motors { id: 21 joint_name: "R_WRIST_Y" }
motors { id: 22 joint_name: "R_WRIST_R" }
motors { id: 16 joint_name: "R_SHOULDER_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 17 joint_name: "R_SHOULDER_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 18 joint_name: "R_SHOULDER_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 19 joint_name: "R_ELBOW_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 20 joint_name: "R_WRIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 21 joint_name: "R_WRIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 22 joint_name: "R_WRIST_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
motor_groups {
id: "head_can"
id: "head_can_motors"
bus_type: MOTOR_BUS_CAN
vendor: MOTOR_VENDOR_TI5
protocol: MOTOR_PROTOCOL_CANOPEN
@ -73,14 +71,14 @@ motor {
joints { joint_name: "HEAD_R" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
}
motors {
motors { id: 32 joint_name: "HEAD_Y" }
motors { id: 30 joint_name: "HEAD_P" }
motors { id: 31 joint_name: "HEAD_R" }
motors { id: 32 joint_name: "HEAD_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 30 joint_name: "HEAD_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 31 joint_name: "HEAD_R" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
motor_groups {
id: "waist_can"
id: "waist_can_motors"
bus_type: MOTOR_BUS_CAN
vendor: MOTOR_VENDOR_TI5
protocol: MOTOR_PROTOCOL_CANOPEN
@ -94,8 +92,8 @@ motor {
joints { joint_name: "WAIST_P" q_lb: -3.14 q_ub: 3.14 qd: 3.0 }
}
motors {
motors { id: 4 joint_name: "WAIST_Y" }
motors { id: 15 joint_name: "WAIST_P" }
motors { id: 4 joint_name: "WAIST_Y" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
motors { id: 15 joint_name: "WAIST_P" encoder_counts_per_rev: 65536 gear_ratio: 101.0 }
}
}
}

7
cmvr-es/config/devices/mujoco/right_arm_eye_to_hand_world.pb.txt Normal file
View File

@ -0,0 +1,7 @@
worlds {
id: "mujoco_world"
model_path: "model/xiaoyan_description/right_arm_eye_to_hand.xml"
timestep_s: 0.001
realtime_factor: 1.0
require_actuator: true
}

2
cmvr-es/config/logger/logger.pb.txt
View File

@ -30,7 +30,7 @@ logger {
max_file_size_mb: 100
flush_interval_seconds: 1
format {
show_time: false
show_time: true
show_level: true
show_thread_id: false
show_source_location: true

63
cmvr-es/config/manager/device_manager.pb.txt
View File

@ -2,40 +2,46 @@ device_manager {
name: "cmvr_es"
version: "0.1"
description: "cmvr edge system version 0.1"
devices {
id: "mujoco_world"
type: DEVICE_TYPE_MUJOCO_WORLD
config_file: "devices/mujoco/mujoco_world.pb.txt"
enable: false
config_file: "devices/mujoco/right_arm_eye_to_hand_world.pb.txt"
enable: true
}
devices {
id: "mujoco_motors"
id: "right_arm_mujoco_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/mujoco_motors.pb.txt"
enable: false
enable: true
}
devices {
id: "mujoco_right_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm_mujoco_qp.pb.txt"
enable: false
enable: true
}
devices {
id: "mujoco_viewer"
type: DEVICE_TYPE_MUJOCO_VIEWER
config_file: "devices/mujoco/mujoco_viewer.pb.txt"
enable: false
enable: true
}
devices {
id: "mujoco_hand_cam"
type: DEVICE_TYPE_CAMERA
config_file: "devices/camera/camera.pb.txt"
enable: false
enable: true
}
devices {
id: "mujoco_external_touch_cam"
type: DEVICE_TYPE_CAMERA
config_file: "devices/camera/camera.pb.txt"
enable: true
}
devices {
@ -68,16 +74,51 @@ device_manager {
}
devices {
id: "ti5_motors"
id: "mujoco_zero_touch_dexhand"
type: DEVICE_TYPE_DEXHAND
config_file: "devices/dexhand/dexhand.pb.txt"
enable: true
}
devices {
id: "left_arm_can_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ti5_motors.pb.txt"
enable: false
}
devices {
id: "right_arm_can_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ti5_motors.pb.txt"
enable: false
}
devices {
id: "head_can_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ti5_motors.pb.txt"
enable: false
}
devices {
id: "waist_can_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ti5_motors.pb.txt"
enable: false
}
devices {
id: "right_arm_ethercat_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ethercat_motors.pb.txt"
enable: false
}
devices {
id: "right_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm.pb.txt"
config_file: "devices/arm/arm_qp.pb.txt"
enable: false
}
@ -92,7 +133,7 @@ device_manager {
id: "huayan_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/huayan_arm.pb.txt"
enable: true
enable: false
}
devices {

10
cmvr-es/config/manager/task_manager.pb.txt
View File

@ -4,7 +4,7 @@ task_manager {
type: TASK_TYPE_TOUCH_SCREEN
run_mode: TASK_RUN_MODE_PERIODIC_STEP
control_period_s: 0.001
config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
config_file: "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt"
enable: false
}
tasks {
@ -14,4 +14,12 @@ task_manager {
config_file: "tasks/grpc_server_task/grpc_server_task.pb.txt"
enable: true
}
tasks {
id: "right_arm_self_collision"
type: TASK_TYPE_SELF_COLLISION
run_mode: TASK_RUN_MODE_PERIODIC_STEP
control_period_s: 0.002
config_file: "tasks/self_collision_task/self_collision_task.pb.txt"
enable: false
}
}

34
cmvr-es/config/tasks/self_collision_task/self_collision_task.pb.txt Normal file
View File

@ -0,0 +1,34 @@
self_collision_task {
id: "right_arm_self_collision"
arm_id: "mujoco_right_arm"
checker {
urdf_path: "model/xiaoyan_description/dual_arm_collision.urdf"
# Simplified compact-wrist bodies overlap in the normal assembled pose.
ignored_pairs {
first: "R_WRIST_P_S"
second: "R_WRIST_R_S"
}
}
sampling {
max_geometry_displacement_m: 0.002
max_check_period_s: 0.01
}
safety {
warning_distance_m: 0.02
stop_distance_m: 0.005
}
recovery {
clear_distance_m: 0.025
stable_period_s: 0.1
max_joint_velocity_rad_s: 0.15
max_joint_acceleration_rad_s2: 0.3
history_duration_s: 10.0
max_distance_regression_m: 0.001
}
}

37
cmvr-es/config/tasks/self_collision_task/self_collision_task_gen2.pb.txt Normal file
View File

@ -0,0 +1,37 @@
self_collision_task {
id: "gen2_right_arm_self_collision"
arm_id: "mujoco_right_arm"
checker {
urdf_path: "model/gen2/collision/robot_collision.urdf"
# These second-neighbor mounting bodies overlap in normal assembled poses.
ignored_pairs {
first: "arm_link_5_2"
second: "arm_link_7_2"
}
ignored_pairs {
first: "body_link"
second: "arm_link_2_2"
}
}
sampling {
max_geometry_displacement_m: 0.002
max_check_period_s: 0.01
}
safety {
warning_distance_m: 0.02
stop_distance_m: 0.005
}
recovery {
clear_distance_m: 0.05
stable_period_s: 0.1
max_joint_velocity_rad_s: 0.3
max_joint_acceleration_rad_s2: 5.0
history_duration_s: 10.0
max_distance_regression_m: 0.001
}
}

75
cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt
View File

@ -1,10 +1,16 @@
touch_screen_task {
id: "touch_screen"
# 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
debug_draw_coordinate_frames: true
# G/H 坐标轴长度，单位为米。
debug_coordinate_axis_length_m: 0.02
devices {
arm_id: "right_arm"
dexhand_id: "paxini_tip_1"
# 手部相机和外部相机的 DeviceManager ID。
camera_id: "right_hand_cam"
external_camera_id: "cam5"
}
initialization {
@ -19,46 +25,56 @@ touch_screen_task {
joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 }
velocity: 1.0
acceleration: 2.0
# 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
skip_position_tolerance_rad: 0.001
# 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
skip_velocity_tolerance_rad_s: 0.01
}
perception {
apriltag {
tag_size_m: 0.012
# 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
tags {
screen {
id: 1
size_m: 0.03
}
hand {
id: 0
size_m: 0.03
}
}
# 手部相机：用于点击目标点和手部目标跟踪。
hand_camera {
depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
}
}
alignment {
ibvs {
camera_link: "R_CAM"
lambda: 0.4
mu: 0.1
qdot_max: 1.0
vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
calibration {
# TCP P 相对于屏幕 Hand Tag H 的目标姿态
hand_tag_to_tcp {
m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
}
}
pbvs {
position_gain { x: 2.0 y: 2.0 z: 1.5 }
rotation_gain { x: 1.5 y: 1.5 z: 1.5 }
vmax6 { x: 0.10 y: 0.10 z: 0.05 rx: 0.50 ry: 0.50 rz: 0.50 }
amax6 { x: 0.50 y: 0.50 z: 0.30 rx: 2.0 ry: 2.0 rz: 2.0 }
twist_filter_alpha: 1.0
r_camera_to_visp {
m00: 1.0 m01: 0.0 m02: 0.0
m10: 0.0 m11: 1.0 m12: 0.0
m20: 0.0 m21: 0.0 m22: 1.0
}
r_camera_to_urdf {
m00: 1.0 m01: 0.0 m02: 0.0
m10: 0.0 m11: 1.0 m12: 0.0
m20: 0.0 m21: 0.0 m22: 1.0
}
control_joint_names: "R_SHOULDER_P"
control_joint_names: "R_SHOULDER_R"
control_joint_names: "R_SHOULDER_Y"
control_joint_names: "R_ELBOW_R"
control_joint_names: "R_WRIST_P"
control_joint_names: "R_WRIST_Y"
control_joint_names: "R_WRIST_R"
}
target {
position_in_camera { x: -0.001 y: 0.08 z: 0.15 }
rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
# Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
# rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
# 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。
# PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。
hand_orientation_G { rx: 0.0 ry: 0.0 rz: 3.141592653589793 }
# 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。
position_offset_G { x: 0.0 y: 0.0 z: 0.05 }
mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
}
error_threshold {
@ -92,6 +108,7 @@ touch_screen_task {
retract {
twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
acceleration: 8.0
duration_s: 0.45
# TCP 后退目标距离，单位为米。
distance_m: 0.02
}
}

102
cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt
View File

@ -1,10 +1,16 @@
touch_screen_task {
id: "touch_screen"
# 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
debug_draw_coordinate_frames: true
# G/H 坐标轴长度，单位为米。
debug_coordinate_axis_length_m: 0.02
devices {
arm_id: "right_arm_mujoco"
arm_id: "mujoco_right_arm"
dexhand_id: "mujoco_zero_touch_dexhand"
camera_id: "hand_cam"
# 手部相机和外部相机的 DeviceManager ID。
camera_id: "mujoco_hand_cam"
external_camera_id: "mujoco_external_touch_cam"
}
initialization {
@ -17,54 +23,73 @@ touch_screen_task {
joint_positions { joint_name: "R_WRIST_P" rad: -2.8792 }
joint_positions { joint_name: "R_WRIST_Y" rad: 0.1150 }
joint_positions { joint_name: "R_WRIST_R" rad: -0.08 }
velocity: 2.8
acceleration: 20.0
velocity: 2.0
acceleration: 3.0
# 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
skip_position_tolerance_rad: 0.001
# 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
skip_velocity_tolerance_rad_s: 0.01
}
perception {
apriltag {
tag_size_m: 0.12
# 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
tags {
screen {
id: 1
size_m: 0.03
}
hand {
id: 0
size_m: 0.03
}
}
# 手部相机：用于点击目标点和手部目标跟踪。
hand_camera {
depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
}
}
alignment {
ibvs {
camera_link: "R_CAM"
lambda: 0.4
mu: 0.1
qdot_max: 0.8
vmax6 { x: 1.0 y: 1.0 z: 1.0 rx: 0.6 ry: 0.6 rz: 0.6 }
amax6 { x: 2.4 y: 2.4 z: 4.5 rx: 2.5 ry: 2.5 rz: 2.5 }
calibration {
# TCP P 相对于屏幕 Hand Tag H 的目标姿态
hand_tag_to_tcp {
m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
}
}
pbvs {
position_gain { x: 2.0 y: 2.0 z: 1.5 }
rotation_gain { x: 1.5 y: 1.5 z: 1.5 }
vmax6 { x: 0.10 y: 0.10 z: 0.05 rx: 0.50 ry: 0.50 rz: 0.50 }
amax6 { x: 0.50 y: 0.50 z: 0.30 rx: 2.0 ry: 2.0 rz: 2.0 }
twist_filter_alpha: 1.0
r_camera_to_visp {
m00: 1.0 m01: 0.0 m02: 0.0
m10: 0.0 m11: -1.0 m12: 0.0
m20: 0.0 m21: 0.0 m22: -1.0
}
r_camera_to_urdf {
m00: 1.0 m01: 0.0 m02: 0.0
m10: 0.0 m11: -1.0 m12: 0.0
m20: 0.0 m21: 0.0 m22: -1.0
}
control_joint_names: "R_SHOULDER_P"
control_joint_names: "R_SHOULDER_R"
control_joint_names: "R_SHOULDER_Y"
control_joint_names: "R_ELBOW_R"
control_joint_names: "R_WRIST_P"
control_joint_names: "R_WRIST_Y"
control_joint_names: "R_WRIST_R"
}
target {
position_in_camera { x: 0.0 y: 0.0 z: 0.30 }
rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
# Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
# rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
# 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。
# PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。
hand_orientation_G {
rx: 0.0
ry: 0.0
rz: 3.141592653589793
}
# 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。
position_offset_G {
x: 0.0
y: 0.0
z: 0.05
}
mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
}
error_threshold {
x: 0.005
y: 0.005
z: 0.010
z: 0.005
rx: 0.08726646259971647
ry: 0.08726646259971647
rz: 0.08726646259971647
@ -78,7 +103,7 @@ touch_screen_task {
speed_l {
twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
acceleration: 6.0
max_distance_m: 0.12
max_distance_m: 0.02
}
tactile {
finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
@ -90,8 +115,9 @@ touch_screen_task {
}
retract {
twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
acceleration: 8.0
duration_s: 5.0
twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
acceleration: 4.0
# TCP 后退目标距离，单位为米。
distance_m: 0.05
}
}

54
cmvr-es/devices/arm/aubo_arm/CMakeLists.txt
View File

@ -4,10 +4,58 @@ add_library(aubo_arm SHARED
target_include_directories(aubo_arm PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
set(AUBO_SDK_INCLUDE_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/include)
set(AUBO_SDK_LIB_DIR ${CMAKE_SOURCE_DIR}/third_party/AuboSdk/linux/lib)
set(AUBO_SDK_ROOT ${CMAKE_SOURCE_DIR}/dependency/${ARCH}/third_party/aubo_sdk/v0.27.1)
set(AUBO_SDK_INCLUDE_DIR ${AUBO_SDK_ROOT}/include)
set(AUBO_SDK_LIB_DIR ${AUBO_SDK_ROOT}/lib)
if (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h")
if (EXISTS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk/aubo_sdkConfig.cmake")
list(APPEND CMAKE_PREFIX_PATH "${AUBO_SDK_LIB_DIR}/cmake")
find_package(Qt5Core QUIET)
if (NOT Qt5Core_FOUND AND NOT TARGET Qt5::Core)
find_library(QT5_CORE_LIBRARY
NAMES Qt5Core libQt5Core.so.5
PATHS /lib /usr/lib /usr/local/lib /lib/x86_64-linux-gnu /usr/lib/x86_64-linux-gnu
)
if (QT5_CORE_LIBRARY)
add_library(Qt5::Core UNKNOWN IMPORTED)
set_target_properties(Qt5::Core PROPERTIES
IMPORTED_LOCATION "${QT5_CORE_LIBRARY}"
)
endif()
endif()
find_package(aubo_sdk REQUIRED CONFIG PATHS "${AUBO_SDK_LIB_DIR}/cmake/aubo_sdk" NO_DEFAULT_PATH)
# The vendor directory contains an old private libstdc++. Keep it out of
# consumers' RUNPATH by staging only the AUBO runtime libraries.
set(AUBO_CLEAN_LIB_DIR "${CMAKE_CURRENT_BINARY_DIR}/aubo_sdk_runtime")
file(MAKE_DIRECTORY "${AUBO_CLEAN_LIB_DIR}")
foreach(AUBO_LIB
libaubo_sdk.so
libaubo_sdkd.so
librobot_proxy.so
librobot_proxyd.so)
file(COPY_FILE
"${AUBO_SDK_LIB_DIR}/${AUBO_LIB}"
"${AUBO_CLEAN_LIB_DIR}/${AUBO_LIB}"
ONLY_IF_DIFFERENT
)
endforeach()
set_target_properties(aubo_sdk::aubo_sdk aubo_sdk::robot_proxy PROPERTIES
MAP_IMPORTED_CONFIG_DEBUG Release
)
set_target_properties(aubo_sdk::aubo_sdk PROPERTIES
IMPORTED_LOCATION_RELEASE "${AUBO_CLEAN_LIB_DIR}/libaubo_sdk.so"
IMPORTED_LOCATION_DEBUG "${AUBO_CLEAN_LIB_DIR}/libaubo_sdkd.so"
)
set_target_properties(aubo_sdk::robot_proxy PROPERTIES
IMPORTED_LOCATION_RELEASE "${AUBO_CLEAN_LIB_DIR}/librobot_proxy.so"
IMPORTED_LOCATION_DEBUG "${AUBO_CLEAN_LIB_DIR}/librobot_proxyd.so"
)
target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK)
target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR})
target_link_libraries(aubo_arm PRIVATE aubo_sdk::aubo_sdk)
elseif (EXISTS "${AUBO_SDK_INCLUDE_DIR}/aubo_sdk/rpc.h")
target_compile_definitions(aubo_arm PRIVATE CMVR_HAS_AUBO_SDK)
target_include_directories(aubo_arm PRIVATE ${AUBO_SDK_INCLUDE_DIR})
if (EXISTS "${AUBO_SDK_LIB_DIR}")

8
cmvr-es/devices/arm/aubo_arm/include/aubo_arm.h
View File

@ -36,6 +36,14 @@ public:
Result calibrateZeroQ(const std::string& joint_name) override;
Result emergencyStop() override;
Result protectiveStop() override { return emergencyStop(); }
Result recoverProtectiveStop(
const JointTrajectory&,
const MotionOptions&) override
{
return Result::failure(
ArmErrorCode::UnsupportedCommand,
"protective recovery is not implemented for AuboArm");
}
Result setSpeedScaling(double scaling) override;
double getSpeedScaling() const override { return speed_scaling_; }
bool isProtectiveStopped() const override { return false; }

10
cmvr-es/devices/arm/huayan_arm/huayan_arm.h
View File

@ -42,6 +42,14 @@ public:
Result calibrateZeroQ(const std::string& joint_name) override;
Result emergencyStop() override;
Result protectiveStop() override { return emergencyStop(); }
Result recoverProtectiveStop(
const JointTrajectory&,
const MotionOptions&) override
{
return Result::failure(
ArmErrorCode::UnsupportedCommand,
"protective recovery is not implemented for HuayanRobot");
}
Result setSpeedScaling(double scaling) override;
double getSpeedScaling() const override { return speed_scaling_; }
bool isProtectiveStopped() const override;
@ -140,4 +148,4 @@ private:
#endif // CMVR_ES_HUAYAN_ROBOT_H
#endif //CMVR_ES_HUAYAN_ARM_H
#endif //CMVR_ES_HUAYAN_ARM_H

18
cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt
View File

@ -34,3 +34,21 @@ target_link_libraries(motor_robot_arm_mujoco_test
gtest_main
pthread
)
add_executable(motor_robot_arm_gen2_mujoco_test
src/motor_robot_arm_gen2_mujoco_test.cpp
)
target_link_libraries(motor_robot_arm_gen2_mujoco_test
PRIVATE
cmvr_es::device::motor_robot_arm
cmvr_es::device::motor_manager
cmvr_es::device::mujoco_motor_driver
cmvr_es::device_manager
cmvr_es::mujoco_viewer
cmvr_es::proto
cmvr_es::task
gtest
gtest_main
pthread
)

31
cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h
View File

@ -41,11 +41,14 @@ public:
Result torqueOff() override;
Result calibrateZeroQ(const std::string& joint_name) override;
Result emergencyStop() override;
Result protectiveStop() override { return emergencyStop(); }
Result protectiveStop() override;
Result recoverProtectiveStop(
const JointTrajectory& path,
const MotionOptions& options) override;
Result setSpeedScaling(double scaling) override;
double getSpeedScaling() const override { return speed_scaling_; }
bool isProtectiveStopped() const override { return false; }
bool isEmergencyStopped() const override { return emergency_stopped_; }
bool isProtectiveStopped() const override { return protective_stopped_.load(); }
bool isEmergencyStopped() const override { return emergency_stopped_.load(); }
bool isFault() const override { return false; }
Result moveJ(const JointPositionCommand& target, const MotionOptions& options) override;
@ -73,10 +76,10 @@ public:
bool isConnected() const override { return motor_manager_ != nullptr; }
Result powerOn() override { return torqueOn(); }
Result powerOff() override { return torqueOff(); }
Result brakeRelease() override { return torqueOn(); }
Result brakeRelease() override;
Result shutdown() override;
Result clearFault() override { return Result::success(); }
Result unlockProtectiveStop() override { return Result::success(); }
Result unlockProtectiveStop() override;
Result loadProgram(const std::string& program_name) override;
Result playProgram() override;
Result pauseProgram() override;
@ -94,11 +97,17 @@ public:
private:
bool containsJoint_(const std::string& joint_name) const;
bool safetyStopRequested_() const;
std::optional<Result> safetyStopResult_(const std::string& command,
bool interrupted = false) const;
Result quickStopMotors_();
bool validatePositionCommand_(const JointPositionCommand& cmd, std::string& error) const;
bool validateVelocityCommand_(const JointVelocityCommand& cmd, std::string& error) const;
std::shared_ptr<AbstractMotor> getMotor_(const std::string& joint_name) const;
bool readArmState_(std::vector<double>& q_now, std::vector<double>& qd_now) const;
std::vector<double> readJointPosition_() const;
Result stopCartesianMotionAndWait_();
Result waitForJointTarget_(const std::vector<double>& target) const;
bool configureAlgorithms_();
bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory);
@ -123,10 +132,20 @@ private:
std::shared_ptr<CartesianMotionPlanner> cartesian_planner_{nullptr};
std::unique_ptr<CartesianVelocityController> cartesian_velocity_controller_{nullptr};
// MoveJ post-trajectory settling criteria. These defaults preserve the
// historical behavior when the optional arm configuration fields are absent.
double move_j_settle_timeout_s_{2.0};
double move_j_position_tolerance_rad_{2e-3};
double move_j_velocity_tolerance_rad_s_{2e-2};
int move_j_stable_sample_count_{3};
mutable std::mutex mutex_;
std::atomic<bool> busy_{false};
double speed_scaling_{1.0};
bool emergency_stopped_{false};
std::atomic<bool> protective_stopped_{false};
std::atomic<bool> emergency_stopped_{false};
std::atomic<bool> protective_recovery_active_{false};
std::atomic<bool> protective_recovery_cancel_requested_{false};
ServoOptions servo_options_;
};

539
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp
View File

@ -1,6 +1,8 @@
#include "arm/motor_robot_arm/include/motor_robot_arm.h"
#include <algorithm>
#include <chrono>
#include <cmath>
#include <Eigen/Dense>
#include <stdexcept>
#include <thread>
@ -28,6 +30,11 @@ struct BusyGuard {
~BusyGuard() { busy.store(false); }
};
struct AtomicFlagGuard {
std::atomic<bool>& flag;
~AtomicFlagGuard() { flag.store(false); }
};
} // namespace
MotorRobotArm::MotorRobotArm(const config::RobotArmConfig& cfg)
@ -132,12 +139,15 @@ bool MotorRobotArm::stop()
ArmState MotorRobotArm::getRobotState() const
{
const bool protective_stopped = protective_stopped_.load();
const bool emergency_stopped = emergency_stopped_.load();
ArmState state;
state.connected = motor_manager_ != nullptr;
state.powered_on = true;
state.brake_released = !emergency_stopped_;
state.brake_released = !emergency_stopped;
state.moving = busy();
state.emergency_stopped = emergency_stopped_;
state.protective_stopped = protective_stopped;
state.emergency_stopped = emergency_stopped;
state.speed_scaling = speed_scaling_;
state.robot_mode = RobotMode::Idle;
state.safety_mode = getSafetyMode();
@ -186,7 +196,13 @@ CartesianPose MotorRobotArm::getTcpPose(const FrameType frame) const
SafetyMode MotorRobotArm::getSafetyMode() const
{
return emergency_stopped_ ? SafetyMode::EmergencyStop : SafetyMode::Normal;
if (emergency_stopped_.load()) {
return SafetyMode::EmergencyStop;
}
if (protective_stopped_.load()) {
return SafetyMode::ProtectiveStop;
}
return SafetyMode::Normal;
}
Result MotorRobotArm::torqueOn()
@ -196,9 +212,12 @@ Result MotorRobotArm::torqueOn()
if (!motor) {
return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
}
motor->brake();
if (!motor->torqueOn()) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to torque on motor for joint: " + joint_name);
}
}
emergency_stopped_ = false;
emergency_stopped_.store(false);
return Result::success();
}
@ -209,7 +228,26 @@ Result MotorRobotArm::torqueOff()
if (!motor) {
return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
}
motor->torqueOff();
if (!motor->torqueOff() || !motor->brakeRelease()) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to torque off motor for joint: " + joint_name);
}
}
return Result::success();
}
Result MotorRobotArm::brakeRelease()
{
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
return Result::failure(ArmErrorCode::RobotNotReady,
"motor not found for joint: " + joint_name);
}
if (!motor->brakeRelease()) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to release brake for joint: " + joint_name);
}
}
return Result::success();
}
@ -233,17 +271,230 @@ Result MotorRobotArm::emergencyStop()
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
}
protective_recovery_cancel_requested_.store(true);
protective_stopped_.store(false);
emergency_stopped_.store(true);
return quickStopMotors_();
}
Result MotorRobotArm::protectiveStop()
{
if (emergency_stopped_.load()) {
return Result::success();
}
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
}
protective_recovery_cancel_requested_.store(true);
protective_stopped_.store(true);
return quickStopMotors_();
}
Result MotorRobotArm::recoverProtectiveStop(
const JointTrajectory& path,
const MotionOptions& options)
{
if (emergency_stopped_.load()) {
return Result::failure(
ArmErrorCode::RobotInEmergencyStop,
"protective recovery rejected: arm is in emergency stop");
}
if (!protective_stopped_.load()) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"protective recovery rejected: arm is not protective stopped");
}
if (path.size() < 2 ||
!std::isfinite(options.velocity) || options.velocity <= 0.0 ||
!std::isfinite(options.acceleration) || options.acceleration <= 0.0) {
return Result::failure(
ArmErrorCode::InvalidArgument,
"protective recovery path or options are invalid");
}
for (std::size_t i = 0; i < path.size(); ++i) {
const auto& sample = path[i];
if (!std::isfinite(sample.time_s) ||
sample.position.size() != joint_names_.size() ||
sample.velocity.size() != joint_names_.size() ||
(i > 0 && sample.time_s <= path[i - 1].time_s)) {
return Result::failure(
ArmErrorCode::InvalidArgument,
"protective recovery sample shape or time is invalid");
}
for (std::size_t joint = 0; joint < sample.position.size(); ++joint) {
if (!std::isfinite(sample.position[joint]) ||
!std::isfinite(sample.velocity[joint])) {
return Result::failure(
ArmErrorCode::InvalidArgument,
"protective recovery sample contains a non-finite value");
}
}
}
if (busy_.exchange(true)) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"protective recovery rejected: arm is busy");
}
BusyGuard busy_guard{busy_};
bool expected = false;
if (!protective_recovery_active_.compare_exchange_strong(expected, true)) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"protective recovery is already active");
}
AtomicFlagGuard recovery_guard{protective_recovery_active_};
protective_recovery_cancel_requested_.store(false);
if (!joint_planner_) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"protective recovery planner is not initialized");
}
JointTrajectory recovery_trajectory;
const auto planning_start = std::chrono::steady_clock::now();
if (!joint_planner_->planReplay(
readJointPosition_(), path, options, recovery_trajectory)) {
return Result::failure(
ArmErrorCode::CommandFailed,
"failed to plan protective recovery replay trajectory");
}
const double planning_ms = std::chrono::duration<double, std::milli>(
std::chrono::steady_clock::now() - planning_start).count();
CMVR_LOG(INFO) << "[MotorRobotArm] protective recovery planned"
<< ", input_samples=" << path.size()
<< ", command_samples=" << recovery_trajectory.size()
<< ", planning_ms=" << planning_ms
<< ", trajectory_duration_s="
<< recovery_trajectory.back().time_s;
if (emergency_stopped_.load()) {
return Result::failure(
ArmErrorCode::RobotInEmergencyStop,
"protective recovery interrupted by emergency stop during planning");
}
if (protective_recovery_cancel_requested_.load()) {
return Result::failure(
ArmErrorCode::RobotInProtectiveStop,
"protective recovery aborted by collision monitor during planning");
}
std::lock_guard<std::mutex> lock(mutex_);
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(joint_names_.size());
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"motor not found for joint: " + joint_name);
}
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION &&
!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return Result::failure(
ArmErrorCode::CommandFailed,
"failed to set recovery position mode for joint: " + joint_name);
}
motors.push_back(std::move(motor));
}
const auto trajectory_start = std::chrono::steady_clock::now();
for (std::size_t i = 0; i < recovery_trajectory.size(); ++i) {
if (emergency_stopped_.load()) {
return Result::failure(
ArmErrorCode::RobotInEmergencyStop,
"protective recovery interrupted by emergency stop");
}
if (protective_recovery_cancel_requested_.load()) {
return Result::failure(
ArmErrorCode::RobotInProtectiveStop,
"protective recovery aborted by collision monitor");
}
const auto& sample = recovery_trajectory[i];
if (!motor_manager_->commandCyclicPositionsAtomic(
motors, sample.position, sample.velocity)) {
return Result::failure(
ArmErrorCode::CommandFailed,
"failed to submit protective recovery sample");
}
if (i + 1 < recovery_trajectory.size()) {
std::this_thread::sleep_until(
trajectory_start +
std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(
recovery_trajectory[i + 1].time_s)));
}
}
const std::vector<double> zero_velocity(joint_names_.size(), 0.0);
if (!motor_manager_->commandCyclicPositionsAtomic(
motors, path.front().position, zero_velocity)) {
return Result::failure(
ArmErrorCode::CommandFailed,
"failed to hold final protective recovery position");
}
return Result::success();
}
Result MotorRobotArm::unlockProtectiveStop()
{
if (emergency_stopped_.load()) {
return Result::failure(
ArmErrorCode::RobotInEmergencyStop,
"cannot unlock protective stop while arm is emergency stopped");
}
if (protective_recovery_active_.load()) {
return Result::failure(
ArmErrorCode::CommandRejected,
"cannot unlock protective stop while recovery is active");
}
protective_stopped_.store(false);
return Result::success();
}
Result MotorRobotArm::quickStopMotors_()
{
for (const auto& joint_name : joint_names_) {
auto motor = getMotor_(joint_name);
if (!motor) {
return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
}
motor->brake();
if (!motor->quickStop()) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to quick stop motor for joint: " + joint_name);
}
}
emergency_stopped_ = true;
return Result::success();
}
bool MotorRobotArm::safetyStopRequested_() const
{
return emergency_stopped_.load() || protective_stopped_.load();
}
std::optional<Result> MotorRobotArm::safetyStopResult_(
const std::string& command,
const bool interrupted) const
{
const char* action = interrupted ? " interrupted by " : " rejected: arm is in ";
if (emergency_stopped_.load()) {
return Result::failure(
ArmErrorCode::RobotInEmergencyStop,
command + action + "emergency stop");
}
if (protective_stopped_.load()) {
return Result::failure(
ArmErrorCode::RobotInProtectiveStop,
command + action + "protective stop");
}
return std::nullopt;
}
Result MotorRobotArm::setSpeedScaling(const double scaling)
{
if (scaling < 0.0 || scaling > 1.0) {
@ -255,6 +506,9 @@ Result MotorRobotArm::setSpeedScaling(const double scaling)
Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOptions& options)
{
if (const auto stopped = safetyStopResult_("moveJ")) {
return *stopped;
}
std::string error;
if (!validatePositionCommand_(target, error)) {
return Result::failure(ArmErrorCode::InvalidArgument, error);
@ -262,13 +516,23 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
if (!joint_planner_) {
return Result::failure(ArmErrorCode::RobotNotReady, "joint planner is not initialized");
}
// speedL runs in a worker thread and sends speedJ commands. A moveJ
// trajectory writes cyclic-position commands directly, so allowing both
// paths to run concurrently can overwrite the motor mode/target and cause
// a short surge at the transition. Finish the Cartesian worker before
// reading the planning start state.
const auto cartesian_stop = stopCartesianMotionAndWait_();
if (!cartesian_stop.ok()) {
return cartesian_stop;
}
if (busy_.exchange(true)) {
return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_);
}
BusyGuard busy_guard{busy_};
std::lock_guard<std::mutex> lock(mutex_);
std::vector<JointTrajectorySample> samples;
JointTrajectory samples;
if (!joint_planner_->planMoveJ(readJointPosition_(), target, options, speed_scaling_, samples)) {
return Result::failure(ArmErrorCode::CommandFailed, "[MotorRobotArm] moveJ planner failed: " + id_);
}
@ -284,29 +548,66 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
return Result::failure(ArmErrorCode::RobotNotReady, "motor not found for joint: " + joint_name);
}
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to set cyclic position mode for joint: " +
joint_name);
}
}
motors.push_back(std::move(motor));
}
const auto t0 = std::chrono::steady_clock::now();
constexpr double fallback_dt = 0.001;
std::vector<double> command_velocity(motors.size(), 0.0);
for (std::size_t k = 1; k < samples.size(); ++k) {
if (const auto stopped = safetyStopResult_("moveJ", true)) {
return *stopped;
}
const auto& sample = samples[k];
if (sample.position.size() != motors.size()) {
return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
}
for (std::size_t i = 0; i < motors.size(); ++i) {
const double qd = i < sample.velocity.size() ? sample.velocity[i] : 0.0;
motors[i]->setTarget(sample.position[i], qd);
std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
// A MoveJ is a rest-to-rest command. Do not let a non-zero numerical
// endpoint velocity from an alternate planner keep the drive moving
// while the position-mode trajectory is being handed back to the arm.
if (k + 1 < samples.size()) {
std::copy_n(sample.velocity.begin(),
std::min(sample.velocity.size(), command_velocity.size()),
command_velocity.begin());
}
if (!motor_manager_->commandCyclicPositionsAtomic(
motors, sample.position, command_velocity)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to submit atomic cyclic position command");
}
if (k + 1 < samples.size()) {
const double next_t = samples[k + 1].t > 0.0 ? samples[k + 1].t
const double next_t = samples[k + 1].time_s > 0.0
? samples[k + 1].time_s
: static_cast<double>(k + 1) * fallback_dt;
std::this_thread::sleep_until(t0 + std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(next_t)));
}
}
// Repeat the final position with zero velocity before checking feedback.
// This seeds the position-mode target after a velocity-to-position switch
// and prevents a stale final velocity command from producing a short
// motion spike at the destination.
if (!motor_manager_->commandCyclicPositionsAtomic(
motors, target.position, std::vector<double>(motors.size(), 0.0))) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to hold final moveJ target");
}
// Allow one servo cycle to consume the explicit hold command before
// evaluating feedback-based settling criteria.
std::this_thread::sleep_for(std::chrono::milliseconds(1));
const auto settle_result = waitForJointTarget_(target.position);
if (!settle_result.ok()) {
return settle_result;
}
return Result::success();
}
@ -315,6 +616,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
const double duration)
{
(void)acceleration;
if (const auto stopped = safetyStopResult_("speedJ")) {
return *stopped;
}
std::string error;
if (!validateVelocityCommand_(velocity, error)) {
return Result::failure(ArmErrorCode::InvalidArgument, error);
@ -329,9 +633,17 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
"motor not found for joint: " + joint_names_[i]);
}
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY);
if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to set cyclic velocity mode for joint: " +
joint_names_[i]);
}
}
if (!motor->commandCyclicVelocity(velocity.velocity[i] * speed_scaling_)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to command cyclic velocity for joint: " +
joint_names_[i]);
}
motor->setTarget(velocity.velocity[i] * speed_scaling_);
}
}
@ -344,6 +656,9 @@ Result MotorRobotArm::speedJ(const JointVelocityCommand& velocity,
Result MotorRobotArm::stopJ(const double acceleration)
{
if (safetyStopRequested_()) {
return Result::success();
}
JointVelocityCommand zero;
zero.velocity.assign(joint_names_.size(), 0.0);
return speedJ(zero, acceleration, 0.0);
@ -353,8 +668,12 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
const MotionOptions& options,
const FrameType frame)
{
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
if (const auto stopped = safetyStopResult_("moveL")) {
return *stopped;
}
const auto cartesian_stop = stopCartesianMotionAndWait_();
if (!cartesian_stop.ok()) {
return cartesian_stop;
}
if (options.asynchronous) {
return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported");
@ -394,8 +713,13 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
<< ", executable_path_m=" << trajectory.executable_path_length;
}
return executeMoveLTrajectory_(trajectory) ? Result::success()
: Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
if (executeMoveLTrajectory_(trajectory)) {
return Result::success();
}
if (const auto stopped = safetyStopResult_("moveL", true)) {
return *stopped;
}
return Result::failure(ArmErrorCode::CommandFailed, "moveL execution failed");
}
Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
@ -403,6 +727,9 @@ Result MotorRobotArm::speedL(const CartesianVelocity& velocity,
const double duration,
const FrameType frame)
{
if (const auto stopped = safetyStopResult_("speedL")) {
return *stopped;
}
if (busy_.load()) {
return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
}
@ -422,7 +749,10 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
Result MotorRobotArm::stopMotion()
{
stopL(0.0);
const auto cartesian_stop = stopCartesianMotionAndWait_();
if (!cartesian_stop.ok()) {
return cartesian_stop;
}
return stopJ(0.0);
}
@ -442,12 +772,17 @@ Result MotorRobotArm::startServoMode(const ServoOptions& options)
Result MotorRobotArm::servoJ(const JointPositionCommand& target)
{
if (const auto stopped = safetyStopResult_("servoJ")) {
return *stopped;
}
std::string error;
if (!validatePositionCommand_(target, error)) {
return Result::failure(ArmErrorCode::InvalidArgument, error);
}
std::lock_guard<std::mutex> lock(mutex_);
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(joint_names_.size());
for (std::size_t i = 0; i < joint_names_.size(); ++i) {
auto motor = getMotor_(joint_names_[i]);
if (!motor) {
@ -455,9 +790,18 @@ Result MotorRobotArm::servoJ(const JointPositionCommand& target)
"motor not found for joint: " + joint_names_[i]);
}
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to set cyclic position mode for joint: " +
joint_names_[i]);
}
}
motor->setTarget(target.position[i], 0.0);
motors.push_back(std::move(motor));
}
const std::vector<double> velocities(motors.size(), 0.0);
if (!motor_manager_->commandCyclicPositionsAtomic(motors, target.position, velocities)) {
return Result::failure(ArmErrorCode::CommandFailed,
"failed to submit atomic cyclic position command");
}
return Result::success();
}
@ -656,8 +1000,142 @@ std::vector<double> MotorRobotArm::readJointPosition_() const
return q_start;
}
Result MotorRobotArm::stopCartesianMotionAndWait_()
{
if (!cartesian_velocity_controller_) {
return Result::success();
}
if (cartesian_velocity_controller_->busy()) {
const auto stop_result = cartesian_velocity_controller_->stop();
if (!stop_result.ok()) {
return stop_result;
}
const auto deadline = std::chrono::steady_clock::now() +
std::chrono::duration<double>(
cartesian_velocity_controller_->stopTimeoutS());
while (cartesian_velocity_controller_->busy() &&
std::chrono::steady_clock::now() < deadline) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
if (cartesian_velocity_controller_->busy()) {
// Do not start a position trajectory while the worker can still
// issue velocity commands. Shutdown joins it and sends one final
// zero-velocity command before reporting the timeout.
cartesian_velocity_controller_->shutdown();
return Result::failure(
ArmErrorCode::Timeout,
"timed out waiting for Cartesian velocity motion to stop");
}
}
// The worker may be idle but still joinable. Joining it here removes any
// last command/worker race before the next motion mode is selected.
cartesian_velocity_controller_->shutdown();
return Result::success();
}
Result MotorRobotArm::waitForJointTarget_(const std::vector<double>& target) const
{
if (target.size() != joint_names_.size()) {
return Result::failure(ArmErrorCode::InvalidArgument,
"moveJ target size mismatch while settling");
}
const auto deadline = std::chrono::steady_clock::now() +
std::chrono::duration<double>(move_j_settle_timeout_s_);
int stable_samples = 0;
while (std::chrono::steady_clock::now() < deadline) {
if (const auto stopped = safetyStopResult_("moveJ", true)) {
return *stopped;
}
bool settled = true;
for (std::size_t i = 0; i < joint_names_.size(); ++i) {
const auto motor = getMotor_(joint_names_[i]);
if (!motor) {
return Result::failure(
ArmErrorCode::RobotNotReady,
"motor not found while waiting for moveJ target: " + joint_names_[i]);
}
const double position = motor->getQ();
const double velocity = motor->getQd();
if (!std::isfinite(position) || !std::isfinite(velocity) ||
std::abs(position - target[i]) > move_j_position_tolerance_rad_ ||
std::abs(velocity) > move_j_velocity_tolerance_rad_s_) {
settled = false;
break;
}
}
stable_samples = settled ? stable_samples + 1 : 0;
if (stable_samples >= move_j_stable_sample_count_) {
return Result::success();
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
CMVR_LOG(ERROR) << "[MotorRobotArm] moveJ target did not settle before timeout: " << id_;
return Result::failure(ArmErrorCode::Timeout,
"timed out waiting for moveJ target to settle");
}
bool MotorRobotArm::configureAlgorithms_()
{
// Optional settling fields are read once during initialization so every
// MoveJ command uses one consistent set of safety criteria.
move_j_settle_timeout_s_ = 2.0;
move_j_position_tolerance_rad_ = 2e-3;
move_j_velocity_tolerance_rad_s_ = 2e-2;
move_j_stable_sample_count_ = 3;
const auto& move_j_config = cfg_.motion().move_j();
if (move_j_config.has_settle_timeout_s()) {
const double value = move_j_config.settle_timeout_s();
if (!std::isfinite(value) || value <= 0.0) {
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_timeout_s: " << value;
return false;
}
move_j_settle_timeout_s_ = value;
}
if (move_j_config.has_settle_position_tolerance_rad()) {
const double value = move_j_config.settle_position_tolerance_rad();
if (!std::isfinite(value) || value < 0.0) {
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_position_tolerance_rad: "
<< value;
return false;
}
move_j_position_tolerance_rad_ = value;
}
if (move_j_config.has_settle_velocity_tolerance_rad_s()) {
const double value = move_j_config.settle_velocity_tolerance_rad_s();
if (!std::isfinite(value) || value < 0.0) {
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_velocity_tolerance_rad_s: "
<< value;
return false;
}
move_j_velocity_tolerance_rad_s_ = value;
}
if (move_j_config.has_settle_stable_sample_count()) {
const int value = move_j_config.settle_stable_sample_count();
if (value < 1) {
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_stable_sample_count: "
<< value;
return false;
}
move_j_stable_sample_count_ = value;
}
const auto& cartesian_controller_config =
cfg_.motion().speed_l().speed_l_controller().cartesian_velocity_controller();
if (cartesian_controller_config.has_stop_timeout_s() &&
(!std::isfinite(cartesian_controller_config.stop_timeout_s()) ||
cartesian_controller_config.stop_timeout_s() <= 0.0)) {
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid Cartesian stop_timeout_s: "
<< cartesian_controller_config.stop_timeout_s();
return false;
}
joint_planner_ = JointMotionPlannerFactory::create(cfg_.motion().move_j());
if (!joint_planner_) {
return false;
@ -725,7 +1203,9 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
return false;
}
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
if (!motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
return false;
}
}
motors.push_back(std::move(motor));
}
@ -733,14 +1213,17 @@ bool MotorRobotArm::executeMoveLTrajectory_(const CartesianJointTrajectory& traj
auto next_deadline = std::chrono::steady_clock::now();
for (std::size_t i = 1; i < trajectory.position.size(); ++i) {
if (safetyStopRequested_()) {
return false;
}
const double dt_segment = std::max(1e-4, trajectory.time[i] - trajectory.time[i - 1]);
const auto& position = trajectory.position[i];
const auto& velocity = trajectory.velocity[i];
if (position.size() != motors.size() || velocity.size() != motors.size()) {
return false;
}
for (std::size_t j = 0; j < motors.size(); ++j) {
motors[j]->setTarget(position[j], velocity[j]);
if (!motor_manager_->commandCyclicPositionsAtomic(motors, position, velocity)) {
return false;
}
next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(dt_segment));
@ -765,6 +1248,10 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController
result.stop_acceleration =
config.stop_acceleration() > 0.0 ? config.stop_acceleration()
: result.stop_acceleration;
if (config.has_stop_timeout_s() && std::isfinite(config.stop_timeout_s()) &&
config.stop_timeout_s() > 0.0) {
result.stop_timeout_s = config.stop_timeout_s();
}
return result;
}

871
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_gen2_mujoco_test.cpp Normal file
View File

@ -0,0 +1,871 @@
#include "arm/motor_robot_arm/include/motor_robot_arm.h"
#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <filesystem>
#include <functional>
#include <iostream>
#include <limits>
#include <memory>
#include <stdexcept>
#include <string>
#include <thread>
#include <utility>
#include <vector>
#include <Eigen/Geometry>
#include <gtest/gtest.h>
#include "common/io/proto_file_io.h"
#include "common/math/transform_math.h"
#include "manager/device_manager/include/device_manager.h"
#include "devices/motor/manager/include/motor_manager.h"
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
#include "task/self_collision_task/include/self_collision_task.h"
namespace cmvr::device {
namespace {
constexpr std::size_t kDof = 7;
constexpr std::array<const char*, kDof> kJointNames = {
"right_arm_J1", "right_arm_J2", "right_arm_J3", "right_arm_J4",
"right_arm_J5", "right_arm_J6", "right_arm_J7"
};
const std::vector<double> kSetupPose{
0.0, -0.50, 1.5708, 1.5708, -0.041, 0.0, 0.0
};
const std::vector<double> kTorsoCollisionPose{
1.57607137794121,
2.06613762981425,
-1.76915077905899,
0.959251437141443,
-0.725973209527894,
1.79390262120717,
0.2223354372144,
};
std::filesystem::path findProjectRoot()
{
const std::filesystem::path marker = "model/gen2/gen2_fixed.xml";
const auto search = [&](std::filesystem::path current) {
while (!current.empty()) {
if (std::filesystem::exists(current / marker)) {
return current;
}
const auto parent = current.parent_path();
if (parent == current) {
break;
}
current = parent;
}
return std::filesystem::path{};
};
auto root = search(std::filesystem::current_path());
if (!root.empty()) {
return root;
}
return search(std::filesystem::path(__FILE__).parent_path());
}
double maxPositionError(const std::vector<double>& actual,
const std::vector<double>& expected)
{
if (actual.size() != expected.size()) {
return std::numeric_limits<double>::infinity();
}
double error = 0.0;
for (std::size_t i = 0; i < actual.size(); ++i) {
error = std::max(error, std::abs(actual[i] - expected[i]));
}
return error;
}
double translationError(const CartesianPose& lhs, const CartesianPose& rhs)
{
return std::sqrt(std::pow(lhs.x - rhs.x, 2.0) +
std::pow(lhs.y - rhs.y, 2.0) +
std::pow(lhs.z - rhs.z, 2.0));
}
double rotationError(const CartesianPose& lhs, const CartesianPose& rhs)
{
const Eigen::Matrix3d lhs_rotation =
common::math::poseToMatrix(lhs).block<3, 3>(0, 0);
const Eigen::Matrix3d rhs_rotation =
common::math::poseToMatrix(rhs).block<3, 3>(0, 0);
return std::abs(Eigen::AngleAxisd(lhs_rotation.transpose() * rhs_rotation).angle());
}
Eigen::Vector3d baseRotationDelta(const CartesianPose& start, const CartesianPose& end)
{
const Eigen::Matrix3d start_rotation =
common::math::poseToMatrix(start).block<3, 3>(0, 0);
const Eigen::Matrix3d end_rotation =
common::math::poseToMatrix(end).block<3, 3>(0, 0);
const Eigen::AngleAxisd delta(end_rotation * start_rotation.transpose());
return delta.axis() * delta.angle();
}
template <class Predicate>
void waitFor(Predicate predicate, const std::chrono::milliseconds timeout)
{
const auto deadline = std::chrono::steady_clock::now() + timeout;
while (!predicate() && std::chrono::steady_clock::now() < deadline) {
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
}
struct ScenarioOutcome {
Result move_j{Result::failure(ArmErrorCode::UnknownError, "not run")};
Result move_l{Result::failure(ArmErrorCode::UnknownError, "not run")};
double move_j_error{std::numeric_limits<double>::infinity()};
double move_l_error{std::numeric_limits<double>::infinity()};
double move_l_rotation_error{std::numeric_limits<double>::infinity()};
std::string worker_error;
};
class MotorRobotArmGen2MujocoTest : public ::testing::Test {
protected:
void SetUp() override
{
DeviceManager::destroyInstance();
project_root_ = findProjectRoot();
ASSERT_FALSE(project_root_.empty());
config::MujocoWorldRootConfig world_root_config;
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
(project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(),
&world_root_config));
ASSERT_GT(world_root_config.worlds_size(), 0);
auto world_config = world_root_config.worlds(0);
world_config.set_model_path(
(project_root_ / "model/gen2/gen2_fixed.xml").string());
world_device_ = std::make_shared<simulate::MujocoWorldDevice>(world_config);
ASSERT_TRUE(world_device_->init());
ASSERT_TRUE(world_device_->start());
config::MotorRootConfig motor_root_config;
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
(project_root_ /
"cmvr-es/config/devices/motor/mujoco_motors_gen2.pb.txt").string(),
&motor_root_config));
motor_system_ = std::make_shared<MotorManager>(
"right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
ASSERT_TRUE(motor_system_->init());
world_ = MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
ASSERT_TRUE(world_);
ASSERT_TRUE(world_->isLoaded());
config::ArmRootConfig root_config;
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
(project_root_ /
"cmvr-es/config/devices/arm/arm_gen2_mujoco.pb.txt").string(),
&root_config));
ASSERT_GT(root_config.arm().robot_arms_size(), 0);
auto arm_config = root_config.arm().robot_arms(0);
arm_config.mutable_kinematics()
->mutable_pinocchio_dls_ik_solver()
->set_urdf_path((project_root_ / "model/gen2/robot.urdf").string());
arm_ = std::make_shared<MotorRobotArm>(arm_config);
ASSERT_TRUE(arm_->init());
const Result torque_result = arm_->torqueOn();
ASSERT_TRUE(torque_result.ok()) << torque_result.message;
config::DeviceManagerConfig device_manager_config;
device_manager_config.set_name("gen2_collision_mujoco_test");
DeviceManager::getInstance(device_manager_config).registerDevice(arm_);
}
void TearDown() override
{
if (arm_) {
arm_->stop();
}
if (motor_system_) {
motor_system_->stop();
}
if (world_device_) {
world_device_->stop();
}
DeviceManager::destroyInstance();
}
std::filesystem::path project_root_;
std::shared_ptr<simulate::MujocoWorldDevice> world_device_;
std::shared_ptr<MotorManager> motor_system_;
std::shared_ptr<simulate::MujocoWorld> world_;
std::shared_ptr<MotorRobotArm> arm_;
};
TEST_F(MotorRobotArmGen2MujocoTest, HoldsInitialPosition)
{
const auto start = arm_->getJointState().position;
ASSERT_EQ(start.size(), kDof);
std::this_thread::sleep_for(std::chrono::milliseconds(500));
const auto end = arm_->getJointState().position;
const double drift = maxPositionError(end, start);
std::cout << "[MotorRobotArmGen2MujocoTest] hold max drift: "
<< drift << std::endl;
EXPECT_LT(drift, 0.02);
}
TEST_F(MotorRobotArmGen2MujocoTest, ProtectiveAndEmergencyStopAreDistinct)
{
ASSERT_TRUE(arm_->protectiveStop().ok());
EXPECT_TRUE(arm_->isProtectiveStopped());
EXPECT_FALSE(arm_->isEmergencyStopped());
EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::ProtectiveStop);
const auto protective_state = arm_->getRobotState();
EXPECT_TRUE(protective_state.protective_stopped);
EXPECT_FALSE(protective_state.emergency_stopped);
MotionOptions options;
options.velocity = 0.6;
options.acceleration = 2.0;
const Result protected_move = arm_->moveJ(
JointPositionCommand{kSetupPose}, options);
EXPECT_EQ(protected_move.code, ArmErrorCode::RobotInProtectiveStop);
ASSERT_TRUE(arm_->unlockProtectiveStop().ok());
EXPECT_FALSE(arm_->isProtectiveStopped());
EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
ASSERT_TRUE(arm_->emergencyStop().ok());
EXPECT_FALSE(arm_->isProtectiveStopped());
EXPECT_TRUE(arm_->isEmergencyStopped());
EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::EmergencyStop);
const auto emergency_state = arm_->getRobotState();
EXPECT_FALSE(emergency_state.protective_stopped);
EXPECT_TRUE(emergency_state.emergency_stopped);
const Result rejected_unlock = arm_->unlockProtectiveStop();
EXPECT_EQ(rejected_unlock.code, ArmErrorCode::RobotInEmergencyStop);
ASSERT_TRUE(arm_->torqueOn().ok());
EXPECT_FALSE(arm_->isEmergencyStopped());
EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
}
TEST_F(MotorRobotArmGen2MujocoTest, MoveJ)
{
MuJocoViewer viewer(world_);
viewer.setupCamera(2.5, -160.0, -20.0);
ScenarioOutcome outcome;
std::thread scenario([&] {
try {
if (!world_ || !world_->isRunning()) {
throw std::runtime_error("MuJoCo world is not running");
}
std::this_thread::sleep_for(std::chrono::milliseconds(300));
MotionOptions options;
options.velocity = 0.6;
options.acceleration = 2.0;
outcome.move_j = arm_->moveJ(JointPositionCommand{kSetupPose}, options);
waitFor([&] {
return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
}, std::chrono::seconds(3));
outcome.move_j_error = maxPositionError(
arm_->getJointState().position, kSetupPose);
} catch (const std::exception& error) {
outcome.worker_error = error.what();
}
std::this_thread::sleep_for(std::chrono::seconds(2));
viewer.requestStop();
});
viewer.setRunning(true);
viewer.run();
scenario.join();
std::cout << "[MotorRobotArmGen2MujocoTest] moveJ max error: "
<< outcome.move_j_error << std::endl;
EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
EXPECT_LT(outcome.move_j_error, 0.08);
}
TEST_F(MotorRobotArmGen2MujocoTest, MoveL)
{
MuJocoViewer viewer(world_);
viewer.setupCamera(2.5, -160.0, -20.0);
ScenarioOutcome outcome;
std::thread scenario([&] {
try {
if (!world_ || !world_->isRunning()) {
throw std::runtime_error("MuJoCo world is not running");
}
std::this_thread::sleep_for(std::chrono::milliseconds(300));
MotionOptions joint_options;
joint_options.velocity = 1.6;
joint_options.acceleration = 12.0;
outcome.move_j = arm_->moveJ(
JointPositionCommand{kSetupPose}, joint_options);
waitFor([&] {
return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
}, std::chrono::seconds(3));
outcome.move_j_error = maxPositionError(
arm_->getJointState().position, kSetupPose);
if (!outcome.move_j.ok()) {
throw std::runtime_error(outcome.move_j.message);
}
MotionOptions cartesian_options;
cartesian_options.velocity = 0.08;
cartesian_options.acceleration = 0.4;
cartesian_options.jerk = 1.0;
MotionOptions rotation_options;
rotation_options.velocity = 0.15;
rotation_options.acceleration = 0.5;
rotation_options.jerk = 2.0;
const auto return_to_setup = [&](const char* step_name) {
outcome.move_j = arm_->moveJ(
JointPositionCommand{kSetupPose}, joint_options);
if (!outcome.move_j.ok()) {
throw std::runtime_error(
std::string("moveJ before moveL ") + step_name +
": " + outcome.move_j.message);
}
waitFor([&] {
return maxPositionError(
arm_->getJointState().position, kSetupPose) < 0.04;
}, std::chrono::seconds(3));
std::this_thread::sleep_for(std::chrono::milliseconds(300));
};
struct CartesianStep {
const char* name;
double dx;
double dy;
double dz;
};
const std::array<CartesianStep, 3> translation_steps{{
{"+X", 0.15, 0.0, 0.0},
{"+Y", 0.0, 0.15, 0.0},
{"+Z", 0.0, 0.0, 0.15},
}};
struct RotationStep {
const char* name;
double drx;
double dry;
double drz;
};
constexpr double kRotationStep =
20.0 * 3.14159265358979323846 / 180.0;
const std::array<RotationStep, 3> rotation_steps{{
{"+RX", kRotationStep, 0.0, 0.0},
{"+RY", 0.0, kRotationStep, 0.0},
{"+RZ", 0.0, 0.0, kRotationStep},
}};
outcome.move_l_error = 0.0;
for (std::size_t i = 0; i < translation_steps.size(); ++i) {
const auto& step = translation_steps[i];
if (i > 0) {
return_to_setup(step.name);
}
CartesianPose target = arm_->getTcpPose();
target.x += step.dx;
target.y += step.dy;
target.z += step.dz;
outcome.move_l = arm_->moveL(
target, cartesian_options, FrameType::Base);
if (!outcome.move_l.ok()) {
throw std::runtime_error(
std::string("moveL ") + step.name + ": " +
outcome.move_l.message);
}
waitFor([&] {
return translationError(arm_->getTcpPose(), target) < 0.005;
}, std::chrono::seconds(5));
const double error = translationError(arm_->getTcpPose(), target);
outcome.move_l_error = std::max(outcome.move_l_error, error);
std::cout << "[MotorRobotArmGen2MujocoTest] moveL "
<< step.name << " translation error: " << error
<< std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
outcome.move_l_rotation_error = 0.0;
for (const auto& step : rotation_steps) {
return_to_setup(step.name);
CartesianPose target = arm_->getTcpPose();
target.rx += step.drx;
target.ry += step.dry;
target.rz += step.drz;
outcome.move_l = arm_->moveL(
target, rotation_options, FrameType::Base);
if (!outcome.move_l.ok()) {
throw std::runtime_error(
std::string("moveL ") + step.name + ": " +
outcome.move_l.message);
}
waitFor([&] {
return rotationError(arm_->getTcpPose(), target) < 0.01;
}, std::chrono::seconds(7));
const double error = rotationError(arm_->getTcpPose(), target);
outcome.move_l_rotation_error = std::max(
outcome.move_l_rotation_error, error);
std::cout << "[MotorRobotArmGen2MujocoTest] moveL "
<< step.name << " rotation error: " << error
<< " rad" << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
} catch (const std::exception& error) {
outcome.worker_error = error.what();
}
std::this_thread::sleep_for(std::chrono::seconds(3));
viewer.requestStop();
});
viewer.setRunning(true);
viewer.run();
scenario.join();
std::cout << "[MotorRobotArmGen2MujocoTest] moveJ setup max error: "
<< outcome.move_j_error
<< ", moveL translation error: " << outcome.move_l_error
<< ", moveL rotation error: "
<< outcome.move_l_rotation_error << " rad" << std::endl;
EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
EXPECT_LT(outcome.move_j_error, 0.08);
EXPECT_TRUE(outcome.move_l.ok()) << outcome.move_l.message;
EXPECT_LT(outcome.move_l_error, 0.01);
EXPECT_LT(outcome.move_l_rotation_error, 0.02);
}
TEST_F(MotorRobotArmGen2MujocoTest, SelfCollisionProtectiveStopMoveJMoveLSpeedL)
{
MuJocoViewer viewer(world_);
viewer.setupCamera(2.5, -160.0, -20.0);
struct CollisionCaseOutcome {
std::string name;
Result setup_move{Result::failure(ArmErrorCode::UnknownError, "not run")};
Result collision_move{Result::failure(ArmErrorCode::UnknownError, "not run")};
Result recovery{Result::failure(ArmErrorCode::UnknownError, "not run")};
task::SelfCollisionTaskStatus initial_status;
task::SelfCollisionTaskStatus stop_status;
task::SelfCollisionTaskStatus recovered_status;
CartesianPose start_tcp;
CartesianPose final_tcp;
std::vector<double> final_position;
bool task_initialized{false};
bool task_started{false};
bool stop_seen{false};
bool recovery_succeeded{false};
bool monitor_ok{true};
bool protective_stopped{false};
bool emergency_stopped{false};
SafetyMode safety_mode{SafetyMode::Unknown};
};
CollisionCaseOutcome move_j_outcome;
CollisionCaseOutcome move_l_outcome;
CollisionCaseOutcome speed_l_outcome;
CartesianPose move_l_target;
CartesianPose collision_tcp_target;
std::string worker_error;
std::thread scenario([&] {
try {
std::this_thread::sleep_for(std::chrono::milliseconds(300));
config::SelfCollisionTaskRootConfig root_config;
const auto config_path = project_root_ /
"cmvr-es/config/tasks/self_collision_task/"
"self_collision_task_gen2.pb.txt";
if (!ProtoMessageIo::getProtoFromAsciiFile(
config_path.string(), &root_config)) {
throw std::runtime_error(
"failed to load self-collision config: " + config_path.string());
}
auto collision_config = root_config.self_collision_task();
collision_config.mutable_checker()->set_urdf_path(
(project_root_ /
"model/gen2/collision/robot_collision.urdf").string());
Eigen::Matrix4d collision_tcp_transform = Eigen::Matrix4d::Identity();
const auto solver = arm_->kinematicsSolver();
if (!solver ||
!solver->fk(kTorsoCollisionPose, collision_tcp_transform, true)) {
throw std::runtime_error("failed to calculate collision TCP target");
}
collision_tcp_target =
common::math::matrixToPose(collision_tcp_transform);
const auto run_collision_case = [&](
const std::string& name,
const std::function<Result()>& start_motion,
const std::chrono::milliseconds stop_timeout) {
CollisionCaseOutcome outcome;
outcome.name = name;
const Result torque_result = arm_->torqueOn();
if (!torque_result.ok()) {
throw std::runtime_error(
name + " torqueOn: " + torque_result.message);
}
MotionOptions setup_options;
setup_options.velocity = 0.6;
setup_options.acceleration = 2.0;
outcome.setup_move = arm_->moveJ(
JointPositionCommand{kSetupPose}, setup_options);
if (!outcome.setup_move.ok()) {
throw std::runtime_error(
name + " setup moveJ: " + outcome.setup_move.message);
}
task::SelfCollisionTask collision_task(collision_config);
outcome.task_initialized = collision_task.init();
if (!outcome.task_initialized) {
throw std::runtime_error(
name + " init: " + collision_task.detailStatusString());
}
outcome.task_started = collision_task.start();
if (!outcome.task_started || !collision_task.step(0.002)) {
throw std::runtime_error(
name + " start: " + collision_task.detailStatusString());
}
outcome.initial_status = collision_task.latestStatus();
if (outcome.initial_status.level != task::CollisionSafetyLevel::SAFE) {
throw std::runtime_error(
name + " setup pose is not SAFE: " +
collision_task.detailStatusString());
}
outcome.start_tcp = arm_->getTcpPose();
std::atomic_bool monitor_running{true};
std::atomic_bool monitor_ok{true};
std::atomic_bool stop_seen{false};
std::thread monitor([&] {
while (monitor_running.load()) {
if (!collision_task.step(0.002)) {
monitor_ok = false;
break;
}
const auto status = collision_task.latestStatus();
if (status.stop_latched && !stop_seen.load()) {
outcome.stop_status = status;
stop_seen.store(true);
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
}
});
outcome.collision_move = start_motion();
waitFor([&] {
return stop_seen.load() || !monitor_ok.load();
}, stop_timeout);
if (!stop_seen.load()) {
arm_->stopMotion();
monitor_running = false;
monitor.join();
collision_task.stop();
throw std::runtime_error(name + " did not trigger protective stop");
}
outcome.recovery = collision_task.requestRecovery(
outcome.stop_status.event_id);
outcome.recovered_status = collision_task.latestStatus();
outcome.recovery_succeeded = outcome.recovery.ok();
monitor_running = false;
monitor.join();
collision_task.stop();
outcome.stop_seen = stop_seen.load();
outcome.monitor_ok = monitor_ok.load();
outcome.final_position = arm_->getJointState().position;
outcome.final_tcp = arm_->getTcpPose();
outcome.protective_stopped = arm_->isProtectiveStopped();
outcome.emergency_stopped = arm_->isEmergencyStopped();
outcome.safety_mode = arm_->getSafetyMode();
std::cout << "[MotorRobotArmGen2MujocoTest] collision "
<< name
<< " stop_seen=" << outcome.stop_seen
<< ", distance_m="
<< outcome.stop_status.result.minimum_distance_m
<< ", pair=" << outcome.stop_status.result.first
<< "/" << outcome.stop_status.result.second
<< ", event_id=" << outcome.stop_status.event_id
<< ", recovery=" << outcome.recovery.message
<< ", recovered_distance_m="
<< outcome.recovered_status.result.minimum_distance_m
<< std::endl;
return outcome;
};
move_j_outcome = run_collision_case(
"MoveJ",
[&] {
MotionOptions options;
options.velocity = 0.45;
options.acceleration = 1.0;
return arm_->moveJ(
JointPositionCommand{kTorsoCollisionPose}, options);
},
std::chrono::seconds(3));
std::this_thread::sleep_for(std::chrono::seconds(1));
move_l_outcome = run_collision_case(
"MoveL",
[&] {
move_l_target = collision_tcp_target;
MotionOptions options;
options.velocity = 0.12;
options.acceleration = 0.5;
options.jerk = 2.0;
return arm_->moveL(
move_l_target, options, FrameType::Base);
},
std::chrono::seconds(3));
std::this_thread::sleep_for(std::chrono::seconds(1));
speed_l_outcome = run_collision_case(
"SpeedL",
[&] {
const CartesianPose start = arm_->getTcpPose();
Eigen::Vector3d linear_direction{
collision_tcp_target.x - start.x,
collision_tcp_target.y - start.y,
collision_tcp_target.z - start.z,
};
Eigen::Vector3d angular_direction =
baseRotationDelta(start, collision_tcp_target);
const double command_duration_s = std::max(
linear_direction.norm() / 0.05,
angular_direction.norm() / 0.20);
if (command_duration_s <= 0.0) {
return Result::failure(
ArmErrorCode::InvalidArgument,
"SpeedL collision target has zero displacement");
}
linear_direction /= command_duration_s;
angular_direction /= command_duration_s;
std::cout
<< "[MotorRobotArmGen2MujocoTest] collision SpeedL target_time="
<< command_duration_s << " s" << std::endl;
return arm_->speedL(
CartesianVelocity{
linear_direction.x(),
linear_direction.y(),
linear_direction.z(),
angular_direction.x(),
angular_direction.y(),
angular_direction.z(),
},
0.5,
0.0,
FrameType::Base);
},
std::chrono::seconds(15));
} catch (const std::exception& error) {
worker_error = error.what();
}
std::this_thread::sleep_for(std::chrono::seconds(3));
viewer.requestStop();
});
viewer.setRunning(true);
viewer.run();
scenario.join();
EXPECT_TRUE(worker_error.empty()) << worker_error;
const auto expect_protective_stop = [&](const CollisionCaseOutcome& outcome) {
EXPECT_TRUE(outcome.setup_move.ok())
<< outcome.name << ": " << outcome.setup_move.message;
EXPECT_TRUE(outcome.task_initialized) << outcome.name;
EXPECT_TRUE(outcome.task_started) << outcome.name;
EXPECT_EQ(outcome.initial_status.level, task::CollisionSafetyLevel::SAFE)
<< outcome.name;
EXPECT_TRUE(outcome.monitor_ok) << outcome.name;
EXPECT_TRUE(outcome.stop_seen) << outcome.name;
EXPECT_EQ(outcome.stop_status.level, task::CollisionSafetyLevel::STOP)
<< outcome.name;
EXPECT_TRUE(outcome.stop_status.stop_latched) << outcome.name;
EXPECT_NE(outcome.stop_status.event_id, 0U) << outcome.name;
EXPECT_EQ(outcome.stop_status.recovery_state,
task::ProtectiveRecoveryState::AVAILABLE)
<< outcome.name;
EXPECT_GE(outcome.stop_status.recovery_sample_count, 2U)
<< outcome.name;
EXPECT_LE(outcome.stop_status.result.minimum_distance_m, 0.005)
<< outcome.name;
EXPECT_TRUE(outcome.stop_status.result.first == "body_link" ||
outcome.stop_status.result.second == "body_link")
<< outcome.name;
EXPECT_TRUE(outcome.recovery_succeeded)
<< outcome.name << ": " << outcome.recovery.message;
EXPECT_FALSE(outcome.recovered_status.stop_latched) << outcome.name;
EXPECT_EQ(outcome.recovered_status.recovery_state,
task::ProtectiveRecoveryState::SUCCEEDED)
<< outcome.name;
EXPECT_GE(outcome.recovered_status.result.minimum_distance_m, 0.025)
<< outcome.name;
EXPECT_FALSE(outcome.protective_stopped) << outcome.name;
EXPECT_FALSE(outcome.emergency_stopped) << outcome.name;
EXPECT_EQ(outcome.safety_mode, SafetyMode::Normal)
<< outcome.name;
};
expect_protective_stop(move_j_outcome);
expect_protective_stop(move_l_outcome);
expect_protective_stop(speed_l_outcome);
EXPECT_EQ(move_j_outcome.collision_move.code,
ArmErrorCode::RobotInProtectiveStop);
EXPECT_GT(maxPositionError(move_j_outcome.final_position, kTorsoCollisionPose), 0.02);
EXPECT_EQ(move_l_outcome.collision_move.code,
ArmErrorCode::RobotInProtectiveStop);
EXPECT_GT(translationError(move_l_outcome.final_tcp, move_l_target), 0.01);
EXPECT_TRUE(speed_l_outcome.collision_move.ok())
<< speed_l_outcome.collision_move.message;
EXPECT_EQ(arm_->getSafetyMode(), SafetyMode::Normal);
}
TEST_F(MotorRobotArmGen2MujocoTest, SpeedL)
{
constexpr auto kCommandDuration = std::chrono::seconds(2);
MuJocoViewer viewer(world_);
viewer.setupCamera(2.5, -160.0, -20.0);
ScenarioOutcome outcome;
std::array<double, 6> measured_deltas{};
std::thread scenario([&] {
try {
if (!world_ || !world_->isRunning()) {
throw std::runtime_error("MuJoCo world is not running");
}
std::this_thread::sleep_for(std::chrono::milliseconds(300));
MotionOptions joint_options;
joint_options.velocity = 0.6;
joint_options.acceleration = 2.0;
outcome.move_j = arm_->moveJ(
JointPositionCommand{kSetupPose}, joint_options);
waitFor([&] {
return maxPositionError(arm_->getJointState().position, kSetupPose) < 0.04;
}, std::chrono::seconds(3));
outcome.move_j_error = maxPositionError(
arm_->getJointState().position, kSetupPose);
if (!outcome.move_j.ok()) {
throw std::runtime_error(outcome.move_j.message);
}
struct SpeedStep {
const char* name;
CartesianVelocity command;
bool angular;
std::size_t axis;
};
const std::array<SpeedStep, 6> steps{{
{"+X", CartesianVelocity{0.05, 0.0, 0.0, 0.0, 0.0, 0.0}, false, 0},
{"+Y", CartesianVelocity{0.0, 0.05, 0.0, 0.0, 0.0, 0.0}, false, 1},
{"+Z", CartesianVelocity{0.0, 0.0, 0.05, 0.0, 0.0, 0.0}, false, 2},
{"+RX", CartesianVelocity{0.0, 0.0, 0.0, 0.30, 0.0, 0.0}, true, 0},
{"+RY", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.30, 0.0}, true, 1},
{"+RZ", CartesianVelocity{0.0, 0.0, 0.0, 0.0, 0.0, 0.30}, true, 2},
}};
for (std::size_t i = 0; i < steps.size(); ++i) {
const auto& step = steps[i];
if (i > 0) {
outcome.move_j = arm_->moveJ(
JointPositionCommand{kSetupPose}, joint_options);
if (!outcome.move_j.ok()) {
throw std::runtime_error(
std::string("moveJ before speedL ") + step.name +
": " + outcome.move_j.message);
}
waitFor([&] {
return maxPositionError(
arm_->getJointState().position, kSetupPose) < 0.04;
}, std::chrono::seconds(3));
std::this_thread::sleep_for(std::chrono::milliseconds(300));
}
const CartesianPose start = arm_->getTcpPose();
const Result speed_result = arm_->speedL(
step.command, 0.5, 0.0, FrameType::Base);
if (!speed_result.ok()) {
throw std::runtime_error(
std::string("speedL ") + step.name + ": " +
speed_result.message);
}
std::this_thread::sleep_for(kCommandDuration);
const Result stop_result = arm_->stopL(0.5);
if (!stop_result.ok()) {
throw std::runtime_error(
std::string("stopL ") + step.name + ": " +
stop_result.message);
}
waitFor([&] { return !arm_->busy(); }, std::chrono::seconds(3));
const CartesianPose end = arm_->getTcpPose();
if (step.angular) {
measured_deltas[i] = baseRotationDelta(start, end)[step.axis];
std::cout << "[MotorRobotArmGen2MujocoTest] speedL "
<< step.name << " rotation delta: "
<< measured_deltas[i] << " rad" << std::endl;
} else {
const Eigen::Vector3d translation_delta{
end.x - start.x, end.y - start.y, end.z - start.z};
measured_deltas[i] = translation_delta[step.axis];
std::cout << "[MotorRobotArmGen2MujocoTest] speedL "
<< step.name << " translation delta: "
<< measured_deltas[i] << " m" << std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
}
} catch (const std::exception& error) {
outcome.worker_error = error.what();
}
std::this_thread::sleep_for(std::chrono::seconds(3));
viewer.requestStop();
});
viewer.setRunning(true);
viewer.run();
scenario.join();
EXPECT_TRUE(outcome.worker_error.empty()) << outcome.worker_error;
EXPECT_TRUE(outcome.move_j.ok()) << outcome.move_j.message;
EXPECT_LT(outcome.move_j_error, 0.08);
for (std::size_t i = 0; i < 3; ++i) {
EXPECT_GT(measured_deltas[i], 0.02);
}
for (std::size_t i = 3; i < measured_deltas.size(); ++i) {
EXPECT_GT(measured_deltas[i], 0.10);
}
}
} // namespace
} // namespace cmvr::device

15
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_mujoco_test.cpp
View File

@ -10,7 +10,6 @@
#include <memory>
#include <string>
#include <thread>
#include <unordered_set>
#include <utility>
#include <vector>
@ -147,17 +146,10 @@ protected:
(project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(),
&motor_root_config));
std::unordered_set<std::string> right_arm_joints;
for (const auto* joint_name : kJointNames) {
right_arm_joints.insert(joint_name);
}
MotorManager::clearActiveJoints();
MotorManager::setActiveJoints(
"mujoco_motors", {{"mujoco_right_arm", std::move(right_arm_joints)}});
motor_system_ = std::make_shared<MotorManager>("mujoco_motors", motor_root_config.motor());
motor_system_ = std::make_shared<MotorManager>(
"right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
ASSERT_NO_THROW(motor_system_->init());
world_ = MotorManager::mujocoWorldFor("mujoco_motors");
world_ = MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
ASSERT_TRUE(world_);
ASSERT_TRUE(world_->isLoaded());
@ -187,7 +179,6 @@ protected:
if (world_device_) {
world_device_->stop();
}
MotorManager::clearActiveJoints();
}
std::filesystem::path project_root_;

352
cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_speedl_stop_test.cpp Normal file
View File

@ -0,0 +1,352 @@
#include "arm/motor_robot_arm/include/motor_robot_arm.h"
#include <algorithm>
#include <array>
#include <atomic>
#include <chrono>
#include <cmath>
#include <filesystem>
#include <fstream>
#include <iomanip>
#include <iostream>
#include <limits>
#include <memory>
#include <sstream>
#include <string>
#include <thread>
#include <vector>
#include <gtest/gtest.h>
#include "algorithms/kinematics/ik_solver/ik_solver_factory.h"
#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.h"
#include "common/io/proto_file_io.h"
#include "devices/motor/manager/include/motor_manager.h"
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
namespace cmvr::device {
namespace {
constexpr std::size_t kDof = 7;
constexpr std::array<const char*, kDof> kJointNames = {
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R",
"R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
constexpr std::array<double, kDof> kInitialJointPosition = {
-0.2423, 1.2929, 1.61, 1.58, -2.8792, 0.1150, -0.08};
constexpr std::array<double, 7> kAccelerationSweep = {
0.1, 0.3, 0.5, 1.0, 2.0, 3.0, 5.0};
constexpr double kCommandVelocity = 0.04;
constexpr double kCommandDurationS = 2.0;
constexpr double kSamplePeriodS = 0.001;
std::filesystem::path findProjectRoot()
{
const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml";
auto search = [&](std::filesystem::path current) {
while (!current.empty()) {
if (std::filesystem::exists(current / marker)) {
return current;
}
const auto parent = current.parent_path();
if (parent == current) {
break;
}
current = parent;
}
return std::filesystem::path{};
};
auto root = search(std::filesystem::current_path());
if (!root.empty()) {
return root;
}
return search(std::filesystem::path(__FILE__).parent_path());
}
void setKinematicsUrdfPath(config::RobotArmConfig& arm_config,
const std::string& urdf_path)
{
switch (arm_config.kinematics().algorithm_case()) {
case config::ArmKinematicsConfig::kPinocchioDlsIkSolver:
arm_config.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path(urdf_path);
break;
case config::ArmKinematicsConfig::kPinocchioQpIkSolver:
arm_config.mutable_kinematics()->mutable_pinocchio_qp_ik_solver()->set_urdf_path(urdf_path);
break;
default:
break;
}
}
struct Sample {
double time_s{0.0};
std::vector<double> q;
std::vector<double> qd;
CartesianVelocity commanded_twist;
};
struct Metrics {
double max_qd_before_stop{0.0};
double max_qd_after_stop{0.0};
double max_qdd{0.0};
double max_tcp_linear_speed_before_stop{0.0};
double max_tcp_linear_speed_after_stop{0.0};
double max_tcp_linear_acceleration{0.0};
};
double vectorNorm(const std::vector<double>& value)
{
double sum = 0.0;
for (const double item : value) {
sum += item * item;
}
return std::sqrt(sum);
}
double linearSpeed(const Eigen::Matrix<double, 6, 1>& twist)
{
return twist.head<3>().norm();
}
std::string numberForFile(const double value)
{
std::ostringstream stream;
stream << std::fixed << std::setprecision(3) << value;
auto result = stream.str();
std::replace(result.begin(), result.end(), '.', '_');
return result;
}
void writeCsv(const std::filesystem::path& path,
const std::vector<Sample>& samples,
const double stop_time_s,
const cmvr::PinocchioIKBase& solver,
Metrics& metrics)
{
std::ofstream output(path);
ASSERT_TRUE(output.is_open()) << "failed to open CSV: " << path;
output << "time_s,phase";
for (const auto* name : kJointNames) {
output << "," << name << "_q";
}
for (const auto* name : kJointNames) {
output << "," << name << "_qd";
}
output << ",command_vx,command_vy,command_vz,command_wx,command_wy,command_wz"
<< ",tcp_vx,tcp_vy,tcp_vz,tcp_wx,tcp_wy,tcp_wz,tcp_linear_speed,tcp_linear_acceleration";
output << '\n';
Eigen::Matrix<double, 6, 1> previous_tcp_twist = Eigen::Matrix<double, 6, 1>::Zero();
bool have_previous_tcp = false;
for (const auto& sample : samples) {
Eigen::Matrix<double, 6, 1> tcp_twist = Eigen::Matrix<double, 6, 1>::Zero();
ASSERT_TRUE(solver.computeTwistBaseAtQ(sample.q, sample.qd, true, tcp_twist));
const bool after_stop = stop_time_s >= 0.0 && sample.time_s >= stop_time_s;
const double qd_norm = vectorNorm(sample.qd);
const double tcp_speed = linearSpeed(tcp_twist);
double tcp_acceleration = 0.0;
if (have_previous_tcp) {
tcp_acceleration = (tcp_twist.head<3>() - previous_tcp_twist.head<3>()).norm() /
std::max(kSamplePeriodS, sample.time_s -
(samples[&sample - samples.data() - 1].time_s));
}
previous_tcp_twist = tcp_twist;
have_previous_tcp = true;
if (after_stop) {
metrics.max_qd_after_stop = std::max(metrics.max_qd_after_stop, qd_norm);
metrics.max_tcp_linear_speed_after_stop =
std::max(metrics.max_tcp_linear_speed_after_stop, tcp_speed);
} else {
metrics.max_qd_before_stop = std::max(metrics.max_qd_before_stop, qd_norm);
metrics.max_tcp_linear_speed_before_stop =
std::max(metrics.max_tcp_linear_speed_before_stop, tcp_speed);
}
metrics.max_tcp_linear_acceleration =
std::max(metrics.max_tcp_linear_acceleration, tcp_acceleration);
if (&sample != samples.data()) {
const auto& previous = samples[&sample - samples.data() - 1];
const double dt = std::max(kSamplePeriodS, sample.time_s - previous.time_s);
for (std::size_t i = 0; i < kDof; ++i) {
metrics.max_qdd = std::max(metrics.max_qdd,
std::abs(sample.qd[i] - previous.qd[i]) / dt);
}
}
output << std::setprecision(9) << sample.time_s << ',' << (after_stop ? "stop" : "run");
for (const double value : sample.q) {
output << ',' << value;
}
for (const double value : sample.qd) {
output << ',' << value;
}
output << ',' << sample.commanded_twist.vx
<< ',' << sample.commanded_twist.vy
<< ',' << sample.commanded_twist.vz
<< ',' << sample.commanded_twist.wx
<< ',' << sample.commanded_twist.wy
<< ',' << sample.commanded_twist.wz;
for (Eigen::Index i = 0; i < 6; ++i) {
output << ',' << tcp_twist[i];
}
output << ',' << tcp_speed << ',' << tcp_acceleration << '\n';
}
}
class MotorRobotArmSpeedLStopTest : public ::testing::Test {
protected:
void SetUp() override
{
project_root_ = findProjectRoot();
ASSERT_FALSE(project_root_.empty());
config::MujocoWorldRootConfig world_root_config;
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
(project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(),
&world_root_config));
ASSERT_GT(world_root_config.worlds_size(), 0);
auto world_config = world_root_config.worlds(0);
world_config.set_model_path(
(project_root_ / "model/xiaoyan_description/dual_arm.xml").string());
world_device_ = std::make_shared<simulate::MujocoWorldDevice>(world_config);
ASSERT_TRUE(world_device_->init());
ASSERT_TRUE(world_device_->start());
config::MotorRootConfig motor_root_config;
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
(project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(),
&motor_root_config));
motor_system_ = std::make_shared<MotorManager>(
"right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors");
ASSERT_TRUE(motor_system_->init());
config::ArmRootConfig root_config;
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
(project_root_ / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt").string(),
&root_config));
ASSERT_GT(root_config.arm().robot_arms_size(), 0);
arm_config_ = root_config.arm().robot_arms(0);
setKinematicsUrdfPath(
arm_config_, (project_root_ / "model/xiaoyan_description/dual_arm.urdf").string());
arm_ = std::make_unique<MotorRobotArm>(arm_config_);
ASSERT_TRUE(arm_->init());
analysis_solver_ = IKSolverFactory::create(arm_config_.kinematics());
ASSERT_TRUE(analysis_solver_);
ASSERT_TRUE(analysis_solver_->init());
analysis_pinocchio_solver_ = std::dynamic_pointer_cast<cmvr::PinocchioIKBase>(analysis_solver_);
ASSERT_TRUE(analysis_pinocchio_solver_);
}
void TearDown() override
{
if (arm_) {
arm_->stop();
}
if (motor_system_) {
motor_system_->stop();
}
if (world_device_) {
world_device_->stop();
}
}
std::filesystem::path project_root_;
config::RobotArmConfig arm_config_;
std::shared_ptr<simulate::MujocoWorldDevice> world_device_;
std::shared_ptr<MotorManager> motor_system_;
std::unique_ptr<MotorRobotArm> arm_;
std::shared_ptr<cmvr::IKSolver> analysis_solver_;
std::shared_ptr<cmvr::PinocchioIKBase> analysis_pinocchio_solver_;
};
TEST_F(MotorRobotArmSpeedLStopTest, SweepAccelerationAndDirection)
{
ASSERT_TRUE(world_device_);
ASSERT_TRUE(world_device_->world());
const std::vector<double> initial(kInitialJointPosition.begin(), kInitialJointPosition.end());
MotionOptions move_options;
move_options.velocity = 2.0;
move_options.acceleration = 3.0;
ASSERT_TRUE(arm_->moveJ(JointPositionCommand{initial}, move_options).ok());
for (const double acceleration : kAccelerationSweep) {
for (const double direction : {1.0, -1.0}) {
ASSERT_FALSE(arm_->busy());
std::vector<Sample> samples;
samples.reserve(5000);
std::atomic<double> stop_time_s{-1.0};
Result command_result = Result::failure(ArmErrorCode::UnknownError, "not run");
const auto start_time = std::chrono::steady_clock::now();
std::thread command_thread([&] {
CartesianVelocity velocity;
velocity.vx = direction * kCommandVelocity;
command_result = arm_->speedL(velocity,
acceleration,
kCommandDurationS,
FrameType::Base);
stop_time_s.store(std::chrono::duration<double>(
std::chrono::steady_clock::now() - start_time).count());
});
while (std::chrono::duration<double>(std::chrono::steady_clock::now() - start_time).count() < 5.0) {
const double time_s = std::chrono::duration<double>(
std::chrono::steady_clock::now() - start_time).count();
const auto state = arm_->getJointState();
ASSERT_EQ(state.position.size(), kDof);
ASSERT_EQ(state.velocity.size(), kDof);
samples.push_back(Sample{time_s,
state.position,
state.velocity,
arm_->getSpeedLCommandTwistBase()});
const double stop = stop_time_s.load();
if (stop >= 0.0 && time_s > stop + 0.8 && !arm_->busy()) {
break;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
command_thread.join();
ASSERT_TRUE(command_result.ok()) << command_result.message;
const double stop = stop_time_s.load();
ASSERT_GT(stop, 1.8);
ASSERT_LT(stop, 2.5);
ASSERT_FALSE(arm_->busy());
const auto csv_path = std::filesystem::path("/tmp") /
("speedl_stop_acc_" + numberForFile(acceleration) +
"_" + (direction > 0.0 ? "pos" : "neg") + ".csv");
Metrics metrics;
writeCsv(csv_path, samples, stop, *analysis_pinocchio_solver_, metrics);
std::cout << "[SpeedLStopTest] acceleration=" << acceleration
<< ", direction=" << direction
<< ", csv=" << csv_path
<< ", max_qd_before=" << metrics.max_qd_before_stop
<< ", max_qd_after=" << metrics.max_qd_after_stop
<< ", max_qdd=" << metrics.max_qdd
<< ", max_tcp_speed_before=" << metrics.max_tcp_linear_speed_before_stop
<< ", max_tcp_speed_after=" << metrics.max_tcp_linear_speed_after_stop
<< ", max_tcp_acceleration=" << metrics.max_tcp_linear_acceleration
<< std::endl;
// Stopping must not create a new velocity peak. A small tolerance
// allows one 1 ms feedback sample of transport jitter.
EXPECT_LE(metrics.max_qd_after_stop,
metrics.max_qd_before_stop + 0.25)
<< "post-stop joint velocity peak for acceleration=" << acceleration
<< ", direction=" << direction;
EXPECT_LE(metrics.max_tcp_linear_speed_after_stop,
metrics.max_tcp_linear_speed_before_stop + 0.01)
<< "post-stop TCP velocity peak for acceleration=" << acceleration
<< ", direction=" << direction;
}
}
}
} // namespace
} // namespace cmvr::device

3
cmvr-es/devices/arm/robot_arm.h
View File

@ -34,6 +34,9 @@ public:
virtual Result emergencyStop() = 0;
virtual Result protectiveStop() = 0;
virtual Result recoverProtectiveStop(
const JointTrajectory& path,
const MotionOptions& options) = 0;
virtual Result setSpeedScaling(double scaling) = 0;
virtual double getSpeedScaling() const = 0;
virtual bool isProtectiveStopped() const = 0;

27
cmvr-es/devices/camera/abstract_camera.h
View File

@ -3,20 +3,22 @@
#pragma once
#include <opencv2/opencv.hpp>
#include <mutex>
#include "../abstract_device.h"
#include <Eigen/Core>
#include "cmvr/config/camera_config/camera_config.pb.h"
#include "devices/camera/common/include/camera_stream_overlay.h"
namespace cmvr::device {
enum CameraMode {PHOTO_MODE, VIDEO_MODE};
struct Rs2Intrinsics
{
float cx;
float cy;
float fx;
float fy;
float coeffs[5];
float cx{0.0F};
float cy{0.0F};
float fx{0.0F};
float fy{0.0F};
float coeffs[5]{};
};
struct StreamFrameData
@ -64,11 +66,26 @@ namespace cmvr::device {
return false;
}
// Video overlay is a presentation-only snapshot. It is deliberately
// kept on the camera so the encoding thread can consume it without
// coupling the camera to AprilTag or task code.
void setStreamOverlay(const CameraStreamOverlay& overlay) {
std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
stream_overlay_ = overlay;
}
CameraStreamOverlay streamOverlay() const {
std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
return stream_overlay_;
}
virtual bool startStreaming() {return true;}
virtual void stopStreaming() {}
virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
protected:
CameraState state_{};
mutable std::mutex stream_overlay_mutex_;
CameraStreamOverlay stream_overlay_{};
void clear_error_() {
this->state_.is_error = false;
this->state_.error_message.clear();

26
cmvr-es/devices/camera/common/include/camera_stream_encoder.h
View File

@ -7,9 +7,12 @@
#include <opencv2/opencv.hpp>
#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
#include "devices/camera/common/include/camera_stream_overlay.h"
namespace cmvr::device {
struct Rs2Intrinsics;
struct FfmpegEncoderInfo {
std::string codec_name;
int width = 0;
@ -27,8 +30,23 @@ struct FfmpegEncoderInfo {
struct CameraStreamEncodeOptions {
bool draw_timestamp = false;
CameraStreamOverlay overlay;
};
// Draw a frame whose pose is expressed as ^C T_Frame onto a BGR/BGRA image.
// The image is modified in place and no camera/perception state is touched.
void drawCoordinateFrame(cv::Mat& image,
const Eigen::Matrix4d& T_C_Frame,
const Rs2Intrinsics& intrinsics,
double axis_length_m,
const std::string& label);
Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
int source_width,
int source_height,
int target_width,
int target_height);
class CameraStreamEncoder {
public:
static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
@ -37,6 +55,14 @@ public:
int height,
int fps);
static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const cv::Mat& frame,
std::vector<uint8_t>& encoded_frame,
bool& is_key,
const Rs2Intrinsics& intrinsics,
const CameraStreamEncodeOptions& options = {});
// Compatibility overload for callers that only need timestamp drawing.
static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const cv::Mat& frame,
std::vector<uint8_t>& encoded_frame,

25
cmvr-es/devices/camera/common/include/camera_stream_overlay.h Normal file
View File

@ -0,0 +1,25 @@
#pragma once
#include <string>
#include <vector>
#include <Eigen/Dense>
namespace cmvr::device {
// A pose snapshot used only by the encoded video overlay. The transform is
// ^C T_Frame: it maps points in the named frame into the camera frame.
struct CoordinateFrameOverlay {
Eigen::Matrix4d T_C_Frame{Eigen::Matrix4d::Identity()};
std::string label;
int tag_id{-1};
bool valid{false};
};
struct CameraStreamOverlay {
bool draw_coordinate_frames{false};
double coordinate_axis_length_m{0.02};
std::vector<CoordinateFrameOverlay> coordinate_frames;
};
} // namespace cmvr::device

141
cmvr-es/devices/camera/common/src/camera_stream_encoder.cpp
View File

@ -1,6 +1,7 @@
#include "devices/camera/common/include/camera_stream_encoder.h"
#include <chrono>
#include <cmath>
#include <ctime>
#include <iomanip>
#include <sstream>
@ -9,6 +10,7 @@
#include <opencv2/imgproc.hpp>
#include "common/base/logging/logger.h"
#include "devices/camera/abstract_camera.h"
namespace cmvr::device {
namespace {
@ -46,6 +48,41 @@ void drawTimeStamp(cv::Mat& image)
cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness);
}
bool projectPoint(const Eigen::Vector3d& point,
const Rs2Intrinsics& intrinsics,
cv::Point& pixel)
{
if (!point.allFinite() || point.z() <= 1e-9 ||
!std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
!std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) {
return false;
}
const double u = static_cast<double>(intrinsics.fx) * point.x() / point.z() +
static_cast<double>(intrinsics.cx);
const double v = static_cast<double>(intrinsics.fy) * point.y() / point.z() +
static_cast<double>(intrinsics.cy);
if (!std::isfinite(u) || !std::isfinite(v)) {
return false;
}
pixel = cv::Point(cvRound(u), cvRound(v));
return true;
}
void drawOutlinedText(cv::Mat& image,
const std::string& text,
const cv::Point& origin,
const cv::Scalar& color)
{
constexpr int font_face = cv::FONT_HERSHEY_SIMPLEX;
constexpr double font_scale = 0.55;
constexpr int thickness = 1;
cv::putText(image, text, origin, font_face, font_scale,
cv::Scalar(0, 0, 0), thickness + 2, cv::LINE_AA);
cv::putText(image, text, origin, font_face, font_scale,
color, thickness, cv::LINE_AA);
}
const AVCodec* findEncoder(const std::string& codec_name)
{
if (codec_name == "h264" || codec_name == "H264") {
@ -75,6 +112,77 @@ AVPixelFormat sourcePixelFormat(const cv::Mat& frame)
} // namespace
void drawCoordinateFrame(cv::Mat& image,
const Eigen::Matrix4d& T_C_Frame,
const Rs2Intrinsics& intrinsics,
const double axis_length_m,
const std::string& label)
{
if (image.empty() || image.channels() < 3 || !T_C_Frame.allFinite() ||
!std::isfinite(axis_length_m) || axis_length_m <= 0.0) {
return;
}
const Eigen::Vector4d origin_h(0.0, 0.0, 0.0, 1.0);
const Eigen::Vector4d x_h(axis_length_m, 0.0, 0.0, 1.0);
const Eigen::Vector4d y_h(0.0, axis_length_m, 0.0, 1.0);
const Eigen::Vector4d z_h(0.0, 0.0, axis_length_m, 1.0);
const Eigen::Vector3d origin = (T_C_Frame * origin_h).head<3>();
const Eigen::Vector3d x = (T_C_Frame * x_h).head<3>();
const Eigen::Vector3d y = (T_C_Frame * y_h).head<3>();
const Eigen::Vector3d z = (T_C_Frame * z_h).head<3>();
cv::Point origin_px;
if (!projectPoint(origin, intrinsics, origin_px)) {
return;
}
cv::Point x_px;
cv::Point y_px;
cv::Point z_px;
constexpr int thickness = 2;
if (projectPoint(x, intrinsics, x_px)) {
cv::arrowedLine(image, origin_px, x_px, cv::Scalar(0, 0, 255), thickness,
cv::LINE_AA, 0, 0.15);
drawOutlinedText(image, "X", x_px + cv::Point(4, -4), cv::Scalar(0, 0, 255));
}
if (projectPoint(y, intrinsics, y_px)) {
cv::arrowedLine(image, origin_px, y_px, cv::Scalar(0, 255, 0), thickness,
cv::LINE_AA, 0, 0.15);
drawOutlinedText(image, "Y", y_px + cv::Point(4, -4), cv::Scalar(0, 255, 0));
}
if (projectPoint(z, intrinsics, z_px)) {
cv::arrowedLine(image, origin_px, z_px, cv::Scalar(255, 0, 0), thickness,
cv::LINE_AA, 0, 0.15);
drawOutlinedText(image, "Z", z_px + cv::Point(4, -4), cv::Scalar(255, 0, 0));
}
cv::drawMarker(image, origin_px, cv::Scalar(255, 255, 255), cv::MARKER_CROSS, 9, 1,
cv::LINE_AA);
if (!label.empty()) {
drawOutlinedText(image, label, origin_px + cv::Point(7, -7),
cv::Scalar(255, 255, 255));
}
}
Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
const int source_width,
const int source_height,
const int target_width,
const int target_height)
{
Rs2Intrinsics scaled = intrinsics;
if (source_width > 0 && source_height > 0 && target_width > 0 && target_height > 0) {
const float sx = static_cast<float>(target_width) / static_cast<float>(source_width);
const float sy = static_cast<float>(target_height) / static_cast<float>(source_height);
scaled.fx *= sx;
scaled.cx *= sx;
scaled.fy *= sy;
scaled.cy *= sy;
}
return scaled;
}
FfmpegEncoderInfo::~FfmpegEncoderInfo()
{
if (frame) {
@ -189,6 +297,7 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const cv::Mat& frame,
std::vector<uint8_t>& encoded_frame,
bool& is_key,
const Rs2Intrinsics& intrinsics,
const CameraStreamEncodeOptions& options)
{
encoded_frame.clear();
@ -205,9 +314,27 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
}
cv::Mat frame_to_encode = frame;
if (options.draw_timestamp) {
if (options.draw_timestamp || options.overlay.draw_coordinate_frames) {
frame_to_encode = frame.clone();
drawTimeStamp(frame_to_encode);
if (options.draw_timestamp) {
drawTimeStamp(frame_to_encode);
}
if (options.overlay.draw_coordinate_frames) {
for (const auto& coordinate_frame : options.overlay.coordinate_frames) {
if (!coordinate_frame.valid) {
continue;
}
std::string label = coordinate_frame.label;
if (coordinate_frame.tag_id >= 0) {
label += " #" + std::to_string(coordinate_frame.tag_id);
}
drawCoordinateFrame(frame_to_encode,
coordinate_frame.T_C_Frame,
intrinsics,
options.overlay.coordinate_axis_length_m,
label);
}
}
}
const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode);
@ -294,4 +421,14 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
return true;
}
bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const cv::Mat& frame,
std::vector<uint8_t>& encoded_frame,
bool& is_key,
const CameraStreamEncodeOptions& options)
{
Rs2Intrinsics intrinsics{};
return encode(encoder, frame, encoded_frame, is_key, intrinsics, options);
}
} // namespace cmvr::device

24
cmvr-es/devices/camera/mujoco_camera/include/mujoco_camera.h
View File

@ -5,10 +5,13 @@
#pragma once
#include <cstdint>
#include <condition_variable>
#include <chrono>
#include <functional>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
#include <mujoco/mujoco.h>
@ -57,6 +60,11 @@ private:
bool initOffscreen_();
void destroyOffscreen_();
bool renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics);
void renderLoop_();
bool fetchCached_(cv::Mat& color,
cv::Mat& depth,
Rs2Intrinsics& intrinsics,
bool consume_new_frame_only);
bool ensureEncoder_(int width, int height, int fps);
void setError_(const std::string& error);
static void flipRgbAndDepth_(std::vector<unsigned char>& rgb,
@ -65,6 +73,14 @@ private:
int height);
static void linearizeDepth_(const mjModel* model, std::vector<float>& depth);
struct CachedFrame {
cv::Mat color;
cv::Mat depth;
Rs2Intrinsics intrinsics{};
uint64_t frame_id{0};
bool valid{false};
};
private:
FetchRgbdFn fetch_rgbd_fn_;
mutable std::mutex mtx_;
@ -78,6 +94,7 @@ private:
mjrContext context_{};
bool scene_initialized_{false};
bool context_initialized_{false};
mjData* render_data_{nullptr};
int camera_id_{-1};
int width_{640};
int height_{480};
@ -92,6 +109,13 @@ private:
size_t stream_frame_index_{0};
bool streaming_{false};
std::shared_ptr<FfmpegEncoderInfo> rgb_encoder_;
mutable std::mutex cache_mtx_;
std::condition_variable cache_cv_;
CachedFrame latest_frame_;
std::thread render_thread_;
bool render_thread_running_{false};
bool render_stop_requested_{false};
};
} // namespace cmvr::device

358
cmvr-es/devices/camera/mujoco_camera/src/mujoco_camera.cpp
View File

@ -19,6 +19,13 @@ constexpr int kDefaultWidth = 640;
constexpr int kDefaultHeight = 480;
constexpr int kMaxGeom = 100000;
// GLFW keeps process-global initialization state. MuJoCo cameras render on
// independent threads, so serialize the one-time init/window creation path.
std::mutex& glfwInitMutex() {
static std::mutex mutex;
return mutex;
}
int positiveOrDefault(const int value, const int fallback)
{
return value > 0 ? value : fallback;
@ -106,10 +113,6 @@ bool MujocoCamera::init()
fovy_deg_ = model->cam_fovy[camera_id_];
}
if (!initOffscreen_()) {
return false;
}
state_.is_initialized = true;
state_.is_opened = true;
state_.fps = positiveOrDefault(config_.render().fps(), 30);
@ -125,37 +128,132 @@ bool MujocoCamera::init()
bool MujocoCamera::start()
{
if (!state_.is_initialized) {
bool initialized = false;
{
std::lock_guard<std::mutex> lock(mtx_);
initialized = state_.is_initialized;
}
if (!initialized) {
if (!init()) {
return false;
}
}
std::lock_guard<std::mutex> lock(mtx_);
auto world = world_.lock();
if (world && !world->isRunning() && !world->start()) {
setError_("[MujocoCamera] failed to start MuJoCo world: " + world->lastError());
return false;
}
state_.is_streaming = true;
state_.is_opened = true;
bool use_external_frames = false;
{
std::lock_guard<std::mutex> lock(mtx_);
state_.is_streaming = true;
state_.is_opened = true;
use_external_frames = static_cast<bool>(fetch_rgbd_fn_);
}
std::thread stale_thread;
{
std::lock_guard<std::mutex> lock(cache_mtx_);
if (render_thread_running_) {
return true;
}
latest_frame_ = CachedFrame{};
last_frame_id_ = 0;
has_last_frame_id_ = false;
render_stop_requested_ = false;
render_thread_running_ = true;
}
{
std::lock_guard<std::mutex> lock(mtx_);
stale_thread = std::move(render_thread_);
}
if (stale_thread.joinable()) {
stale_thread.join();
}
try {
std::lock_guard<std::mutex> lock(mtx_);
render_thread_ = std::thread(&MujocoCamera::renderLoop_, this);
} catch (const std::exception& e) {
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_running_ = false;
render_stop_requested_ = true;
}
cache_cv_.notify_all();
setError_("[MujocoCamera] failed to start render thread: " + std::string(e.what()));
return false;
}
// External PiP callbacks are not ready until the viewer enters its render
// loop, so let their polling thread warm up asynchronously.
if (use_external_frames) {
return true;
}
std::unique_lock<std::mutex> cache_lock(cache_mtx_);
const bool ready = cache_cv_.wait_for(
cache_lock,
std::chrono::seconds(5),
[this] { return latest_frame_.valid || !render_thread_running_ || render_stop_requested_; });
const bool has_frame = latest_frame_.valid;
cache_lock.unlock();
if (!ready || !has_frame) {
stop();
if (ready) {
setError_("[MujocoCamera] render thread stopped before producing a frame");
} else {
setError_("[MujocoCamera] timed out waiting for the first rendered frame");
}
return false;
}
return true;
}
bool MujocoCamera::stop()
{
std::lock_guard<std::mutex> lock(mtx_);
state_.is_streaming = false;
state_.is_opened = false;
destroyOffscreen_();
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_stop_requested_ = true;
}
cache_cv_.notify_all();
std::thread thread_to_join;
{
std::lock_guard<std::mutex> lock(mtx_);
state_.is_streaming = false;
state_.is_opened = false;
thread_to_join = std::move(render_thread_);
}
if (thread_to_join.joinable()) {
thread_to_join.join();
}
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_running_ = false;
latest_frame_ = CachedFrame{};
}
cache_cv_.notify_all();
return true;
}
void MujocoCamera::setFetchRgbdFn(FetchRgbdFn fetch_rgbd_fn)
{
bool render_thread_active = false;
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_active = render_thread_running_;
}
if (render_thread_active) {
stop();
}
std::lock_guard<std::mutex> lock(mtx_);
fetch_rgbd_fn_ = std::move(fetch_rgbd_fn);
if (fetch_rgbd_fn_) {
destroyOffscreen_();
state_.is_initialized = true;
state_.is_opened = true;
state_.fps = positiveOrDefault(config_.render().fps(), 30);
@ -203,9 +301,10 @@ void MujocoCamera::getRGBDImages(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics&
bool MujocoCamera::startStreaming()
{
if (!state_.is_initialized && !init()) {
if (!start()) {
return false;
}
std::lock_guard<std::mutex> lock(mtx_);
streaming_ = true;
state_.is_streaming = true;
@ -246,12 +345,17 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
}
frame_data.rgbImage = color.clone();
frame_data.intrinsics = intrinsics;
CameraStreamEncodeOptions encode_options;
encode_options.draw_timestamp = enable_stream_timestamp_;
encode_options.overlay = streamOverlay();
const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
intrinsics, color.cols, color.rows, color_to_encode.cols, color_to_encode.rows);
if (!CameraStreamEncoder::encode(rgb_encoder_,
color_to_encode,
frame_data.rgbFrame,
frame_data.bKey,
encode_intrinsics,
encode_options)) {
return false;
}
@ -261,7 +365,6 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
const auto* depth_end = depth_begin + depth.total() * depth.elemSize();
frame_data.depthFrame.assign(depth_begin, depth_end);
}
frame_data.intrinsics = intrinsics;
frame_data.width = color_to_encode.cols;
frame_data.height = color_to_encode.rows;
frame_data.fps = fps;
@ -273,14 +376,29 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsics)
{
std::lock_guard<std::mutex> lock(mtx_);
if (fetch_rgbd_fn_) {
FetchRgbdFn fetch_rgbd_fn;
bool consume_new_frame_only = false;
bool render_thread_active = false;
{
std::lock_guard<std::mutex> lock(mtx_);
fetch_rgbd_fn = fetch_rgbd_fn_;
consume_new_frame_only = consume_new_frame_only_;
}
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_active = render_thread_running_;
}
// Keep compatibility with callback-only cameras that have not been
// started. Once start() owns a polling thread, reads are cache-only.
if (fetch_rgbd_fn && !render_thread_active) {
std::vector<unsigned char> rgb_raw;
std::vector<float> depth_raw;
int width = 0;
int height = 0;
std::uint64_t frame_id = 0;
if (!fetch_rgbd_fn_(rgb_raw, depth_raw, width, height, frame_id)) {
if (!fetch_rgbd_fn(rgb_raw, depth_raw, width, height, frame_id)) {
return false;
}
if (width <= 0 || height <= 0) {
@ -292,11 +410,14 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
if (!depth_raw.empty() && static_cast<int>(depth_raw.size()) != width * height) {
return false;
}
if (consume_new_frame_only_ && has_last_frame_id_ && frame_id == last_frame_id_) {
return false;
{
std::lock_guard<std::mutex> lock(cache_mtx_);
if (consume_new_frame_only && has_last_frame_id_ && frame_id == last_frame_id_) {
return false;
}
last_frame_id_ = frame_id;
has_last_frame_id_ = true;
}
last_frame_id_ = frame_id;
has_last_frame_id_ = true;
cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
@ -310,7 +431,31 @@ bool MujocoCamera::fetch(cv::Mat& color, cv::Mat& depth, Rs2Intrinsics& intrinsi
return true;
}
return renderOffscreen_(color, depth, intrinsics);
return fetchCached_(color, depth, intrinsics, consume_new_frame_only);
}
bool MujocoCamera::fetchCached_(cv::Mat& color,
cv::Mat& depth,
Rs2Intrinsics& intrinsics,
const bool consume_new_frame_only)
{
std::lock_guard<std::mutex> lock(cache_mtx_);
if (!latest_frame_.valid || latest_frame_.color.empty()) {
return false;
}
if (consume_new_frame_only && has_last_frame_id_ &&
latest_frame_.frame_id == last_frame_id_) {
return false;
}
// cv::Mat copies are reference-counted; keep the cache immutable while the
// consumer reads the published frame and avoid a full image copy per poll.
color = latest_frame_.color;
depth = latest_frame_.depth;
intrinsics = latest_frame_.intrinsics;
last_frame_id_ = latest_frame_.frame_id;
has_last_frame_id_ = true;
return !color.empty();
}
bool MujocoCamera::initOffscreen_()
@ -319,6 +464,7 @@ bool MujocoCamera::initOffscreen_()
return true;
}
std::lock_guard<std::mutex> glfw_lock(glfwInitMutex());
if (!glfwInit()) {
setError_("[MujocoCamera] glfwInit failed");
return false;
@ -345,10 +491,25 @@ bool MujocoCamera::initOffscreen_()
return false;
}
std::lock_guard<std::mutex> world_lock(world->mutex());
const mjModel* model = world->model();
if (model == nullptr) {
setError_("[MujocoCamera] world model is null");
mjModel* model = nullptr;
{
std::lock_guard<std::mutex> world_lock(world->mutex());
model = world->model();
if (model == nullptr) {
setError_("[MujocoCamera] world model is null");
return false;
}
// MuJoCo clips rendering to the model's offscreen buffer. Make sure
// the buffer is large enough before creating this camera's context;
// otherwise a larger requested frame is only partially populated.
model->vis.global.offwidth = std::max(model->vis.global.offwidth, width_);
model->vis.global.offheight = std::max(model->vis.global.offheight, height_);
}
render_data_ = mj_makeData(model);
if (render_data_ == nullptr) {
setError_("[MujocoCamera] failed to allocate render data");
return false;
}
mjv_makeScene(model, &scene_, kMaxGeom);
@ -360,9 +521,116 @@ bool MujocoCamera::initOffscreen_()
setError_("[MujocoCamera] MuJoCo offscreen buffer is not available");
return false;
}
if (context_.offWidth < width_ || context_.offHeight < height_) {
setError_("[MujocoCamera] offscreen buffer is smaller than requested frame: " +
std::to_string(context_.offWidth) + "x" +
std::to_string(context_.offHeight) + " < " +
std::to_string(width_) + "x" + std::to_string(height_));
return false;
}
return true;
}
void MujocoCamera::renderLoop_()
{
FetchRgbdFn external_fetch;
{
std::lock_guard<std::mutex> lock(mtx_);
external_fetch = fetch_rgbd_fn_;
}
const bool use_external_frames = static_cast<bool>(external_fetch);
if (!use_external_frames && !initOffscreen_()) {
destroyOffscreen_();
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_running_ = false;
}
cache_cv_.notify_all();
return;
}
const int fps = positiveOrDefault(config_.render().fps(), 30);
const auto period = std::chrono::duration<double>(1.0 / static_cast<double>(fps));
const auto period_ticks = std::chrono::duration_cast<std::chrono::steady_clock::duration>(period);
auto next_tick = std::chrono::steady_clock::now();
while (true) {
{
std::lock_guard<std::mutex> lock(cache_mtx_);
if (render_stop_requested_) {
break;
}
}
cv::Mat color;
cv::Mat depth;
Rs2Intrinsics intrinsics{};
uint64_t external_frame_id = 0;
bool got_frame = false;
if (use_external_frames) {
std::vector<unsigned char> rgb_raw;
std::vector<float> depth_raw;
int width = 0;
int height = 0;
if (external_fetch(rgb_raw, depth_raw, width, height, external_frame_id) &&
width > 0 && height > 0 &&
static_cast<int>(rgb_raw.size()) == width * height * 3 &&
(depth_raw.empty() || static_cast<int>(depth_raw.size()) == width * height)) {
cv::Mat rgb(height, width, CV_8UC3, rgb_raw.data());
cv::cvtColor(rgb, color, cv::COLOR_RGB2BGR);
if (!depth_raw.empty()) {
cv::Mat dep(height, width, CV_32FC1, depth_raw.data());
depth = dep.clone();
}
fillIntrinsics(width, height, intrinsics);
got_frame = !color.empty();
}
} else {
got_frame = renderOffscreen_(color, depth, intrinsics);
}
if (got_frame) {
{
std::lock_guard<std::mutex> lock(cache_mtx_);
latest_frame_.color = std::move(color);
latest_frame_.depth = std::move(depth);
latest_frame_.intrinsics = intrinsics;
latest_frame_.frame_id = use_external_frames && external_frame_id != 0
? external_frame_id
: latest_frame_.frame_id + 1;
latest_frame_.valid = true;
}
{
std::lock_guard<std::mutex> lock(mtx_);
clear_error_();
}
cache_cv_.notify_all();
}
next_tick += period_ticks;
std::unique_lock<std::mutex> lock(cache_mtx_);
if (cache_cv_.wait_until(lock, next_tick, [this] { return render_stop_requested_; })) {
break;
}
const auto now = std::chrono::steady_clock::now();
if (next_tick < now) {
next_tick = now + period_ticks;
}
}
if (!use_external_frames) {
destroyOffscreen_();
}
{
std::lock_guard<std::mutex> lock(cache_mtx_);
render_thread_running_ = false;
}
cache_cv_.notify_all();
}
void MujocoCamera::destroyOffscreen_()
{
if (window_ != nullptr) {
@ -376,6 +644,10 @@ void MujocoCamera::destroyOffscreen_()
mjv_freeScene(&scene_);
scene_initialized_ = false;
}
if (render_data_ != nullptr) {
mj_deleteData(render_data_);
render_data_ = nullptr;
}
if (window_ != nullptr) {
glfwDestroyWindow(window_);
window_ = nullptr;
@ -388,7 +660,8 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
setError_("[MujocoCamera] camera is not initialized: " + id_);
return false;
}
if (!initOffscreen_()) {
if (window_ == nullptr || !context_initialized_ || !scene_initialized_) {
setError_("[MujocoCamera] offscreen renderer is not initialized: " + id_);
return false;
}
@ -402,15 +675,27 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
std::vector<unsigned char> rgb(static_cast<std::size_t>(width_) * height_ * 3);
std::vector<float> depth_raw(static_cast<std::size_t>(width_) * height_);
mjModel* model = nullptr;
{
std::lock_guard<std::mutex> world_lock(world->mutex());
mjModel* model = world->model();
mjData* data = world->data();
if (model == nullptr || data == nullptr) {
std::unique_lock<std::mutex> world_lock(world->mutex(), std::try_to_lock);
if (!world_lock.owns_lock()) {
// Never make the simulation wait for a camera frame. The next
// scheduled capture will use a newer state if this one is busy.
return false;
}
model = world->model();
const mjData* data = world->data();
if (model == nullptr || data == nullptr || render_data_ == nullptr) {
setError_("[MujocoCamera] world model/data is null");
return false;
}
// Keep the world lock limited to the state copy. GPU rendering runs on
// the camera thread using its private data snapshot.
mjv_copyData(render_data_, model, data);
}
{
camera_.type = mjCAMERA_FIXED;
camera_.fixedcamid = camera_id_;
camera_.trackbodyid = -1;
@ -421,7 +706,7 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
viewport.width = width_;
viewport.height = height_;
mjv_updateScene(model, data, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
mjv_updateScene(model, render_data_, &option_, &perturb_, &camera_, mjCAT_ALL, &scene_);
mjr_render(viewport, &scene_, &context_);
mjr_readPixels(rgb.data(), depth_raw.data(), viewport, &context_);
flipRgbAndDepth_(rgb, depth_raw, width_, height_);
@ -429,13 +714,12 @@ bool MujocoCamera::renderOffscreen_(cv::Mat& color, cv::Mat& depth, Rs2Intrinsic
}
cv::Mat rgb_mat(height_, width_, CV_8UC3, rgb.data());
color = rgb_mat.clone();
// mjr_readPixels returns RGB, while the rest of the camera API exposes
// OpenCV-compatible BGR frames (as UVC and RealSense do).
cv::cvtColor(rgb_mat, color, cv::COLOR_RGB2BGR);
cv::Mat depth_mat(height_, width_, CV_32FC1, depth_raw.data());
depth = depth_mat.clone();
fillIntrinsics(width_, height_, intrinsics);
++last_frame_id_;
has_last_frame_id_ = true;
clear_error_();
return true;
}

8
cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
View File

@ -772,10 +772,18 @@ void RealsenseCamera::streaming_worker_() {
}
CameraStreamEncodeOptions encode_options;
encode_options.draw_timestamp = enable_stream_timestamp_;
encode_options.overlay = streamOverlay();
const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
frame_data.intrinsics,
frame_data.rgbImage.cols,
frame_data.rgbImage.rows,
rgb_to_encode.cols,
rgb_to_encode.rows);
success = CameraStreamEncoder::encode(rgbEncoder_,
rgb_to_encode,
frame_data.rgbFrame,
frame_data.bKey,
encode_intrinsics,
encode_options);
// 深度图编码
// success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);

8
cmvr-es/devices/camera/uvc_camera/src/uvc_camera.cpp
View File

@ -504,10 +504,18 @@ void UVCCamera::streaming_worker_() {
}
CameraStreamEncodeOptions encode_options;
encode_options.draw_timestamp = enable_stream_timestamp_;
encode_options.overlay = streamOverlay();
const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
frame_data.intrinsics,
frame_data.rgbImage.cols,
frame_data.rgbImage.rows,
rgb_to_encode.cols,
rgb_to_encode.rows);
success = CameraStreamEncoder::encode(rgbEncoder_,
rgb_to_encode,
frame_data.rgbFrame,
frame_data.bKey,
encode_intrinsics,
encode_options);
if (success) {
frame_data.fps = fps_;

3
cmvr-es/devices/canbus/can_comm/can_sender.h
View File

@ -247,6 +247,9 @@ namespace cmvr {
curr_period_ = period_;
Update();
if (send_with_once_) {
has_sent_ = true;
}
}
template<typename SensorType>

13
cmvr-es/devices/canbus/can_comm/can_sender_test.cc
View File

@ -77,6 +77,19 @@ namespace cmvr {
sensor_data->enable = (bytes[2] != 0);
}
TEST(CanSenderTest, OneShotMessageWaitsForExplicitUpdate) {
MyProtocol protocol;
SenderMessage<MySensorData> message(MyProtocol::ID, &protocol, true);
EXPECT_TRUE(message.has_sent());
protocol.SetSpeed(12.3);
protocol.SetEnable(true);
message.Update();
EXPECT_FALSE(message.has_sent());
}
TEST(CanSenderTest, OneRunCase) {
cmvr::config::SocketCanConfig cfg;

2
cmvr-es/devices/canbus/canopen/sdo_request_protocol.h
View File

@ -30,7 +30,7 @@ namespace cmvr {
return BASE_ID + sdo_frame_.node_id();
}
void SetFrameData(msgs::CommandSpecifier cs, msgs::ObIndex index,msgs::ObSubIndex sub_index, uint32_t data) {
void SetFrameData(msgs::CommandSpecifier cs, uint32_t index, uint32_t sub_index, uint32_t data) {
std::lock_guard<std::mutex> lock(mutex_);
sdo_frame_.set_cs(cs);
sdo_frame_.set_index(index);

6
cmvr-es/devices/canbus/canopen/sdo_response_protocol.h
View File

@ -49,10 +49,10 @@ namespace cmvr {
auto command = static_cast<msgs::CommandSpecifier>(bytes[0]);
// 解析 index（字节1和字节2，低字节优先）
auto index = static_cast<msgs::ObIndex>(bytes[1] + (bytes[2] << 8));
const uint32_t index = bytes[1] + (bytes[2] << 8);
// 解析 subindex（字节3）
auto subindex = static_cast<msgs::ObSubIndex>(bytes[3]);
const uint32_t subindex = bytes[3];
// 根据 command 解析 data（字节4~7）
uint32_t data = 0;
@ -94,4 +94,4 @@ namespace cmvr {
ParseSdoData(sdo_response_, sensor_data);
}
}
}
}

2
cmvr-es/devices/dexhand/CMakeLists.txt
View File

@ -1,5 +1,6 @@
add_subdirectory(rh56dftp_dexhand)
add_subdirectory(px_6ax_gen3)
add_subdirectory(zero_sim_touch_dexhand)
add_library(dexhand INTERFACE)
@ -9,6 +10,7 @@ target_link_libraries(dexhand
INTERFACE
cmvr_es::device::rh56dftp_dexhand
cmvr_es::device::px_6ax_gen3
cmvr_es::device::zero_sim_touch_dexhand
cmvr_es::proto
)

5
cmvr-es/devices/dexhand/dexhand_factory.h
View File

@ -10,6 +10,7 @@
#include "devices/dexhand/abstract_dexhand.h"
#include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h"
#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
#include "devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h"
namespace cmvr::device {
@ -32,6 +33,10 @@ public:
return std::make_shared<PX6AXGen3>(
backendWithId_(cfg.id(), cfg.px_6ax_gen3()));
case config::DexHandDeviceConfig::kZeroSimTouch:
return std::make_shared<ZeroSimTouchDexHand>(
backendWithId_(cfg.id(), cfg.zero_sim_touch()));
case config::DexHandDeviceConfig::BACKEND_NOT_SET:
default:
{

13
cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt Normal file
View File

@ -0,0 +1,13 @@
add_library(zero_sim_touch_dexhand SHARED src/zero_sim_touch_dexhand.cpp)
target_include_directories(zero_sim_touch_dexhand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
add_library(cmvr_es::device::zero_sim_touch_dexhand ALIAS zero_sim_touch_dexhand)
target_link_libraries(zero_sim_touch_dexhand
PRIVATE
cmvr_es::proto
glog
)
install(TARGETS zero_sim_touch_dexhand LIBRARY DESTINATION lib)

57
cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h Normal file
View File

@ -0,0 +1,57 @@
#ifndef CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
#define CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
#include <array>
#include <mutex>
#include <string>
#include <vector>
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
#include "../../abstract_dexhand.h"
namespace cmvr::device {
class ZeroSimTouchDexHand final : public AbstractDexHand {
public:
using FingerType = AbstractDexHand::FingerType;
using ResultantForce = AbstractDexHand::ResultantForce;
using TactilePoint = AbstractDexHand::TactilePoint;
using TactileRegion = AbstractDexHand::TactileRegion;
using TactileRegionKey = AbstractDexHand::TactileRegionKey;
using TactileRegionData = AbstractDexHand::TactileRegionData;
using Status = AbstractDexHand::Status;
explicit ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg);
~ZeroSimTouchDexHand() override = default;
std::string typeName() const override { return "ZeroSimTouchDexHand"; }
bool init() override;
bool start() override;
bool stop() override;
Status state() const override;
std::string lastError() const override;
void getState(DexHandState& state) override;
void setAngles(const std::vector<int>& finger_joint_angles) override;
void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
std::vector<TactileRegionData> getSensorData() override;
TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
private:
TactileRegionData makeRegionData(FingerType finger, TactileRegion region);
mutable std::mutex mutex_;
std::vector<TactileRegionKey> polling_regions_;
std::array<TactilePoint, 1> tactile_points_{};
Status lifecycle_state_{Status::CREATED};
std::string last_error_;
};
// Keep the historical spelling available to callers that used the test double.
using ZeroSImTouchDexHand = ZeroSimTouchDexHand;
} // namespace cmvr::device
#endif // CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H

96
cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp Normal file
View File

@ -0,0 +1,96 @@
#include "../include/zero_sim_touch_dexhand.h"
#include <utility>
namespace cmvr::device {
ZeroSimTouchDexHand::ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg) {
id_ = cfg.id();
}
bool ZeroSimTouchDexHand::init() {
std::lock_guard<std::mutex> lock(mutex_);
lifecycle_state_ = Status::STREAMING;
last_error_.clear();
return true;
}
bool ZeroSimTouchDexHand::start() {
std::lock_guard<std::mutex> lock(mutex_);
lifecycle_state_ = Status::STREAMING;
last_error_.clear();
return true;
}
bool ZeroSimTouchDexHand::stop() {
std::lock_guard<std::mutex> lock(mutex_);
lifecycle_state_ = Status::STOPPED;
return true;
}
ZeroSimTouchDexHand::Status ZeroSimTouchDexHand::state() const {
std::lock_guard<std::mutex> lock(mutex_);
return lifecycle_state_;
}
std::string ZeroSimTouchDexHand::lastError() const {
std::lock_guard<std::mutex> lock(mutex_);
return last_error_;
}
void ZeroSimTouchDexHand::getState(DexHandState& state) {
std::lock_guard<std::mutex> lock(mutex_);
state = DexHandState{};
state.is_initialized = lifecycle_state_ == Status::INITIALIZED ||
lifecycle_state_ == Status::STREAMING;
if (!last_error_.empty()) {
state.hands[0].error_message.push_back(last_error_);
}
}
void ZeroSimTouchDexHand::setAngles(const std::vector<int>&) {
// The simulation has no finger actuators; commands are intentionally ignored.
}
void ZeroSimTouchDexHand::setTactilePollingRegions(
const std::vector<TactileRegionKey>& regions) {
std::lock_guard<std::mutex> lock(mutex_);
polling_regions_ = regions;
}
std::vector<ZeroSimTouchDexHand::TactileRegionData> ZeroSimTouchDexHand::getSensorData() {
std::lock_guard<std::mutex> lock(mutex_);
if (polling_regions_.empty()) {
polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP});
}
std::vector<TactileRegionData> data;
data.reserve(polling_regions_.size());
for (const auto& region : polling_regions_) {
data.push_back(makeRegionData(region.first, region.second));
}
return data;
}
ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::getSensorData(
const FingerType finger, const TactileRegion region) {
std::lock_guard<std::mutex> lock(mutex_);
return makeRegionData(finger, region);
}
ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce(
const FingerType, const TactileRegion) {
return TactilePoint::fromFz(0);
}
ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData(
const FingerType finger, const TactileRegion region) {
tactile_points_[0] = TactilePoint::fromFz(0);
TactileMatrixView view;
view.data = tactile_points_.data();
view.rows = 1;
view.cols = 1;
return {finger, region, view, "mujoco_touch_tip"};
}
} // namespace cmvr::device

7
cmvr-es/devices/motor/CMakeLists.txt
View File

@ -1,6 +1,10 @@
add_library(motor_core INTERFACE)
target_include_directories(motor_core INTERFACE ${CMAKE_SOURCE_DIR}/cmvr-es/devices)
target_include_directories(motor_core
INTERFACE
${CMAKE_SOURCE_DIR}/cmvr-es
${CMAKE_SOURCE_DIR}/cmvr-es/devices
)
target_link_libraries(motor_core
INTERFACE
@ -12,4 +16,5 @@ add_library(cmvr_es::device::motor_core ALIAS motor_core)
add_subdirectory(drivers/ti5_canopen)
add_subdirectory(drivers/mujoco)
add_subdirectory(bus_runtime)
add_subdirectory(drivers/ethercat_motor)
add_subdirectory(manager)

105
cmvr-es/devices/motor/abstract_motor.h
View File

@ -48,13 +48,13 @@ namespace cmvr::device{
DeviceKind kind() const noexcept override { return DeviceKind::Motor; }
virtual void setMode(msgs::RunMode mode) {
virtual bool setMode(msgs::RunMode mode) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return;
return false;
}
protocol_->setMode(node_id_, mode);
return protocol_->setMode(node_id_, mode);
}
virtual msgs::RunMode getMode() {
@ -66,13 +66,22 @@ namespace cmvr::device{
return protocol_->getMode(node_id_);
}
virtual void torqueOff() {
virtual bool torqueOn() {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return;
return false;
}
protocol_->torqueOff(node_id_);
return protocol_->torqueOn(node_id_);
}
virtual bool torqueOff() {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return false;
}
return protocol_->torqueOff(node_id_);
}
virtual void setLimitQ(double ub, double lb) {
@ -101,43 +110,69 @@ namespace cmvr::device{
}
// virtual void setLimitTau(double tau) = 0;
// virtual void setLimitCurrent(double tau) = 0;
virtual void brake() {
virtual bool brakeRelease() {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return;
return false;
}
protocol_->brake(node_id_);
}
/**
*
* @param q unit : rad
*/
virtual void setQ(double q) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return;
}
protocol_->setQ(node_id_, q);
return protocol_->brakeRelease(node_id_);
}
virtual void setTarget(double q,double qd) {
virtual bool quickStop() {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return;
return false;
}
protocol_->setTarget(node_id_,q, qd);
return protocol_->quickStop(node_id_);
}
virtual bool commandProfilePosition(double target_q,
double max_qd = 0.0,
double max_qdd = 0.0) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return false;
}
return protocol_->commandProfilePosition(node_id_, target_q, max_qd, max_qdd);
}
virtual void setTarget(double qd) {
virtual bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return;
return false;
}
protocol_->setTarget(node_id_, qd);
return protocol_->commandProfileVelocity(node_id_, target_qd, max_qdd);
}
virtual bool commandCyclicPosition(double target_q,
double target_qd = 0.0) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return false;
}
return protocol_->commandCyclicPosition(node_id_, target_q, target_qd);
}
virtual bool commandCyclicVelocity(double target_qd) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return false;
}
return protocol_->commandCyclicVelocity(node_id_, target_qd);
}
virtual bool commandCyclicTorque(double target_tau) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return false;
}
return protocol_->commandCyclicTorque(node_id_, target_tau);
}
virtual bool calibrateZeroQ() {
@ -156,16 +191,6 @@ namespace cmvr::device{
}
return protocol_->reachedTargetQ(node_id_);
}
// rad /s
virtual void setQd(double qd) {
std::scoped_lock lock(mtx_);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return;
}
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;
@ -189,14 +214,14 @@ namespace cmvr::device{
// 使用的通讯协议
virtual void setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {
virtual bool setProtocol(std::shared_ptr<MotorProtocolInterface> protocol) {
std::scoped_lock lock(mtx_);
protocol_ = std::move(protocol);
if (!protocol_) {
CMVR_LOG(ERROR) << "Protocol not set for motor";
return;
return false;
}
protocol_->initNode(node_id_);
return protocol_->initNode(node_id_);
}
uint8_t id() const {

32
cmvr-es/devices/motor/bus_runtime/CMakeLists.txt
View File

@ -4,7 +4,18 @@ add_library(motor_bus_runtime SHARED
ethercat/src/ethercat_motor_bus_runtime.cpp
)
target_include_directories(motor_bus_runtime PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
set(IGH_ETHERCAT_ROOT
${CMAKE_SOURCE_DIR}/dependency/x86/third_party/ethercat/v1.7.0
)
target_include_directories(motor_bus_runtime
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}
PRIVATE
${IGH_ETHERCAT_ROOT}/include
)
target_link_directories(motor_bus_runtime PRIVATE ${IGH_ETHERCAT_ROOT}/lib)
target_link_libraries(motor_bus_runtime
PUBLIC
@ -12,9 +23,28 @@ target_link_libraries(motor_bus_runtime
cmvr_es::device::motor_core
cmvr_es::mujoco_world
PRIVATE
ethercat
cmvr_es::device::canbus
glog
)
add_library(cmvr_es::device::motor_bus_runtime ALIAS motor_bus_runtime)
install(TARGETS motor_bus_runtime LIBRARY DESTINATION lib)
add_executable(ethercat_motor_bus_runtime_real_test
ethercat/src/ethercat_motor_bus_runtime_real_test.cpp
)
target_include_directories(ethercat_motor_bus_runtime_real_test
PRIVATE
${CMAKE_SOURCE_DIR}/cmvr-es
)
target_link_libraries(ethercat_motor_bus_runtime_real_test
PRIVATE
cmvr_es::device::motor_bus_runtime
gtest
gtest_main
pthread
glog
)

8
cmvr-es/devices/motor/bus_runtime/can/src/can_motor_bus_runtime.cpp
View File

@ -68,16 +68,16 @@ bool CanMotorBusRuntime::start()
return false;
}
auto ret = sender_->Start();
auto ret = receiver_->Start();
if (ret != ErrorCode::OK) {
CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
stop();
return false;
}
ret = receiver_->Start();
ret = sender_->Start();
if (ret != ErrorCode::OK) {
CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN receiver: " << id_;
CMVR_LOG(ERROR) << "[CanMotorBusRuntime] failed to start CAN sender: " << id_;
stop();
return false;
}

214
cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h
View File

@ -1,15 +1,45 @@
#ifndef CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
#define CMVR_ES_ETHERCAT_MOTOR_BUS_RUNTIME_H
#include <atomic>
#include <chrono>
#include <cstddef>
#include <cstdint>
#include <cstring>
#include <mutex>
#include <string>
#include <thread>
#include <type_traits>
#include <unordered_map>
#include <vector>
#include "../../abstract_motor_bus_runtime.h"
#include "motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h"
typedef struct ec_domain ec_domain_t;
typedef struct ec_master ec_master_t;
typedef struct ec_slave_config ec_slave_config_t;
namespace cmvr::device {
class EthercatMotorBusRuntime final : public AbstractMotorBusRuntime {
public:
struct PdoWrite {
int motor_id{0};
std::uint16_t index{0};
std::uint8_t subindex{0};
std::uint8_t bit_length{0};
std::uint64_t raw_value{0};
};
struct PdoRead {
int motor_id{0};
std::uint16_t index{0};
std::uint8_t subindex{0};
std::uint8_t bit_length{0};
std::uint64_t raw_value{0};
};
bool init(const config::MotorGroupConfig& group_cfg) override;
bool start() override;
void stop() override;
@ -17,12 +47,194 @@ public:
const std::string& id() const { return id_; }
const config::EtherCATConfig& config() const { return config_; }
void setPdoMapping(EthercatPdoMapping mapping);
const EthercatPdoMapping& pdoMapping() const { return pdo_mapping_; }
const config::EthercatSlaveConfig* slaveForMotor(int motor_id) const;
bool hasMotor(int motor_id) const;
bool hasPdoEntry(int motor_id, std::uint16_t index, std::uint8_t subindex) const;
template <typename T>
bool writePdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value)
{
return writePdoRaw_(motor_id, index, subindex, valueBitLength_<T>(),
toRawValue_(value));
}
template <typename T>
static PdoWrite makePdoWrite(int motor_id,
std::uint16_t index,
std::uint8_t subindex,
T value)
{
return PdoWrite{motor_id, index, subindex, valueBitLength_<T>(), toRawValue_(value)};
}
bool writePdosAtomic(const PdoWrite* writes, std::size_t count);
template <typename T>
static PdoRead makePdoRead(int motor_id,
std::uint16_t index,
std::uint8_t subindex)
{
return PdoRead{motor_id, index, subindex, valueBitLength_<T>(), 0};
}
template <typename T>
static T pdoReadValue(const PdoRead& read)
{
return fromRawValue_<T>(read.raw_value);
}
bool readPdosAtomic(PdoRead* reads, std::size_t count) const;
std::uint64_t commandGeneration() const { return command_generation_.load(); }
std::uint64_t sentCommandGeneration() const { return sent_command_generation_.load(); }
bool isHealthy() const { return healthy_.load(); }
template <typename T>
bool readPdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value) const
{
std::uint64_t raw = 0;
if (!readPdoRaw_(motor_id, index, subindex, valueBitLength_<T>(), raw)) {
return false;
}
value = fromRawValue_<T>(raw);
return true;
}
template <typename T>
bool writeSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T value)
{
return writeSdoRaw_(motor_id, index, subindex, valueBitLength_<T>(),
toRawValue_(value));
}
template <typename T>
bool readSdo(int motor_id, std::uint16_t index, std::uint8_t subindex, T& value)
{
std::uint64_t raw = 0;
if (!readSdoRaw_(motor_id, index, subindex, valueBitLength_<T>(), raw)) {
return false;
}
value = fromRawValue_<T>(raw);
return true;
}
private:
struct PdoEntryRuntime {
EthercatPdoEntryConfig cfg;
unsigned int offset{0};
bool rx{false};
std::uint64_t value{0};
};
struct SlaveRuntime {
config::EthercatSlaveConfig cfg;
ec_slave_config_t* slave_config{nullptr};
std::unordered_map<std::uint32_t, PdoEntryRuntime> pdo_entries;
};
struct BusHealthState {
bool initialized{false};
bool healthy{false};
unsigned int slaves_responding{0};
unsigned int master_al_states{0};
bool link_up{false};
int domain_result{0};
unsigned int working_counter{0};
unsigned int wc_state{0};
};
struct SlaveHealthState {
bool initialized{false};
bool healthy{false};
int result{0};
bool online{false};
bool operational{false};
unsigned int al_state{0};
};
bool configureSlave_(SlaveRuntime& slave);
bool configureDc_();
bool waitSlavesOperational_();
void cyclicLoop_();
void monitorBusHealth_();
void readFeedbackLocked_();
void writeCommandsLocked_();
void releaseMaster_();
bool hasValidPdoMapping_() const;
bool writePdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
std::uint8_t bit_len, std::uint64_t value);
bool readPdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
std::uint8_t bit_len, std::uint64_t& value) const;
bool writeSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
std::uint8_t bit_len, std::uint64_t value);
bool readSdoRaw_(int motor_id, std::uint16_t index, std::uint8_t subindex,
std::uint8_t bit_len, std::uint64_t& value);
template <typename T>
static constexpr std::uint8_t valueBitLength_()
{
using ValueType = std::remove_cv_t<T>;
static_assert(std::is_integral_v<ValueType>, "EtherCAT object values must be integral");
static_assert(!std::is_same_v<ValueType, bool>, "bool is not a valid EtherCAT object value");
static_assert(sizeof(ValueType) == 1 || sizeof(ValueType) == 2 || sizeof(ValueType) == 4,
"only 8/16/32-bit EtherCAT object values are supported");
return static_cast<std::uint8_t>(sizeof(ValueType) * 8);
}
template <typename T>
static std::uint64_t toRawValue_(T value)
{
using ValueType = std::remove_cv_t<T>;
using UnsignedType = std::make_unsigned_t<ValueType>;
return static_cast<std::uint64_t>(static_cast<UnsignedType>(value));
}
template <typename T>
static T fromRawValue_(std::uint64_t raw)
{
using ValueType = std::remove_cv_t<T>;
using UnsignedType = std::make_unsigned_t<ValueType>;
const auto unsigned_value = static_cast<UnsignedType>(raw);
ValueType value{};
std::memcpy(&value, &unsigned_value, sizeof(ValueType));
return value;
}
static std::uint32_t pdoEntryKey_(std::uint16_t index, std::uint8_t subindex);
static std::string hexIndex_(std::uint32_t index);
static std::uint64_t maskValue_(std::uint64_t value, std::uint8_t bit_len);
static bool isSupportedBitLength_(std::uint8_t bit_len);
static std::uint64_t readEntryValue_(const std::uint8_t* domain_data,
const PdoEntryRuntime& entry);
static void writeEntryValue_(std::uint8_t* domain_data,
const PdoEntryRuntime& entry);
static std::uint64_t steadyTimeNs_();
static std::uint64_t timePointNs_(std::chrono::steady_clock::time_point time_point);
static std::uint32_t usToNs_(std::uint32_t value_us);
static std::int32_t usToNs_(std::int32_t value_us);
std::string id_;
config::EtherCATConfig config_;
std::unordered_map<int, const config::EthercatSlaveConfig*> slaves_by_motor_id_;
EthercatPdoMapping pdo_mapping_;
std::unordered_map<int, SlaveRuntime> slaves_by_motor_id_;
ec_master_t* master_{nullptr};
ec_domain_t* domain_{nullptr};
std::uint8_t* domain_data_{nullptr};
mutable std::mutex data_mutex_;
std::thread cyclic_thread_;
std::atomic<bool> running_{false};
std::atomic<bool> healthy_{false};
std::atomic<bool> health_monitor_enabled_{false};
std::atomic<std::uint64_t> command_generation_{0};
std::atomic<std::uint64_t> sent_command_generation_{0};
BusHealthState last_bus_health_;
std::unordered_map<int, SlaveHealthState> last_slave_health_;
bool initialized_{false};
bool started_{false};
};

35
cmvr-es/devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h Normal file
View File

@ -0,0 +1,35 @@
#ifndef CMVR_ES_ETHERCAT_PDO_MAPPING_H
#define CMVR_ES_ETHERCAT_PDO_MAPPING_H
#include <cstdint>
#include <string>
#include <vector>
namespace cmvr::device {
struct EthercatPdoEntryConfig {
std::uint16_t index{0};
std::uint8_t subindex{0};
std::uint8_t bit_len{0};
std::string name;
bool padding{false};
};
struct EthercatPdoConfig {
std::uint16_t index{0};
std::uint8_t sync_manager{0};
bool rx{false};
std::vector<EthercatPdoEntryConfig> entries;
};
struct EthercatPdoMapping {
std::uint32_t vendor_id{0};
std::uint32_t product_code{0};
std::string name;
std::vector<EthercatPdoConfig> rx_pdos;
std::vector<EthercatPdoConfig> tx_pdos;
};
} // namespace cmvr::device
#endif // CMVR_ES_ETHERCAT_PDO_MAPPING_H

1065
cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime.cpp
View File

File diff suppressed because it is too large Load Diff

128
cmvr-es/devices/motor/bus_runtime/ethercat/src/ethercat_motor_bus_runtime_real_test.cpp Normal file
View File

@ -0,0 +1,128 @@
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include <algorithm>
#include <array>
#include <chrono>
#include <cstdint>
#include <iostream>
#include <string>
#include <thread>
#include <gtest/gtest.h>
#include "cmvr/msgs/cia402.pb.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
namespace cmvr::device {
namespace {
config::MotorGroupConfig createSingleSlaveGroup()
{
config::MotorGroupConfig group;
group.set_id("ethercat_real_test");
group.set_bus_type(config::MOTOR_BUS_ETHERCAT);
group.set_vendor(config::MOTOR_VENDOR_EYOU);
group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402);
auto* ethercat = group.mutable_ethercat();
ethercat->set_master_index(0);
ethercat->set_cycle_us(1000);
ethercat->set_slave_op_timeout_ms(12000);
ethercat->set_slave_state_poll_period_ms(10);
auto* dc = ethercat->mutable_dc();
dc->set_enable(true);
dc->set_reference_motor_id(1);
dc->set_sync0_cycle_us(1000);
dc->set_sync0_shift_us(0);
dc->set_sync_reference_clock_period(1);
dc->set_assign_activate(768);
dc->set_sync_monitor_period_ms(1000);
auto* slave = ethercat->add_slaves();
slave->set_motor_id(1);
slave->set_alias(0);
slave->set_position(0);
return group;
}
} // namespace
TEST(EthercatMotorBusRuntimeRealTest, InitStartAndReadStatusword)
{
EthercatMotorBusRuntime runtime;
runtime.setPdoMapping(createEyouCia402PdoMapping());
ASSERT_TRUE(runtime.init(createSingleSlaveGroup()));
EXPECT_EQ(runtime.busType(), config::MOTOR_BUS_ETHERCAT);
EXPECT_TRUE(runtime.hasMotor(1));
EXPECT_NE(runtime.slaveForMotor(1), nullptr);
EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_CONTROL_WORD_6040, 0x00));
EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_STATUS_WORD_6041, 0x00));
EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_OPERATION_MODE_6060, 0x00));
EXPECT_TRUE(runtime.hasPdoEntry(1, msgs::CIA402_MODE_DISPLAY_6061, 0x00));
const bool started = runtime.start();
EXPECT_TRUE(started);
if (!started) {
runtime.stop();
return;
}
const std::array command_writes{
EthercatMotorBusRuntime::makePdoWrite<std::uint16_t>(
1, msgs::CIA402_CONTROL_WORD_6040, 0x00, 0x0000),
EthercatMotorBusRuntime::makePdoWrite<std::int8_t>(
1, msgs::CIA402_OPERATION_MODE_6060, 0x00, 0),
};
auto invalid_writes = command_writes;
invalid_writes[1].bit_length = 16;
const auto generation_before = runtime.commandGeneration();
EXPECT_FALSE(runtime.writePdosAtomic(invalid_writes.data(), invalid_writes.size()));
EXPECT_EQ(runtime.commandGeneration(), generation_before);
EXPECT_TRUE(runtime.writePdosAtomic(command_writes.data(), command_writes.size()));
EXPECT_EQ(runtime.commandGeneration(), generation_before + 1);
const int settle_ms = 1000;
std::this_thread::sleep_for(std::chrono::milliseconds(settle_ms));
EXPECT_GE(runtime.sentCommandGeneration(), generation_before + 1);
EXPECT_TRUE(runtime.isHealthy());
std::array feedback_reads{
EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
1, msgs::CIA402_STATUS_WORD_6041, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
1, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
};
auto invalid_feedback_reads = feedback_reads;
invalid_feedback_reads[1].bit_length = 16;
EXPECT_FALSE(runtime.readPdosAtomic(invalid_feedback_reads.data(),
invalid_feedback_reads.size()));
ASSERT_TRUE(runtime.readPdosAtomic(feedback_reads.data(), feedback_reads.size()));
const auto statusword =
EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(feedback_reads[0]);
const auto mode_display =
EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(feedback_reads[1]);
std::cout << "CIA402 statusword: 0x" << std::hex << statusword
<< ", mode display: " << std::dec << static_cast<int>(mode_display)
<< std::endl;
const int hold_ms = 10000;
if (hold_ms > 0) {
std::cout << "Holding EtherCAT runtime for " << hold_ms
<< " ms. Check slave state in another terminal." << std::endl;
std::this_thread::sleep_for(std::chrono::milliseconds(hold_ms));
}
runtime.stop();
}
} // namespace cmvr::device

50
cmvr-es/devices/motor/drivers/ethercat_motor/CMakeLists.txt Normal file
View File

@ -0,0 +1,50 @@
add_library(ethercat_motor_driver SHARED
src/cia402/cia402_protocol.cpp
src/cia402/cia402_status_monitor.cpp
src/vendor/eyou/eyou_motor.cpp
src/vendor/eyou/eyou_motor_adapter.cpp
)
target_include_directories(ethercat_motor_driver
PUBLIC
${CMAKE_CURRENT_SOURCE_DIR}/include
)
target_link_libraries(ethercat_motor_driver
PUBLIC
cmvr_es::device::motor_core
cmvr_es::device::motor_bus_runtime
PRIVATE
cmvr_es::proto
glog
)
add_library(cmvr_es::device::ethercat_motor_driver ALIAS ethercat_motor_driver)
install(TARGETS ethercat_motor_driver LIBRARY DESTINATION lib)
add_executable(eyou_motor_real_test
src/vendor/eyou/eyou_motor_real_test.cpp
)
target_link_libraries(eyou_motor_real_test
PRIVATE
cmvr_es::device::ethercat_motor_driver
gtest
gtest_main
pthread
glog
)
add_executable(eyou_motor_device_manager_real_test
src/vendor/eyou/eyou_motor_device_manager_real_test.cpp
)
target_link_libraries(eyou_motor_device_manager_real_test
PRIVATE
cmvr_es::device_manager
cmvr_es::device::motor_manager
gtest
gtest_main
pthread
glog
)

166
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h Normal file
View File

@ -0,0 +1,166 @@
#ifndef CMVR_ES_CIA402_OBJECTS_H
#define CMVR_ES_CIA402_OBJECTS_H
#include <cstdint>
namespace cmvr::device::cia402 {
union Controlword {
std::uint16_t value;
struct {
std::uint16_t switch_on : 1;
std::uint16_t enable_voltage : 1;
std::uint16_t quick_stop : 1;
std::uint16_t enable_operation : 1;
std::uint16_t new_set_point : 1;
std::uint16_t change_set_immediately : 1;
std::uint16_t relative : 1;
std::uint16_t fault_reset : 1;
std::uint16_t halt : 1;
std::uint16_t reserved : 2;
std::uint16_t manufacturer_specific : 5;
};
};
union Statusword {
std::uint16_t value;
struct {
std::uint16_t ready_to_switch_on : 1;
std::uint16_t switched_on : 1;
std::uint16_t operation_enabled : 1;
std::uint16_t fault : 1;
std::uint16_t voltage_enabled : 1;
std::uint16_t quick_stop : 1;
std::uint16_t switch_on_disabled : 1;
std::uint16_t warning : 1;
std::uint16_t manufacturer_specific_8 : 1;
std::uint16_t remote : 1;
std::uint16_t target_reached : 1;
std::uint16_t internal_limit_active : 1;
std::uint16_t operation_mode_specific : 2;
std::uint16_t manufacturer_specific : 2;
};
};
static_assert(sizeof(Controlword) == sizeof(std::uint16_t));
static_assert(sizeof(Statusword) == sizeof(std::uint16_t));
enum class DeviceState {
SwitchOnDisabled,
ReadyToSwitchOn,
SwitchedOn,
OperationEnabled,
};
namespace detail {
struct StateRule {
std::uint16_t relevant_bits;
std::uint16_t expected_bits;
};
inline StateRule stateRule(const DeviceState state)
{
// CiA402 device states are matched by selected 0x6041 statusword bits.
switch (state) {
case DeviceState::SwitchOnDisabled:
return {0x004F, 0x0040};
case DeviceState::ReadyToSwitchOn:
return {0x006F, 0x0021};
case DeviceState::SwitchedOn:
return {0x006F, 0x0023};
case DeviceState::OperationEnabled:
return {0x006F, 0x0027};
}
return {0x006F, 0x0000};
}
} // namespace detail
inline Controlword controlword(const std::uint16_t value)
{
Controlword cw{};
cw.value = value;
return cw;
}
inline Statusword statusword(const std::uint16_t value)
{
Statusword sw{};
sw.value = value;
return sw;
}
inline Controlword shutdownControlword()
{
Controlword cw{};
cw.quick_stop = 1;
cw.enable_voltage = 1;
return cw;
}
inline Controlword switchOnControlword()
{
auto cw = shutdownControlword();
cw.switch_on = 1;
return cw;
}
inline Controlword enableOperationControlword()
{
auto cw = switchOnControlword();
cw.enable_operation = 1;
return cw;
}
inline Controlword quickStopControlword()
{
auto cw = enableOperationControlword();
cw.quick_stop = 0;
return cw;
}
inline Controlword faultResetControlword()
{
Controlword cw{};
cw.fault_reset = 1;
return cw;
}
inline Controlword profilePositionControlword(const bool new_set_point)
{
auto cw = enableOperationControlword();
cw.change_set_immediately = 1;
cw.new_set_point = new_set_point ? 1 : 0;
return cw;
}
inline bool hasState(const Statusword status, const DeviceState state)
{
const auto rule = detail::stateRule(state);
return (status.value & rule.relevant_bits) == rule.expected_bits;
}
inline bool isSwitchOnDisabled(const Statusword status)
{
return hasState(status, DeviceState::SwitchOnDisabled);
}
inline bool isOperationEnabled(const Statusword status)
{
return hasState(status, DeviceState::OperationEnabled);
}
inline bool targetReached(const Statusword status)
{
return status.target_reached != 0;
}
inline bool setPointAcknowledged(const Statusword status)
{
return (status.value & (1U << 12U)) != 0;
}
} // namespace cmvr::device::cia402
#endif // CMVR_ES_CIA402_OBJECTS_H

149
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h Normal file
View File

@ -0,0 +1,149 @@
#ifndef CMVR_ES_CIA402_PROTOCOL_H
#define CMVR_ES_CIA402_PROTOCOL_H
#include <cstdint>
#include <memory>
#include <mutex>
#include <unordered_map>
#include <vector>
#include "cmvr/config/motor_config/motor_config.pb.h"
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
#include "devices/motor/motor_protocol_interface.h"
namespace cmvr::device {
class Cia402StatusMonitor;
class Cia402Protocol final : public MotorProtocolInterface {
public:
struct CyclicPositionCommand {
std::uint8_t node_id{0};
double target_q{0.0};
double target_qd{0.0};
};
struct MotorFeedback {
std::uint8_t node_id{0};
double q{0.0};
double qd{0.0};
};
explicit Cia402Protocol(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
const config::Cia402ProtocolConfig& config);
~Cia402Protocol() override;
bool initNode(std::uint8_t node_id) override;
bool setMode(std::uint8_t node_id, msgs::RunMode mode) override;
msgs::RunMode getMode(std::uint8_t node_id) override;
void setLimitQdd(std::uint8_t node_id, double u_qdd, double l_qdd) override;
void setLimitQd(std::uint8_t node_id, double qd) override;
void setLimitQ(std::uint8_t node_id, double ub, double lb) override;
bool calibrateZeroQ(std::uint8_t node_id) override;
bool reachedTargetQ(std::uint8_t node_id) override;
bool commandProfilePosition(std::uint8_t node_id,
double target_q,
double max_qd,
double max_qdd) override;
bool commandProfileVelocity(std::uint8_t node_id,
double target_qd,
double max_qdd) override;
bool commandCyclicPosition(std::uint8_t node_id,
double target_q,
double target_qd) override;
bool commandCyclicPositionsAtomic(const CyclicPositionCommand* commands,
std::size_t count);
bool readFeedbacksAtomic(MotorFeedback* feedbacks, std::size_t count) const;
bool commandCyclicVelocity(std::uint8_t node_id,
double target_qd) override;
bool commandCyclicTorque(std::uint8_t node_id, double target_tau) override;
void setMotorConversion(std::uint8_t node_id,
double encoder_counts_per_rev,
double gear_ratio) override;
bool torqueOn(std::uint8_t node_id) override;
bool torqueOff(std::uint8_t node_id) override;
bool brakeRelease(std::uint8_t node_id) override;
bool quickStop(std::uint8_t node_id) override;
double getQ(std::uint8_t node_id) override;
double getQd(std::uint8_t node_id) override;
bool syncTargetToActualPosition(std::uint8_t node_id);
private:
struct NodeState {
msgs::RunMode mode{msgs::RUN_MODE_CYCLIC_SYNC_POSITION};
cia402::Controlword controlword{};
std::int32_t target_position{0};
std::int32_t target_velocity{0};
std::int16_t target_torque{0};
std::int32_t profile_velocity{0};
std::int32_t profile_acceleration{0};
std::int32_t profile_deceleration{0};
double limit_q_lb{0.0};
double limit_q_ub{0.0};
double limit_qd{0.0};
double limit_qdd{0.0};
double encoder_counts_per_rev{0.0};
double gear_ratio{0.0};
};
static std::int8_t toCia402Mode_(msgs::RunMode mode);
static msgs::RunMode fromCia402Mode_(std::int8_t mode);
static cia402::Controlword nextControlword_(cia402::Statusword statusword);
static bool isOperationEnabled_(cia402::Statusword statusword);
static bool targetReached_(cia402::Statusword statusword);
std::int32_t radToCounts_(double angle_rad, const NodeState& state) const;
double countsToRad_(std::int32_t counts, const NodeState& state) const;
std::int32_t radPerSecToCounts_(double velocity_rad_s, const NodeState& state) const;
std::int32_t radPerSec2ToCounts_(double acceleration_rad_s2, const NodeState& state) const;
double countsToRadPerSec_(std::int32_t velocity_counts_s, const NodeState& state) const;
NodeState& nodeState_(std::uint8_t node_id);
const NodeState* findNodeState_(std::uint8_t node_id) const;
bool hasValidConversion_(std::uint8_t node_id, const NodeState& state) const;
bool validateNodePdos_(std::uint8_t node_id) const;
bool readStatusword_(std::uint8_t node_id, std::uint16_t& statusword) const;
bool readActualPosition_(std::uint8_t node_id, std::int32_t& actual_position) const;
bool readActualVelocity_(std::uint8_t node_id, std::int32_t& actual_velocity) const;
bool readModeDisplay_(std::uint8_t node_id, std::int8_t& mode_display) const;
bool writeControlword_(std::uint8_t node_id, cia402::Controlword controlword);
bool writeControlwordAndWait_(std::uint8_t node_id,
cia402::Controlword controlword,
cia402::DeviceState target_state,
const char* state_name);
bool waitStatus_(std::uint8_t node_id,
cia402::DeviceState target_state,
const char* state_name) const;
bool waitMode_(std::uint8_t node_id, std::int8_t target_mode) const;
bool waitSetPointAcknowledged_(std::uint8_t node_id, bool acknowledged) const;
bool waitVelocityNearZero_(std::uint8_t node_id, const char* action_name) const;
bool writePositionLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
bool writeVelocityLimitToDictionary_(std::uint8_t node_id, const NodeState& state) const;
bool writeAccelerationLimitsToDictionary_(std::uint8_t node_id, const NodeState& state) const;
bool prepareSafeTargetsForMode_(std::uint8_t node_id, msgs::RunMode mode, NodeState& state);
bool writeTargetsForMode_(std::uint8_t node_id,
msgs::RunMode mode,
const NodeState& state) const;
bool appendTargetWritesForMode_(
std::uint8_t node_id,
msgs::RunMode mode,
const NodeState& state,
EthercatMotorBusRuntime::PdoWrite* writes,
std::size_t capacity,
std::size_t& count) const;
bool writeProfilePositionTarget_(std::uint8_t node_id, NodeState& state);
bool writeNode_(std::uint8_t node_id, NodeState& state);
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
std::unique_ptr<Cia402StatusMonitor> status_monitor_;
config::Cia402ProtocolConfig config_;
std::unordered_map<std::uint8_t, NodeState> nodes_;
std::mutex cyclic_position_mutex_;
std::vector<EthercatMotorBusRuntime::PdoWrite> cyclic_position_writes_;
};
} // namespace cmvr::device
#endif // CMVR_ES_CIA402_PROTOCOL_H

78
cmvr-es/devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h Normal file
View File

@ -0,0 +1,78 @@
#ifndef CMVR_ES_CIA402_STATUS_MONITOR_H
#define CMVR_ES_CIA402_STATUS_MONITOR_H
#include <atomic>
#include <chrono>
#include <cstdint>
#include <memory>
#include <mutex>
#include <thread>
#include <unordered_map>
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
namespace cmvr::device {
class Cia402StatusMonitor final {
public:
Cia402StatusMonitor(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
std::chrono::milliseconds poll_period);
~Cia402StatusMonitor();
Cia402StatusMonitor(const Cia402StatusMonitor&) = delete;
Cia402StatusMonitor& operator=(const Cia402StatusMonitor&) = delete;
void addNode(std::uint8_t node_id);
void setExpectedOperationEnabled(std::uint8_t node_id, bool expected);
bool isNodeOperational(std::uint8_t node_id) const;
private:
struct StatusSample {
bool read_ok{false};
bool transport_healthy{false};
bool expected_operation_enabled{false};
bool operation_enabled{false};
bool status_problem{false};
bool command_blocked{false};
std::uint16_t statusword{0};
std::uint16_t error_code{0};
std::int8_t mode_display{0};
std::int32_t actual_position{0};
std::int32_t actual_velocity{0};
std::int16_t actual_torque{0};
};
struct NodeMonitorState {
bool expected_operation_enabled{false};
bool has_last_sample{false};
StatusSample last_sample;
};
void monitorLoop_();
void monitorNode_(std::uint8_t node_id, bool expected_operation_enabled);
bool readStatusSample_(std::uint8_t node_id,
bool expected_operation_enabled,
StatusSample& sample) const;
void reportErrorCodeTransition_(std::uint8_t node_id,
bool had_previous,
const StatusSample& previous,
const StatusSample& current) const;
void reportStatuswordTransition_(std::uint8_t node_id,
bool had_previous,
const StatusSample& previous,
const StatusSample& current) const;
static const char* deviceStateName_(std::uint16_t statusword);
static const char* errorCodeDescription_(std::uint16_t error_code);
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
std::chrono::milliseconds poll_period_;
std::mutex monitor_mutex_;
mutable std::mutex states_mutex_;
std::unordered_map<std::uint8_t, NodeMonitorState> states_;
std::atomic<bool> running_{true};
std::thread monitor_thread_;
};
} // namespace cmvr::device
#endif // CMVR_ES_CIA402_STATUS_MONITOR_H

81
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h vendored Normal file
View File

@ -0,0 +1,81 @@
#ifndef CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
#define CMVR_ES_EYOU_CIA402_PDO_MAPPING_H
#include <cstdint>
#include <string>
#include <utility>
#include "cmvr/msgs/canopen.pb.h"
#include "cmvr/msgs/cia402.pb.h"
#include "devices/motor/bus_runtime/ethercat/include/ethercat_pdo_mapping.h"
namespace cmvr::device {
namespace eyou_cia402_pdo_mapping_detail {
inline constexpr std::uint32_t VENDOR_ID = 0x00001097;
inline constexpr std::uint32_t PRODUCT_CODE = 0x00002406;
inline EthercatPdoEntryConfig entry(const std::uint16_t index,
const std::uint8_t subindex,
const std::uint8_t bit_len,
std::string name)
{
EthercatPdoEntryConfig cfg;
cfg.index = index;
cfg.subindex = subindex;
cfg.bit_len = bit_len;
cfg.name = std::move(name);
cfg.padding = index == 0 || bit_len == 0;
return cfg;
}
} // namespace eyou_cia402_pdo_mapping_detail
inline EthercatPdoMapping createEyouCia402PdoMapping()
{
using namespace eyou_cia402_pdo_mapping_detail;
EthercatPdoConfig rx_pdo;
rx_pdo.index = msgs::CANOPEN_RPDO2_MAP_1601;
rx_pdo.sync_manager = 2;
rx_pdo.rx = true;
rx_pdo.entries = {
entry(msgs::CIA402_CONTROL_WORD_6040, 0x00, 16, "Control Word"),
entry(msgs::CIA402_TARGET_POSITION_607A, 0x00, 32, "Target Position"),
entry(msgs::CIA402_TARGET_VELOCITY_60FF, 0x00, 32, "Target Velocity"),
entry(msgs::CIA402_TARGET_TORQUE_6071, 0x00, 16, "Target Torque"),
entry(msgs::CIA402_PROFILE_ACCELERATION_6083, 0x00, 32, "Profile Acceleration"),
entry(msgs::CIA402_PROFILE_DECELERATION_6084, 0x00, 32, "Profile Deceleration"),
entry(msgs::CIA402_PROFILE_VELOCITY_6081, 0x00, 32, "Profile Velocity"),
entry(msgs::CIA402_TORQUE_SLOPE_6087, 0x00, 32, "Torque Slope"),
entry(msgs::CIA402_OPERATION_MODE_6060, 0x00, 8, "Mode Of Operation"),
entry(0x0000, 0x00, 8, "Padding"),
};
EthercatPdoConfig tx_pdo;
tx_pdo.index = msgs::CANOPEN_TPDO1_MAP_1A00;
tx_pdo.sync_manager = 3;
tx_pdo.rx = false;
tx_pdo.entries = {
entry(msgs::CIA402_STATUS_WORD_6041, 0x00, 16, "Status Word"),
entry(msgs::CIA402_ACTUAL_POSITION_6064, 0x00, 32, "Actual Position"),
entry(msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00, 32, "Actual Velocity"),
entry(msgs::CIA402_ACTUAL_TORQUE_6077, 0x00, 16, "Actual Torque"),
entry(msgs::CIA402_MODE_DISPLAY_6061, 0x00, 8, "Mode Of Operation Display"),
entry(msgs::CIA402_ERROR_CODE_603F, 0x00, 16, "Error Code"),
entry(0x0000, 0x00, 8, "Padding"),
};
EthercatPdoMapping mapping;
mapping.vendor_id = VENDOR_ID;
mapping.product_code = PRODUCT_CODE;
mapping.name = "EYOU ServoModule ECAT V145 CiA402";
mapping.rx_pdos.push_back(std::move(rx_pdo));
mapping.tx_pdos.push_back(std::move(tx_pdo));
return mapping;
}
} // namespace cmvr::device
#endif // CMVR_ES_EYOU_CIA402_PDO_MAPPING_H

53
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h vendored Normal file
View File

@ -0,0 +1,53 @@
#ifndef CMVR_ES_EYOU_MOTOR_H
#define CMVR_ES_EYOU_MOTOR_H
#include <cstdint>
#include <memory>
#include <vector>
#include "cmvr/config/motor_config/motor_config.pb.h"
#include "devices/motor/abstract_motor.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
namespace cmvr::device {
class EyouMotor final : public AbstractMotor {
public:
EyouMotor(const config::MotorConfigItem& config,
std::shared_ptr<Cia402Protocol> cia402_protocol,
std::unique_ptr<EyouMotorAdapter> vendor_adapter);
std::string typeName() const override { return "EyouMotor"; }
bool init() override;
void setLimitQ(double ub, double lb) override;
void setLimitQd(double qd) override;
bool calibrateZeroQ() override;
bool brakeRelease() override;
static bool commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities);
static bool readFeedbacksAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
std::vector<double>& positions,
std::vector<double>& velocities);
private:
bool hasDependencies_() const;
bool hasValidConversion_() const;
bool writeVendorPositionLimits_() const;
bool writeVendorVelocityLimit_() const;
std::int32_t radToCounts_(double angle_rad) const;
std::uint32_t radPerSecToCounts_(double velocity_rad_s) const;
std::shared_ptr<Cia402Protocol> cia402_protocol_;
std::unique_ptr<EyouMotorAdapter> vendor_adapter_;
double encoder_counts_per_rev_{0.0};
double gear_ratio_{0.0};
};
} // namespace cmvr::device
#endif // CMVR_ES_EYOU_MOTOR_H

34
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h vendored Normal file
View File

@ -0,0 +1,34 @@
#ifndef CMVR_ES_EYOU_MOTOR_ADAPTER_H
#define CMVR_ES_EYOU_MOTOR_ADAPTER_H
#include <cstdint>
#include <memory>
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h"
namespace cmvr::device {
class EyouMotorAdapter final : public MotorVendorAdapter {
public:
explicit EyouMotorAdapter(std::shared_ptr<EthercatMotorBusRuntime> bus_runtime);
~EyouMotorAdapter() override = default;
bool initNode(std::uint8_t node_id) override;
bool writePositionLimits(std::uint8_t node_id,
std::int32_t lower_limit,
std::int32_t upper_limit) override;
bool writeVelocityLimit(std::uint8_t node_id,
std::uint32_t velocity_limit) override;
bool calibrateZero(std::uint8_t node_id,
std::int64_t counts_per_joint_revolution,
std::int32_t& zeroed_position) override;
bool brakeRelease(std::uint8_t node_id) override;
private:
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime_;
};
} // namespace cmvr::device
#endif // CMVR_ES_EYOU_MOTOR_ADAPTER_H

20
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h vendored Normal file
View File

@ -0,0 +1,20 @@
#ifndef CMVR_ES_EYOU_OBJECTS_H
#define CMVR_ES_EYOU_OBJECTS_H
#include <cstdint>
namespace cmvr::device::eyou {
inline constexpr std::uint16_t EYOU_SOFT_LIMIT_STATE_2003 = 0x2003;
inline constexpr std::uint16_t EYOU_BRAKE_CONTROL_2014 = 0x2014;
inline constexpr std::uint16_t EYOU_OVER_SPEED_THRESHOLD_2024 = 0x2024;
inline constexpr std::uint16_t EYOU_FIRST_ENCODER_VALUE_202A = 0x202A;
inline constexpr std::uint16_t EYOU_SECOND_ENCODER_VALUE_202B = 0x202B;
inline constexpr std::uint16_t EYOU_STORE_PARAMETERS_1010 = 0x1010;
} // namespace cmvr::device::eyou
#endif // CMVR_ES_EYOU_OBJECTS_H

26
cmvr-es/devices/motor/drivers/ethercat_motor/include/vendor/motor_vendor_adapter.h vendored Normal file
View File

@ -0,0 +1,26 @@
#ifndef CMVR_ES_MOTOR_VENDOR_ADAPTER_H
#define CMVR_ES_MOTOR_VENDOR_ADAPTER_H
#include <cstdint>
namespace cmvr::device {
class MotorVendorAdapter {
public:
virtual ~MotorVendorAdapter() = default;
virtual bool initNode(std::uint8_t node_id) = 0;
virtual bool writePositionLimits(std::uint8_t node_id,
std::int32_t lower_limit,
std::int32_t upper_limit) = 0;
virtual bool writeVelocityLimit(std::uint8_t node_id,
std::uint32_t velocity_limit) = 0;
virtual bool calibrateZero(std::uint8_t node_id,
std::int64_t counts_per_joint_revolution,
std::int32_t& zeroed_position) = 0;
virtual bool brakeRelease(std::uint8_t node_id) = 0;
};
} // namespace cmvr::device
#endif // CMVR_ES_MOTOR_VENDOR_ADAPTER_H

1123
cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_protocol.cpp Normal file
View File

File diff suppressed because it is too large Load Diff

284
cmvr-es/devices/motor/drivers/ethercat_motor/src/cia402/cia402_status_monitor.cpp Normal file
View File

@ -0,0 +1,284 @@
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_status_monitor.h"
#include <algorithm>
#include <array>
#include <iomanip>
#include <utility>
#include "cmvr/msgs/cia402.pb.h"
#include "common/base/logging/logger.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_objects.h"
namespace cmvr::device {
Cia402StatusMonitor::Cia402StatusMonitor(
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime,
const std::chrono::milliseconds poll_period)
: bus_runtime_(std::move(bus_runtime)),
poll_period_(std::max(poll_period, std::chrono::milliseconds{1})),
monitor_thread_(&Cia402StatusMonitor::monitorLoop_, this)
{
}
Cia402StatusMonitor::~Cia402StatusMonitor()
{
running_.store(false);
if (monitor_thread_.joinable()) {
monitor_thread_.join();
}
}
void Cia402StatusMonitor::addNode(const std::uint8_t node_id)
{
{
std::lock_guard<std::mutex> lock(states_mutex_);
states_.try_emplace(node_id);
}
monitorNode_(node_id, false);
}
void Cia402StatusMonitor::setExpectedOperationEnabled(
const std::uint8_t node_id,
const bool expected)
{
{
std::lock_guard<std::mutex> lock(states_mutex_);
states_[node_id].expected_operation_enabled = expected;
}
monitorNode_(node_id, expected);
}
bool Cia402StatusMonitor::isNodeOperational(const std::uint8_t node_id) const
{
if (!bus_runtime_ || !bus_runtime_->isHealthy()) {
return false;
}
std::lock_guard<std::mutex> lock(states_mutex_);
const auto it = states_.find(node_id);
return it != states_.end() && it->second.has_last_sample &&
it->second.last_sample.read_ok &&
it->second.last_sample.transport_healthy &&
it->second.last_sample.operation_enabled &&
!it->second.last_sample.command_blocked;
}
void Cia402StatusMonitor::monitorLoop_()
{
while (running_.load()) {
std::array<std::pair<std::uint8_t, bool>, 256> nodes{};
std::size_t node_count = 0;
{
std::lock_guard<std::mutex> lock(states_mutex_);
for (const auto& [node_id, state] : states_) {
if (node_count >= nodes.size()) {
break;
}
nodes[node_count++] = {node_id, state.expected_operation_enabled};
}
}
for (std::size_t i = 0; i < node_count; ++i) {
monitorNode_(nodes[i].first, nodes[i].second);
}
std::this_thread::sleep_for(poll_period_);
}
}
void Cia402StatusMonitor::monitorNode_(
const std::uint8_t node_id,
const bool expected_operation_enabled)
{
std::lock_guard<std::mutex> monitor_lock(monitor_mutex_);
StatusSample current;
readStatusSample_(node_id, expected_operation_enabled, current);
StatusSample previous;
bool had_previous = false;
{
std::lock_guard<std::mutex> lock(states_mutex_);
auto& state = states_[node_id];
had_previous = state.has_last_sample;
previous = state.last_sample;
state.last_sample = current;
state.has_last_sample = true;
}
if (!current.transport_healthy) {
return;
}
if (!current.read_ok) {
if (!had_previous || previous.read_ok) {
CMVR_LOG(ERROR) << "[Cia402StatusMonitor] failed to read node status snapshot"
<< ", node=" << static_cast<int>(node_id);
}
return;
}
if (had_previous && !previous.read_ok) {
CMVR_LOG(INFO) << "[Cia402StatusMonitor] node status snapshot recovered"
<< ", node=" << static_cast<int>(node_id);
}
reportErrorCodeTransition_(node_id, had_previous, previous, current);
reportStatuswordTransition_(node_id, had_previous, previous, current);
}
void Cia402StatusMonitor::reportErrorCodeTransition_(
const std::uint8_t node_id,
const bool had_previous,
const StatusSample& previous,
const StatusSample& current) const
{
const bool error_code_changed =
!had_previous || !previous.read_ok ||
previous.error_code != current.error_code;
if (current.error_code != 0 && error_code_changed) {
CMVR_LOG(ERROR) << "[Cia402StatusMonitor] [error code] 0x"
<< std::hex << std::uppercase << std::setw(4)
<< std::setfill('0') << current.error_code
<< std::dec << std::nouppercase << std::setfill(' ')
<< ' ' << errorCodeDescription_(current.error_code)
<< ", node=" << static_cast<int>(node_id);
} else if (current.error_code == 0 && had_previous && previous.read_ok &&
previous.error_code != 0) {
CMVR_LOG(INFO) << "[Cia402StatusMonitor] [error code] recovered"
<< ", node=" << static_cast<int>(node_id)
<< ", previous_code=0x" << std::hex << std::uppercase
<< std::setw(4) << std::setfill('0') << previous.error_code
<< std::dec << std::nouppercase << std::setfill(' ');
}
}
void Cia402StatusMonitor::reportStatuswordTransition_(
const std::uint8_t node_id,
const bool had_previous,
const StatusSample& previous,
const StatusSample& current) const
{
const bool status_changed =
!had_previous || !previous.read_ok ||
previous.status_problem != current.status_problem ||
(previous.statusword & 0x0888) != (current.statusword & 0x0888);
if (current.status_problem && status_changed) {
const auto status = cia402::statusword(current.statusword);
CMVR_LOG(WARNING) << "[Cia402StatusMonitor] [statusword] 0x"
<< std::hex << std::uppercase << std::setw(4)
<< std::setfill('0') << current.statusword
<< std::dec << std::nouppercase << std::setfill(' ')
<< ' ' << deviceStateName_(current.statusword)
<< ", node=" << static_cast<int>(node_id)
<< (status.fault != 0 ? ", fault" : "")
<< (status.warning != 0 ? ", warning" : "")
<< (status.internal_limit_active != 0
? ", internal limit active"
: "")
<< (current.expected_operation_enabled &&
!current.operation_enabled
? ", operation not enabled"
: "");
} else if (!current.status_problem && had_previous && previous.read_ok &&
previous.status_problem) {
CMVR_LOG(INFO) << "[Cia402StatusMonitor] [statusword] recovered"
<< ", node=" << static_cast<int>(node_id)
<< ", previous_statusword=0x" << std::hex << std::uppercase
<< std::setw(4) << std::setfill('0') << previous.statusword
<< std::dec << std::nouppercase << std::setfill(' ');
}
}
bool Cia402StatusMonitor::readStatusSample_(
const std::uint8_t node_id,
const bool expected_operation_enabled,
StatusSample& sample) const
{
sample.expected_operation_enabled = expected_operation_enabled;
sample.transport_healthy = bus_runtime_ && bus_runtime_->isHealthy();
if (!sample.transport_healthy) {
return false;
}
std::array reads{
EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
node_id, msgs::CIA402_STATUS_WORD_6041, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::uint16_t>(
node_id, msgs::CIA402_ERROR_CODE_603F, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int8_t>(
node_id, msgs::CIA402_MODE_DISPLAY_6061, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
node_id, msgs::CIA402_ACTUAL_POSITION_6064, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int32_t>(
node_id, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00),
EthercatMotorBusRuntime::makePdoRead<std::int16_t>(
node_id, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00),
};
if (!bus_runtime_->readPdosAtomic(reads.data(), reads.size())) {
return false;
}
sample.statusword = EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(reads[0]);
sample.error_code = EthercatMotorBusRuntime::pdoReadValue<std::uint16_t>(reads[1]);
sample.mode_display = EthercatMotorBusRuntime::pdoReadValue<std::int8_t>(reads[2]);
sample.actual_position = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[3]);
sample.actual_velocity = EthercatMotorBusRuntime::pdoReadValue<std::int32_t>(reads[4]);
sample.actual_torque = EthercatMotorBusRuntime::pdoReadValue<std::int16_t>(reads[5]);
sample.read_ok = true;
const auto status = cia402::statusword(sample.statusword);
sample.operation_enabled = cia402::isOperationEnabled(status);
sample.status_problem = status.fault != 0 || status.warning != 0 ||
status.internal_limit_active != 0 ||
(expected_operation_enabled && !sample.operation_enabled);
sample.command_blocked = status.fault != 0 || status.warning != 0 ||
sample.error_code != 0 ||
(expected_operation_enabled && !sample.operation_enabled);
return true;
}
const char* Cia402StatusMonitor::deviceStateName_(const std::uint16_t statusword)
{
if ((statusword & 0x004F) == 0x000F) {
return "FaultReactionActive";
}
if ((statusword & 0x004F) == 0x0008) {
return "Fault";
}
if ((statusword & 0x006F) == 0x0007) {
return "QuickStopActive";
}
const auto status = cia402::statusword(statusword);
if (cia402::isOperationEnabled(status)) {
return "OperationEnabled";
}
if (cia402::hasState(status, cia402::DeviceState::SwitchedOn)) {
return "SwitchedOn";
}
if (cia402::hasState(status, cia402::DeviceState::ReadyToSwitchOn)) {
return "ReadyToSwitchOn";
}
if (cia402::isSwitchOnDisabled(status)) {
return "SwitchOnDisabled";
}
return "NotReadyToSwitchOn";
}
const char* Cia402StatusMonitor::errorCodeDescription_(const std::uint16_t error_code)
{
switch (error_code) {
case 0x0000:
return "no error";
case 0x2310:
return "continuous over-current";
case 0x3210:
return "DC bus over-voltage";
case 0x3220:
return "DC bus under-voltage";
case 0x4210:
return "device over-temperature";
case 0x4310:
return "drive over-temperature";
case 0x8611:
return "position following error";
default:
return "unknown or vendor-specific error";
}
}
} // namespace cmvr::device

277
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor.cpp vendored Normal file
View File

@ -0,0 +1,277 @@
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
#include <algorithm>
#include <array>
#include <cmath>
#include <cstdint>
#include <functional>
#include <mutex>
#include <utility>
#include "common/base/logging/logger.h"
namespace cmvr::device {
EyouMotor::EyouMotor(const config::MotorConfigItem& config,
std::shared_ptr<Cia402Protocol> cia402_protocol,
std::unique_ptr<EyouMotorAdapter> vendor_adapter)
: cia402_protocol_(std::move(cia402_protocol)),
vendor_adapter_(std::move(vendor_adapter))
{
info_.id = config.id();
info_.joint_name = config.joint_name();
info_.limit_q_lb = config.limit_q_lb();
info_.limit_q_ub = config.limit_q_ub();
info_.limit_qd = config.limit_qd();
info_.limit_qdd = config.limit_qdd();
encoder_counts_per_rev_ = config.encoder_counts_per_rev();
gear_ratio_ = config.gear_ratio();
node_id_ = static_cast<std::uint8_t>(info_.id);
id_ = info_.joint_name;
protocol_ = cia402_protocol_;
}
bool EyouMotor::init()
{
std::scoped_lock lock(mtx_);
if (!hasDependencies_() || !hasValidConversion_()) {
return false;
}
if (!cia402_protocol_->initNode(node_id_)) {
CMVR_LOG(ERROR) << "[EyouMotor] failed to init CiA402 node: " << info_.joint_name;
return false;
}
if (!vendor_adapter_->initNode(node_id_)) {
CMVR_LOG(ERROR) << "[EyouMotor] failed to init vendor adapter: " << info_.joint_name;
return false;
}
cia402_protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_);
cia402_protocol_->setLimitQd(node_id_, info_.limit_qd);
if (!writeVendorVelocityLimit_()) {
return false;
}
if (info_.limit_qdd > 0.0) {
cia402_protocol_->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
}
if (!writeVendorPositionLimits_()) {
return false;
}
return true;
}
void EyouMotor::setLimitQ(const double ub, const double lb)
{
std::scoped_lock lock(mtx_);
info_.limit_q_ub = ub;
info_.limit_q_lb = lb;
if (!hasDependencies_() || !hasValidConversion_()) {
return;
}
writeVendorPositionLimits_();
}
void EyouMotor::setLimitQd(const double qd)
{
std::scoped_lock lock(mtx_);
info_.limit_qd = qd;
if (!hasDependencies_() || !hasValidConversion_()) {
return;
}
cia402_protocol_->setLimitQd(node_id_, info_.limit_qd);
writeVendorVelocityLimit_();
}
bool EyouMotor::calibrateZeroQ()
{
std::scoped_lock lock(mtx_);
if (!hasDependencies_() || !hasValidConversion_()) {
return false;
}
const auto counts_per_joint_revolution = static_cast<std::int64_t>(
std::llround(encoder_counts_per_rev_ * gear_ratio_));
std::int32_t zeroed_position = 0;
if (!vendor_adapter_->calibrateZero(node_id_, counts_per_joint_revolution,
zeroed_position)) {
CMVR_LOG(ERROR) << "[EyouMotor] zero calibration failed: " << info_.joint_name;
return false;
}
if (!cia402_protocol_->syncTargetToActualPosition(node_id_)) {
return false;
}
if (!writeVendorPositionLimits_()) {
return false;
}
return true;
}
bool EyouMotor::commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities)
{
if (motors.empty() || motors.size() != positions.size() ||
motors.size() != velocities.size() || motors.size() > 256) {
return false;
}
std::array<EyouMotor*, 256> lock_order{};
std::array<Cia402Protocol::CyclicPositionCommand, 256> commands{};
std::shared_ptr<Cia402Protocol> protocol;
for (std::size_t i = 0; i < motors.size(); ++i) {
const auto motor = std::dynamic_pointer_cast<EyouMotor>(motors[i]);
if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) {
return false;
}
if (!protocol) {
protocol = motor->cia402_protocol_;
} else if (protocol.get() != motor->cia402_protocol_.get()) {
CMVR_LOG(ERROR) << "[EyouMotor] batch target motors belong to different "
<< "CiA402 protocols";
return false;
}
for (std::size_t previous = 0; previous < i; ++previous) {
if (lock_order[previous] == motor.get()) {
return false;
}
}
lock_order[i] = motor.get();
commands[i] = Cia402Protocol::CyclicPositionCommand{
motor->node_id_, positions[i], velocities[i]};
}
std::sort(lock_order.begin(), lock_order.begin() + motors.size(),
std::less<EyouMotor*>{});
std::array<std::unique_lock<std::mutex>, 256> locks{};
for (std::size_t i = 0; i < motors.size(); ++i) {
locks[i] = std::unique_lock<std::mutex>(lock_order[i]->mtx_);
}
return protocol && protocol->commandCyclicPositionsAtomic(commands.data(), motors.size());
}
bool EyouMotor::readFeedbacksAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
std::vector<double>& positions,
std::vector<double>& velocities)
{
if (motors.empty() || motors.size() > 256) {
return false;
}
std::array<EyouMotor*, 256> lock_order{};
std::array<Cia402Protocol::MotorFeedback, 256> feedbacks{};
std::shared_ptr<Cia402Protocol> protocol;
for (std::size_t i = 0; i < motors.size(); ++i) {
const auto motor = std::dynamic_pointer_cast<EyouMotor>(motors[i]);
if (!motor || !motor->hasDependencies_() || !motor->hasValidConversion_()) {
return false;
}
if (!protocol) {
protocol = motor->cia402_protocol_;
} else if (protocol.get() != motor->cia402_protocol_.get()) {
CMVR_LOG(ERROR) << "[EyouMotor] batch feedback motors belong to different "
<< "CiA402 protocols";
return false;
}
for (std::size_t previous = 0; previous < i; ++previous) {
if (lock_order[previous] == motor.get()) {
return false;
}
}
lock_order[i] = motor.get();
feedbacks[i].node_id = motor->node_id_;
}
std::sort(lock_order.begin(), lock_order.begin() + motors.size(),
std::less<EyouMotor*>{});
std::array<std::unique_lock<std::mutex>, 256> locks{};
for (std::size_t i = 0; i < motors.size(); ++i) {
locks[i] = std::unique_lock<std::mutex>(lock_order[i]->mtx_);
}
if (!protocol || !protocol->readFeedbacksAtomic(feedbacks.data(), motors.size())) {
return false;
}
positions.resize(motors.size());
velocities.resize(motors.size());
for (std::size_t i = 0; i < motors.size(); ++i) {
positions[i] = feedbacks[i].q;
velocities[i] = feedbacks[i].qd;
}
return true;
}
bool EyouMotor::brakeRelease()
{
std::scoped_lock lock(mtx_);
if (!hasDependencies_()) {
return false;
}
return vendor_adapter_->brakeRelease(node_id_);
}
bool EyouMotor::hasDependencies_() const
{
if (!cia402_protocol_ || !vendor_adapter_) {
CMVR_LOG(ERROR) << "[EyouMotor] missing protocol or vendor adapter: "
<< info_.joint_name;
return false;
}
if (cia402_protocol_->comm_proto != MotorProtocolInterface::CommProto::ETHERCAT) {
CMVR_LOG(ERROR) << "[EyouMotor] invalid protocol for motor: " << info_.joint_name;
return false;
}
return true;
}
bool EyouMotor::hasValidConversion_() const
{
if (encoder_counts_per_rev_ > 0.0 && gear_ratio_ > 0.0) {
return true;
}
CMVR_LOG(ERROR) << "[EyouMotor] missing encoder conversion config: "
<< info_.joint_name
<< ", encoder_counts_per_rev=" << encoder_counts_per_rev_
<< ", gear_ratio=" << gear_ratio_;
return false;
}
bool EyouMotor::writeVendorPositionLimits_() const
{
if (!std::isfinite(info_.limit_q_lb) || !std::isfinite(info_.limit_q_ub) ||
info_.limit_q_ub <= info_.limit_q_lb) {
return true;
}
cia402_protocol_->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
return vendor_adapter_->writePositionLimits(node_id_,
radToCounts_(info_.limit_q_lb),
radToCounts_(info_.limit_q_ub));
}
bool EyouMotor::writeVendorVelocityLimit_() const
{
if (!std::isfinite(info_.limit_qd) || info_.limit_qd <= 0.0) {
return true;
}
return vendor_adapter_->writeVelocityLimit(node_id_, radPerSecToCounts_(info_.limit_qd));
}
std::int32_t EyouMotor::radToCounts_(const double angle_rad) const
{
const double rev = angle_rad / (2.0 * M_PI);
return static_cast<std::int32_t>(
std::llround(rev * gear_ratio_ * encoder_counts_per_rev_));
}
std::uint32_t EyouMotor::radPerSecToCounts_(const double velocity_rad_s) const
{
const double rev_per_sec = std::abs(velocity_rad_s) / (2.0 * M_PI);
return static_cast<std::uint32_t>(
std::llround(rev_per_sec * gear_ratio_ * encoder_counts_per_rev_));
}
} // namespace cmvr::device

376
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_adapter.cpp vendored Normal file
View File

@ -0,0 +1,376 @@
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
#include <chrono>
#include <cmath>
#include <cstdlib>
#include <limits>
#include <thread>
#include <utility>
#include "cmvr/msgs/cia402.pb.h"
#include "common/base/logging/logger.h"
#include "common/math/support_functions.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_objects.h"
namespace cmvr::device {
EyouMotorAdapter::EyouMotorAdapter(
std::shared_ptr<EthercatMotorBusRuntime> bus_runtime)
: bus_runtime_(std::move(bus_runtime))
{
}
bool EyouMotorAdapter::initNode(const std::uint8_t node_id)
{
return bus_runtime_ && bus_runtime_->hasMotor(node_id);
}
bool EyouMotorAdapter::writePositionLimits(const std::uint8_t node_id,
const std::int32_t lower_limit,
const std::int32_t upper_limit)
{
if (!bus_runtime_) {
return false;
}
const auto write_limits = [&]() {
return bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
0x00, 0) &&
bus_runtime_->writeSdo<std::int32_t>(
node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
0x02, upper_limit) &&
bus_runtime_->writeSdo<std::int32_t>(
node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
0x01, lower_limit) &&
bus_runtime_->writeSdo<std::uint32_t>(
node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
0x00, 0x4C494D54);
};
const auto readback_matches = [&]() {
std::int32_t actual_lower = 0;
std::int32_t actual_upper = 0;
return bus_runtime_->readSdo<std::int32_t>(
node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
0x01, actual_lower) &&
bus_runtime_->readSdo<std::int32_t>(
node_id, msgs::CIA402_SOFTWARE_POSITION_LIMIT_607D,
0x02, actual_upper) &&
actual_lower == lower_limit &&
actual_upper == upper_limit;
};
const bool ok = write_limits() && readback_matches();
if (!ok) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write software position "
<< "limits, node=" << static_cast<int>(node_id)
<< ", soft_limit_state=" << 0x4C494D54
<< ", lower=" << lower_limit
<< ", upper=" << upper_limit;
}
return ok;
}
bool EyouMotorAdapter::writeVelocityLimit(const std::uint8_t node_id,
const std::uint32_t velocity_limit)
{
if (!bus_runtime_) {
return false;
}
std::uint32_t actual_velocity_limit = 0;
const bool ok =
bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024,
0x00, velocity_limit) &&
bus_runtime_->readSdo<std::uint32_t>(node_id, eyou::EYOU_OVER_SPEED_THRESHOLD_2024,
0x00, actual_velocity_limit) &&
actual_velocity_limit == velocity_limit;
if (!ok) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write over speed "
<< "threshold, node=" << static_cast<int>(node_id)
<< ", expected=" << velocity_limit
<< ", actual=" << actual_velocity_limit;
}
return ok;
}
bool EyouMotorAdapter::calibrateZero(const std::uint8_t node_id,
const std::int64_t counts_per_joint_revolution,
std::int32_t& zeroed_position)
{
if (!bus_runtime_) {
return false;
}
const auto& zero_config = bus_runtime_->config().zero_calibration();
const auto home_offset_timeout =
std::chrono::milliseconds{zero_config.timeout_ms()};
const auto home_offset_poll_period =
std::chrono::milliseconds{zero_config.poll_period_ms()};
const auto home_offset_stable_samples = zero_config.stable_sample_count();
const auto home_offset_position_tolerance_counts =
zero_config.position_tolerance_counts();
const auto home_offset_stable_delta_counts =
zero_config.stable_delta_counts();
std::uint32_t original_soft_limit_state = 0;
std::int32_t original_home_offset = 0;
std::int32_t original_position = 0;
if (!bus_runtime_->readSdo<std::uint32_t>(
node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
0x00, original_soft_limit_state) ||
!bus_runtime_->readSdo<std::int32_t>(
node_id, msgs::CIA402_HOME_OFFSET_607C,
0x00, original_home_offset) ||
!bus_runtime_->readSdo<std::int32_t>(
node_id, msgs::CIA402_ACTUAL_POSITION_6064,
0x00, original_position)) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to snapshot calibration state, node="
<< static_cast<int>(node_id);
return false;
}
// EYOU applies HomeOffset additively, so clearing it exposes this raw position.
const auto expected_cleared_position_wide =
static_cast<std::int64_t>(original_position) -
static_cast<std::int64_t>(original_home_offset);
if (expected_cleared_position_wide < std::numeric_limits<std::int32_t>::min() ||
expected_cleared_position_wide > std::numeric_limits<std::int32_t>::max()) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] cleared position would overflow, node="
<< static_cast<int>(node_id)
<< ", original_position=" << original_position
<< ", original_home_offset=" << original_home_offset;
return false;
}
const auto expected_cleared_position =
static_cast<std::int32_t>(expected_cleared_position_wide);
const auto write_home_offset = [&](const std::int32_t value) {
return bus_runtime_->writeSdo<std::int32_t>(
node_id, msgs::CIA402_HOME_OFFSET_607C, 0x00, value);
};
const auto save_parameters = [&]() {
return bus_runtime_->writeSdo<std::uint32_t>(
node_id, eyou::EYOU_STORE_PARAMETERS_1010,
0x01, 0x65766173);
};
const auto wait_for_soft_limit = [&](const std::uint32_t expected_state) {
const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout;
do {
std::uint32_t actual_soft_limit_state = 0;
if (bus_runtime_->readSdo<std::uint32_t>(
node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
0x00, actual_soft_limit_state) &&
actual_soft_limit_state == expected_state) {
return true;
}
std::this_thread::sleep_for(home_offset_poll_period);
} while (std::chrono::steady_clock::now() < deadline);
return false;
};
const auto restore_soft_limit = [&]() {
return bus_runtime_->writeSdo<std::uint32_t>(
node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
0x00, original_soft_limit_state) &&
wait_for_soft_limit(original_soft_limit_state);
};
const auto wait_for_position = [&](const char* phase,
const std::int32_t expected_offset,
const std::int32_t expected_position,
std::int32_t& observed_position) {
const auto deadline = std::chrono::steady_clock::now() + home_offset_timeout;
std::uint32_t stable_samples = 0;
bool has_previous_position = false;
std::int32_t previous_position = 0;
std::int32_t observed_offset = 0;
do {
const bool read_ok =
bus_runtime_->readSdo<std::int32_t>(
node_id, msgs::CIA402_HOME_OFFSET_607C,
0x00, observed_offset) &&
bus_runtime_->readSdo<std::int32_t>(
node_id, msgs::CIA402_ACTUAL_POSITION_6064,
0x00, observed_position);
const bool position_stable =
!has_previous_position ||
SupportFunctions::cyclicAbsoluteDifference(
observed_position, previous_position,
counts_per_joint_revolution) <= home_offset_stable_delta_counts;
const bool sample_matches =
read_ok && observed_offset == expected_offset &&
SupportFunctions::cyclicAbsoluteDifference(
observed_position, expected_position,
counts_per_joint_revolution) <= home_offset_position_tolerance_counts &&
position_stable;
stable_samples = sample_matches ? stable_samples + 1 : 0;
if (stable_samples >= home_offset_stable_samples) {
return true;
}
if (read_ok) {
previous_position = observed_position;
has_previous_position = true;
}
std::this_thread::sleep_for(home_offset_poll_period);
} while (std::chrono::steady_clock::now() < deadline);
CMVR_LOG(ERROR) << "[EyouMotorAdapter] timed out waiting for home offset state, node="
<< static_cast<int>(node_id)
<< ", phase=" << phase
<< ", expected_offset=" << expected_offset
<< ", actual_offset=" << observed_offset
<< ", expected_position=" << expected_position
<< ", actual_position=" << observed_position
<< ", cyclic_position_distance="
<< SupportFunctions::cyclicAbsoluteDifference(
observed_position, expected_position,
counts_per_joint_revolution)
<< ", counts_per_joint_revolution="
<< counts_per_joint_revolution
<< ", stable_samples=" << stable_samples;
return false;
};
// Follow EYOU's required clear -> set -> save sequence during rollback too.
const auto rollback = [&](const char* failed_phase) {
const bool clear_written = write_home_offset(0);
std::int32_t cleared_position = 0;
const bool clear_applied =
clear_written &&
wait_for_position("rollback_clear_home_offset", 0,
expected_cleared_position, cleared_position);
const bool offset_written = write_home_offset(original_home_offset);
std::int32_t restored_position = 0;
const bool offset_applied =
offset_written &&
wait_for_position("rollback_apply_home_offset", original_home_offset,
original_position, restored_position);
const bool parameters_saved = offset_written && save_parameters();
const bool saved_state_confirmed =
parameters_saved &&
wait_for_position("rollback_save_home_offset", original_home_offset,
original_position, restored_position);
const bool soft_limit_restored = restore_soft_limit();
const bool rollback_ok = clear_applied && offset_applied &&
saved_state_confirmed && soft_limit_restored;
CMVR_LOG(ERROR) << "[EyouMotorAdapter] calibration failed and original state was "
<< (rollback_ok ? "restored" : "not fully restored")
<< ", node=" << static_cast<int>(node_id)
<< ", phase=" << failed_phase
<< ", original_home_offset=" << original_home_offset
<< ", original_soft_limit_state=" << original_soft_limit_state
<< ", clear_applied=" << clear_applied
<< ", offset_applied=" << offset_applied
<< ", saved_state_confirmed=" << saved_state_confirmed
<< ", soft_limit_restored=" << soft_limit_restored;
return false;
};
if (!bus_runtime_->writeSdo<std::uint32_t>(node_id, eyou::EYOU_SOFT_LIMIT_STATE_2003,
0x00, 0)) {
const bool soft_limit_restored = restore_soft_limit();
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to disable software position "
<< "limit before home offset calibration, node="
<< static_cast<int>(node_id)
<< ", soft_limit_restored=" << soft_limit_restored;
return false;
}
if (!wait_for_soft_limit(0)) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] software position limit did not disable, node="
<< static_cast<int>(node_id);
return rollback("disable_soft_limit");
}
if (!write_home_offset(0)) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to clear home offset, node="
<< static_cast<int>(node_id);
return rollback("clear_home_offset");
}
std::int32_t actual_position = 0;
if (!wait_for_position("clear_home_offset", 0,
expected_cleared_position, actual_position)) {
return rollback("wait_for_cleared_position");
}
if (actual_position == std::numeric_limits<std::int32_t>::min()) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] invalid actual position for home "
<< "offset calibration, node=" << static_cast<int>(node_id)
<< ", actual_position=" << actual_position;
return rollback("negate_actual_position");
}
const auto home_offset = static_cast<std::int32_t>(-actual_position);
if (!write_home_offset(home_offset)) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to write home offset, node="
<< static_cast<int>(node_id)
<< ", home_offset=" << home_offset;
return rollback("write_home_offset");
}
if (!wait_for_position("apply_home_offset", home_offset, 0,
zeroed_position)) {
return rollback("wait_for_zero_before_save");
}
if (!save_parameters()) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to save home offset parameter, node="
<< static_cast<int>(node_id);
return rollback("save_parameters");
}
if (!wait_for_position("save_home_offset", home_offset, 0,
zeroed_position)) {
return rollback("wait_for_zero_after_save");
}
if (!restore_soft_limit()) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to restore software position "
<< "limit after home offset calibration, node="
<< static_cast<int>(node_id)
<< ", original_soft_limit_state=" << original_soft_limit_state;
return rollback("restore_soft_limit");
}
CMVR_LOG(INFO) << "[EyouMotorAdapter] home offset calibration completed, node="
<< static_cast<int>(node_id)
<< ", original_home_offset=" << original_home_offset
<< ", cleared_position=" << actual_position
<< ", home_offset=" << home_offset
<< ", zeroed_position=" << zeroed_position
<< ", soft_limit_state=" << original_soft_limit_state;
return true;
}
bool EyouMotorAdapter::brakeRelease(const std::uint8_t node_id)
{
if (!bus_runtime_) {
return false;
}
if (!bus_runtime_->writeSdo<std::uint8_t>(
node_id, eyou::EYOU_BRAKE_CONTROL_2014,
0x01,
1)) {
CMVR_LOG(ERROR) << "[EyouMotorAdapter] failed to release brake, node="
<< static_cast<int>(node_id);
return false;
}
const auto deadline =
std::chrono::steady_clock::now() + std::chrono::milliseconds{1000};
do {
std::uint8_t brake_state = 0;
if (bus_runtime_->readSdo<std::uint8_t>(
node_id, eyou::EYOU_BRAKE_CONTROL_2014,
0x02, brake_state) &&
(brake_state == 1 || brake_state == 2)) {
return true;
}
std::this_thread::sleep_for(std::chrono::milliseconds{10});
} while (std::chrono::steady_clock::now() < deadline);
CMVR_LOG(ERROR) << "[EyouMotorAdapter] brake release timeout, node="
<< static_cast<int>(node_id);
return false;
}
} // namespace cmvr::device

380
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_device_manager_real_test.cpp vendored Normal file
View File

@ -0,0 +1,380 @@
#include <array>
#include <algorithm>
#include <chrono>
#include <cmath>
#include <cstdint>
#include <iostream>
#include <limits>
#include <memory>
#include <thread>
#include <utility>
#include <vector>
#include <gtest/gtest.h>
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
#include "common/config/config_files.h"
#include "devices/motor/abstract_motor.h"
#include "devices/motor/manager/include/motor_manager.h"
#include "manager/device_manager/include/device_manager.h"
namespace cmvr::device {
namespace {
constexpr const char* kMotorManagerId = "right_arm_ethercat_motors";
constexpr const char* kMotorConfigFile =
"devices/motor/ethercat_motors_four_real_test.pb.txt";
constexpr std::array<int, 4> kFourMotorIds{1, 2, 3, 4};
constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
constexpr std::chrono::milliseconds kStatsSamplePeriod{10};
constexpr std::chrono::milliseconds kHoldAfterTrajectoryDuration{500};
constexpr std::chrono::milliseconds kFourMotorTrajectoryDuration{20000};
constexpr double kPi = 3.14159265358979323846;
constexpr double kRaisedCosineCoefficientRad = 0.2;
constexpr double kFourMotorPeriodS = 2.0;
constexpr std::array<double, 4> kFourMotorPhaseRad{0.0, 0.0, 0.0, 0.0};
constexpr double kMinimumPositionExcursionRad = 0.2;
constexpr double kMaximumAbsoluteTrackingErrorRad = 0.15;
constexpr double kMaximumRmsTrackingErrorRad = 0.08;
constexpr double kMaximumErrorSpreadRad = 0.10;
constexpr double kFinalPositionToleranceRad = 0.05;
class DeviceManagerDestroyGuard {
public:
~DeviceManagerDestroyGuard()
{
DeviceManager::destroyInstance();
}
};
class MotorManagerStopGuard {
public:
explicit MotorManagerStopGuard(std::shared_ptr<MotorManager> motor_manager)
: motor_manager_(std::move(motor_manager))
{
}
~MotorManagerStopGuard()
{
if (motor_manager_ && !motor_manager_->stop()) {
std::cerr << "failed to stop motor manager during test cleanup" << std::endl;
}
}
private:
std::shared_ptr<MotorManager> motor_manager_;
};
class MultiMotorSafetyGuard {
public:
explicit MultiMotorSafetyGuard(
const std::vector<std::shared_ptr<AbstractMotor>>& motors)
: motors_(motors)
{
}
~MultiMotorSafetyGuard()
{
if (armed_) {
stop();
}
}
bool stop()
{
bool all_ok = true;
for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
if (*it && !(*it)->quickStop()) {
all_ok = false;
}
}
for (auto it = motors_.rbegin(); it != motors_.rend(); ++it) {
if (*it && !(*it)->torqueOff()) {
all_ok = false;
}
}
armed_ = !all_ok;
return all_ok;
}
private:
const std::vector<std::shared_ptr<AbstractMotor>>& motors_;
bool armed_{true};
};
struct TrackingErrorStats {
std::int64_t sample_count{0};
double sum_error{0.0};
double sum_error_sq{0.0};
double max_abs_error{0.0};
double sin_projection{0.0};
double cos_projection{0.0};
void add(const double error, const double theta)
{
++sample_count;
sum_error += error;
sum_error_sq += error * error;
max_abs_error = std::max(max_abs_error, std::fabs(error));
sin_projection += error * std::sin(theta);
cos_projection += error * std::cos(theta);
}
double mean() const
{
return sample_count > 0 ? sum_error / static_cast<double>(sample_count) : 0.0;
}
double rms() const
{
return sample_count > 0
? std::sqrt(sum_error_sq / static_cast<double>(sample_count))
: 0.0;
}
double fundamentalAmplitude() const
{
if (sample_count == 0) {
return 0.0;
}
const double scale = 2.0 / static_cast<double>(sample_count);
return scale * std::sqrt(sin_projection * sin_projection +
cos_projection * cos_projection);
}
double phaseRad() const
{
return std::atan2(cos_projection, sin_projection);
}
};
double normalizePhaseRad(double phase)
{
while (phase > kPi) {
phase -= 2.0 * kPi;
}
while (phase < -kPi) {
phase += 2.0 * kPi;
}
return phase;
}
double radToDeg(const double rad)
{
return rad * 180.0 / kPi;
}
config::DeviceManagerConfig createEthercatOnlyDeviceManagerConfig()
{
config::DeviceManagerConfig config;
config.set_name("eyou_motor_device_manager_real_test");
config.set_version("test");
auto* motor_entry = config.add_devices();
motor_entry->set_id(kMotorManagerId);
motor_entry->set_type(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
motor_entry->set_config_file(kMotorConfigFile);
motor_entry->set_enable(true);
return config;
}
void printMotorState(const int motor_id, const std::shared_ptr<AbstractMotor>& motor)
{
ASSERT_NE(motor, nullptr);
std::cout << "motor_id=" << motor_id
<< ", joint_name=" << motor->jointName()
<< ", q=" << motor->getQ() << " rad"
<< ", qd=" << motor->getQd() << " rad/s"
<< std::endl;
}
} // namespace
TEST(EyouMotorDeviceManagerRealTest, InitFourEthercatMotorsAndPrintState)
{
ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
DeviceManagerDestroyGuard guard;
auto& device_manager =
DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
ASSERT_NE(motor_manager, nullptr);
MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
for (const int motor_id : kFourMotorIds) {
auto motor = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
ASSERT_NE(motor, nullptr);
printMotorState(motor_id, motor);
}
}
TEST(EyouMotorDeviceManagerRealTest, CommandFourCyclicPositionRaisedCosineTrajectory)
{
ConfigHelper::setConfigRootFromFile("cmvr-es/config/cmvr_es.pb.txt");
DeviceManagerDestroyGuard guard;
auto& device_manager =
DeviceManager::getInstance(createEthercatOnlyDeviceManagerConfig());
auto motor_manager = device_manager.getDevice<MotorManager>(kMotorManagerId);
ASSERT_NE(motor_manager, nullptr);
MotorManagerStopGuard motor_manager_stop_guard(motor_manager);
std::vector<std::shared_ptr<AbstractMotor>> motors(kFourMotorIds.size());
for (std::size_t i = 0; i < kFourMotorIds.size(); ++i) {
const int motor_id = kFourMotorIds[i];
motors[i] = motor_manager->getMotor(static_cast<std::uint8_t>(motor_id));
ASSERT_NE(motors[i], nullptr);
printMotorState(motor_id, motors[i]);
}
MultiMotorSafetyGuard safety_guard(motors);
std::cout << "calibrate zero for four EtherCAT motors" << std::endl;
for (const auto& motor : motors) {
ASSERT_TRUE(motor->torqueOff());
}
for (std::size_t i = 0; i < motors.size(); ++i) {
const int motor_id = kFourMotorIds[i];
std::cout << "before calibrateZeroQ: ";
printMotorState(motor_id, motors[i]);
ASSERT_TRUE(motors[i]->calibrateZeroQ());
std::cout << "after calibrateZeroQ: ";
printMotorState(motor_id, motors[i]);
}
for (const auto& motor : motors) {
ASSERT_TRUE(motor->torqueOn());
}
for (const auto& motor : motors) {
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
}
std::vector<double> center_q;
std::vector<double> actual_qd;
ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, center_q, actual_qd));
const double omega = 2.0 * kPi / kFourMotorPeriodS;
std::cout << "command four motors in CSP, duration="
<< kFourMotorTrajectoryDuration.count()
<< " ms, command_period=" << kCyclicCommandPeriod.count()
<< " ms, raised_cosine_coefficient=" << kRaisedCosineCoefficientRad
<< " rad, position_excursion=" << 2.0 * kRaisedCosineCoefficientRad
<< " rad, period=" << kFourMotorPeriodS
<< " s" << std::endl;
const auto start_time = std::chrono::steady_clock::now();
const auto end_time = start_time + kFourMotorTrajectoryDuration;
auto next_command_time = start_time;
auto next_stats_time = start_time;
std::uint64_t missed_command_deadlines = 0;
std::array<TrackingErrorStats, kFourMotorIds.size()> error_stats;
std::vector<double> target_q(motors.size(), 0.0);
std::vector<double> target_qd(motors.size(), 0.0);
std::vector<double> actual_q;
std::array<double, kFourMotorIds.size()> minimum_actual_q{};
std::array<double, kFourMotorIds.size()> maximum_actual_q{};
std::copy(center_q.begin(), center_q.end(), minimum_actual_q.begin());
std::copy(center_q.begin(), center_q.end(), maximum_actual_q.begin());
double max_error_spread_rad = 0.0;
double sum_error_spread_sq = 0.0;
std::int64_t error_spread_sample_count = 0;
while (true) {
std::this_thread::sleep_until(next_command_time);
const auto now = std::chrono::steady_clock::now();
if (now > end_time) {
break;
}
const double t_s = std::chrono::duration<double>(now - start_time).count();
for (std::size_t i = 0; i < motors.size(); ++i) {
const double theta = omega * t_s + kFourMotorPhaseRad[i];
target_q[i] =
center_q[i] + kRaisedCosineCoefficientRad * (1.0 - std::cos(theta));
target_qd[i] = kRaisedCosineCoefficientRad * omega * std::sin(theta);
}
ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
if (now >= next_stats_time) {
ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
double min_error = std::numeric_limits<double>::max();
double max_error = std::numeric_limits<double>::lowest();
for (std::size_t i = 0; i < motors.size(); ++i) {
const double theta = omega * t_s + kFourMotorPhaseRad[i];
const double error = actual_q[i] - target_q[i];
error_stats[i].add(error, theta);
minimum_actual_q[i] = std::min(minimum_actual_q[i], actual_q[i]);
maximum_actual_q[i] = std::max(maximum_actual_q[i], actual_q[i]);
min_error = std::min(min_error, error);
max_error = std::max(max_error, error);
}
const double error_spread = max_error - min_error;
max_error_spread_rad = std::max(max_error_spread_rad, error_spread);
sum_error_spread_sq += error_spread * error_spread;
++error_spread_sample_count;
next_stats_time = now + kStatsSamplePeriod;
}
next_command_time += kCyclicCommandPeriod;
const auto command_complete_time = std::chrono::steady_clock::now();
if (next_command_time <= command_complete_time) {
const auto skipped_periods =
(command_complete_time - next_command_time) / kCyclicCommandPeriod + 1;
missed_command_deadlines += static_cast<std::uint64_t>(skipped_periods);
next_command_time += skipped_periods * kCyclicCommandPeriod;
}
}
std::copy(center_q.begin(), center_q.end(), target_q.begin());
std::fill(target_qd.begin(), target_qd.end(), 0.0);
ASSERT_TRUE(motor_manager->commandCyclicPositionsAtomic(motors, target_q, target_qd));
std::this_thread::sleep_for(kHoldAfterTrajectoryDuration);
ASSERT_TRUE(motor_manager->readFeedbacksAtomic(motors, actual_q, actual_qd));
std::cout << "after four motor CSP trajectory" << std::endl;
for (std::size_t i = 0; i < motors.size(); ++i) {
printMotorState(kFourMotorIds[i], motors[i]);
}
const double reference_phase = error_stats.front().phaseRad();
const double rms_error_spread =
error_spread_sample_count > 0
? std::sqrt(sum_error_spread_sq / static_cast<double>(error_spread_sample_count))
: 0.0;
std::cout << "four motor CSP tracking error statistics, sample_period="
<< kStatsSamplePeriod.count()
<< " ms, samples=" << error_stats.front().sample_count
<< ", missed_command_deadlines=" << missed_command_deadlines
<< ", max_error_spread=" << max_error_spread_rad
<< " rad, rms_error_spread=" << rms_error_spread
<< " rad" << std::endl;
for (std::size_t i = 0; i < motors.size(); ++i) {
const double phase = error_stats[i].phaseRad();
const double relative_phase = normalizePhaseRad(phase - reference_phase);
const double relative_phase_ms = relative_phase / omega * 1000.0;
std::cout << " motor_id=" << kFourMotorIds[i]
<< ", mean_error=" << error_stats[i].mean()
<< " rad, rms_error=" << error_stats[i].rms()
<< " rad, max_abs_error=" << error_stats[i].max_abs_error
<< " rad, error_fundamental_amp="
<< error_stats[i].fundamentalAmplitude()
<< " rad, error_phase=" << phase
<< " rad (" << radToDeg(phase)
<< " deg), relative_phase_to_motor1=" << relative_phase
<< " rad (" << radToDeg(relative_phase)
<< " deg, " << relative_phase_ms
<< " ms)" << std::endl;
EXPECT_GE(maximum_actual_q[i] - minimum_actual_q[i],
kMinimumPositionExcursionRad)
<< "motor_id=" << kFourMotorIds[i] << " did not complete enough motion";
EXPECT_LE(error_stats[i].max_abs_error,
kMaximumAbsoluteTrackingErrorRad)
<< "motor_id=" << kFourMotorIds[i] << " exceeded maximum tracking error";
EXPECT_LE(error_stats[i].rms(), kMaximumRmsTrackingErrorRad)
<< "motor_id=" << kFourMotorIds[i] << " exceeded RMS tracking error";
EXPECT_NEAR(actual_q[i], center_q[i], kFinalPositionToleranceRad)
<< "motor_id=" << kFourMotorIds[i] << " did not return to its start position";
}
EXPECT_LE(max_error_spread_rad, kMaximumErrorSpreadRad);
EXPECT_TRUE(safety_guard.stop());
}
} // namespace cmvr::device

569
cmvr-es/devices/motor/drivers/ethercat_motor/src/vendor/eyou/eyou_motor_real_test.cpp vendored Normal file
View File

@ -0,0 +1,569 @@
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include <chrono>
#include <cmath>
#include <cstdint>
#include <iomanip>
#include <iostream>
#include <memory>
#include <sstream>
#include <thread>
#include <utility>
#include <gtest/gtest.h>
#include "cmvr/msgs/cia402.pb.h"
#include "devices/motor/abstract_motor.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
namespace cmvr::device {
namespace {
constexpr int kMotorId = 1;
constexpr std::chrono::milliseconds kCommandSamplePeriod{100};
constexpr std::chrono::milliseconds kCyclicCommandPeriod{1};
constexpr std::chrono::milliseconds kFeedbackSampleDuration{5000};
constexpr double kDefaultGearRatio = 101.0;
constexpr double kEncoderCountsPerMotorRev = 65536.0;
constexpr double kPi = 3.14159265358979323846;
config::MotorGroupConfig createSingleSlaveGroup()
{
config::MotorGroupConfig group;
group.set_id("eyou_motor_real_test");
group.set_bus_type(config::MOTOR_BUS_ETHERCAT);
group.set_vendor(config::MOTOR_VENDOR_EYOU);
group.set_protocol(config::MOTOR_PROTOCOL_ETHERCAT_CIA402);
auto* ethercat = group.mutable_ethercat();
ethercat->set_master_index(0);
ethercat->set_cycle_us(1000);
ethercat->set_slave_op_timeout_ms(12000);
ethercat->set_slave_state_poll_period_ms(10);
auto* cia402 = ethercat->mutable_cia402();
cia402->set_state_transition_timeout_ms(1200);
cia402->set_velocity_stop_timeout_ms(2000);
cia402->set_status_poll_period_ms(10);
cia402->set_stopped_velocity_tolerance_rad_s(0.001);
auto* zero_calibration = ethercat->mutable_zero_calibration();
zero_calibration->set_timeout_ms(2000);
zero_calibration->set_poll_period_ms(10);
zero_calibration->set_stable_sample_count(5);
zero_calibration->set_position_tolerance_counts(10000);
zero_calibration->set_stable_delta_counts(1000);
auto* dc = ethercat->mutable_dc();
dc->set_enable(false);
dc->set_reference_motor_id(kMotorId);
dc->set_sync0_cycle_us(1000);
dc->set_sync0_shift_us(0);
dc->set_sync_reference_clock_period(1);
dc->set_assign_activate(768);
dc->set_sync_monitor_period_ms(1000);
auto* slave = ethercat->add_slaves();
slave->set_motor_id(kMotorId);
slave->set_alias(0);
slave->set_position(0);
return group;
}
class RuntimeStopGuard {
public:
explicit RuntimeStopGuard(std::shared_ptr<EthercatMotorBusRuntime> runtime)
: runtime_(std::move(runtime))
{
}
~RuntimeStopGuard()
{
if (runtime_) {
runtime_->stop();
}
}
private:
std::shared_ptr<EthercatMotorBusRuntime> runtime_;
};
std::shared_ptr<EthercatMotorBusRuntime> startRuntime()
{
auto runtime = std::make_shared<EthercatMotorBusRuntime>();
runtime->setPdoMapping(createEyouCia402PdoMapping());
if (!runtime->init(createSingleSlaveGroup())) {
return nullptr;
}
if (!runtime->start()) {
runtime->stop();
return nullptr;
}
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
return runtime;
}
std::shared_ptr<Cia402Protocol> createProtocol(
const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
{
return std::make_shared<Cia402Protocol>(runtime, runtime->config().cia402());
}
config::MotorConfigItem createMotorConfig()
{
config::MotorConfigItem config;
config.set_id(kMotorId);
config.set_joint_name("ethercat_test_joint");
config.set_limit_q_lb(-36.14);
config.set_limit_q_ub(36.14);
config.set_limit_qd(10.0);
config.set_limit_qdd(100.0);
config.set_encoder_counts_per_rev(kEncoderCountsPerMotorRev);
config.set_gear_ratio(kDefaultGearRatio);
return config;
}
std::unique_ptr<AbstractMotor> createMotor(
const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
{
auto motor = std::make_unique<EyouMotor>(
createMotorConfig(),
createProtocol(runtime),
std::make_unique<EyouMotorAdapter>(runtime));
if (!motor->init()) {
return nullptr;
}
return motor;
}
void printMotorState(const char* label, AbstractMotor& motor)
{
std::cout << label
<< ": motor_q=" << motor.getQ() << " rad"
<< ", motor_qd=" << motor.getQd() << " rad/s"
<< std::endl;
}
std::string hex16(const std::uint16_t value)
{
std::ostringstream oss;
oss << "0x" << std::uppercase << std::hex << std::setw(4) << std::setfill('0')
<< value;
return oss.str();
}
void printRawEthercatFeedback(const char* label,
const std::shared_ptr<EthercatMotorBusRuntime>& runtime)
{
std::uint16_t statusword = 0;
std::int8_t mode_display = 0;
std::int32_t actual_position = 0;
std::int32_t actual_velocity = 0;
std::int16_t actual_torque = 0;
std::uint16_t error_code = 0;
runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_STATUS_WORD_6041, 0x00, statusword);
runtime->readPdo<std::int8_t>(kMotorId, msgs::CIA402_MODE_DISPLAY_6061, 0x00, mode_display);
runtime->readPdo<std::int32_t>(kMotorId, msgs::CIA402_ACTUAL_POSITION_6064, 0x00,
actual_position);
runtime->readPdo<std::int32_t>(kMotorId, msgs::CIA402_ACTUAL_VELOCITY_606C, 0x00,
actual_velocity);
runtime->readPdo<std::int16_t>(kMotorId, msgs::CIA402_ACTUAL_TORQUE_6077, 0x00,
actual_torque);
runtime->readPdo<std::uint16_t>(kMotorId, msgs::CIA402_ERROR_CODE_603F, 0x00, error_code);
std::cout << label
<< ": statusword=" << hex16(statusword)
<< ", mode_display=" << static_cast<int>(mode_display)
<< ", actual_position=" << actual_position
<< ", actual_velocity=" << actual_velocity
<< ", actual_torque=" << actual_torque
<< ", error_code=" << hex16(error_code)
<< std::endl;
}
void sampleMotorState(AbstractMotor& motor,
const std::chrono::milliseconds duration)
{
for (auto elapsed = std::chrono::milliseconds{0};
elapsed < duration;
elapsed += kCommandSamplePeriod) {
std::this_thread::sleep_for(kCommandSamplePeriod);
std::cout << "t=" << (elapsed + kCommandSamplePeriod).count() << " ms";
printMotorState("", motor);
}
}
double nearbySafeTarget(const double current_q, const double delta_rad)
{
return current_q + (current_q > 0.0 ? -std::abs(delta_rad) : std::abs(delta_rad));
}
} // namespace
TEST(EyouMotorRealTest, ReadMotorStateOnly)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOn());
printMotorState("motor state", *motor);
printRawEthercatFeedback("raw feedback", runtime);
for (int i = 1; i <= 10; ++i) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
printMotorState("motor state", *motor);
printRawEthercatFeedback("raw feedback", runtime);
}
}
TEST(EyouMotorRealTest, CalibrateZeroQPrintBeforeAndAfter)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
printMotorState("before calibrateZeroQ", *motor);
ASSERT_TRUE(motor->calibrateZeroQ());
printMotorState("after calibrateZeroQ", *motor);
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
ASSERT_TRUE(motor->commandProfilePosition(1.5,0.8,3.0));
sampleMotorState(*motor, kFeedbackSampleDuration);
}
TEST(EyouMotorRealTest, CommandProfilePosition)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
ASSERT_TRUE(motor->commandProfilePosition(-3.0, 0.5, 1.0));
sampleMotorState(*motor, kFeedbackSampleDuration);
}
TEST(EyouMotorRealTest, CommandProfileVelocity)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
std::cout << "motor.commandProfileVelocity(0.3 rad/s, 1.0 rad/s^2)" << std::endl;
ASSERT_TRUE(motor->commandProfileVelocity(0.3, 1.0));
sampleMotorState(*motor, kFeedbackSampleDuration);
std::cout << "motor.commandProfileVelocity(0 rad/s, 1.0 rad/s^2)" << std::endl;
ASSERT_TRUE(motor->commandProfileVelocity(0.0, 1.0));
sampleMotorState(*motor, std::chrono::milliseconds{1000});
}
TEST(EyouMotorRealTest, CommandCyclicPosition)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
const std::chrono::milliseconds trajectory_duration{12000};
const double period_s = 6.0;
const double excursion_rad = 3.0;
const double center_q = motor->getQ();
const double omega = 2.0 * kPi / period_s;
std::cout << "motor.commandCyclicPosition(raised cosine), center_q=" << center_q
<< " rad, period=" << period_s
<< " s, excursion=" << excursion_rad
<< " rad, command_period=" << kCyclicCommandPeriod.count()
<< " ms" << std::endl;
const auto start_time = std::chrono::steady_clock::now();
const auto total_ticks = trajectory_duration / kCyclicCommandPeriod;
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
const auto elapsed = tick * kCyclicCommandPeriod;
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
const double theta = omega * t_s;
const double target_q =
center_q + 0.5 * excursion_rad * (1.0 - std::cos(theta));
const double target_qd =
0.5 * excursion_rad * omega * std::sin(theta);
ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
std::cout << "t=" << elapsed.count()
<< " ms, target_q=" << target_q
<< " rad, target_qd=" << target_qd
<< " rad/s";
printMotorState("", *motor);
}
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
}
}
TEST(EyouMotorRealTest, CommandCyclicVelocity)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
const std::chrono::milliseconds trajectory_duration{12000};
const double period_s = 5.0;
const double excursion_rad = 5.0;
const double phase_rad = 0.0;
const double omega = 2.0 * kPi / period_s;
const double velocity_amplitude_rad_s = 0.5 * excursion_rad * omega;
std::cout << "motor.commandCyclicVelocity(sin), period=" << period_s
<< " s, velocity_amplitude=" << velocity_amplitude_rad_s
<< " rad/s, excursion=" << excursion_rad
<< " rad, phase=" << phase_rad
<< " rad, command_period=" << kCyclicCommandPeriod.count()
<< " ms" << std::endl;
const auto start_time = std::chrono::steady_clock::now();
const auto total_ticks = trajectory_duration / kCyclicCommandPeriod;
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
const auto elapsed = tick * kCyclicCommandPeriod;
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
const double theta = omega * t_s + phase_rad;
const double target_qd = velocity_amplitude_rad_s * std::sin(theta);
ASSERT_TRUE(motor->commandCyclicVelocity(target_qd));
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
std::cout << "t=" << elapsed.count()
<< " ms, target_qd=" << target_qd
<< " rad/s";
printMotorState("", *motor);
printRawEthercatFeedback("raw feedback", runtime);
}
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
}
std::cout << "motor.commandCyclicVelocity(0 rad/s)" << std::endl;
ASSERT_TRUE(motor->commandCyclicVelocity(0.0));
sampleMotorState(*motor, std::chrono::milliseconds{500});
printRawEthercatFeedback("raw feedback after stop", runtime);
}
TEST(EyouMotorRealTest, QuickStopAfterTwoSeconds)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY));
const std::chrono::milliseconds run_duration{2000};
const double period_s = 6.0;
const double velocity_amplitude_rad_s = 4.5;
const double phase_rad = 0.0;
const double omega = 2.0 * kPi / period_s;
std::cout << "motor.commandCyclicVelocity(sin), then quickStop at "
<< run_duration.count()
<< " ms, period=" << period_s
<< " s, velocity_amplitude=" << velocity_amplitude_rad_s
<< " rad/s, phase=" << phase_rad
<< " rad, command_period=" << kCyclicCommandPeriod.count()
<< " ms" << std::endl;
const auto start_time = std::chrono::steady_clock::now();
const auto total_ticks = run_duration / kCyclicCommandPeriod;
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
const auto elapsed = tick * kCyclicCommandPeriod;
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
const double theta = omega * t_s + phase_rad;
const double target_qd = velocity_amplitude_rad_s * std::sin(theta);
ASSERT_TRUE(motor->commandCyclicVelocity(target_qd));
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
std::cout << "t=" << elapsed.count()
<< " ms, target_qd=" << target_qd
<< " rad/s";
printMotorState("", *motor);
printRawEthercatFeedback("raw feedback", runtime);
}
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
}
std::cout << "motor.quickStop()" << std::endl;
ASSERT_TRUE(motor->quickStop());
printMotorState("after quickStop", *motor);
printRawEthercatFeedback("raw feedback", runtime);
sampleMotorState(*motor, std::chrono::milliseconds{1000});
printRawEthercatFeedback("raw feedback", runtime);
}
TEST(EyouMotorRealTest, QuickStopInProfilePosition)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_POSITION));
const std::chrono::milliseconds quick_stop_time{1000};
const double start_q = motor->getQ();
const double target_q = nearbySafeTarget(start_q, 4.0);
const double max_qd = 2.0;
const double max_qdd = 10.0;
std::cout << "motor.commandProfilePosition(" << target_q
<< " rad, " << max_qd
<< " rad/s, " << max_qdd
<< " rad/s^2), then quickStop at "
<< quick_stop_time.count() << " ms" << std::endl;
ASSERT_TRUE(motor->commandProfilePosition(target_q, max_qd, max_qdd));
std::cout << "wait " << quick_stop_time.count()
<< " ms before quickStop" << std::endl;
sampleMotorState(*motor, quick_stop_time);
printRawEthercatFeedback("raw feedback before quickStop", runtime);
std::cout << "motor.quickStop()" << std::endl;
ASSERT_TRUE(motor->quickStop());
printMotorState("after quickStop", *motor);
printRawEthercatFeedback("raw feedback", runtime);
sampleMotorState(*motor, std::chrono::milliseconds{1000});
printRawEthercatFeedback("raw feedback", runtime);
}
TEST(EyouMotorRealTest, QuickStopInProfileVelocity)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_PROFILE_VELOCITY));
const std::chrono::milliseconds quick_stop_time{2000};
const double target_qd = motor->getQ() > 0.0 ? -2.0 : 2.0;
const double max_qdd = 10.0;
std::cout << "motor.commandProfileVelocity(" << target_qd
<< " rad/s, " << max_qdd
<< " rad/s^2), then quickStop at "
<< quick_stop_time.count() << " ms" << std::endl;
ASSERT_TRUE(motor->commandProfileVelocity(target_qd, max_qdd));
std::cout << "wait " << quick_stop_time.count()
<< " ms before quickStop" << std::endl;
sampleMotorState(*motor, quick_stop_time);
printRawEthercatFeedback("raw feedback before quickStop", runtime);
std::cout << "motor.quickStop()" << std::endl;
ASSERT_TRUE(motor->quickStop());
printMotorState("after quickStop", *motor);
printRawEthercatFeedback("raw feedback", runtime);
sampleMotorState(*motor, std::chrono::milliseconds{1000});
printRawEthercatFeedback("raw feedback", runtime);
}
TEST(EyouMotorRealTest, QuickStopInCyclicPosition)
{
auto runtime = startRuntime();
ASSERT_NE(runtime, nullptr);
RuntimeStopGuard runtime_guard(runtime);
auto motor = createMotor(runtime);
ASSERT_NE(motor, nullptr);
ASSERT_TRUE(motor->torqueOff());
ASSERT_TRUE(motor->calibrateZeroQ());
ASSERT_TRUE(motor->torqueOn());
ASSERT_TRUE(motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION));
const std::chrono::milliseconds run_duration{2000};
const double start_q = motor->getQ();
const double target_qd = start_q > 0.0 ? -2.0 : 2.0;
std::cout << "motor.commandCyclicPosition(linear), start_q=" << start_q
<< " rad, target_qd=" << target_qd
<< " rad/s, then quickStop at "
<< run_duration.count()
<< " ms, command_period=" << kCyclicCommandPeriod.count()
<< " ms" << std::endl;
const auto start_time = std::chrono::steady_clock::now();
const auto total_ticks = run_duration / kCyclicCommandPeriod;
for (std::int64_t tick = 0; tick <= total_ticks; ++tick) {
const auto elapsed = tick * kCyclicCommandPeriod;
const double t_s = static_cast<double>(elapsed.count()) / 1000.0;
const double target_q = start_q + target_qd * t_s;
ASSERT_TRUE(motor->commandCyclicPosition(target_q, target_qd));
if (elapsed.count() % kCommandSamplePeriod.count() == 0) {
std::cout << "t=" << elapsed.count()
<< " ms, target_q=" << target_q
<< " rad, target_qd=" << target_qd
<< " rad/s";
printMotorState("", *motor);
printRawEthercatFeedback("raw feedback", runtime);
}
std::this_thread::sleep_until(start_time + (tick + 1) * kCyclicCommandPeriod);
}
std::cout << "motor.quickStop()" << std::endl;
ASSERT_TRUE(motor->quickStop());
printMotorState("after quickStop", *motor);
printRawEthercatFeedback("raw feedback", runtime);
sampleMotorState(*motor, std::chrono::milliseconds{1000});
printRawEthercatFeedback("raw feedback", runtime);
}
} // namespace cmvr::device

28
cmvr-es/devices/motor/drivers/mujoco/include/mujoco_motor.h
View File

@ -21,29 +21,37 @@ public:
std::string typeName() const override { return "MujocoMotor"; }
bool init() override;
void setMode(msgs::RunMode mode) override;
bool setMode(msgs::RunMode mode) override;
msgs::RunMode getMode() override;
void torqueOff() override;
bool torqueOn() override;
bool torqueOff() override;
bool brakeRelease() override;
bool quickStop() override;
void setLimitQ(double ub, double lb) override;
void setLimitQd(double qd) override;
void setLimitQdd(double u_qdd, double l_qdd) override;
void brake() override;
void setQ(double q) override;
void setTarget(double q, double qd) override;
void setTarget(double qd) override;
bool calibrateZeroQ() override;
bool reachedTargetQ() override;
void setQd(double qd) override;
bool commandProfilePosition(double target_q,
double max_qd = 0.0,
double max_qdd = 0.0) override;
bool commandProfileVelocity(double target_qd, double max_qdd = 0.0) override;
bool commandCyclicPosition(double target_q,
double target_qd = 0.0) override;
bool commandCyclicVelocity(double target_qd) override;
bool commandCyclicTorque(double target_tau) override;
double getQ() override;
double getQd() override;
static bool setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities);
static bool commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities);
private:
bool holdPosition_();
double clampQ_(double q) const;
double clampQd_(double qd) const;
std::shared_ptr<simulate::MujocoWorld> worldLocked_() const;

112
cmvr-es/devices/motor/drivers/mujoco/src/mujoco_motor.cpp
View File

@ -42,10 +42,11 @@ bool MujocoMotor::init()
return true;
}
void MujocoMotor::setMode(const msgs::RunMode mode)
bool MujocoMotor::setMode(const msgs::RunMode mode)
{
std::scoped_lock lock(mtx_);
mode_ = mode;
return true;
}
msgs::RunMode MujocoMotor::getMode()
@ -54,11 +55,27 @@ msgs::RunMode MujocoMotor::getMode()
return mode_;
}
void MujocoMotor::torqueOff()
bool MujocoMotor::torqueOn()
{
brake();
return holdPosition_();
}
bool MujocoMotor::torqueOff()
{
const bool ok = holdPosition_();
std::scoped_lock lock(mtx_);
mode_ = msgs::RUN_MODE_UNSPECIFIED;
return ok;
}
bool MujocoMotor::brakeRelease()
{
return true;
}
bool MujocoMotor::quickStop()
{
return holdPosition_();
}
void MujocoMotor::setLimitQ(const double ub, const double lb)
@ -82,38 +99,78 @@ void MujocoMotor::setLimitQdd(const double u_qdd, const double l_qdd)
info_.limit_qdd = std::max(limit_qdd_upper_, std::abs(limit_qdd_lower_));
}
void MujocoMotor::brake()
bool MujocoMotor::holdPosition_()
{
const auto world = worldLocked_();
double q = 0.0;
if (!world || !world->getJointPosition(info_.joint_name, q)) {
return;
return false;
}
std::scoped_lock lock(mtx_);
target_q_ = q;
mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
world->setJointTargetState(info_.joint_name, q, 0.0);
return true;
}
void MujocoMotor::setQ(const double q)
bool MujocoMotor::commandProfilePosition(const double target_q,
const double max_qd,
const double max_qdd)
{
setTarget(q, 0.0);
(void)max_qdd;
std::scoped_lock lock(mtx_);
const auto world = worldLocked_();
if (!world) {
return false;
}
mode_ = msgs::RUN_MODE_PROFILE_POSITION;
target_q_ = clampQ_(target_q);
const double profile_qd = max_qd > 0.0 ? max_qd : info_.limit_qd;
return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(profile_qd));
}
void MujocoMotor::setTarget(const double q, const double qd)
bool MujocoMotor::commandProfileVelocity(const double target_qd, const double max_qdd)
{
(void)max_qdd;
std::scoped_lock lock(mtx_);
const auto world = worldLocked_();
if (!world) {
return false;
}
mode_ = msgs::RUN_MODE_PROFILE_VELOCITY;
return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd));
}
bool MujocoMotor::commandCyclicPosition(const double target_q,
const double target_qd)
{
std::scoped_lock lock(mtx_);
const auto world = worldLocked_();
if (!world) {
return;
return false;
}
target_q_ = clampQ_(q);
world->setJointTargetState(info_.joint_name, target_q_, clampQd_(qd));
mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
target_q_ = clampQ_(target_q);
return world->setJointTargetState(info_.joint_name, target_q_, clampQd_(target_qd));
}
void MujocoMotor::setTarget(const double qd)
bool MujocoMotor::commandCyclicVelocity(const double target_qd)
{
setQd(qd);
std::scoped_lock lock(mtx_);
const auto world = worldLocked_();
if (!world) {
return false;
}
mode_ = msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY;
return world->setJointTargetVelocity(info_.joint_name, clampQd_(target_qd));
}
bool MujocoMotor::commandCyclicTorque(const double target_tau)
{
(void)target_tau;
CMVR_LOG(ERROR) << "[MujocoMotor] cyclic torque command is not implemented: "
<< info_.joint_name;
return false;
}
bool MujocoMotor::calibrateZeroQ()
@ -140,16 +197,6 @@ bool MujocoMotor::reachedTargetQ()
}
}
void MujocoMotor::setQd(const double qd)
{
std::scoped_lock lock(mtx_);
const auto world = worldLocked_();
if (!world) {
return;
}
world->setJointTargetVelocity(info_.joint_name, clampQd_(qd));
}
double MujocoMotor::getQ()
{
const auto world = worldLocked_();
@ -170,9 +217,10 @@ double MujocoMotor::getQd()
return qd;
}
bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities)
bool MujocoMotor::commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities)
{
if (motors.size() != positions.size() || motors.size() != velocities.size() || motors.empty()) {
return false;
@ -187,7 +235,7 @@ bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor
clamped_velocities.reserve(motors.size());
for (std::size_t i = 0; i < motors.size(); ++i) {
const auto& motor = motors[i];
const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
if (!motor) {
return false;
}
@ -217,9 +265,13 @@ bool MujocoMotor::setTargetsAtomic(const std::vector<std::shared_ptr<MujocoMotor
}
for (std::size_t i = 0; i < motors.size(); ++i) {
std::scoped_lock lock(motors[i]->mtx_);
motors[i]->target_q_ = clamped_positions[i];
motors[i]->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
const auto motor = std::dynamic_pointer_cast<MujocoMotor>(motors[i]);
if (!motor) {
return false;
}
std::scoped_lock lock(motor->mtx_);
motor->target_q_ = clamped_positions[i];
motor->mode_ = msgs::RUN_MODE_CYCLIC_SYNC_POSITION;
}
return true;
}

30
cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor.h
View File

@ -22,6 +22,8 @@ namespace cmvr {
info_.limit_q_ub = config.limit_q_ub();
info_.limit_qd = config.limit_qd() > 0.0 ? config.limit_qd() : 0.5;
info_.limit_qdd = config.limit_qdd() > 0.0 ? config.limit_qdd() : 10.0;
encoder_counts_per_rev_ = config.encoder_counts_per_rev();
gear_ratio_ = config.gear_ratio();
node_id_ = info_.id;
}
@ -37,6 +39,19 @@ namespace cmvr {
}
if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) {
auto canopen_protocol = std::dynamic_pointer_cast<Ti5MotorCanopenProtocol>(protocol_);
if (!canopen_protocol) {
CMVR_LOG(ERROR) << "[Ti5Motor] invalid CANopen protocol for motor: "
<< info_.joint_name;
return false;
}
if (encoder_counts_per_rev_ <= 0.0 || gear_ratio_ <= 0.0) {
CMVR_LOG(ERROR) << "[Ti5Motor] missing encoder conversion config: "
<< info_.joint_name
<< ", encoder_counts_per_rev=" << encoder_counts_per_rev_
<< ", gear_ratio=" << gear_ratio_;
return false;
}
protocol_->setMotorConversion(node_id_, encoder_counts_per_rev_, gear_ratio_);
// torqueOff(node_id_);
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION);
// canopen_protocol->torqueOff(node_id_);
@ -44,9 +59,14 @@ namespace cmvr {
// canopen_protocol->configProfile(node_id_,4000,8000,8000);
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL);
canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE);
canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
// canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15);
// canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
if (!canopen_protocol->setMode(
node_id_, msgs::RUN_MODE_CYCLIC_SYNC_POSITION)) {
CMVR_LOG(ERROR) << "[Ti5Motor] failed to initialize operation mode: "
<< info_.joint_name;
return false;
}
// canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15);
// canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CIA402_CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15);
canopen_protocol->setLimitQ(node_id_, info_.limit_q_ub, info_.limit_q_lb);
canopen_protocol->setLimitQd(node_id_, info_.limit_qd);
canopen_protocol->setLimitQdd(node_id_, info_.limit_qdd, -info_.limit_qdd);
@ -55,6 +75,10 @@ namespace cmvr {
}
return true;
}
private:
double encoder_counts_per_rev_{0.0};
double gear_ratio_{0.0};
};

69
cmvr-es/devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h
View File

@ -18,6 +18,7 @@
#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo1.h"
#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_rpdo2.h"
#include <cmath>
#include <unordered_map>
namespace cmvr {
namespace device {
@ -29,28 +30,40 @@ namespace cmvr {
bool initNode(uint8_t node_id) override;
void setMode(uint8_t node_id, msgs::RunMode mode);
void setTarget(uint8_t node_id, double angle_rad, double vel) override;
void setTarget(uint8_t node_id, double vel) override;
void setQ(uint8_t node_id, double angle_rad) override;
bool setMode(uint8_t node_id, msgs::RunMode mode) override;
void setLimitQ(uint8_t node_id, double ub, double lb) override;
void setLimitQd(uint8_t node_id, double qd) override;
void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) override;
bool calibrateZeroQ(uint8_t node_id) override;
void brake(uint8_t node_id) override;
bool torqueOn(uint8_t node_id) override;
bool torqueOff(uint8_t node_id) override;
bool brakeRelease(uint8_t node_id) override;
bool quickStop(uint8_t node_id) override;
bool reachedTargetQ(uint8_t node_id) override;
double getQ(uint8_t node_id) override;
double getQd(uint8_t node_id) override;
void setQd(uint8_t node_id, double qd) override;
void setQdd(uint8_t node_id, double qdd) override;
void torqueOff(uint8_t node_id) override;
bool commandProfilePosition(uint8_t node_id,
double target_q,
double max_qd,
double max_qdd) override;
bool commandProfileVelocity(uint8_t node_id,
double target_qd,
double max_qdd) override;
bool commandCyclicPosition(uint8_t node_id,
double target_q,
double target_qd) override;
bool commandCyclicVelocity(uint8_t node_id,
double target_qd) override;
bool commandCyclicTorque(uint8_t node_id, double target_tau) override;
void setMotorConversion(uint8_t node_id,
double encoder_counts_per_rev,
double gear_ratio) override;
void seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms = 10);
void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, msgs::ObIndex index,
msgs::ObSubIndex sub_index, uint32_t data, uint32_t delay_ms = 10);
void seedSdoRequest(uint8_t node_id, msgs::CommandSpecifier cs, uint32_t index,
uint32_t sub_index, uint32_t data, uint32_t delay_ms = 10);
void configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel);
void configPdo(uint8_t node_id);
@ -61,23 +74,24 @@ namespace cmvr {
return data_ptr;
}
msgs::RunMode getMode(uint8_t node_id) override {
return GetRobotDetail()->motors().at(node_id).run_mode();
// cur_mode_[node_id] = feed_mode;
// return feed_mode;
// return cur_mode_[node_id];
}
msgs::RunMode getMode(uint8_t node_id) override;
private:
static constexpr double GearRatio = 101.0; // 电机减速比
static constexpr double RADTODEG = 180.0 / M_PI;
static constexpr double Ti5VelocityUnitScale = 100.0;
static constexpr double Ti5AccelerationTimeScale = 1000.0;
struct MotorConversion {
double encoder_counts_per_rev{0.0};
double gear_ratio{0.0};
};
std::shared_ptr<AbstractCanbus> can_client_{nullptr};
// key node_id
// std::unordered_map<uint8_t,msgs::RunMode> cur_mode_{};
std::unordered_map<uint8_t,uint32_t> last_Qd_{};
std::unordered_map<uint8_t,uint32_t> last_Qdd_{};
std::unordered_map<uint8_t, MotorConversion> motor_conversions_{};
std::shared_ptr<device::CanSender<msgs::RobotDetail> > can_sender_{nullptr};
std::shared_ptr<device::MessageManager<msgs::RobotDetail> > message_manager_{nullptr};
@ -94,11 +108,8 @@ namespace cmvr {
std::map<uint8_t, motor::Ti5MotorRPDO1 *> rpdo1_commands_{};
std::map<uint8_t, motor::Ti5MotorRPDO2 *> rpdo2_commands_{};
void setPPTargetPosBySdo(uint8_t node_id, int32_t pos);
void setPPTargetPosByPdo(uint8_t node_id, int32_t pos);
void setCSPTargetPosByPdo(uint8_t node_id, int32_t pos);
bool getMotorStatus(uint8_t node_id, msgs::MotorStatus* status) const;
void writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos);
void configTPDO1(uint8_t node_id);
@ -107,6 +118,14 @@ namespace cmvr {
void configRPDO1(uint8_t node_id, bool enable);
void configRPDO2(uint8_t node_id, bool enable);
const MotorConversion* conversionForNode(uint8_t node_id) const;
double radToCounts(double angle_rad, const MotorConversion& conversion) const;
double countsToRad(int32_t counts, const MotorConversion& conversion) const;
double radPerSecToVelocityRaw(double velocity_rad_s, const MotorConversion& conversion) const;
uint32_t radPerSec2ToAccelerationRaw(double acceleration_rad_s2,
const MotorConversion& conversion) const;
double velocityRawToRadPerSec(int32_t velocity_raw, const MotorConversion& conversion) const;
bool waitUntil(std::function<bool()> condition, int timeout_ms) {
auto start = std::chrono::steady_clock::now();

9
cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_sdo_response.cpp
View File

@ -3,6 +3,7 @@
// Created by lgv on 2025/7/24.
//
#include "cmvr/msgs/cia402.pb.h"
#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_sdo_response.h"
using namespace cmvr::device::motor;
@ -19,17 +20,17 @@ void Ti5MotorSdoResponse::ParseSdoData(const msgs::SdoFrame &sdo_response,
switch (sdo_response.index()) {
case msgs::CONTROL_WORD_6040:
case msgs::CIA402_CONTROL_WORD_6040:
motor_status->set_ctrl_word(sdo_response.data());
break;
case msgs::STATUS_WORD_6041:
case msgs::CIA402_STATUS_WORD_6041:
motor_status->set_status_word(sdo_response.data());
break;
case msgs::ACTUAL_POSITION_6064:
case msgs::CIA402_ACTUAL_POSITION_6064:
motor_status->set_position(static_cast<int32_t>(sdo_response.data()));
CMVR_LOG(INFO) << "pos = " << motor_status->position();
break;
case msgs::POSITION_OFFSET_2008:
case msgs::CANOPEN_POSITION_OFFSET_2008:
motor_status->set_position_offset(sdo_response.data());
}
//

12
cmvr-es/devices/motor/drivers/ti5_canopen/src/protocol/ti5_motor_tpdo2.cpp
View File

@ -19,16 +19,4 @@ void Ti5MotorTPDO2::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot
motor_status->set_position(bytes[3] << 24 | bytes[2] << 16 | bytes[1] << 8 | bytes[0]);
motor_status->set_speed(bytes[7] << 24 | bytes[6] << 16 | bytes[5] << 8 | bytes[4]);
double gearRatio = 101.0;
double radToDeg = 180.0 / M_PI;
auto speed = (motor_status->speed() * 360.0) / (radToDeg * gearRatio * 100.0);
auto angle_rad = (motor_status->position() * 360.0) / (gearRatio * 65536.0 * radToDeg);
// CMVR_LOG(INFO) << " Motor ID " << int(this->node_id_) << " pos = " << angle_rad << " rad speed = " << speed << " rad/s";
}

535
cmvr-es/devices/motor/drivers/ti5_canopen/src/ti5_motor_canopen_protocol.cpp
View File

@ -3,6 +3,7 @@
// Created by lgv on 2025/8/1.
//
#include "cmvr/msgs/cia402.pb.h"
#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
#include "canbus/canopen/register.h"
#include "motor/drivers/ti5_canopen/include/protocol/ti5_motor_tpdo1.h"
@ -66,7 +67,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
if (sdo_commands_[node_id] == nullptr) {
CMVR_LOG(ERROR) << "Ti5 Motor SDO Request Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
return false;
}
can_sender_->AddMessage(sdo_commands_[node_id]->ID(), sdo_commands_[node_id], true);
@ -77,7 +78,7 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
if (rpdo1_commands_[node_id] == nullptr) {
CMVR_LOG(ERROR) << "Ti5 Motor RPDO1 Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
return false;
}
can_sender_->AddMessage(rpdo1_commands_[node_id]->ID(), rpdo1_commands_[node_id], true);
@ -87,52 +88,187 @@ bool Ti5MotorCanopenProtocol::initNode(uint8_t node_id) {
if (rpdo2_commands_[node_id] == nullptr) {
CMVR_LOG(ERROR) << "Ti5 Motor RPDO2 Protocol does not exist in the MessageManager!";
return ErrorCode::CANBUS_ERROR;
return false;
}
can_sender_->AddMessage(rpdo2_commands_[node_id]->ID(), rpdo2_commands_[node_id], true);
return ErrorCode::OK;
return true;
}
bool Ti5MotorCanopenProtocol::getMotorStatus(
const uint8_t node_id,
msgs::MotorStatus* const status) const {
if (!message_manager_ || !status) {
return false;
}
msgs::RobotDetail robot_detail;
if (message_manager_->GetSensorData(&robot_detail) != ErrorCode::OK) {
return false;
}
const auto it = robot_detail.motors().find(node_id);
if (it == robot_detail.motors().end()) {
return false;
}
status->CopyFrom(it->second);
return true;
}
cmvr::msgs::RunMode Ti5MotorCanopenProtocol::getMode(const uint8_t node_id) {
msgs::MotorStatus status;
if (!getMotorStatus(node_id, &status)) {
return msgs::RUN_MODE_UNSPECIFIED;
}
return status.run_mode();
}
void Ti5MotorCanopenProtocol::setMotorConversion(
const uint8_t node_id,
const double encoder_counts_per_rev,
const double gear_ratio) {
motor_conversions_[node_id] = {encoder_counts_per_rev, gear_ratio};
}
const Ti5MotorCanopenProtocol::MotorConversion*
Ti5MotorCanopenProtocol::conversionForNode(const uint8_t node_id) const {
const auto it = motor_conversions_.find(node_id);
if (it != motor_conversions_.end() &&
it->second.encoder_counts_per_rev > 0.0 &&
it->second.gear_ratio > 0.0) {
return &it->second;
}
CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] missing conversion config for node "
<< static_cast<int>(node_id);
return nullptr;
}
double Ti5MotorCanopenProtocol::radToCounts(
const double angle_rad,
const MotorConversion& conversion) const {
return (angle_rad * RADTODEG) / 360.0 *
conversion.gear_ratio * conversion.encoder_counts_per_rev;
}
double Ti5MotorCanopenProtocol::countsToRad(
const int32_t counts,
const MotorConversion& conversion) const {
return (counts * 360.0) /
(conversion.gear_ratio * conversion.encoder_counts_per_rev * RADTODEG);
}
double Ti5MotorCanopenProtocol::radPerSecToVelocityRaw(
const double velocity_rad_s,
const MotorConversion& conversion) const {
return ((velocity_rad_s * RADTODEG) * conversion.gear_ratio * Ti5VelocityUnitScale) /
360.0;
}
uint32_t Ti5MotorCanopenProtocol::radPerSec2ToAccelerationRaw(
const double acceleration_rad_s2,
const MotorConversion& conversion) const {
const auto raw = ((std::abs(acceleration_rad_s2) * RADTODEG) *
conversion.gear_ratio * Ti5VelocityUnitScale) /
360.0 / Ti5AccelerationTimeScale;
return static_cast<uint32_t>(std::abs(raw));
}
double Ti5MotorCanopenProtocol::velocityRawToRadPerSec(
const int32_t velocity_raw,
const MotorConversion& conversion) const {
return (velocity_raw * 360.0) /
(conversion.gear_ratio * Ti5VelocityUnitScale * RADTODEG);
}
void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, ObIndex index, ObSubIndex sub_index,
void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier cs, uint32_t index, uint32_t sub_index,
uint32_t data, uint32_t delay_ms) {
sdo_commands_[node_id]->SetFrameData(cs, index, sub_index, data);
can_sender_->Update(sdo_commands_[node_id]->ID());
std::this_thread::sleep_for(std::chrono::milliseconds(delay_ms));
}
void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) {
auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
switch (getMode(node_id)) {
// case RUN_MODE_CYCLIC_SYNC_POSITION:
// setCSPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
// break;
case RUN_MODE_PROFILE_POSITION:
// setPPTargetPosByPdo(node_id, static_cast<int32_t>(cmd));
setPPTargetPosBySdo(node_id, static_cast<int32_t>(cmd));
break;
bool Ti5MotorCanopenProtocol::commandProfilePosition(uint8_t node_id,
double target_q,
double max_qd,
double max_qdd) {
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return false;
}
if (max_qd > 0.0) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081,
SUB_INDEX_0,
static_cast<uint32_t>(std::abs(radPerSecToVelocityRaw(max_qd, *conversion))),
0);
}
if (max_qdd > 0.0) {
const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083,
SUB_INDEX_0, accel, 0);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084,
SUB_INDEX_0, accel, 0);
}
writeProfilePositionTargetBySdo(node_id, static_cast<int32_t>(radToCounts(target_q, *conversion)));
return true;
}
void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double angle_rad, double vel) {
auto pos_cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0;
auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
bool Ti5MotorCanopenProtocol::commandProfileVelocity(uint8_t node_id,
double target_qd,
double max_qdd) {
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return false;
}
if (max_qdd > 0.0) {
const auto accel = radPerSec2ToAccelerationRaw(max_qdd, *conversion);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083,
SUB_INDEX_0, accel, 0);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084,
SUB_INDEX_0, accel, 0);
}
const auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_VELOCITY_60FF,
SUB_INDEX_0,
static_cast<uint32_t>(static_cast<int32_t>(std::llround(speed))),
0);
return true;
}
bool Ti5MotorCanopenProtocol::commandCyclicPosition(uint8_t node_id,
double target_q,
double target_qd) {
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return false;
}
auto pos_cmd = radToCounts(target_q, *conversion);
auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
rpdo1_commands_[node_id]->SetTargetPos(pos_cmd);
rpdo1_commands_[node_id]->SetTargetVel(uint32_t(std::abs(speed)));
can_sender_->Update(rpdo1_commands_[node_id]->ID());
return true;
}
void Ti5MotorCanopenProtocol::setTarget(uint8_t node_id, double vel) {
auto speed = ((vel * RADTODEG) * GearRatio * 100.0) / 360.0;
bool Ti5MotorCanopenProtocol::commandCyclicVelocity(uint8_t node_id,
double target_qd) {
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return false;
}
auto speed = radPerSecToVelocityRaw(target_qd, *conversion);
rpdo2_commands_[node_id]->SetTargetVel(int16_t(speed));
can_sender_->Update(rpdo2_commands_[node_id]->ID());
return true;
}
bool Ti5MotorCanopenProtocol::commandCyclicTorque(uint8_t node_id, double target_tau) {
(void)target_tau;
CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] cyclic torque command is not implemented, node="
<< static_cast<int>(node_id);
return false;
}
void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos) {
void Ti5MotorCanopenProtocol::writeProfilePositionTargetBySdo(uint8_t node_id, int32_t pos) {
controlword_t cw = {};
cw.switch_on = 1;
cw.enable_voltage = 1;
@ -141,45 +277,20 @@ void Ti5MotorCanopenProtocol::setPPTargetPosBySdo(uint8_t node_id, int32_t pos)
cw.change_set_immediately = 1;
// 1. 设置目标位置
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, pos);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A, SUB_INDEX_0, pos);
// 2. 设置触发位（bit4 = 1）
cw.new_set_point = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
// 3. 清除触发位（bit4 = 0），准备下一次触发
cw.new_set_point = 0;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
}
void Ti5MotorCanopenProtocol::setPPTargetPosByPdo(uint8_t node_id, int32_t pos) {
// 触发目标位置运动
controlword_t cw;
cw.value = 0x0F;
cw.new_set_point = 1;
cw.change_set_immediately = 1;
rpdo1_commands_[node_id]->SetTargetPos(pos);
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
std::this_thread::sleep_for(std::chrono::milliseconds(10));
cw.new_set_point = 0;
rpdo1_commands_[node_id]->SetCtrlWord(cw.value);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) {
rpdo1_commands_[node_id]->SetTargetPos(pos);
rpdo1_commands_[node_id]->SetCtrlWord(0x0F);
can_sender_->Update(rpdo1_commands_[node_id]->ID());
}
void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
bool Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
// cur_mode_[node_id] = mode;
@ -188,56 +299,72 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
controlword_t cw = {};
cw.quick_stop = 1;
cw.enable_voltage = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
// configRPDO1(node_id, false);
// configRPDO2(node_id, false);
// 1 : 先设置模式
auto data = static_cast<uint32_t>(mode);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CIA402_OPERATION_MODE_6060, SUB_INDEX_0, data);
// 3 : 状态机步进 —— Switch On & Enable Operation（0x0F）
cw.switch_on = 1;
cw.enable_operation = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value, 20);
int32_t current_position = 0;
if (mode == RUN_MODE_PROFILE_POSITION || mode == RUN_MODE_CYCLIC_SYNC_POSITION) {
seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
msgs::MotorStatus status;
if (!waitUntil([&]() {
return getMotorStatus(node_id, &status) &&
status.has_sdo_response() &&
status.sdo_response().index() == CIA402_ACTUAL_POSITION_6064;
}, 500)) {
CMVR_LOG(ERROR) << "motor " << static_cast<int>(node_id)
<< ": actual-position feedback timed out";
return false;
}
current_position = status.position();
}
switch (mode) {
case RUN_MODE_PROFILE_POSITION: {
// 4 : 设置目标位置（为当前位置）
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A,
SUB_INDEX_0, current_position);
// 5 : 触发位置运动（new_set_point 翻转）
cw.new_set_point = 1;
cw.change_set_immediately = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
// 6 : 清除 new_set_point（必须，不清除则无法再次触发新目标）
cw.new_set_point = 0;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
case RUN_MODE_CYCLIC_SYNC_POSITION: {
// configRPDO1(node_id, true);
// 设置目标位置为当前位置
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_POSITION_607A, SUB_INDEX_0, cur_pos);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_TARGET_POSITION_607A,
SUB_INDEX_0, current_position);
//3 : 使能 15
cw.enable_operation = 1;
cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
case RUN_MODE_PROFILE_VELOCITY: {
cw.enable_operation = 1;
cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
@ -245,13 +372,21 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) {
// configRPDO2(node_id, true);
cw.enable_operation = 1;
cw.switch_on = 1;
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, cw.value);
break;
}
default:
// TODO: Handle unspecified or unknown mode
break;
return false;
}
if (!waitUntil([&]() { return getMode(node_id) == mode; }, 500)) {
CMVR_LOG(ERROR) << "motor " << static_cast<int>(node_id)
<< ": operation mode switch failed, target_mode="
<< static_cast<int>(mode);
return false;
}
return true;
}
void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand command, uint32_t delay_ms) {
@ -262,9 +397,9 @@ void Ti5MotorCanopenProtocol::seedNmtRequest(uint8_t node_id, msgs::NmtCommand c
void Ti5MotorCanopenProtocol::configProfile(uint8_t node_id, uint32_t speed, uint32_t accel, uint32_t decel) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_VELOCITY_6081, SUB_INDEX_0, speed);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, decel);
}
@ -272,128 +407,128 @@ void Ti5MotorCanopenProtocol::configTPDO1(uint8_t node_id) {
//TDPO1 配置 状态字 和 控制字
// 1: 失能 pdo
uint32_t cob_id = TPDO1_BASE_ID_180 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 0);
// 2: 配置为异步
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
// 3：配置约束时间 unit:0.1ms
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_3, 10);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_3, 10);
// 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO1_COMM_1800, SUB_INDEX_5, 0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_5, 0);
// 5 ：映射控制字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_1,
CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_1,
CIA402_CONTROL_WORD_6040 << 16 | SUB_INDEX_0 << 8 | 16);
//6 : 映射状态字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_2,
STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_2,
CIA402_STATUS_WORD_6041 << 16 | SUB_INDEX_0 << 8 | 16);
//7 : 映射模式
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_3,
MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_3,
CIA402_MODE_DISPLAY_6061 << 16 | SUB_INDEX_0 << 8 | 8);
//8 映射错误码
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_MAP_1A00, SUB_INDEX_4,
ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_4,
CIA402_ERROR_CODE_603F << 16 | SUB_INDEX_0 << 8 | 16);
//9 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO1_MAP_1A00, SUB_INDEX_0, 4);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO1_MAP_1A00, SUB_INDEX_0, 4);
//10 使能
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO1_COMM_1800, SUB_INDEX_1, cob_id | (0U << 31));
}
void Ti5MotorCanopenProtocol::configTPDO2(uint8_t node_id) {
// 1: 失能 pdo
uint32_t cob_id = TPDO2_BASE_ID_280 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 0);
// 2: 配置为异步
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
// 3：配置约束时间 unit:0.1ms
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_3, 100);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_3, 100);
// 4 : 配置周期发送时间 unit : ms
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, TPDO2_COMM_1801, SUB_INDEX_5, 0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_5, 0);
// 5 ：映射当前位置
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_1,
ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_1,
CIA402_ACTUAL_POSITION_6064 << 16 | SUB_INDEX_0 << 8 | 32);
//6 : 映射当前速度
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_MAP_1A01, SUB_INDEX_2,
ACTUAL_SPEED_606C << 16 | SUB_INDEX_0 << 8 | 32);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_2,
CIA402_ACTUAL_VELOCITY_606C << 16 | SUB_INDEX_0 << 8 | 32);
//9 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, TPDO2_MAP_1A01, SUB_INDEX_0, 2);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_TPDO2_MAP_1A01, SUB_INDEX_0, 2);
//10 使能
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_TPDO2_COMM_1801, SUB_INDEX_1, cob_id | (0U << 31));
}
void Ti5MotorCanopenProtocol::configRPDO1(uint8_t node_id, bool enable) {
// 1: 失能 pdo
uint32_t cob_id = RPDO1_BASE_ID_200 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 0);
// 2: 配置为
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
// // 3：配置约束时间 unit:0.1ms
// seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_3,10);
// seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_3,10);
//
// // 4 : 配置周期发送时间 unit : ms 0 为 数据改变时发送
// seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,RPDO1_COMM_1400,SUB_INDEX_5,0);
// seedSdoRequest(node_id,CS_WRITE_TWO_BYTES,CANOPEN_RPDO1_COMM_1400,SUB_INDEX_5,0);
// 5 ：映射位置
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_1,
TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_1,
CIA402_TARGET_POSITION_607A << 16 | SUB_INDEX_0 << 8 | 32);
//6 : 映射控制字
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_MAP_1600, SUB_INDEX_2,
PROFILE_SPEED_6081 << 16 | SUB_INDEX_0 << 8 | 32);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_2,
CIA402_PROFILE_VELOCITY_6081 << 16 | SUB_INDEX_0 << 8 | 32);
//7 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO1_MAP_1600, SUB_INDEX_0, 2);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO1_MAP_1600, SUB_INDEX_0, 2);
//8 使能
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO1_COMM_1400, SUB_INDEX_1, cob_id | (0U << 31));
}
void Ti5MotorCanopenProtocol::configRPDO2(uint8_t node_id, bool enable) {
// 1: 失能 pdo
uint32_t cob_id = RPDO2_BASE_ID_300 + node_id;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (1U << 31));
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 0);
if (!enable) return;
// 2: 配置为
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_2, SYNC_EVENT_DRIVEN);
// 5 ：映射位置
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_MAP_1601, SUB_INDEX_1,
TARGET_SPEED_60FF << 16 | SUB_INDEX_0 << 8 | 32);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_1,
CIA402_TARGET_VELOCITY_60FF << 16 | SUB_INDEX_0 << 8 | 32);
//7 写入该PDO映射对象总个数
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, RPDO2_MAP_1601, SUB_INDEX_0, 1);
seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, CANOPEN_RPDO2_MAP_1601, SUB_INDEX_0, 1);
//8 使能
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_RPDO2_COMM_1401, SUB_INDEX_1, cob_id | (0U << 31));
}
@ -405,140 +540,166 @@ void Ti5MotorCanopenProtocol::configPdo(uint8_t node_id) {
}
void Ti5MotorCanopenProtocol::setLimitQdd(uint8_t node_id, double u_qdd, double l_qdd) {
auto accel = ((u_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
auto decel = ((l_qdd * RADTODEG) * GearRatio * 100.0) / 360.0 / 1000.0;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return;
}
auto accel = ((u_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) /
360.0 / Ti5AccelerationTimeScale;
auto decel = ((l_qdd * RADTODEG) * conversion->gear_ratio * Ti5VelocityUnitScale) /
360.0 / Ti5AccelerationTimeScale;
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_ACCELERATION_6083, SUB_INDEX_0, std::abs(accel));
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_PROFILE_DECELERATION_6084, SUB_INDEX_0, std::abs(decel));
}
void Ti5MotorCanopenProtocol::setLimitQd(uint8_t node_id, double qd) {
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
// seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, MAX_SPEED_607F, SUB_INDEX_0, speed);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, speed);
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return;
}
auto speed = radPerSecToVelocityRaw(qd, *conversion);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_MAX_PROFILE_VELOCITY_607F, SUB_INDEX_0, speed);
}
void Ti5MotorCanopenProtocol::setLimitQ(uint8_t node_id, double ub, double lb) {
ub = (ub * RADTODEG) / 360.0 * GearRatio * 65536.0;
lb = (lb * RADTODEG) / 360.0 * GearRatio * 65536.0;
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return;
}
ub = radToCounts(ub, *conversion);
lb = radToCounts(lb, *conversion);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_1, lb);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_SOFTWARE_POSITION_LIMIT_607D, SUB_INDEX_2, ub);
}
bool Ti5MotorCanopenProtocol::calibrateZeroQ(uint8_t node_id) {
// 0: 设置控制字为 0x06，确保停机状态
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x06, 1000);
// 1: 清除偏置值 0x2008 ← 0
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0);
// 2: 等待确认清除成功
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0);
if (!waitUntil([&]() {
return GetRobotDetail()->motors().at(node_id).position_offset() == 0;
msgs::MotorStatus status;
return getMotorStatus(node_id, &status) &&
status.has_sdo_response() &&
status.sdo_response().index() == CANOPEN_POSITION_OFFSET_2008 &&
status.position_offset() == 0;
}, 1000)) {
CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set zero failed";
return false;
}
// 3: 读取当前位置 0x6064
seedSdoRequest(node_id, CS_READ_REQUEST, ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
auto cur_pos = GetRobotDetail()->motors().at(node_id).position();
seedSdoRequest(node_id, CS_READ_REQUEST, CIA402_ACTUAL_POSITION_6064, SUB_INDEX_0, 0, 20);
msgs::MotorStatus status;
if (!waitUntil([&]() {
return getMotorStatus(node_id, &status) &&
status.has_sdo_response() &&
status.sdo_response().index() == CIA402_ACTUAL_POSITION_6064;
}, 500)) {
CMVR_LOG(ERROR) << "motor " << static_cast<int>(node_id)
<< ": actual-position feedback timed out during calibration";
return false;
}
const auto cur_pos = status.position();
// 4: 将当前位置写入偏置寄存器
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, cur_pos);
// 5: 保存参数到永久区（0x2000 ← 1）
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CANOPEN_USER_SAVE_PARA_2000, SUB_INDEX_0, 1, 100);
// 6: 确认写入成功
seedSdoRequest(node_id, CS_READ_REQUEST, POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
seedSdoRequest(node_id, CS_READ_REQUEST, CANOPEN_POSITION_OFFSET_2008, SUB_INDEX_0, 0, 20);
if (!waitUntil([&]() {
return GetRobotDetail()->motors().at(node_id).position_offset() == cur_pos;
msgs::MotorStatus latest_status;
return getMotorStatus(node_id, &latest_status) &&
latest_status.has_sdo_response() &&
latest_status.sdo_response().index() == CANOPEN_POSITION_OFFSET_2008 &&
latest_status.position_offset() == cur_pos;
}, 500)) {
return false;
CMVR_LOG(ERROR) << "motor " << node_id << ": 0x2008 set current position failed";
return false;
}
return true;
}
void Ti5MotorCanopenProtocol::brake(uint8_t node_id) {
bool Ti5MotorCanopenProtocol::torqueOn(uint8_t node_id) {
return setMode(node_id, msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
}
bool Ti5MotorCanopenProtocol::brakeRelease(uint8_t node_id) {
// (void)node_id;
// CMVR_LOG(ERROR) << "[Ti5MotorCanopenProtocol] brakeRelease is not implemented";
torqueOff(node_id);
return true;
}
bool Ti5MotorCanopenProtocol::quickStop(uint8_t node_id) {
// // 开机未使能电机时调用
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
// 6 抱闸 0 ： 立即停机 自由
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, QUICK_STOP_DECEL_6085, SUB_INDEX_0, 0XFFFFFFF0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, CIA402_QUICK_STOP_DECELERATION_6085, SUB_INDEX_0, 0XFFFFFFF0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 6);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 100);
// 必须要发送 0xf 才能按照6085中设定的减速度减速
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
return true;
}
bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) {
msgs::MotorStatus motor_status;
if (!getMotorStatus(node_id, &motor_status)) {
return false;
}
statusword_t st{};
st.value = GetRobotDetail()->motors().at(node_id).status_word();
st.value = motor_status.status_word();
return st.target_reached == 1;
}
void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) {
auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0;
switch (getMode(node_id)) {
case msgs::RUN_MODE_CYCLIC_SYNC_POSITION:
case msgs::RUN_MODE_PROFILE_POSITION: {
auto it = last_Qd_.find(node_id);
if (it == last_Qd_.end() || it->second != speed) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_SPEED_6081, SUB_INDEX_0, uint32_t(std::abs(speed)),
0);
last_Qd_[node_id] = speed;
}
break;
}
case msgs::RUN_MODE_PROFILE_VELOCITY:
case msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY: {
// 在速度模式下，直接设置目标速度
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, TARGET_SPEED_60FF, SUB_INDEX_0, uint32_t(speed), 0);
break;
}
default:
break;
}
}
void Ti5MotorCanopenProtocol::setQdd(uint8_t node_id, double qdd) {
uint32_t accel = ((std::abs(qdd) * RADTODEG) * GearRatio * 100.0 * 65536.0) / (360.0 * 1000.0);
auto it = last_Qdd_.find(node_id);
if (it == last_Qdd_.end() || it->second != accel) {
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_ACCELERATION_6083, SUB_INDEX_0, accel);
seedSdoRequest(node_id, CS_WRITE_FOUR_BYTES, PROFILE_DECELERATION_6084, SUB_INDEX_0, accel);
last_Qdd_[node_id] = accel;
}
}
void Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
bool Ti5MotorCanopenProtocol::torqueOff(uint8_t node_id) {
// 0 ： 立即停机 自由
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_QUICK_STOP_OPTION_605A, SUB_INDEX_0, 0);
seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x02, 20);
// 必须要发送 0xf 才能按照6085中设定的减速度减速
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
// seedSdoRequest(node_id, CS_WRITE_TWO_BYTES, CIA402_CONTROL_WORD_6040, SUB_INDEX_0, 0x0F);
// 停机之后，要重新使能？
// cur_mode_[node_id] = msgs::RUN_MODE_UNSPECIFIED;
return true;
}
double Ti5MotorCanopenProtocol::getQ(uint8_t node_id) {
auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get());
auto cnt = data_ptr->motors().at(node_id).position();
return (cnt * 360.0) / (GearRatio * 65536.0 * RADTODEG);
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return 0.0;
}
msgs::MotorStatus motor_status;
if (!getMotorStatus(node_id, &motor_status)) {
return 0.0;
}
const auto cnt = motor_status.position();
return countsToRad(cnt, *conversion);
}
double Ti5MotorCanopenProtocol::getQd(uint8_t node_id) {
auto data_ptr = std::make_unique<msgs::RobotDetail>();
message_manager_->GetSensorData(data_ptr.get());
auto cnt = data_ptr->motors().at(node_id).speed();
return (cnt * 360.0) / (GearRatio * 100.0 * RADTODEG);
const auto* conversion = conversionForNode(node_id);
if (!conversion) {
return 0.0;
}
msgs::MotorStatus motor_status;
if (!getMotorStatus(node_id, &motor_status)) {
return 0.0;
}
const auto cnt = motor_status.speed();
return velocityRawToRadPerSec(cnt, *conversion);
}

1
cmvr-es/devices/motor/manager/CMakeLists.txt
View File

@ -12,6 +12,7 @@ target_link_libraries(motor_manager
PRIVATE
cmvr_es::device::ti5_canopen_motor_driver
cmvr_es::device::mujoco_motor_driver
cmvr_es::device::ethercat_motor_driver
cmvr_es::ik_solver
glog
)

24
cmvr-es/devices/motor/manager/include/motor_manager.h
View File

@ -5,7 +5,6 @@
#include <memory>
#include <mutex>
#include <string>
#include <unordered_set>
#include <unordered_map>
#include <vector>
@ -32,7 +31,9 @@ class AbstractMotorBusRuntime;
class MotorManager final : public AbstractDevice,
public std::enable_shared_from_this<MotorManager> {
public:
MotorManager(std::string id, const config::MotorConfig& cfg);
MotorManager(std::string id,
const config::MotorConfig& cfg,
std::string selected_group_id);
~MotorManager() override;
DeviceKind kind() const noexcept override { return DeviceKind::MotorSystem; }
@ -45,19 +46,18 @@ public:
std::shared_ptr<AbstractMotor> getMotor(std::uint8_t node_id) const;
std::shared_ptr<AbstractMotor> getMotor(const std::string& joint_name) const;
const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& motorsMap() const;
bool commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities) const;
bool readFeedbacksAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
std::vector<double>& positions,
std::vector<double>& velocities) const;
static std::shared_ptr<MotorManager> managerFor(const std::string& id);
static std::shared_ptr<simulate::MujocoWorld> mujocoWorldFor(const std::string& id);
static void setActiveJoints(const std::string& motor_manager_id,
std::unordered_map<std::string, std::unordered_set<std::string>> group_joints);
static void clearActiveJoints();
private:
using ActiveJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
bool selectActiveMotors_(const std::string& group_name,
const google::protobuf::RepeatedPtrField<config::MotorConfigItem>& source,
std::vector<config::MotorConfigItem>& selected) const;
bool applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
std::vector<config::MotorConfigItem>& selected) const;
std::shared_ptr<AbstractMotorBusRuntime> createBusRuntime_(
@ -81,6 +81,7 @@ private:
private:
config::MotorConfig cfg_;
std::string selected_group_id_;
std::vector<std::shared_ptr<AbstractMotorBusRuntime>> bus_runtimes_;
mutable std::mutex motors_mutex_;
std::unordered_map<std::uint8_t, std::shared_ptr<AbstractMotor>> motors_by_id_;
@ -90,7 +91,6 @@ private:
static std::mutex registry_mutex_;
static std::unordered_map<std::string, std::weak_ptr<MotorManager>> managers_;
static std::unordered_map<std::string, std::weak_ptr<simulate::MujocoWorld>> mujoco_world_registry_;
static std::unordered_map<std::string, ActiveJointSelection> active_joints_;
};
} // namespace cmvr::device

266
cmvr-es/devices/motor/manager/src/motor_manager.cpp
View File

@ -1,38 +1,41 @@
#include "motor/manager/include/motor_manager.h"
#include "devices/motor/manager/include/motor_manager.h"
#include <chrono>
#include <cmath>
#include <cstddef>
#include <cstdint>
#include <thread>
#include <unordered_map>
#include <utility>
#include "algorithms/kinematics/ik_solver/common/include/urdf_parser.h"
#include "common/base/logging/logger.h"
#include "common/config/config_files.h"
#include "../../bus_runtime/abstract_motor_bus_runtime.h"
#include "motor/bus_runtime/can/include/can_motor_bus_runtime.h"
#include "motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include "motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
#include "motor/drivers/mujoco/include/mujoco_motor.h"
#include "motor/drivers/ti5_canopen/include/ti5_motor.h"
#include "motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
#include "devices/motor/bus_runtime/abstract_motor_bus_runtime.h"
#include "devices/motor/bus_runtime/can/include/can_motor_bus_runtime.h"
#include "devices/motor/bus_runtime/ethercat/include/ethercat_motor_bus_runtime.h"
#include "devices/motor/bus_runtime/mujoco/include/mujoco_motor_bus_runtime.h"
#include "devices/motor/drivers/ethercat_motor/include/cia402/cia402_protocol.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_cia402_pdo_mapping.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor.h"
#include "devices/motor/drivers/ethercat_motor/include/vendor/eyou/eyou_motor_adapter.h"
#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
#include "devices/motor/drivers/ti5_canopen/include/ti5_motor.h"
#include "devices/motor/drivers/ti5_canopen/include/ti5_motor_canopen_protocol.h"
namespace cmvr::device {
std::mutex MotorManager::registry_mutex_;
std::unordered_map<std::string, std::weak_ptr<MotorManager>> MotorManager::managers_;
std::unordered_map<std::string, std::weak_ptr<simulate::MujocoWorld>> MotorManager::mujoco_world_registry_;
std::unordered_map<std::string, MotorManager::ActiveJointSelection> MotorManager::active_joints_;
MotorManager::MotorManager(std::string id, const config::MotorConfig& cfg)
: cfg_(cfg)
MotorManager::MotorManager(std::string id,
const config::MotorConfig& cfg,
std::string selected_group_id)
: cfg_(cfg),
selected_group_id_(std::move(selected_group_id))
{
id_ = std::move(id);
if (!cfg_.id().empty() && cfg_.id() != id_) {
CMVR_LOG(ERROR) << "[MotorManager] config id '" << cfg_.id()
<< "' does not match device id '" << id_ << "'";
id_.clear();
}
}
MotorManager::~MotorManager() = default;
@ -46,6 +49,14 @@ bool MotorManager::init()
CMVR_LOG(ERROR) << "[MotorManager] id is empty";
return false;
}
if (selected_group_id_.empty()) {
CMVR_LOG(ERROR) << "[MotorManager] selected motor group id is empty: " << id_;
return false;
}
if (cfg_.motor_groups_size() == 0) {
CMVR_LOG(ERROR) << "[MotorManager] no motor groups configured: " << id_;
return false;
}
bus_runtimes_.clear();
bus_runtimes_.reserve(static_cast<std::size_t>(cfg_.motor_groups_size()));
@ -56,25 +67,27 @@ bool MotorManager::init()
}
bool all_ok = true;
std::size_t selected_group_count = 0;
for (const auto& motor_group_cfg : cfg_.motor_groups()) {
const auto& group_name = motor_group_cfg.id();
if (group_name.empty()) {
CMVR_LOG(ERROR) << "[MotorManager] motor group id is empty in manager: " << id_;
all_ok = false;
if (group_name != selected_group_id_) {
continue;
}
++selected_group_count;
if (!motor_group_cfg.has_motors()) {
CMVR_LOG(ERROR) << "[MotorManager] motor group missing motors: " << group_name;
all_ok = false;
continue;
}
std::vector<config::MotorConfigItem> selected_motor_cfgs;
const bool selected_active_group = selectActiveMotors_(
group_name, motor_group_cfg.motors().motors(), selected_motor_cfgs);
if (!selected_active_group) {
if (motor_group_cfg.motors().motors_size() == 0) {
CMVR_LOG(ERROR) << "[MotorManager] motor group has no motors: " << group_name;
all_ok = false;
continue;
}
std::vector<config::MotorConfigItem> selected_motor_cfgs(
motor_group_cfg.motors().motors().begin(),
motor_group_cfg.motors().motors().end());
if (!applyConfiguredJointLimits_(motor_group_cfg, selected_motor_cfgs)) {
all_ok = false;
@ -91,11 +104,17 @@ bool MotorManager::init()
all_ok = false;
continue;
}
if (!bus_runtime->start()) {
const bool start_before_motor_creation =
motor_group_cfg.bus_type() == config::MOTOR_BUS_ETHERCAT;
if (start_before_motor_creation && !bus_runtime->start()) {
bus_runtime->stop();
all_ok = false;
continue;
}
if (start_before_motor_creation) {
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
}
auto motors = createMotors_(motor_group_cfg, selected_motor_cfgs, bus_runtime);
if (motors.empty()) {
@ -104,7 +123,29 @@ bool MotorManager::init()
all_ok = false;
continue;
}
if (!start_before_motor_creation && !bus_runtime->start()) {
bus_runtime->stop();
all_ok = false;
continue;
}
bool group_ok = true;
if (motor_group_cfg.bus_type() == config::MOTOR_BUS_CAN) {
for (auto& motor : motors) {
if (!motor->init()) {
CMVR_LOG(ERROR) << "[MotorManager] failed to initialize CAN motor: "
<< motor->jointName();
group_ok = false;
break;
}
}
}
if (!group_ok) {
bus_runtime->stop();
all_ok = false;
continue;
}
for (auto& motor : motors) {
if (!addMotor(motor)) {
group_ok = false;
@ -120,6 +161,12 @@ bool MotorManager::init()
bus_runtimes_.push_back(std::move(bus_runtime));
}
if (selected_group_count != 1) {
CMVR_LOG(ERROR) << "[MotorManager] selected motor group '" << selected_group_id_
<< "' must occur exactly once in config: " << id_;
all_ok = false;
}
if (!all_ok) {
for (auto& bus_runtime : bus_runtimes_) {
if (bus_runtime) {
@ -222,6 +269,66 @@ const std::unordered_map<std::string, std::shared_ptr<AbstractMotor>>& MotorMana
return motors_by_joint_;
}
bool MotorManager::commandCyclicPositionsAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
const std::vector<double>& positions,
const std::vector<double>& velocities) const
{
if (motors.empty() || motors.size() != positions.size() ||
motors.size() != velocities.size()) {
return false;
}
if (std::dynamic_pointer_cast<EyouMotor>(motors.front())) {
return EyouMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
}
if (std::dynamic_pointer_cast<MujocoMotor>(motors.front())) {
return MujocoMotor::commandCyclicPositionsAtomic(motors, positions, velocities);
}
if (std::dynamic_pointer_cast<Ti5Motor>(motors.front())) {
// TI5 currently exposes only a per-motor RPDO command. Keep the
// fallback here so callers retain one manager-level entry point.
static std::once_flag ti5_non_atomic_warning;
std::call_once(ti5_non_atomic_warning, []() {
CMVR_LOG(WARNING)
<< "[MotorManager] TI5 motors use non-atomic cyclic position commands";
});
for (std::size_t i = 0; i < motors.size(); ++i) {
if (!std::dynamic_pointer_cast<Ti5Motor>(motors[i])) {
CMVR_LOG(ERROR) << "[MotorManager] mixed motor types in cyclic position command";
return false;
}
if (!motors[i]->commandCyclicPosition(positions[i], velocities[i])) {
CMVR_LOG(ERROR) << "[MotorManager] failed to command TI5 motor at index " << i;
return false;
}
}
return true;
}
CMVR_LOG(ERROR) << "[MotorManager] atomic cyclic position is unsupported for motor type: "
<< motors.front()->typeName();
return false;
}
bool MotorManager::readFeedbacksAtomic(
const std::vector<std::shared_ptr<AbstractMotor>>& motors,
std::vector<double>& positions,
std::vector<double>& velocities) const
{
if (motors.empty()) {
return false;
}
if (std::dynamic_pointer_cast<EyouMotor>(motors.front())) {
return EyouMotor::readFeedbacksAtomic(motors, positions, velocities);
}
CMVR_LOG(ERROR) << "[MotorManager] atomic feedback is unsupported for motor type: "
<< motors.front()->typeName();
return false;
}
std::shared_ptr<MotorManager> MotorManager::managerFor(const std::string& id)
{
std::lock_guard<std::mutex> lock(registry_mutex_);
@ -242,68 +349,6 @@ std::shared_ptr<simulate::MujocoWorld> MotorManager::mujocoWorldFor(const std::s
return it->second.lock();
}
void MotorManager::setActiveJoints(const std::string& motor_manager_id,
ActiveJointSelection group_joints)
{
std::lock_guard<std::mutex> lock(registry_mutex_);
active_joints_[motor_manager_id] = std::move(group_joints);
}
void MotorManager::clearActiveJoints()
{
std::lock_guard<std::mutex> lock(registry_mutex_);
active_joints_.clear();
}
bool MotorManager::selectActiveMotors_(
const std::string& group_name,
const google::protobuf::RepeatedPtrField<config::MotorConfigItem>& source,
std::vector<config::MotorConfigItem>& selected) const
{
selected.clear();
ActiveJointSelection selection;
{
std::lock_guard<std::mutex> lock(registry_mutex_);
const auto it = active_joints_.find(id_);
if (it != active_joints_.end()) {
selection = it->second;
}
}
if (selection.empty()) {
CMVR_LOG(WARNING) << "[MotorManager] No active joints selected for motor manager " << id_
<< ", motor group " << group_name << " will not initialize motors.";
return false;
}
const auto group_it = selection.find(group_name);
if (group_it == selection.end() || group_it->second.empty()) {
return false;
}
for (const auto& motor_cfg : source) {
if (group_it->second.count(motor_cfg.joint_name()) > 0) {
selected.push_back(motor_cfg);
}
}
if (selected.size() != group_it->second.size()) {
std::unordered_set<std::string> found;
for (const auto& motor_cfg : selected) {
found.insert(motor_cfg.joint_name());
}
for (const auto& joint_name : group_it->second) {
if (found.count(joint_name) == 0) {
CMVR_LOG(ERROR) << "[MotorManager] active joint '" << joint_name
<< "' not found in motor group '" << group_name << "'";
return false;
}
}
}
return !selected.empty();
}
bool MotorManager::applyConfiguredJointLimits_(const config::MotorGroupConfig& group_cfg,
std::vector<config::MotorConfigItem>& selected) const
{
@ -398,8 +443,19 @@ std::shared_ptr<AbstractMotorBusRuntime> MotorManager::createBusRuntime_(
return std::make_shared<CanMotorBusRuntime>();
case config::MOTOR_BUS_MUJOCO:
return std::make_shared<MujocoMotorBusRuntime>();
case config::MOTOR_BUS_ETHERCAT:
return std::make_shared<EthercatMotorBusRuntime>();
case config::MOTOR_BUS_ETHERCAT: {
auto runtime = std::make_shared<EthercatMotorBusRuntime>();
if (group_cfg.vendor() == config::MOTOR_VENDOR_EYOU &&
group_cfg.protocol() == config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
runtime->setPdoMapping(createEyouCia402PdoMapping());
return runtime;
}
CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor="
<< config::MotorVendor_Name(group_cfg.vendor())
<< ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
<< ", group=" << group_cfg.id();
return nullptr;
}
default:
CMVR_LOG(ERROR) << "[MotorManager] unsupported motor bus type: "
<< config::MotorBusType_Name(group_cfg.bus_type())
@ -464,9 +520,8 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createCanMotors_(
motors.reserve(motor_cfgs.size());
for (const auto& cfg : motor_cfgs) {
auto motor = std::make_shared<Ti5Motor>(cfg);
motor->setProtocol(protocol);
if (!motor->init()) {
CMVR_LOG(ERROR) << "[MotorManager] failed to init TI5 motor: "
if (!motor->setProtocol(protocol)) {
CMVR_LOG(ERROR) << "[MotorManager] failed to register TI5 motor protocols: "
<< cfg.joint_name();
return {};
}
@ -534,6 +589,14 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT config: " << group_cfg.id();
return {};
}
if (group_cfg.vendor() != config::MOTOR_VENDOR_EYOU ||
group_cfg.protocol() != config::MOTOR_PROTOCOL_ETHERCAT_CIA402) {
CMVR_LOG(ERROR) << "[MotorManager] unsupported EtherCAT motor: vendor="
<< config::MotorVendor_Name(group_cfg.vendor())
<< ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
<< ", group=" << group_cfg.id();
return {};
}
for (const auto& motor_cfg : motor_cfgs) {
if (!ethercat_bus_runtime->slaveForMotor(motor_cfg.id())) {
CMVR_LOG(ERROR) << "[MotorManager] missing EtherCAT slave config for motor id "
@ -542,11 +605,22 @@ std::vector<std::shared_ptr<AbstractMotor>> MotorManager::createEthercatMotors_(
}
}
CMVR_LOG(ERROR) << "[MotorManager] EtherCAT motor creation is not implemented: vendor="
<< config::MotorVendor_Name(group_cfg.vendor())
<< ", protocol=" << config::MotorProtocol_Name(group_cfg.protocol())
<< ", group=" << group_cfg.id();
return {};
auto protocol = std::make_shared<Cia402Protocol>(
ethercat_bus_runtime, group_cfg.ethercat().cia402());
std::vector<std::shared_ptr<AbstractMotor>> motors;
motors.reserve(motor_cfgs.size());
for (const auto& cfg : motor_cfgs) {
auto motor = std::make_shared<EyouMotor>(
cfg, protocol, std::make_unique<EyouMotorAdapter>(ethercat_bus_runtime));
if (!motor->init()) {
CMVR_LOG(ERROR) << "[MotorManager] failed to init EYOU EtherCAT motor: "
<< cfg.joint_name();
return {};
}
motors.push_back(std::move(motor));
}
return motors;
}
} // namespace cmvr::device

43
cmvr-es/devices/motor/motor_protocol_interface.h
View File

@ -15,7 +15,8 @@ namespace cmvr {
public:
enum class CommProto : uint8_t {
CANOPEN = 1,
CUSTOM = 2
ETHERCAT = 2,
CUSTOM = 3
};
virtual ~MotorProtocolInterface() = default;
@ -26,22 +27,42 @@ namespace cmvr {
*/
virtual bool initNode(uint8_t node_id) = 0;
virtual void setQ(uint8_t node_id, double angle_rad) = 0;
virtual void setTarget(uint8_t node_id, double angle_rad,double vel) = 0;
virtual void setTarget(uint8_t node_id,double vel) = 0;
virtual void setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
virtual bool setMode(uint8_t node_id,msgs::RunMode mode ) = 0;
virtual msgs::RunMode getMode(uint8_t node_id) = 0;
virtual void setLimitQdd(uint8_t node_id, double u_qdd,double l_qdd) = 0;
virtual void setLimitQd(uint8_t node_id,double qd) = 0;
virtual void setLimitQ(uint8_t node_id, double ub, double lb) = 0;
virtual bool calibrateZeroQ(uint8_t node_id) = 0;
virtual bool reachedTargetQ(uint8_t node_id) = 0;
virtual void setQd(uint8_t node_id, double qd) = 0;
virtual void setQdd(uint8_t node_id,double qdd) = 0;
// virtual void setVelocity(uint8_t node_id, double velocity) = 0;
// virtual void clearError(uint8_t node_id) = 0;
virtual void brake(uint8_t node_id) = 0;
virtual void torqueOff(uint8_t node_id) = 0;
// target_q: rad, max_qd: rad/s, max_qdd: rad/s^2.
// Profile Position 写入目标位置和轮廓速度/加速度，并触发一次新目标。
virtual bool commandProfilePosition(uint8_t node_id,
double target_q,
double max_qd,
double max_qdd) = 0;
// target_qd: rad/s, max_qdd: rad/s^2.
// Profile Velocity 写入目标速度和轮廓加速度。
virtual bool commandProfileVelocity(uint8_t node_id,
double target_qd,
double max_qdd) = 0;
// target_q: rad, target_qd: rad/s.
// Cyclic Position 周期写入目标位置和目标速度。
virtual bool commandCyclicPosition(uint8_t node_id,
double target_q,
double target_qd) = 0;
// target_qd: rad/s.
// Cyclic Velocity 周期写入目标速度。
virtual bool commandCyclicVelocity(uint8_t node_id,
double target_qd) = 0;
// target_tau: N*m.
virtual bool commandCyclicTorque(uint8_t node_id, double target_tau) = 0;
virtual void setMotorConversion(uint8_t node_id,
double encoder_counts_per_rev,
double gear_ratio) = 0;
virtual bool torqueOn(uint8_t node_id) = 0;
virtual bool torqueOff(uint8_t node_id) = 0;
virtual bool brakeRelease(uint8_t node_id) = 0;
virtual bool quickStop(uint8_t node_id) = 0;
virtual double getQ(uint8_t node_id) = 0;
virtual double getQd(uint8_t node_id) = 0;

2
cmvr-es/manager/device_manager/include/device_manager.h
View File

@ -8,7 +8,6 @@
#include <list>
#include <mutex>
#include <string>
#include <unordered_set>
#include <unordered_map>
#include "device_factory.h"
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
@ -49,7 +48,6 @@ namespace cmvr::device {
explicit DeviceManager(const config::DeviceManagerConfig &cfg);
void log_device_plan_() const;
void pre_scan_robot_arm_dependencies_() const;
void init_devices_();
void configure_mujoco_viewer_pip_();
};

49
cmvr-es/manager/device_manager/src/device_factory.cpp
View File

@ -226,22 +226,41 @@ DeviceFactory::DeviceFactory()
});
registerCreator(config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM,
[](const auto& entry) {
if (entry.id().empty()) {
CMVR_LOG(ERROR) << "[DeviceFactory]: MotorManager id is required";
return DeviceRecord{};
}
if (entry.config_file().empty()) {
CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for MotorManager ID: " << entry.id();
return DeviceRecord{};
}
config::MotorRootConfig root_cfg;
if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail";
return DeviceRecord{};
}
[](const auto& entry) {
if (entry.id().empty()) {
CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id is required";
return DeviceRecord{};
}
if (entry.config_file().empty()) {
CMVR_LOG(ERROR) << "[DeviceFactory]: Empty config_file for motor group ID: " << entry.id();
return DeviceRecord{};
}
config::MotorRootConfig root_cfg;
if (!cmvr::ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
CMVR_LOG(ERROR) << "[DeviceFactory]: Read device config fail";
return DeviceRecord{};
}
const config::MotorGroupConfig* selected_group = nullptr;
for (const auto& group_cfg : root_cfg.motor().motor_groups()) {
if (group_cfg.id() != entry.id()) {
continue;
}
if (selected_group != nullptr) {
CMVR_LOG(ERROR) << "[DeviceFactory]: Duplicate motor group id '"
<< entry.id() << "' in config: " << entry.config_file();
return DeviceRecord{};
}
selected_group = &group_cfg;
}
if (selected_group == nullptr) {
CMVR_LOG(ERROR) << "[DeviceFactory]: Motor group id '" << entry.id()
<< "' not found in config: " << entry.config_file();
return DeviceRecord{};
}
CMVR_LOG(INFO) << "[DeviceFactory]: Read device config success";
auto device = std::make_shared<MotorManager>(entry.id(), root_cfg.motor());
auto device = std::make_shared<MotorManager>(
entry.id(), root_cfg.motor(), entry.id());
DeviceRecord record;
record.id = entry.id();
record.kind = device->kind();

168
cmvr-es/manager/device_manager/src/device_manager.cpp
View File

@ -18,8 +18,6 @@
#include "devices/speaker/abstract_speaker.h"
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
#include "common/config/config_files.h"
#include "cmvr/config/arm_config/arm_config.pb.h"
#include "cmvr/config/motor_config/motor_config.pb.h"
using namespace std;
using namespace cmvr::device;
@ -60,17 +58,6 @@ const char* deviceTypeToString(const cmvr::config::DeviceConfigEntry::DeviceType
}
}
bool motorGroupHasJoint(const cmvr::config::MotorGroupConfig& motor_group,
const std::string& joint_name)
{
for (const auto& motor : motor_group.motors().motors()) {
if (motor.joint_name() == joint_name) {
return true;
}
}
return false;
}
} // namespace
template std::shared_ptr<AbstractAGV> DeviceManager::getDevice(const std::string& device_id);
@ -96,7 +83,6 @@ DeviceManager::DeviceManager(const config::DeviceManagerConfig& cfg) {
dev_factory_ = std::make_unique<DeviceFactory>();
logSection("Device Plan");
log_device_plan_();
pre_scan_robot_arm_dependencies_();
logSection("Initialize Devices");
init_devices_();
configure_mujoco_viewer_pip_();
@ -121,7 +107,6 @@ DeviceManager& DeviceManager::getInstance() {
void DeviceManager::destroyInstance() {
std::lock_guard lock(init_mutex_);
instance_.reset();
MotorManager::clearActiveJoints();
}
void DeviceManager::start(){
@ -254,145 +239,6 @@ void DeviceManager::log_device_plan_() const
CMVR_LOG(INFO) << "[DeviceManager]: Device plan end";
}
void DeviceManager::pre_scan_robot_arm_dependencies_() const
{
using GroupJointSelection = std::unordered_map<std::string, std::unordered_set<std::string>>;
std::unordered_map<std::string, GroupJointSelection> selections;
std::unordered_map<std::string, config::MotorRootConfig> motor_roots;
for (const auto& entry : cfg_.devices()) {
if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM) {
continue;
}
if (entry.id().empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager device id is empty";
return;
}
if (entry.config_file().empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: Enabled MotorManager config_file is empty: " << entry.id();
return;
}
config::MotorRootConfig root_cfg;
if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load motor config: " << entry.config_file();
return;
}
if (!root_cfg.motor().id().empty() && root_cfg.motor().id() != entry.id()) {
CMVR_LOG(ERROR) << "[DeviceManager]: MotorManager entry id '" << entry.id()
<< "' does not match config id '" << root_cfg.motor().id() << "'";
return;
}
motor_roots.emplace(entry.id(), std::move(root_cfg));
}
for (const auto& entry : cfg_.devices()) {
if (!entry.enable() || entry.type() != config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM) {
continue;
}
if (entry.id().empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm device id is empty";
return;
}
if (entry.config_file().empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: Enabled RobotArm config_file is empty: " << entry.id();
return;
}
config::ArmRootConfig root_cfg;
if (!ConfigHelper::loadConfigFileSilent(entry.config_file(), root_cfg)) {
CMVR_LOG(ERROR) << "[DeviceManager]: Failed to load arm config: " << entry.config_file();
return;
}
const config::RobotArmConfig* arm_cfg = nullptr;
for (const auto& candidate : root_cfg.arm().robot_arms()) {
if (candidate.id() == entry.id()) {
arm_cfg = &candidate;
break;
}
}
if (!arm_cfg) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm ID '" << entry.id()
<< "' not found in config: " << entry.config_file();
return;
}
if (arm_cfg->backend_case() == config::RobotArmConfig::kVendor) {
continue;
}
if (arm_cfg->backend_case() != config::RobotArmConfig::kMotor) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm backend is not configured: " << entry.id();
return;
}
const auto& motor_config = arm_cfg->motor();
if (motor_config.motor_system_id().empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_system_id: " << entry.id();
return;
}
if (motor_config.motor_group_ids_size() == 0) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing motor_group_ids: " << entry.id();
return;
}
if (motor_config.joint_names_size() == 0) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm missing joint_names: " << entry.id();
return;
}
const auto motor_root_it = motor_roots.find(motor_config.motor_system_id());
if (motor_root_it == motor_roots.end()) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
<< "' depends on disabled or missing MotorManager: "
<< motor_config.motor_system_id();
return;
}
std::unordered_set<std::string> allowed_groups;
allowed_groups.reserve(static_cast<size_t>(motor_config.motor_group_ids_size()));
for (const auto& group_id : motor_config.motor_group_ids()) {
if (group_id.empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty motor_group_id: " << entry.id();
return;
}
allowed_groups.insert(group_id);
}
auto& group_selection = selections[motor_config.motor_system_id()];
for (const auto& joint_name : motor_config.joint_names()) {
if (joint_name.empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm has empty joint_name: " << entry.id();
return;
}
std::string matched_group;
for (const auto& motor_group : motor_root_it->second.motor().motor_groups()) {
const auto& group_id = motor_group.id();
if (allowed_groups.count(group_id) == 0) {
continue;
}
if (motorGroupHasJoint(motor_group, joint_name)) {
matched_group = group_id;
break;
}
}
if (matched_group.empty()) {
CMVR_LOG(ERROR) << "[DeviceManager]: RobotArm '" << entry.id()
<< "' joint '" << joint_name
<< "' not found in configured motor_group_ids";
return;
}
group_selection[matched_group].insert(joint_name);
}
}
MotorManager::clearActiveJoints();
for (auto& [motor_system_id, group_selection] : selections) {
MotorManager::setActiveJoints(motor_system_id, std::move(group_selection));
}
}
void DeviceManager::init_devices_() {
for (const auto& entry : cfg_.devices()) {
if (!entry.enable()) {
@ -462,12 +308,14 @@ void DeviceManager::configure_mujoco_viewer_pip_()
auto viewer = getDevice<cmvr::MujocoViewerDevice>(viewer_id);
if (viewer && viewer->setPiPCameraConfig(camera_config)) {
camera->setFetchRgbdFn([viewer](std::vector<unsigned char>& rgb,
std::vector<float>& depth,
int& width,
int& height,
uint64_t& frame_id) {
return viewer->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
const std::string camera_name = camera_config.camera_name();
camera->setFetchRgbdFn([viewer, camera_name](std::vector<unsigned char>& rgb,
std::vector<float>& depth,
int& width,
int& height,
uint64_t& frame_id) {
return viewer->getPiPCameraRGBD(
camera_name, rgb, depth, width, height, frame_id);
});
break;
}

2
cmvr-es/manager/task_manager/src/task_manager.cpp
View File

@ -36,6 +36,8 @@ const char* taskConfigTypeToString(const config::TaskConfigEntry::TaskType type)
return "TASK_TYPE_TOUCH_SCREEN";
case config::TaskConfigEntry::TASK_TYPE_GRPC_SERVER:
return "TASK_TYPE_GRPC_SERVER";
case config::TaskConfigEntry::TASK_TYPE_SELF_COLLISION:
return "TASK_TYPE_SELF_COLLISION";
case config::TaskConfigEntry::TASK_TYPE_UNKNOWN:
default:
return "TASK_TYPE_UNKNOWN";

5
cmvr-es/simulate/mujoco/mujoco_viewer/CMakeLists.txt
View File

@ -35,4 +35,9 @@ target_link_libraries(mujoco_viewer_test
gtest_main
cmvr_es::mujoco_viewer
cmvr_es::mujoco_world
cmvr_es::device_manager
cmvr_es::device::camera
cmvr_es::device::motor_manager
cmvr_es::device::mujoco_motor_driver
cmvr_es::device::motor_robot_arm
)

77
cmvr-es/simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h
View File

@ -5,8 +5,11 @@
#pragma once
#include <atomic>
#include <cstdint>
#include <deque>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
@ -52,6 +55,14 @@ namespace cmvr {
int display_height,
int render_width,
int render_height);
// Append another fixed camera view to the same viewer window.
void addPiPCamera(const char *camera_name,
int left,
int bottom,
int display_width,
int display_height,
int render_width,
int render_height);
void disablePiPCamera();
// 获取 PiP 相机 RGB+Depth（Depth 已线性化为米）
// depth 可不取（传 nullptr 或者用 getPiPCameraRGB 旧接口）
@ -60,9 +71,16 @@ namespace cmvr {
int &width,
int &height,
uint64_t &frame_id) const;
bool getPiPCameraRGBD(const std::string &camera_name,
std::vector<unsigned char> &rgb,
std::vector<float> &depth,
int &width,
int &height,
uint64_t &frame_id) const;
// 只拿 frame_id，便于 physics 线程判断是否新帧
uint64_t getPiPCameraFrameId() const;
uint64_t getPiPCameraFrameId(const std::string &camera_name) const;
public:
void setupCamera(double distance = 3.0,
@ -79,9 +97,34 @@ namespace cmvr {
void printCameraState() const;
private:
struct PiPCameraState {
std::string name;
int camera_id = -1;
int width = 320;
int height = 240;
int render_width = 320;
int render_height = 240;
bool render_size_warning_logged = false;
int margin = 10;
bool custom_pos = false;
int left = 0;
int bottom = 0;
mjvCamera camera{};
mjvScene scene{};
bool scene_inited = false;
mjModel *scene_model = nullptr;
std::vector<unsigned char> rgb;
std::vector<float> depth;
int rgb_width = 0;
int rgb_height = 0;
bool rgb_valid = false;
uint64_t frame_id = 0;
};
void renderPiP();
void initSim();
void syncThreadFunc();
void clearPiPCameras();
private:
std::shared_ptr<simulate::MujocoWorld> world_;
@ -93,29 +136,10 @@ namespace cmvr {
std::unique_ptr<mujoco::Simulate> sim_;
std::thread sync_thread_;
bool pip_enabled_ = false;
std::string pip_camera_name_;
int pip_camera_id_ = -1;
int pip_width_ = 320;
int pip_height_ = 240;
int pip_render_width_ = 320;
int pip_render_height_ = 240;
bool pip_render_size_warning_logged_ = false;
int pip_margin_ = 10;
bool pip_custom_pos_ = false;
int pip_left_ = 0;
int pip_bottom_ = 0;
mjvCamera pip_cam_;
mjvScene pip_scene_;
bool pip_scene_inited_ = false;
mjModel *pip_scene_model_ = nullptr;
std::deque<PiPCameraState> pip_cameras_;
mjData *pip_render_data_ = nullptr;
mjModel *pip_render_data_model_ = nullptr;
mutable std::mutex pip_rgb_mtx_;
std::vector<unsigned char> pip_rgb_;
std::vector<float> pip_depth_; // 新增：z-buffer
int pip_rgb_width_ = 0;
int pip_rgb_height_ = 0;
bool pip_rgb_valid_ = false;
uint64_t pip_frame_id_ = 0; // 新增：帧序号
};
@ -137,15 +161,20 @@ namespace cmvr {
int& width,
int& height,
uint64_t& frame_id) const;
bool getPiPCameraRGBD(const std::string& camera_name,
std::vector<unsigned char>& rgb,
std::vector<float>& depth,
int& width,
int& height,
uint64_t& frame_id) const;
private:
config::MujocoViewerConfig config_;
config::MujocoCameraConfig pip_camera_config_;
std::vector<config::MujocoCameraConfig> pip_camera_configs_;
std::shared_ptr<simulate::MujocoWorld> world_;
std::unique_ptr<MuJocoViewer> viewer_;
std::thread viewer_thread_;
mutable std::mutex mtx_;
bool has_pip_camera_config_ = false;
bool running_ = false;
bool stop_requested_ = false;
};

501
cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer.cpp
View File

@ -53,8 +53,6 @@ namespace cmvr {
mjv_defaultCamera(&cam_);
mjv_defaultOption(&opt_);
mjv_defaultPerturb(&pert_);
mjv_defaultCamera(&pip_cam_);
mjv_defaultScene(&pip_scene_);
auto platform_ui = std::make_unique<PiPGlfwAdapter>(this);
sim_ = std::make_unique<mj::Simulate>(
@ -72,9 +70,11 @@ namespace cmvr {
sync_thread_.join();
}
if (pip_scene_inited_) {
mjv_freeScene(&pip_scene_);
pip_scene_inited_ = false;
clearPiPCameras();
if (pip_render_data_ != nullptr) {
mj_deleteData(pip_render_data_);
pip_render_data_ = nullptr;
pip_render_data_model_ = nullptr;
}
}
@ -115,14 +115,12 @@ namespace cmvr {
}
void MuJocoViewer::enablePiPCamera(const char *camera_name) {
pip_enabled_ = true;
pip_camera_name_ = camera_name ? camera_name : "";
pip_camera_id_ = -1;
pip_width_ = 320;
pip_height_ = 240;
pip_render_width_ = pip_width_;
pip_render_height_ = pip_height_;
pip_custom_pos_ = false;
clearPiPCameras();
PiPCameraState state;
state.name = camera_name ? camera_name : "";
mjv_defaultCamera(&state.camera);
mjv_defaultScene(&state.scene);
pip_cameras_.push_back(std::move(state));
}
void MuJocoViewer::enablePiPCamera(const char *camera_name,
@ -140,183 +138,241 @@ namespace cmvr {
int display_height,
int render_width,
int render_height) {
pip_enabled_ = true;
pip_camera_name_ = camera_name ? camera_name : "";
pip_camera_id_ = -1;
pip_left_ = left;
pip_bottom_ = bottom;
pip_width_ = display_width > 0 ? display_width : 320;
pip_height_ = display_height > 0 ? display_height : 240;
pip_render_width_ = render_width > 0 ? render_width : pip_width_;
pip_render_height_ = render_height > 0 ? render_height : pip_height_;
pip_custom_pos_ = true;
clearPiPCameras();
addPiPCamera(camera_name,
left,
bottom,
display_width,
display_height,
render_width,
render_height);
}
void MuJocoViewer::addPiPCamera(const char *camera_name,
int left,
int bottom,
int display_width,
int display_height,
int render_width,
int render_height) {
PiPCameraState state;
state.name = camera_name ? camera_name : "";
state.left = left;
state.bottom = bottom;
state.width = display_width > 0 ? display_width : 320;
state.height = display_height > 0 ? display_height : 240;
state.render_width = render_width > 0 ? render_width : state.width;
state.render_height = render_height > 0 ? render_height : state.height;
state.custom_pos = true;
mjv_defaultCamera(&state.camera);
mjv_defaultScene(&state.scene);
pip_cameras_.push_back(std::move(state));
}
void MuJocoViewer::disablePiPCamera() {
pip_enabled_ = false;
clearPiPCameras();
}
void MuJocoViewer::clearPiPCameras() {
for (auto &pip : pip_cameras_) {
if (pip.scene_inited) {
mjv_freeScene(&pip.scene);
pip.scene_inited = false;
pip.scene_model = nullptr;
}
}
pip_cameras_.clear();
}
void MuJocoViewer::renderPiP() {
if (!pip_enabled_ || !sim_) return;
if (pip_camera_name_.empty()) return;
if (!sim_ || pip_cameras_.empty()) return;
mjModel* render_model = sim_->m_passive_ ? sim_->m_passive_ : sim_->m_;
mjData* render_data = sim_->d_passive_ ? sim_->d_passive_ : sim_->d_;
if (!render_model || !render_data) return;
if (pip_camera_id_ < 0) {
pip_camera_id_ = mj_name2id(render_model, mjOBJ_CAMERA, pip_camera_name_.c_str());
if (pip_camera_id_ < 0) {
return;
}
}
auto [fb_width, fb_height] = sim_->platform_ui->GetFramebufferSize();
if (fb_width <= 0 || fb_height <= 0) return;
int left = 0;
int bottom = 0;
int width = 0;
int height = 0;
if (pip_custom_pos_) {
width = std::min(pip_width_, fb_width);
height = std::min(pip_height_, fb_height);
const int max_left = fb_width - width;
const int max_bottom = fb_height - height;
left = pip_left_ >= 0
? std::max(0, std::min(pip_left_, max_left))
: std::max(0, std::min(fb_width - width + pip_left_, max_left));
bottom = pip_bottom_ >= 0
? std::max(0, std::min(pip_bottom_, max_bottom))
: std::max(0, std::min(fb_height - height + pip_bottom_, max_bottom));
} else {
width = std::min(pip_width_, fb_width - 2 * pip_margin_);
height = std::min(pip_height_, fb_height - 2 * pip_margin_);
left = fb_width - pip_margin_ - width;
bottom = pip_margin_;
}
if (width <= 0 || height <= 0) return;
mjrRect display_rect;
display_rect.width = width;
display_rect.height = height;
display_rect.left = left;
display_rect.bottom = bottom;
const std::unique_lock<std::recursive_mutex> lock(sim_->mtx);
if (!pip_scene_inited_ || pip_scene_model_ != render_model) {
if (pip_scene_inited_) {
mjv_freeScene(&pip_scene_);
// Copy one visualization snapshot for all PiP cameras. Keep the
// simulation lock out of scene updates, GPU rendering, and readback.
{
std::unique_lock<std::recursive_mutex> lock(sim_->mtx, std::try_to_lock);
if (lock.owns_lock()) {
if (pip_render_data_model_ != render_model) {
if (pip_render_data_ != nullptr) {
mj_deleteData(pip_render_data_);
pip_render_data_ = nullptr;
}
pip_render_data_ = mj_makeData(render_model);
pip_render_data_model_ = render_model;
}
if (pip_render_data_ != nullptr) {
mjv_copyData(pip_render_data_, render_model, render_data);
}
}
mjv_makeScene(render_model, &pip_scene_, kPiPMaxGeom);
pip_scene_inited_ = true;
pip_scene_model_ = render_model;
}
pip_cam_.type = mjCAMERA_FIXED;
pip_cam_.fixedcamid = pip_camera_id_;
pip_cam_.trackbodyid = -1;
mjv_updateScene(render_model, render_data, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
// If the sync thread owns the lock, use the last complete snapshot.
if (pip_render_data_ == nullptr || pip_render_data_model_ != render_model) {
return;
}
auto& context = sim_->platform_ui->mjr_context();
const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;
const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height;
const int render_width = std::min(std::max(1, pip_render_width_), offscreen_width);
const int render_height = std::min(std::max(1, pip_render_height_), offscreen_height);
if (!pip_render_size_warning_logged_ &&
(render_width != pip_render_width_ || render_height != pip_render_height_)) {
CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped"
<< ", requested=" << pip_render_width_ << "x" << pip_render_height_
<< ", actual=" << render_width << "x" << render_height
<< ", offscreen=" << offscreen_width << "x" << offscreen_height;
pip_render_size_warning_logged_ = true;
}
const bool use_offscreen = render_width != display_rect.width || render_height != display_rect.height;
for (size_t pip_index = 0; pip_index < pip_cameras_.size(); ++pip_index) {
auto& pip = pip_cameras_[pip_index];
if (pip.name.empty()) continue;
mjrRect render_rect;
render_rect.left = 0;
render_rect.bottom = 0;
render_rect.width = render_width;
render_rect.height = render_height;
if (pip.camera_id < 0) {
pip.camera_id = mj_name2id(render_model, mjOBJ_CAMERA, pip.name.c_str());
if (pip.camera_id < 0) {
CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera not found: " << pip.name;
continue;
}
}
bool rendered_offscreen = false;
if (use_offscreen) {
mjr_setBuffer(mjFB_OFFSCREEN, &context);
if (context.currentBuffer == mjFB_OFFSCREEN) {
mjr_render(render_rect, &pip_scene_, &context);
rendered_offscreen = true;
int left = 0;
int bottom = 0;
int width = 0;
int height = 0;
if (pip.custom_pos) {
width = std::min(pip.width, fb_width);
height = std::min(pip.height, fb_height);
const int max_left = fb_width - width;
const int max_bottom = fb_height - height;
left = pip.left >= 0
? std::max(0, std::min(pip.left, max_left))
: std::max(0, std::min(fb_width - width + pip.left, max_left));
bottom = pip.bottom >= 0
? std::max(0, std::min(pip.bottom, max_bottom))
: std::max(0, std::min(fb_height - height + pip.bottom, max_bottom));
} else {
mjr_render(display_rect, &pip_scene_, &context);
width = std::min(pip.width, fb_width - 2 * pip.margin);
height = std::min(pip.height, fb_height - 2 * pip.margin);
left = fb_width - pip.margin - width;
bottom = pip.margin;
}
if (width <= 0 || height <= 0) continue;
mjrRect display_rect;
display_rect.width = width;
display_rect.height = height;
display_rect.left = left;
display_rect.bottom = bottom;
if (!pip.scene_inited || pip.scene_model != render_model) {
if (pip.scene_inited) {
mjv_freeScene(&pip.scene);
}
mjv_makeScene(render_model, &pip.scene, kPiPMaxGeom);
pip.scene_inited = true;
pip.scene_model = render_model;
}
pip.camera.type = mjCAMERA_FIXED;
pip.camera.fixedcamid = pip.camera_id;
pip.camera.trackbodyid = -1;
mjv_updateScene(render_model,
pip_render_data_,
&opt_,
&pert_,
&pip.camera,
mjCAT_ALL,
&pip.scene);
const int offscreen_width = context.offWidth > 0 ? context.offWidth : display_rect.width;
const int offscreen_height = context.offHeight > 0 ? context.offHeight : display_rect.height;
const int render_width = std::min(std::max(1, pip.render_width), offscreen_width);
const int render_height = std::min(std::max(1, pip.render_height), offscreen_height);
if (!pip.render_size_warning_logged &&
(render_width != pip.render_width || render_height != pip.render_height)) {
CMVR_LOG(WARNING) << "[MuJocoViewer] PiP camera render size clamped"
<< ", camera=" << pip.name
<< ", requested=" << pip.render_width << "x" << pip.render_height
<< ", actual=" << render_width << "x" << render_height
<< ", offscreen=" << offscreen_width << "x" << offscreen_height;
pip.render_size_warning_logged = true;
}
const bool use_offscreen =
render_width != display_rect.width || render_height != display_rect.height;
mjrRect render_rect;
render_rect.left = 0;
render_rect.bottom = 0;
render_rect.width = render_width;
render_rect.height = render_height;
bool rendered_offscreen = false;
if (use_offscreen) {
mjr_setBuffer(mjFB_OFFSCREEN, &context);
if (context.currentBuffer == mjFB_OFFSCREEN) {
mjr_render(render_rect, &pip.scene, &context);
rendered_offscreen = true;
} else {
mjr_render(display_rect, &pip.scene, &context);
render_rect = display_rect;
}
} else {
mjr_render(display_rect, &pip.scene, &context);
render_rect = display_rect;
}
} else {
mjr_render(display_rect, &pip_scene_, &context);
render_rect = display_rect;
}
{
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
const int w = render_rect.width;
const int h = render_rect.height;
if (w > 0 && h > 0) {
pip_rgb_.resize(static_cast<size_t>(3 * w * h));
pip_depth_.resize(static_cast<size_t>(w * h));
{
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
const int w = render_rect.width;
const int h = render_rect.height;
if (w > 0 && h > 0) {
pip.rgb.resize(static_cast<size_t>(3 * w * h));
pip.depth.resize(static_cast<size_t>(w * h));
mjr_readPixels(pip.rgb.data(), pip.depth.data(), render_rect, &context);
// 同时读 RGB 和 depth(z-buffer 0..1)
mjr_readPixels(pip_rgb_.data(), pip_depth_.data(),
render_rect, &context);
if (rendered_offscreen) {
mjr_setBuffer(mjFB_WINDOW, &context);
mjr_render(display_rect, &pip_scene_, &context);
}
// OpenGL's pixel origin is bottom-left; expose top-left images.
for (int r = 0; r < h / 2; ++r) {
unsigned char *top_row = pip.rgb.data() + 3 * w * r;
unsigned char *bottom_row = pip.rgb.data() + 3 * w * (h - 1 - r);
std::swap_ranges(top_row, top_row + 3 * w, bottom_row);
// OpenGL 像素原点在左下，需要竖直翻转 RGB 和 depth
for (int r = 0; r < h / 2; ++r) {
// flip rgb row
unsigned char *top_row = pip_rgb_.data() + 3 * w * r;
unsigned char *bottom_row = pip_rgb_.data() + 3 * w * (h - 1 - r);
std::swap_ranges(top_row, top_row + 3 * w, bottom_row);
// flip depth row
float *top_d = pip_depth_.data() + w * r;
float *bot_d = pip_depth_.data() + w * (h - 1 - r);
std::swap_ranges(top_d, top_d + w, bot_d);
}
// 将 OpenGL depth buffer(0..1) 线性化为相机前向距离（米）。
const double znear = static_cast<double>(render_model->vis.map.znear) *
static_cast<double>(render_model->stat.extent);
const double zfar = static_cast<double>(render_model->vis.map.zfar) *
static_cast<double>(render_model->stat.extent);
if (znear > 0.0 && zfar > znear) {
const double two_nf = 2.0 * znear * zfar;
const double f_plus_n = zfar + znear;
const double f_minus_n = zfar - znear;
for (float &d : pip_depth_) {
if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) {
d = std::numeric_limits<float>::infinity();
continue;
}
const double z_ndc = 2.0 * static_cast<double>(d) - 1.0; // [-1,1]
const double denom = f_plus_n - z_ndc * f_minus_n;
if (denom <= 1e-12) {
d = std::numeric_limits<float>::infinity();
continue;
}
d = static_cast<float>(two_nf / denom);
float *top_d = pip.depth.data() + w * r;
float *bot_d = pip.depth.data() + w * (h - 1 - r);
std::swap_ranges(top_d, top_d + w, bot_d);
}
}
pip_rgb_width_ = w;
pip_rgb_height_ = h;
pip_rgb_valid_ = true;
++pip_frame_id_; // 新帧
// Linearize the OpenGL depth buffer to camera-forward meters.
const double znear = static_cast<double>(render_model->vis.map.znear) *
static_cast<double>(render_model->stat.extent);
const double zfar = static_cast<double>(render_model->vis.map.zfar) *
static_cast<double>(render_model->stat.extent);
if (znear > 0.0 && zfar > znear) {
const double two_nf = 2.0 * znear * zfar;
const double f_plus_n = zfar + znear;
const double f_minus_n = zfar - znear;
for (float &d : pip.depth) {
if (!std::isfinite(d) || d <= 0.0f || d >= 1.0f) {
d = std::numeric_limits<float>::infinity();
continue;
}
const double z_ndc = 2.0 * static_cast<double>(d) - 1.0;
const double denom = f_plus_n - z_ndc * f_minus_n;
if (denom <= 1e-12) {
d = std::numeric_limits<float>::infinity();
continue;
}
d = static_cast<float>(two_nf / denom);
}
}
pip.rgb_width = w;
pip.rgb_height = h;
pip.rgb_valid = true;
++pip.frame_id;
}
}
if (rendered_offscreen) {
mjr_setBuffer(mjFB_WINDOW, &context);
mjr_render(display_rect, &pip.scene, &context);
}
}
}
@ -331,12 +387,18 @@ namespace cmvr {
if (!world_->model() || !world_->data()) {
mju_error("MuJocoViewer world has null model/data");
}
if (pip_enabled_ && pip_render_width_ > 0 && pip_render_height_ > 0) {
int max_pip_render_width = 0;
int max_pip_render_height = 0;
for (const auto &pip : pip_cameras_) {
max_pip_render_width = std::max(max_pip_render_width, pip.render_width);
max_pip_render_height = std::max(max_pip_render_height, pip.render_height);
}
if (!pip_cameras_.empty() && max_pip_render_width > 0 && max_pip_render_height > 0) {
mjModel* model = world_->model();
const int old_width = model->vis.global.offwidth;
const int old_height = model->vis.global.offheight;
model->vis.global.offwidth = std::max(model->vis.global.offwidth, pip_render_width_);
model->vis.global.offheight = std::max(model->vis.global.offheight, pip_render_height_);
model->vis.global.offwidth = std::max(model->vis.global.offwidth, max_pip_render_width);
model->vis.global.offheight = std::max(model->vis.global.offheight, max_pip_render_height);
if (model->vis.global.offwidth != old_width || model->vis.global.offheight != old_height) {
CMVR_LOG(INFO) << "[MuJocoViewer] resize offscreen buffer before context creation"
<< ", old=" << old_width << "x" << old_height
@ -386,7 +448,15 @@ namespace cmvr {
uint64_t MuJocoViewer::getPiPCameraFrameId() const {
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
return pip_frame_id_;
return pip_cameras_.empty() ? 0 : pip_cameras_.front().frame_id;
}
uint64_t MuJocoViewer::getPiPCameraFrameId(const std::string &camera_name) const {
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
const auto it = std::find_if(
pip_cameras_.begin(), pip_cameras_.end(),
[&camera_name](const PiPCameraState &pip) { return pip.name == camera_name; });
return it == pip_cameras_.end() ? 0 : it->frame_id;
}
bool MuJocoViewer::getPiPCameraRGBD(std::vector<unsigned char> &rgb,
@ -395,13 +465,35 @@ namespace cmvr {
int &height,
uint64_t &frame_id) const {
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
if (!pip_rgb_valid_ || pip_rgb_.empty()) return false;
if (pip_cameras_.empty()) return false;
const auto &pip = pip_cameras_.front();
if (!pip.rgb_valid || pip.rgb.empty()) return false;
rgb = pip_rgb_;
depth = pip_depth_;
width = pip_rgb_width_;
height = pip_rgb_height_;
frame_id = pip_frame_id_;
rgb = pip.rgb;
depth = pip.depth;
width = pip.rgb_width;
height = pip.rgb_height;
frame_id = pip.frame_id;
return true;
}
bool MuJocoViewer::getPiPCameraRGBD(const std::string &camera_name,
std::vector<unsigned char> &rgb,
std::vector<float> &depth,
int &width,
int &height,
uint64_t &frame_id) const {
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
const auto it = std::find_if(
pip_cameras_.begin(), pip_cameras_.end(),
[&camera_name](const PiPCameraState &pip) { return pip.name == camera_name; });
if (it == pip_cameras_.end() || !it->rgb_valid || it->rgb.empty()) return false;
rgb = it->rgb;
depth = it->depth;
width = it->rgb_width;
height = it->rgb_height;
frame_id = it->frame_id;
return true;
}
@ -456,6 +548,7 @@ namespace cmvr {
}
std::shared_ptr<simulate::MujocoWorld> world;
std::vector<config::MujocoCameraConfig> pip_camera_configs;
{
std::lock_guard<std::mutex> lock(mtx_);
if (running_) {
@ -466,6 +559,7 @@ namespace cmvr {
return false;
}
world = world_;
pip_camera_configs = pip_camera_configs_;
stop_requested_ = false;
running_ = true;
}
@ -486,19 +580,30 @@ namespace cmvr {
config_.camera_azimuth(),
config_.camera_elevation());
if (has_pip_camera_config_ && !pip_camera_config_.camera_name().empty()) {
const auto& pip = pip_camera_config_.viewer_pip();
const auto& render = pip_camera_config_.render();
if (pip.width() > 0 && pip.height() > 0) {
viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str(),
bool first_pip_camera = true;
for (const auto& camera_config : pip_camera_configs) {
if (camera_config.camera_name().empty()) {
continue;
}
const auto& pip = camera_config.viewer_pip();
const auto& render = camera_config.render();
if (first_pip_camera) {
viewer->enablePiPCamera(camera_config.camera_name().c_str(),
pip.left(),
pip.bottom(),
pip.width(),
pip.height(),
render.width(),
render.height());
first_pip_camera = false;
} else {
viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str());
viewer->addPiPCamera(camera_config.camera_name().c_str(),
pip.left(),
pip.bottom(),
pip.width(),
pip.height(),
render.width(),
render.height());
}
}
@ -525,19 +630,31 @@ namespace cmvr {
if (camera_config.world_id() != config_.world_id()) {
return false;
}
std::lock_guard<std::mutex> lock(mtx_);
if (has_pip_camera_config_) {
CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured, keep first"
<< ", viewer_id=" << id_
<< ", current_camera=" << pip_camera_config_.camera_name()
<< ", ignored_camera=" << camera_config.camera_name();
if (camera_config.camera_name().empty()) {
return false;
}
pip_camera_config_ = camera_config;
has_pip_camera_config_ = true;
CMVR_LOG(INFO) << "[MujocoViewerDevice] set PiP camera"
std::lock_guard<std::mutex> lock(mtx_);
if (running_) {
CMVR_LOG(WARNING) << "[MujocoViewerDevice] cannot add PiP camera while viewer is running"
<< ", viewer_id=" << id_
<< ", camera=" << camera_config.camera_name();
return false;
}
const auto duplicate = std::find_if(
pip_camera_configs_.begin(), pip_camera_configs_.end(),
[&camera_config](const config::MujocoCameraConfig& configured) {
return configured.camera_name() == camera_config.camera_name();
});
if (duplicate != pip_camera_configs_.end()) {
CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured"
<< ", viewer_id=" << id_
<< ", camera=" << camera_config.camera_name();
return false;
}
pip_camera_configs_.push_back(camera_config);
CMVR_LOG(INFO) << "[MujocoViewerDevice] add PiP camera"
<< ", viewer_id=" << id_
<< ", camera=" << camera_config.camera_name()
<< ", world_id=" << camera_config.world_id();
@ -556,6 +673,20 @@ namespace cmvr {
return viewer_->getPiPCameraRGBD(rgb, depth, width, height, frame_id);
}
bool MujocoViewerDevice::getPiPCameraRGBD(const std::string& camera_name,
std::vector<unsigned char>& rgb,
std::vector<float>& depth,
int& width,
int& height,
uint64_t& frame_id) const {
std::lock_guard<std::mutex> lock(mtx_);
if (!viewer_) {
return false;
}
return viewer_->getPiPCameraRGBD(
camera_name, rgb, depth, width, height, frame_id);
}
bool MujocoViewerDevice::stop() {
std::thread thread_to_join;
{

131
cmvr-es/simulate/mujoco/mujoco_viewer/src/mujoco_viewer_test.cpp
View File

@ -1,11 +1,21 @@
#include <algorithm>
#include <array>
#include <cstdlib>
#include <filesystem>
#include <iostream>
#include <memory>
#include <string>
#include <vector>
#include <gtest/gtest.h>
#include "common/config/config_files.h"
#include "cmvr/config/device_manager_config/device_manager_config.pb.h"
#include "devices/arm/robot_arm.h"
#include "devices/camera/abstract_camera.h"
#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
#include "devices/motor/manager/include/motor_manager.h"
#include "manager/device_manager/include/device_manager.h"
#include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
@ -37,6 +47,63 @@ std::string defaultModelPath()
return (root / "model/xiaoyan_description/dual_arm.xml").string();
}
cmvr::device::DeviceManager& createEyeToHandDeviceManager(
const std::filesystem::path& project_root)
{
cmvr::device::DeviceManager::destroyInstance();
cmvr::ConfigHelper::setConfigRootFromFile(
(project_root / "cmvr-es/config/cmvr_es.pb.txt").string());
cmvr::config::DeviceManagerConfig config;
config.set_name("mujoco_viewer_eye_to_hand_test");
config.set_version("test");
auto* world = config.add_devices();
world->set_id("mujoco_world");
world->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD);
world->set_config_file("devices/mujoco/right_arm_eye_to_hand_world.pb.txt");
world->set_enable(true);
auto* motors = config.add_devices();
motors->set_id("right_arm_mujoco_motors");
motors->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
motors->set_config_file("devices/motor/mujoco_motors.pb.txt");
motors->set_enable(true);
auto* arm = config.add_devices();
arm->set_id("mujoco_right_arm");
arm->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM);
arm->set_config_file("devices/arm/arm_mujoco.pb.txt");
arm->set_enable(true);
for (const auto* camera_id : {"mujoco_hand_cam", "mujoco_external_touch_cam"}) {
auto* camera = config.add_devices();
camera->set_id(camera_id);
camera->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA);
camera->set_config_file("devices/camera/camera.pb.txt");
camera->set_enable(true);
}
return cmvr::device::DeviceManager::getInstance(config);
}
cmvr::device::Result moveArmToInitialization(cmvr::device::RobotArm& arm)
{
const std::vector<double> positions = {
-0.2423,
1.2929,
1.61,
1.58,
-2.8792,
0.1150,
-0.08,
};
cmvr::device::MotionOptions options;
options.velocity = 2.8;
options.acceleration = 20.0;
return arm.moveJ(cmvr::device::JointPositionCommand{positions}, options);
}
} // namespace
TEST(MujocoViewerTest, ShowsUiWithMujocoWorld)
@ -63,3 +130,67 @@ TEST(MujocoViewerTest, ShowsUiWithMujocoWorld)
world->stop();
}
TEST(MujocoViewerTest, ShowsRightArmEyeToHandCamera)
{
const auto project_root = findProjectRoot();
ASSERT_FALSE(project_root.empty());
const auto model_path = project_root / "model/xiaoyan_description/right_arm_eye_to_hand.xml";
auto& device_manager = createEyeToHandDeviceManager(project_root);
auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
ASSERT_NE(arm, nullptr);
auto hand_camera_base =
device_manager.getDevice<cmvr::device::AbstractCamera>("mujoco_hand_cam");
auto external_camera_base =
device_manager.getDevice<cmvr::device::AbstractCamera>("mujoco_external_touch_cam");
auto hand_camera = std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(hand_camera_base);
auto external_camera =
std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(external_camera_base);
ASSERT_NE(hand_camera, nullptr);
ASSERT_NE(external_camera, nullptr);
auto world = cmvr::device::MotorManager::mujocoWorldFor("right_arm_mujoco_motors");
ASSERT_NE(world, nullptr);
ASSERT_TRUE(world->isLoaded());
ASSERT_TRUE(world->isRunning());
const auto move_result = moveArmToInitialization(*arm);
ASSERT_TRUE(move_result.ok()) << move_result.message;
cmvr::MuJocoViewer viewer(world);
ASSERT_NE(viewer.model(), nullptr);
ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "hand_cam"), 0)
<< "right_arm_eye_to_hand.xml does not contain hand_cam";
ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "external_touch_cam"), 0)
<< "right_arm_eye_to_hand.xml does not contain external_touch_cam";
viewer.setupCamera(2.5, -160.0, -25.0);
const auto& external_config = external_camera->config();
const auto& hand_config = hand_camera->config();
ASSERT_TRUE(external_config.viewer_pip().enable());
ASSERT_TRUE(hand_config.viewer_pip().enable());
viewer.enablePiPCamera(external_config.camera_name().c_str(),
external_config.viewer_pip().left(),
external_config.viewer_pip().bottom(),
external_config.viewer_pip().width(),
external_config.viewer_pip().height(),
external_config.encoder().width(),
external_config.encoder().height());
viewer.addPiPCamera(hand_config.camera_name().c_str(),
hand_config.viewer_pip().left(),
hand_config.viewer_pip().bottom(),
hand_config.viewer_pip().width(),
hand_config.viewer_pip().height(),
hand_config.encoder().width(),
hand_config.encoder().height());
std::cout << "MujocoWorld loaded: " << model_path.string() << std::endl;
std::cout << "Showing hand_cam and external_touch_cam in PiP. "
"Close the MuJoCo window to exit."
<< std::endl;
viewer.run();
world->stop();
}

40
cmvr-es/task/CMakeLists.txt
View File

@ -1,5 +1,6 @@
add_library(task
touch_screen_task/src/touch_screen_task.cpp
self_collision_task/src/self_collision_task.cpp
)
target_include_directories(task PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
@ -10,6 +11,7 @@ target_link_libraries(task
cmvr_es::common
cmvr_es::ik_solver
cmvr_es::base_motion
cmvr_es::self_collision_checker
PRIVATE
cmvr_es::device_manager
)
@ -17,22 +19,22 @@ target_link_libraries(task
add_library(cmvr_es::task ALIAS task)
install(TARGETS task LIBRARY DESTINATION lib)
#add_executable(touch_screen_task_test
# touch_screen_task/src/touch_screen_task_test.cpp
#)
#
#target_link_libraries(touch_screen_task_test PRIVATE
# cmvr_es::task
# cmvr_es::device::arm
# cmvr_es::device::motor_manager
# cmvr_es::device::mujoco_motor_driver
# cmvr_es::device::mujoco_camera
# cmvr_es::mujoco_viewer
# cmvr_es::proto
# cmvr_es::device_manager
# cmvr_es::service
# gtest
# gtest_main
# pthread
# glog
#)
add_executable(touch_screen_task_test
touch_screen_task/src/touch_screen_task_test.cpp
)
target_link_libraries(touch_screen_task_test PRIVATE
cmvr_es::task
cmvr_es::device::arm
cmvr_es::device::motor_manager
cmvr_es::device::mujoco_motor_driver
cmvr_es::device::mujoco_camera
cmvr_es::mujoco_viewer
cmvr_es::proto
cmvr_es::device_manager
cmvr_es::service
gtest
gtest_main
pthread
glog
)

98
cmvr-es/task/self_collision_task/include/self_collision_task.h Normal file
View File

@ -0,0 +1,98 @@
#ifndef CMVR_ES_SELF_COLLISION_TASK_H
#define CMVR_ES_SELF_COLLISION_TASK_H
#include <condition_variable>
#include <cstdint>
#include <deque>
#include <memory>
#include <mutex>
#include <optional>
#include <string>
#include <vector>
#include "algorithms/collision_detection/self_collision/include/distance_sampling_policy.h"
#include "algorithms/collision_detection/self_collision/include/self_collision_checker.h"
#include "cmvr/config/self_collision_task_config/self_collision_task_config.pb.h"
#include "devices/arm/robot_arm.h"
#include "task/task.h"
namespace cmvr::task {
enum class CollisionSafetyLevel {
UNKNOWN = 0,
SAFE,
WARNING,
STOP,
};
enum class ProtectiveRecoveryState {
IDLE = 0,
AVAILABLE,
RECOVERING,
SUCCEEDED,
FAILED,
};
struct SelfCollisionTaskStatus {
CollisionSafetyLevel level{CollisionSafetyLevel::UNKNOWN};
SelfCollisionResult result;
bool stop_latched{false};
std::uint64_t event_id{0};
ProtectiveRecoveryState recovery_state{ProtectiveRecoveryState::IDLE};
std::size_t recovery_sample_count{0};
std::string recovery_error;
};
class SelfCollisionTask final : public Task {
public:
explicit SelfCollisionTask(const config::SelfCollisionTaskConfig& config);
const std::string& id() const override { return id_; }
TaskRunMode runMode() const override { return TaskRunMode::PERIODIC_STEP; }
bool init() override;
bool start() override;
bool step(double dt) override;
void stop() override;
TaskState state() const override;
bool isBusy() const override;
bool isFinished() const override;
bool isFailed() const override;
std::string stateString() const override;
std::string detailStatusString() const override;
SelfCollisionTaskStatus latestStatus() const;
device::Result requestRecovery(std::uint64_t event_id);
private:
static bool validateConfig(const config::SelfCollisionTaskConfig& config,
std::string* error);
static const char* safetyLevelToString(CollisionSafetyLevel level);
static const char* recoveryStateToString(ProtectiveRecoveryState state);
void recordJointSample_(const device::JointGroupState& joint_state,
DistanceSamplingPolicy::Clock::time_point now);
config::SelfCollisionTaskConfig config_;
std::string id_;
std::shared_ptr<device::RobotArm> arm_;
SelfCollisionChecker checker_;
DistanceSamplingPolicy sampling_;
mutable std::mutex mutex_;
std::condition_variable recovery_cv_;
TaskState state_{TaskState::UNINITIALIZED};
SelfCollisionTaskStatus latest_status_{};
std::deque<device::JointTrajectoryPoint> joint_history_;
device::JointTrajectory recovery_path_;
DistanceSamplingPolicy::Clock::time_point history_epoch_{};
std::optional<DistanceSamplingPolicy::Clock::time_point> recovery_clear_since_;
double recovery_best_distance_m_{0.0};
bool recovery_clear_confirmed_{false};
std::uint64_t next_event_id_{1};
std::string last_error_;
};
} // namespace cmvr::task
#endif // CMVR_ES_SELF_COLLISION_TASK_H

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