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

Compare commits

base: SmartBench:141e9813c2539d46c8ca78ad825b4612a8bc94d0
Branches Tags
SmartBench:main
SmartBench:linbo_dev
SmartBench:dev
SmartBench:lgv_dev
SmartBench:xtkuang_dev3
SmartBench:linbo_dev_new
SmartBench:xtkuang_dev2
SmartBench:backup-dev-before-reset-20260903
SmartBench:xtkuang_dev
SmartBench:lgv_refator_code
SmartBench:backup-dev-before-rollback
SmartBench:dev_touch_screen
SmartBench:lgv_test_0227
SmartBench:lgv_11_12
SmartBench:backup/dev_before_replace
SmartBench:v1
SmartBench:refactor_dev
SmartBench:061bfc50
SmartBench:v1.1
SmartBench:v1.0
..
compare: SmartBench:29a899b305491652903b550e1a8573eae0f269bd
Branches Tags
SmartBench:linbo_dev
SmartBench:dev
SmartBench:lgv_dev
SmartBench:xtkuang_dev3
SmartBench:linbo_dev_new
SmartBench:xtkuang_dev2
SmartBench:backup-dev-before-reset-20260903
SmartBench:xtkuang_dev
SmartBench:lgv_refator_code
SmartBench:backup-dev-before-rollback
SmartBench:dev_touch_screen
SmartBench:lgv_test_0227
SmartBench:lgv_11_12
SmartBench:backup/dev_before_replace
SmartBench:v1
SmartBench:refactor_dev
SmartBench:061bfc50
SmartBench:main
SmartBench:v1.1
SmartBench:v1.0

No commits in common. "141e9813c2539d46c8ca78ad825b4612a8bc94d0" and "29a899b305491652903b550e1a8573eae0f269bd" have entirely different histories.
141e9813c2 ... 29a899b305

53 changed files with 865 additions and 5333 deletions
Show Stats Expand all files Collapse all files

6
MUJOCO_LOG.TXT
View File

@ -4,9 +4,3 @@ 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

12
cmvr-es/algorithms/controllers/CMakeLists.txt
View File

@ -12,7 +12,7 @@ add_subdirectory(arm_control)
#) 其他动态库类似
file(GLOB SRC
${CMAKE_CURRENT_SOURCE_DIR}/pid/src/pid_controller.cpp
${CMAKE_CURRENT_SOURCE_DIR}/pbvs/src/pbvs_controller.cpp
${CMAKE_CURRENT_SOURCE_DIR}/ibvs/src/ibvs_controller.cpp
)
@ -58,13 +58,3 @@ target_link_libraries(controller PUBLIC
add_library(cmvr_es::algorithms::controller ALIAS controller)
install(TARGETS controller LIBRARY DESTINATION lib)
add_executable(pbvs_controller_test
pbvs/src/pbvs_controller_test.cpp
)
target_link_libraries(pbvs_controller_test PRIVATE
cmvr_es::algorithms::controller
gtest
gtest_main
)

2
cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h
View File

@ -25,7 +25,6 @@ public:
double stop_command_velocity_norm{1e-3};
double stop_measured_velocity_norm{1e-2};
double stop_acceleration{0.5};
double stop_timeout_s{2.0};
};
using ReadStateCallback = std::function<bool(std::vector<double>& q, std::vector<double>& qd)>;
@ -49,7 +48,6 @@ public:
void shutdown();
bool busy() const { return busy_.load(); }
double stopTimeoutS() const { return config_.stop_timeout_s; }
CartesianVelocity getCommandTwistBase() const;
private:

9
cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp
View File

@ -29,9 +29,6 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController:
if (config.stop_acceleration <= 0.0) {
config.stop_acceleration = defaults.stop_acceleration;
}
if (!std::isfinite(config.stop_timeout_s) || config.stop_timeout_s <= 0.0) {
config.stop_timeout_s = defaults.stop_timeout_s;
}
return config;
}
@ -111,12 +108,6 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
if (!worker_ || !worker_->joinable()) {
return Result::success();
}
// A completed speedL command leaves the worker thread joinable but idle.
// Do not turn that idle worker into a new command just because a caller
// requests a stop during a task transition.
if (!busy_.load()) {
return Result::success();
}
requestStop_(acceleration);
return Result::success();
}

453
cmvr-es/algorithms/controllers/pbvs/include/pbvs_controller.h
View File

@ -1,453 +0,0 @@
#pragma once
#ifndef CMVR_PBVS_CONTROLLER_H
#define CMVR_PBVS_CONTROLLER_H
#include <array>
#include <Eigen/Dense>
#include "common/types/arm/arm_types.h"
namespace cmvr {
/**
* @brief 基于相对 3D 位姿的 PBVS 控制器。
*
* 坐标系：
*
* G : Screen Tag 坐标系，同时作为屏幕参考坐标系
* P : TCP / 触控点坐标系
*
* 输入：
*
* ^G T_P_des : TCP 相对于 Screen Tag 的目标位姿
* ^G T_P_cur : TCP 相对于 Screen Tag 的当前位姿
*
* 输出：
*
* ^G V_P =
*
* [ vx ]
* [ vy ]
* [ vz ]
* [ wx ]
* [ wy ]
* [ wz ]
*
* 即 TCP 在 Screen Tag 坐标系 G 下表达的 6D Cartesian Twist。
*
* 位置误差：
*
* e_p = p_des - p_cur
*
* 姿态误差：
*
* R_err = R_des * R_cur^T
*
* e_R = Log(R_err)^vee
*
* 控制律：
*
* v = Kp * e_p
* w = Kr * e_R
*
* 本类只负责视觉伺服 6D Twist 计算，不负责：
*
* - AprilTag 检测
* - RobotArm
* - G -> Base 坐标转换
* - IK
* - speedL 下发
*/
class PbvsController {
public:
enum class ComputeStatus {
OK = 0,
TARGET_NOT_SET,
INVALID_INPUT,
INVALID_DT,
INVALID_TARGET,
INVALID_CURRENT_POSE
};
struct Output {
// 当前位置误差，单位 m
Eigen::Vector3d position_error_G{
Eigen::Vector3d::Zero()
};
// 当前姿态误差 rotation-vector，单位 rad
Eigen::Vector3d rotation_error_G{
Eigen::Vector3d::Zero()
};
// PBVS 计算得到的原始线速度，单位 m/s
Eigen::Vector3d raw_linear_velocity_G{
Eigen::Vector3d::Zero()
};
// PBVS 计算得到的原始角速度，单位 rad/s
Eigen::Vector3d raw_angular_velocity_G{
Eigen::Vector3d::Zero()
};
// 经过限幅 / 加速度 / 滤波后的线速度
Eigen::Vector3d linear_velocity_G{
Eigen::Vector3d::Zero()
};
// 经过限幅 / 加速度 / 滤波后的角速度
Eigen::Vector3d angular_velocity_G{
Eigen::Vector3d::Zero()
};
// 可直接取出的 CartesianVelocity。
//
// 注意：
// 这里仍然是在 G / ScreenTag frame 下表达。
device::CartesianVelocity twist_G{};
bool position_reached{false};
bool orientation_reached{false};
bool reached{false};
bool valid{false};
};
public:
PbvsController();
/**
* @brief 设置完整目标位姿。
*
* @param T_G_P_des TCP(P) 相对于 ScreenTag(G) 的目标位姿。
*/
bool setTargetPose(
const Eigen::Matrix4d& T_G_P_des);
/**
* @brief 使用目标位置 + 目标姿态设置期望位姿。
*/
bool setTargetPose(
const Eigen::Vector3d& position_G,
const Eigen::Matrix3d& rotation_G_P);
/**
* @brief 当前是否已经设置有效目标。
*/
bool hasTarget() const {
return target_valid_;
}
/**
* @brief 获取目标位姿。
*/
const Eigen::Matrix4d& targetPose() const {
return T_G_P_des_;
}
/**
* @brief 根据当前 TCP 位姿计算 PBVS 速度命令。
*
* @param T_G_P_cur 当前 TCP 相对于 Screen Tag 的位姿。
* @param dt 控制周期，单位 s。
* @param output 输出结果。
*
* @return 成功返回 true。
*/
bool compute(
const Eigen::Matrix4d& T_G_P_cur,
double dt,
Output& output);
/**
* @brief 便捷接口，只输出 CartesianVelocity。
*
* 注意输出仍然是 G frame。
*/
bool compute(
const Eigen::Matrix4d& T_G_P_cur,
double dt,
device::CartesianVelocity& twist_G_out);
/**
* @brief 设置位置比例增益。
*
* x/y/z 分别控制。
*/
void setPositionGain(
const Eigen::Vector3d& kp);
/**
* @brief 设置姿态比例增益。
*
* rx/ry/rz 分别控制。
*/
void setRotationGain(
const Eigen::Vector3d& kr);
/**
* @brief 设置六维速度上限。
*
* [vx vy vz wx wy wz]
*
* 前三维 m/s；
* 后三维 rad/s。
*/
void setVelocityLimit6(
const std::array<double, 6>& vmax6);
/**
* @brief 设置六维加速度限制。
*
* 前三维 m/s^2；
* 后三维 rad/s^2。
*
* <= 0 表示对应维度不限制。
*/
void setAccelerationLimit6(
const std::array<double, 6>& amax6);
/**
* @brief 设置六维误差阈值。
*
* [x y z rx ry rz]
*
* 前三维 m；
* 后三维 rad。
*/
void setTolerance6(
const std::array<double, 6>& tolerance6);
/**
* @brief 设置一阶低通 alpha。
*
* alpha = 1：
* 不滤波。
*
* 0 < alpha < 1：
*
* cmd =
* alpha * current
* +
* (1-alpha) * previous
*/
void setTwistFilterAlpha(double alpha);
/**
* @brief 是否启用每个轴。
*
* 默认 6DoF 全部开启。
*
* 例如以后如果不希望控制 yaw：
*
* enabled[5] = false;
*/
void setAxisEnabled(
const std::array<bool, 6>& enabled);
/**
* @brief 清空速度历史，但保留目标。
*/
void resetTwistCommandState();
/**
* @brief 清空整个 PBVS 状态和目标。
*/
void reset();
ComputeStatus lastComputeStatus() const {
return last_compute_status_;
}
static const char* statusToString(
ComputeStatus status);
const Eigen::Vector3d& lastPositionError() const {
return last_position_error_G_;
}
const Eigen::Vector3d& lastRotationError() const {
return last_rotation_error_G_;
}
const Eigen::Vector3d& positionGain() const {
return kp_position_;
}
const Eigen::Vector3d& rotationGain() const {
return kp_rotation_;
}
const std::array<double, 6>& velocityLimit6() const {
return vmax6_;
}
const std::array<double, 6>& accelerationLimit6() const {
return amax6_;
}
const std::array<double, 6>& tolerance6() const {
return tolerance6_;
}
double twistFilterAlpha() const {
return twist_lpf_alpha_;
}
const Eigen::Matrix<double, 6, 1>&
lastTwistCommandG() const {
return last_twist_cmd_G_;
}
bool lastReached() const {
return last_reached_;
}
private:
static bool isFiniteTransform(
const Eigen::Matrix4d& T);
static bool hasValidBottomRow(
const Eigen::Matrix4d& T,
double tolerance = 1e-6);
static bool isValidTransform(
const Eigen::Matrix4d& T);
/**
* @brief 将可能有微小数值误差的旋转矩阵投影到 SO(3)。
*/
static Eigen::Matrix3d projectToSO3(
const Eigen::Matrix3d& R);
/**
* @brief 计算空间旋转误差，在 G frame 表达。
*
* R_err = R_des * R_cur^T
*
* e_R = Log(R_err)^vee
*/
static Eigen::Vector3d rotationError(
const Eigen::Matrix3d& R_des,
const Eigen::Matrix3d& R_cur);
static double clampValue(
double value,
double lower,
double upper);
static device::CartesianVelocity
toCartesianVelocity(
const Eigen::Matrix<double, 6, 1>& twist);
private:
// ---------------- target ----------------
Eigen::Matrix4d T_G_P_des_{
Eigen::Matrix4d::Identity()
};
bool target_valid_{false};
// ---------------- gains ----------------
Eigen::Vector3d kp_position_{
2.0,
2.0,
1.5
};
Eigen::Vector3d kp_rotation_{
1.5,
1.5,
1.5
};
// ---------------- velocity limits ----------------
//
// [vx vy vz wx wy wz]
//
std::array<double, 6> vmax6_{{
0.10,
0.10,
0.05,
0.50,
0.50,
0.50
}};
// ---------------- acceleration limits ----------------
std::array<double, 6> amax6_{{
0.50,
0.50,
0.30,
2.0,
2.0,
2.0
}};
// ---------------- tolerance ----------------
std::array<double, 6> tolerance6_{{
0.0015, // x 1.5 mm
0.0015, // y 1.5 mm
0.0020, // z 2.0 mm
0.05, // rx ~2.9 deg
0.05, // ry
0.05 // rz
}};
// ---------------- axis enable ----------------
std::array<bool, 6> axis_enabled_{{
true,
true,
true,
true,
true,
true
}};
// ---------------- LPF ----------------
double twist_lpf_alpha_{1.0};
// ---------------- command history ----------------
Eigen::Matrix<double, 6, 1>
previous_twist_cmd_G_{
Eigen::Matrix<double, 6, 1>::Zero()
};
bool has_previous_twist_{false};
// ---------------- last output ----------------
Eigen::Vector3d last_position_error_G_{
Eigen::Vector3d::Zero()
};
Eigen::Vector3d last_rotation_error_G_{
Eigen::Vector3d::Zero()
};
Eigen::Matrix<double, 6, 1>
last_twist_cmd_G_{
Eigen::Matrix<double, 6, 1>::Zero()
};
bool last_reached_{false};
ComputeStatus last_compute_status_{
ComputeStatus::TARGET_NOT_SET
};
};
} // namespace cmvr
#endif // CMVR_PBVS_CONTROLLER_H

774
cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller.cpp
View File

@ -1,774 +0,0 @@
#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
#include <algorithm>
#include <cmath>
#include <Eigen/Geometry>
#include <Eigen/SVD>
namespace cmvr {
PbvsController::PbvsController()
{
resetTwistCommandState();
}
bool PbvsController::setTargetPose(
const Eigen::Matrix4d& T_G_P_des)
{
if (!isValidTransform(T_G_P_des)) {
target_valid_ = false;
last_compute_status_ =
ComputeStatus::INVALID_TARGET;
return false;
}
T_G_P_des_ = T_G_P_des;
//
// 视觉位姿可能有微小数值误差。
// 强制把 rotation 投影到 SO(3)。
//
T_G_P_des_.block<3, 3>(0, 0) =
projectToSO3(
T_G_P_des.block<3, 3>(0, 0));
target_valid_ = true;
last_compute_status_ =
ComputeStatus::OK;
return true;
}
bool PbvsController::setTargetPose(
const Eigen::Vector3d& position_G,
const Eigen::Matrix3d& rotation_G_P)
{
if (!position_G.allFinite() ||
!rotation_G_P.allFinite()) {
target_valid_ = false;
last_compute_status_ =
ComputeStatus::INVALID_TARGET;
return false;
}
Eigen::Matrix4d T =
Eigen::Matrix4d::Identity();
T.block<3, 3>(0, 0) =
projectToSO3(rotation_G_P);
T.block<3, 1>(0, 3) =
position_G;
return setTargetPose(T);
}
bool PbvsController::compute(
const Eigen::Matrix4d& T_G_P_cur,
const double dt,
Output& output)
{
output = Output{};
last_reached_ = false;
last_position_error_G_.setZero();
last_rotation_error_G_.setZero();
last_twist_cmd_G_.setZero();
if (!target_valid_) {
last_compute_status_ =
ComputeStatus::TARGET_NOT_SET;
return false;
}
if (!std::isfinite(dt) ||
dt <= 0.0) {
last_compute_status_ =
ComputeStatus::INVALID_DT;
return false;
}
if (!isValidTransform(T_G_P_cur)) {
last_compute_status_ =
ComputeStatus::INVALID_CURRENT_POSE;
return false;
}
// =====================================================
// 1. Current pose
// =====================================================
const Eigen::Vector3d p_cur_G =
T_G_P_cur.block<3, 1>(0, 3);
const Eigen::Matrix3d R_cur_G_P =
projectToSO3(
T_G_P_cur.block<3, 3>(0, 0));
// =====================================================
// 2. Desired pose
// =====================================================
const Eigen::Vector3d p_des_G =
T_G_P_des_.block<3, 1>(0, 3);
const Eigen::Matrix3d R_des_G_P =
T_G_P_des_.block<3, 3>(0, 0);
// =====================================================
// 3. Position error
//
// e_p = p_des - p_cur
//
// expressed in G frame.
// =====================================================
Eigen::Vector3d e_pos_G =
p_des_G -
p_cur_G;
// =====================================================
// 4. Rotation error
//
// R_err =
// R_des * R_cur^T
//
// e_R =
// Log(R_err)^vee
//
// e_R is also expressed in G frame.
// =====================================================
Eigen::Vector3d e_rot_G =
rotationError(
R_des_G_P,
R_cur_G_P);
if (!e_pos_G.allFinite() ||
!e_rot_G.allFinite()) {
last_compute_status_ =
ComputeStatus::INVALID_INPUT;
return false;
}
// =====================================================
// 5. Disabled axes
// =====================================================
for (int i = 0; i < 3; ++i) {
if (!axis_enabled_[i]) {
e_pos_G[i] = 0.0;
}
if (!axis_enabled_[i + 3]) {
e_rot_G[i] = 0.0;
}
}
last_position_error_G_ =
e_pos_G;
last_rotation_error_G_ =
e_rot_G;
output.position_error_G =
e_pos_G;
output.rotation_error_G =
e_rot_G;
// =====================================================
// 6. Reached check
// =====================================================
bool position_reached = true;
bool orientation_reached = true;
for (int i = 0; i < 3; ++i) {
if (axis_enabled_[i] &&
std::abs(e_pos_G[i]) >
tolerance6_[i]) {
position_reached = false;
}
if (axis_enabled_[i + 3] &&
std::abs(e_rot_G[i]) >
tolerance6_[i + 3]) {
orientation_reached = false;
}
}
const bool reached =
position_reached &&
orientation_reached;
output.position_reached =
position_reached;
output.orientation_reached =
orientation_reached;
output.reached =
reached;
last_reached_ =
reached;
// =====================================================
// 7. PBVS P control
//
// v = Kp * e_pos
//
// w = Kr * e_rot
// =====================================================
Eigen::Matrix<double, 6, 1>
twist_raw_G =
Eigen::Matrix<double, 6, 1>::Zero();
twist_raw_G.head<3>() =
kp_position_.cwiseProduct(
e_pos_G);
twist_raw_G.tail<3>() =
kp_rotation_.cwiseProduct(
e_rot_G);
// =====================================================
// 8. Dead zone
//
// 某个轴已经进入误差阈值，则这个轴不再主动运动。
// =====================================================
for (int i = 0; i < 6; ++i) {
if (!axis_enabled_[i]) {
twist_raw_G[i] = 0.0;
continue;
}
const double error_value =
i < 3
? e_pos_G[i]
: e_rot_G[i - 3];
if (std::abs(error_value) <=
tolerance6_[i]) {
twist_raw_G[i] = 0.0;
}
}
output.raw_linear_velocity_G =
twist_raw_G.head<3>();
output.raw_angular_velocity_G =
twist_raw_G.tail<3>();
// =====================================================
// 9. Velocity limits
// =====================================================
Eigen::Matrix<double, 6, 1>
twist_vel_limited =
twist_raw_G;
for (int i = 0; i < 6; ++i) {
const double vmax =
vmax6_[i];
if (!std::isfinite(vmax) ||
vmax <= 0.0) {
twist_vel_limited[i] = 0.0;
continue;
}
twist_vel_limited[i] =
clampValue(
twist_vel_limited[i],
-vmax,
vmax);
}
// =====================================================
// 10. Reached:
//
// 进入完整目标阈值后直接输出 0。
//
// 上层可以随后调用 RobotArm::stopL()。
// =====================================================
if (reached) {
previous_twist_cmd_G_.setZero();
has_previous_twist_ = true;
last_twist_cmd_G_.setZero();
output.linear_velocity_G.setZero();
output.angular_velocity_G.setZero();
output.twist_G =
device::CartesianVelocity{};
output.valid = true;
last_compute_status_ =
ComputeStatus::OK;
return true;
}
// =====================================================
// 11. Acceleration limits
// =====================================================
Eigen::Matrix<double, 6, 1>
twist_acc_limited;
if (!has_previous_twist_) {
previous_twist_cmd_G_.setZero();
has_previous_twist_ = true;
}
twist_acc_limited =
previous_twist_cmd_G_;
for (int i = 0; i < 6; ++i) {
if (!axis_enabled_[i]) {
twist_acc_limited[i] = 0.0;
continue;
}
const double amax =
amax6_[i];
// <=0：不做加速度限制
if (!std::isfinite(amax) ||
amax <= 0.0) {
twist_acc_limited[i] =
twist_vel_limited[i];
continue;
}
const double dv_max =
amax * dt;
const double dv_des =
twist_vel_limited[i]
-
previous_twist_cmd_G_[i];
const double dv =
clampValue(
dv_des,
-dv_max,
dv_max);
twist_acc_limited[i] =
previous_twist_cmd_G_[i]
+
dv;
}
// =====================================================
// 12. First-order LPF
// =====================================================
Eigen::Matrix<double, 6, 1>
twist_filtered =
twist_acc_limited;
const double alpha =
std::clamp(
twist_lpf_alpha_,
0.0,
1.0);
if (alpha > 0.0 &&
alpha < 1.0) {
twist_filtered =
alpha *
twist_acc_limited
+
(1.0 - alpha) *
previous_twist_cmd_G_;
}
// =====================================================
// 13. Make sure disabled axis is zero
// =====================================================
for (int i = 0; i < 6; ++i) {
if (!axis_enabled_[i]) {
twist_filtered[i] = 0.0;
}
}
// =====================================================
// 14. Save state
// =====================================================
previous_twist_cmd_G_ =
twist_filtered;
last_twist_cmd_G_ =
twist_filtered;
// =====================================================
// 15. Output
// =====================================================
output.linear_velocity_G =
twist_filtered.head<3>();
output.angular_velocity_G =
twist_filtered.tail<3>();
output.twist_G =
toCartesianVelocity(
twist_filtered);
output.valid = true;
last_compute_status_ =
ComputeStatus::OK;
return true;
}
bool PbvsController::compute(
const Eigen::Matrix4d& T_G_P_cur,
const double dt,
device::CartesianVelocity& twist_G_out)
{
Output output;
if (!compute(
T_G_P_cur,
dt,
output)) {
twist_G_out =
device::CartesianVelocity{};
return false;
}
twist_G_out =
output.twist_G;
return true;
}
void PbvsController::setPositionGain(
const Eigen::Vector3d& kp)
{
for (int i = 0; i < 3; ++i) {
if (std::isfinite(kp[i]) &&
kp[i] >= 0.0) {
kp_position_[i] =
kp[i];
}
}
}
void PbvsController::setRotationGain(
const Eigen::Vector3d& kr)
{
for (int i = 0; i < 3; ++i) {
if (std::isfinite(kr[i]) &&
kr[i] >= 0.0) {
kp_rotation_[i] =
kr[i];
}
}
}
void PbvsController::setVelocityLimit6(
const std::array<double, 6>& vmax6)
{
for (int i = 0; i < 6; ++i) {
if (std::isfinite(vmax6[i]) &&
vmax6[i] >= 0.0) {
vmax6_[i] =
vmax6[i];
}
}
}
void PbvsController::setAccelerationLimit6(
const std::array<double, 6>& amax6)
{
for (int i = 0; i < 6; ++i) {
if (std::isfinite(amax6[i])) {
amax6_[i] =
amax6[i];
}
}
}
void PbvsController::setTolerance6(
const std::array<double, 6>& tolerance6)
{
for (int i = 0; i < 6; ++i) {
if (std::isfinite(tolerance6[i]) &&
tolerance6[i] >= 0.0) {
tolerance6_[i] =
tolerance6[i];
}
}
}
void PbvsController::setTwistFilterAlpha(
const double alpha)
{
if (!std::isfinite(alpha)) {
return;
}
twist_lpf_alpha_ =
std::clamp(
alpha,
0.0,
1.0);
}
void PbvsController::setAxisEnabled(
const std::array<bool, 6>& enabled)
{
axis_enabled_ =
enabled;
}
void PbvsController::resetTwistCommandState()
{
previous_twist_cmd_G_.setZero();
last_twist_cmd_G_.setZero();
has_previous_twist_ = false;
}
void PbvsController::reset()
{
T_G_P_des_.setIdentity();
target_valid_ = false;
last_position_error_G_.setZero();
last_rotation_error_G_.setZero();
last_reached_ = false;
resetTwistCommandState();
last_compute_status_ =
ComputeStatus::TARGET_NOT_SET;
}
const char* PbvsController::statusToString(
const ComputeStatus status)
{
switch (status) {
case ComputeStatus::OK:
return "ok";
case ComputeStatus::TARGET_NOT_SET:
return "target_not_set";
case ComputeStatus::INVALID_INPUT:
return "invalid_input";
case ComputeStatus::INVALID_DT:
return "invalid_dt";
case ComputeStatus::INVALID_TARGET:
return "invalid_target";
case ComputeStatus::INVALID_CURRENT_POSE:
return "invalid_current_pose";
default:
return "unknown";
}
}
bool PbvsController::isFiniteTransform(
const Eigen::Matrix4d& T)
{
return T.allFinite();
}
bool PbvsController::hasValidBottomRow(
const Eigen::Matrix4d& T,
const double tolerance)
{
return
std::abs(T(3, 0)) <= tolerance &&
std::abs(T(3, 1)) <= tolerance &&
std::abs(T(3, 2)) <= tolerance &&
std::abs(T(3, 3) - 1.0) <= tolerance;
}
bool PbvsController::isValidTransform(
const Eigen::Matrix4d& T)
{
if (!isFiniteTransform(T)) {
return false;
}
if (!hasValidBottomRow(T)) {
return false;
}
return true;
}
Eigen::Matrix3d PbvsController::projectToSO3(
const Eigen::Matrix3d& R)
{
if (!R.allFinite()) {
return Eigen::Matrix3d::Identity();
}
Eigen::JacobiSVD<Eigen::Matrix3d> svd(
R,
Eigen::ComputeFullU |
Eigen::ComputeFullV);
Eigen::Matrix3d U =
svd.matrixU();
const Eigen::Matrix3d V =
svd.matrixV();
Eigen::Matrix3d R_projected =
U * V.transpose();
//
// 确保 det = +1，而不是 reflection。
//
if (R_projected.determinant() < 0.0) {
U.col(2) *= -1.0;
R_projected =
U * V.transpose();
}
return R_projected;
}
Eigen::Vector3d PbvsController::rotationError(
const Eigen::Matrix3d& R_des,
const Eigen::Matrix3d& R_cur)
{
const Eigen::Matrix3d R_d =
projectToSO3(R_des);
const Eigen::Matrix3d R_c =
projectToSO3(R_cur);
//
// 空间旋转误差，在 G frame 表达。
//
// 当前为 I，目标绕 +Z 旋转 theta：
//
// R_err = R_des
//
// 得到 +theta Z，
// 因此角速度方向正确。
//
const Eigen::Matrix3d R_err =
R_d *
R_c.transpose();
Eigen::AngleAxisd aa(
R_err);
const double angle =
aa.angle();
if (!std::isfinite(angle) ||
std::abs(angle) <= 1e-12) {
return Eigen::Vector3d::Zero();
}
const Eigen::Vector3d axis =
aa.axis();
if (!axis.allFinite()) {
return Eigen::Vector3d::Zero();
}
return axis * angle;
}
double PbvsController::clampValue(
const double value,
const double lower,
const double upper)
{
return std::max(
lower,
std::min(
value,
upper));
}
device::CartesianVelocity
PbvsController::toCartesianVelocity(
const Eigen::Matrix<double, 6, 1>& twist)
{
device::CartesianVelocity velocity;
velocity.vx =
twist[0];
velocity.vy =
twist[1];
velocity.vz =
twist[2];
velocity.wx =
twist[3];
velocity.wy =
twist[4];
velocity.wz =
twist[5];
return velocity;
}
} // namespace cmvr

61
cmvr-es/algorithms/controllers/pbvs/src/pbvs_controller_test.cpp
View File

@ -1,61 +0,0 @@
#include "gtest/gtest.h"
#include <array>
#include <Eigen/Geometry>
#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
namespace {
TEST(PbvsControllerTest, AppliesConfigurationSetters) {
cmvr::PbvsController controller;
const Eigen::Vector3d position_gain(1.0, 2.0, 3.0);
const Eigen::Vector3d rotation_gain(4.0, 5.0, 6.0);
const std::array<double, 6> vmax{{0.1, 0.2, 0.3, 0.4, 0.5, 0.6}};
const std::array<double, 6> amax{{1.0, 2.0, 3.0, 4.0, 5.0, 6.0}};
const std::array<double, 6> tolerance{{0.001, 0.002, 0.003, 0.01, 0.02, 0.03}};
controller.setPositionGain(position_gain);
controller.setRotationGain(rotation_gain);
controller.setVelocityLimit6(vmax);
controller.setAccelerationLimit6(amax);
controller.setTolerance6(tolerance);
controller.setTwistFilterAlpha(0.75);
EXPECT_TRUE(controller.positionGain().isApprox(position_gain));
EXPECT_TRUE(controller.rotationGain().isApprox(rotation_gain));
EXPECT_EQ(controller.velocityLimit6(), vmax);
EXPECT_EQ(controller.accelerationLimit6(), amax);
EXPECT_EQ(controller.tolerance6(), tolerance);
EXPECT_DOUBLE_EQ(controller.twistFilterAlpha(), 0.75);
}
TEST(PbvsControllerTest, CommandsTowardPositionAndRotationError) {
cmvr::PbvsController controller;
controller.setPositionGain(Eigen::Vector3d::Ones());
controller.setRotationGain(Eigen::Vector3d::Ones());
controller.setVelocityLimit6({{1.0, 1.0, 1.0, 1.0, 1.0, 1.0}});
controller.setAccelerationLimit6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
controller.setTolerance6({{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}});
controller.setTwistFilterAlpha(1.0);
Eigen::Matrix4d target = Eigen::Matrix4d::Identity();
target.block<3, 3>(0, 0) =
Eigen::AngleAxisd(0.25, Eigen::Vector3d::UnitZ()).toRotationMatrix();
target.block<3, 1>(0, 3) = Eigen::Vector3d(0.1, -0.2, 0.3);
ASSERT_TRUE(controller.setTargetPose(target));
cmvr::PbvsController::Output output;
ASSERT_TRUE(controller.compute(Eigen::Matrix4d::Identity(), 0.01, output));
ASSERT_TRUE(output.valid);
EXPECT_GT(output.linear_velocity_G.x(), 0.0);
EXPECT_LT(output.linear_velocity_G.y(), 0.0);
EXPECT_GT(output.linear_velocity_G.z(), 0.0);
EXPECT_NEAR(output.angular_velocity_G.x(), 0.0, 1e-12);
EXPECT_NEAR(output.angular_velocity_G.y(), 0.0, 1e-12);
EXPECT_GT(output.angular_velocity_G.z(), 0.0);
}
} // namespace

13
cmvr-es/algorithms/kinematics/ik_solver/CMakeLists.txt
View File

@ -25,15 +25,4 @@ target_link_libraries(ik_solver PUBLIC
add_library(cmvr_es::ik_solver ALIAS ik_solver)
install(TARGETS ik_solver LIBRARY DESTINATION lib)
add_executable(pinocchio_qp_ik_solver_test
pinocchio/src/pinocchio_qp_ik_solver_test.cpp
)
target_link_libraries(pinocchio_qp_ik_solver_test PRIVATE
cmvr_es::ik_solver
cmvr_es::proto
gtest
gtest_main
)
install(TARGETS ik_solver LIBRARY DESTINATION lib)

5
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h
View File

@ -45,11 +45,6 @@ public:
std::vector<double>& qdot_out,
double qdot_abs_max = std::numeric_limits<double>::infinity()) const override;
// Projects a secondary joint velocity into the Cartesian task null space.
static Eigen::VectorXd projectJointLimitAvoidanceToNullspace(
const Eigen::MatrixXd& jacobian_base,
const Eigen::VectorXd& qdot_avoid);
/// 如你有更严格的速度 / 加速度限位，可以覆盖默认值
void setVelocityLimits(const Eigen::VectorXd &qd_max);
void setAccelerationLimits(const Eigen::VectorXd &qdd_max);

20
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_ik_base.cpp
View File

@ -164,9 +164,7 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
const double margin_ratio = positiveOr(config.margin_ratio(), 0.08);
const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02);
// Apply one common scale factor instead of changing individual joints.
// Per-joint scaling changes J*qdot and can disturb the Cartesian task.
double scale = 1.0;
Eigen::VectorXd limited = qdot;
for (Eigen::Index i = 0; i < q_chain.size(); ++i) {
const double lower = joint_pos_lower_limits_[i];
const double upper = joint_pos_upper_limits_[i];
@ -176,15 +174,21 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
const double span = upper - lower;
const double margin = std::max(min_margin_rad, margin_ratio * span);
if (qdot[i] < 0.0 && q_chain[i] < lower + margin) {
if (limited[i] < 0.0 && q_chain[i] < lower + margin) {
const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0);
scale = std::min(scale, ratio);
} else if (qdot[i] > 0.0 && q_chain[i] > upper - margin) {
limited[i] *= ratio;
if (q_chain[i] <= lower) {
limited[i] = std::max(0.0, limited[i]);
}
} else if (limited[i] > 0.0 && q_chain[i] > upper - margin) {
const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0);
scale = std::min(scale, ratio);
limited[i] *= ratio;
if (q_chain[i] >= upper) {
limited[i] = std::min(0.0, limited[i]);
}
}
}
return scale * qdot;
return limited;
}
void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) {

135
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver.cpp
View File

@ -13,66 +13,11 @@
#include <pinocchio/spatial/explog.hpp>
#include <algorithm> // std::clamp, std::max, std::min
#include <atomic>
#include <cmath> // std::sqrt
#include <cstdint>
#include <limits>
#include <sstream>
#include <unordered_map>
#include <Eigen/SVD>
namespace cmvr {
namespace {
Eigen::MatrixXd moorePenrosePseudoInverse(const Eigen::MatrixXd& matrix)
{
if (matrix.rows() == 0 || matrix.cols() == 0 || !matrix.allFinite()) {
return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
}
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
if (svd.info() != Eigen::Success) {
return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
}
const Eigen::VectorXd singular_values = svd.singularValues();
const double max_singular = singular_values.size() > 0
? singular_values.maxCoeff()
: 0.0;
const double tolerance =
std::numeric_limits<double>::epsilon() *
static_cast<double>(std::max(matrix.rows(), matrix.cols())) *
std::max(1.0, max_singular);
Eigen::VectorXd inverse_singular = singular_values;
for (Eigen::Index i = 0; i < inverse_singular.size(); ++i) {
inverse_singular[i] = singular_values[i] > tolerance
? 1.0 / singular_values[i]
: 0.0;
}
const Eigen::Index rank_dimension = singular_values.size();
return svd.matrixV().leftCols(rank_dimension) *
inverse_singular.asDiagonal() *
svd.matrixU().leftCols(rank_dimension).transpose();
}
std::string vectorToString(const Eigen::VectorXd& value)
{
std::ostringstream stream;
stream << '[';
for (Eigen::Index i = 0; i < value.size(); ++i) {
if (i > 0) {
stream << ' ';
}
stream << value[i];
}
stream << ']';
return stream.str();
}
} // namespace
using Eigen::Matrix4d;
using Eigen::VectorXd;
using Eigen::MatrixXd;
@ -263,24 +208,6 @@ namespace cmvr {
}
}
Eigen::VectorXd PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace(
const Eigen::MatrixXd& jacobian_base,
const Eigen::VectorXd& qdot_avoid)
{
if (jacobian_base.cols() != qdot_avoid.size() ||
jacobian_base.rows() == 0 || jacobian_base.cols() == 0 ||
!jacobian_base.allFinite() || !qdot_avoid.allFinite()) {
return Eigen::VectorXd::Zero(qdot_avoid.size());
}
const Eigen::MatrixXd jacobian_pinv =
moorePenrosePseudoInverse(jacobian_base);
const Eigen::MatrixXd nullspace =
Eigen::MatrixXd::Identity(jacobian_base.cols(), jacobian_base.cols()) -
jacobian_pinv * jacobian_base;
return nullspace * qdot_avoid;
}
bool PinocchioQpIKSolver::ik(const Matrix4d &target_pose,
std::vector<double> &joints_angle,
bool is_tcp) {
@ -462,7 +389,7 @@ namespace cmvr {
const int dof = chain_v_dof_;
const auto& avoidance = jointLimitPolicy().avoidance();
const bool use_joint_limit_avoidance =
!jointLimitsDisabled() && avoidance.enable() && avoidance.gain() > 0.0;
!jointLimitsDisabled() && avoidance.enable() && avoidance.weight() > 0.0;
const int avoidance_rows = use_joint_limit_avoidance ? dof : 0;
MatrixXd cost(6 + dof + avoidance_rows, dof);
@ -479,7 +406,7 @@ namespace cmvr {
VectorXd upper(dof);
const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof);
if (use_joint_limit_avoidance) {
const VectorXd qdot_avoid_raw =
const VectorXd qdot_avoid =
cmvr::kinematics::computeJointLimitAvoidanceVelocity(
q_chain,
joint_pos_lower_limits_,
@ -488,32 +415,10 @@ namespace cmvr {
positiveOr(avoidance.gain(), 0.2),
positiveOr(avoidance.margin_ratio(), 0.15),
positiveOr(avoidance.max_push(), 0.25));
const Eigen::MatrixXd jacobian_pinv =
moorePenrosePseudoInverse(jacobian_base);
const bool jacobian_pinv_valid =
jacobian_pinv.rows() == dof && jacobian_pinv.cols() == 6 &&
jacobian_pinv.allFinite();
Eigen::MatrixXd nullspace = MatrixXd::Zero(dof, dof);
if (jacobian_pinv_valid) {
nullspace = MatrixXd::Identity(dof, dof) -
jacobian_pinv * jacobian_base;
}
const VectorXd qdot_avoid_null = nullspace * qdot_avoid_raw;
const double sqrt_weight = std::sqrt(positiveOr(avoidance.weight(), 0.05));
cost.middleRows(6 + dof, dof) =
nullspace;
target.segment(6 + dof, dof) = qdot_avoid_null;
static std::atomic<std::uint64_t> avoidance_debug_counter{0};
const auto debug_index =
avoidance_debug_counter.fetch_add(1, std::memory_order_relaxed);
if (debug_index % 1000 == 0) {
CMVR_LOG(DEBUG)
<< "[PinocchioQpIKSolver][JOINT_LIMIT_AVOIDANCE]"
<< " qdot_avoid_raw=" << vectorToString(qdot_avoid_raw)
<< " qdot_avoid_null=" << vectorToString(qdot_avoid_null)
<< " norm(J*qdot_avoid_null)="
<< (jacobian_base * qdot_avoid_null).norm();
}
sqrt_weight * MatrixXd::Identity(dof, dof);
target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid;
}
for (int i = 0; i < dof; ++i) {
double limit = std::numeric_limits<double>::infinity();
@ -547,35 +452,6 @@ namespace cmvr {
}
}
}
const auto& soft_limit = jointLimitPolicy().soft_limit();
if (!jointLimitsDisabled() && soft_limit.enable() &&
joint_pos_lower_limits_.size() == dof &&
joint_pos_upper_limits_.size() == dof) {
const double q_min = joint_pos_lower_limits_[i];
const double q_max = joint_pos_upper_limits_[i];
if (std::isfinite(q_min) && std::isfinite(q_max) && q_max > q_min) {
const double span = q_max - q_min;
const double margin = std::max(
positiveOr(soft_limit.min_margin_rad(), 0.02),
positiveOr(soft_limit.margin_ratio(), 0.08) * span);
if (q_chain[i] < q_min + margin) {
const double ratio = std::clamp(
(q_chain[i] - q_min) / margin, 0.0, 1.0);
lower[i] = std::max(lower[i], -limit * ratio);
if (q_chain[i] <= q_min) {
lower[i] = std::max(0.0, lower[i]);
}
} else if (q_chain[i] > q_max - margin) {
const double ratio = std::clamp(
(q_max - q_chain[i]) / margin, 0.0, 1.0);
upper[i] = std::min(upper[i], limit * ratio);
if (q_chain[i] >= q_max) {
upper[i] = std::min(0.0, upper[i]);
}
}
}
}
}
QPSolver solver;
@ -599,6 +475,7 @@ namespace cmvr {
if (qdot.size() != dof) {
return false;
}
qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
qdot_out.assign(qdot.data(), qdot.data() + qdot.size());
return true;
}

104
cmvr-es/algorithms/kinematics/ik_solver/pinocchio/src/pinocchio_qp_ik_solver_test.cpp
View File

@ -1,104 +0,0 @@
#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_qp_ik_solver.h"
#include <filesystem>
#include <vector>
#include <gtest/gtest.h>
#include "common/io/proto_file_io.h"
#include "cmvr/config/arm_config/arm_config.pb.h"
namespace cmvr {
namespace {
std::filesystem::path findProjectRoot()
{
std::filesystem::path current = std::filesystem::current_path();
while (!current.empty()) {
if (std::filesystem::exists(
current / "model/xiaoyan_description/dual_arm.urdf")) {
return current;
}
const auto parent = current.parent_path();
if (parent == current) {
break;
}
current = parent;
}
return {};
}
config::PinocchioQpIKConfig loadQpConfig(const std::filesystem::path& root)
{
config::ArmRootConfig root_config;
const auto config_path =
root / "cmvr-es/config/devices/arm/arm_mujoco_qp.pb.txt";
EXPECT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(config_path.string(), &root_config));
EXPECT_GT(root_config.arm().robot_arms_size(), 0);
auto solver_config =
root_config.arm().robot_arms(0).kinematics().pinocchio_qp_ik_solver();
solver_config.set_urdf_path(
(root / "model/xiaoyan_description/dual_arm.urdf").string());
return solver_config;
}
TEST(PinocchioQpIKSolverTest, JointLimitAvoidanceIsInCartesianNullspace)
{
Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7);
jacobian.leftCols(6).setIdentity();
jacobian.col(6) << 0.3, -0.2, 0.4, -0.1, 0.25, 0.15;
Eigen::VectorXd qdot_avoid(7);
qdot_avoid << 0.4, -0.3, 0.2, 0.1, -0.5, 0.6, -0.7;
const Eigen::VectorXd qdot_null =
PinocchioQpIKSolver::projectJointLimitAvoidanceToNullspace(
jacobian, qdot_avoid);
ASSERT_EQ(qdot_null.size(), 7);
EXPECT_LT((jacobian * qdot_null).norm(), 1e-12);
EXPECT_GT(qdot_null.norm(), 0.0);
}
TEST(PinocchioQpIKSolverTest, AvoidanceDoesNotDisturbReachableCartesianTwist)
{
const auto root = findProjectRoot();
ASSERT_FALSE(root.empty());
auto disabled_config = loadQpConfig(root);
disabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(false);
auto enabled_config = disabled_config;
enabled_config.mutable_joint_limit_policy()->mutable_avoidance()->set_enable(true);
PinocchioQpIKSolver solver_disabled(disabled_config);
PinocchioQpIKSolver solver_enabled(enabled_config);
ASSERT_TRUE(solver_disabled.init());
ASSERT_TRUE(solver_enabled.init());
Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, 7);
jacobian.leftCols(6).setIdentity();
Eigen::Matrix<double, 6, 1> target_twist;
target_twist << 0.15, -0.10, 0.08, 0.05, -0.04, 0.03;
// The last joint is inside its configured soft-limit margin, while the
// first six columns fully span the Cartesian task.
const std::vector<double> q_chain = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 1.56};
std::vector<double> qdot_disabled;
std::vector<double> qdot_enabled;
ASSERT_TRUE(solver_disabled.solveVelocityBase(
jacobian, target_twist, q_chain, qdot_disabled, 10.0));
ASSERT_TRUE(solver_enabled.solveVelocityBase(
jacobian, target_twist, q_chain, qdot_enabled, 10.0));
const Eigen::Map<const Eigen::VectorXd> qdot_disabled_eigen(
qdot_disabled.data(), static_cast<Eigen::Index>(qdot_disabled.size()));
const Eigen::Map<const Eigen::VectorXd> qdot_enabled_eigen(
qdot_enabled.data(), static_cast<Eigen::Index>(qdot_enabled.size()));
const Eigen::VectorXd achieved_disabled = jacobian * qdot_disabled_eigen;
const Eigen::VectorXd achieved_enabled = jacobian * qdot_enabled_eigen;
EXPECT_LT((achieved_enabled - achieved_disabled).norm(), 1e-6);
EXPECT_LT((achieved_enabled - target_twist).norm(), 5e-5);
}
} // namespace
} // namespace cmvr

1
cmvr-es/algorithms/perception/CMakeLists.txt
View File

@ -4,7 +4,6 @@ find_package(OpenCV REQUIRED)
add_library(perception SHARED
apriltag/src/tag_relative_target_3d.cpp
apriltag/src/apriltag_perception.cpp
apriltag/src/tag_relative_tcp_pose.cpp
)
target_include_directories(perception PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

4
cmvr-es/algorithms/perception/apriltag/include/apriltag_perception.h
View File

@ -84,10 +84,6 @@ public:
// Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
// Semantic alias used by visualization and downstream consumers:
// `T_C_Tag` maps points in this tag frame into camera frame C.
const Eigen::Matrix4d& T_C_Tag() const { return T_c_t; }
};
struct FrameCache {

289
cmvr-es/algorithms/perception/apriltag/include/tag_relative_tcp_pose.h
View File

@ -1,289 +0,0 @@
#pragma once
#ifndef CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H
#define CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H
#include <memory>
#include <Eigen/Dense>
#include "algorithms/perception/apriltag/include/apriltag_perception.h"
namespace cmvr::perception {
/**
* @brief 根据同一台相机同时观测到的 Screen Tag 和 Hand Tag，
* 计算 TCP 相对于 Screen Tag 的位姿。
*
* 坐标系：
*
* C : 固定外部相机坐标系
* G : Screen Tag 坐标系，同时作为屏幕参考坐标系
* H : Hand Tag 坐标系
* P : TCP / 触控点坐标系
*
* 已知：
*
* T_C_G : Screen Tag -> Camera
* T_C_H : Hand Tag -> Camera
* T_H_P : TCP -> Hand Tag
*
* 其中 T_H_P 是外部传入的一次标定结果。
*
* 计算：
*
* T_G_H = inverse(T_C_G) * T_C_H
*
* T_G_P = T_G_H * T_H_P
*
* 即：
*
* T_G_P = inverse(T_C_G) * T_C_H * T_H_P
*
* 最终：
*
* p_G_P = T_G_P.block<3, 1>(0, 3)
*
* 得到 TCP 原点在 Screen Tag 坐标系下的位置。
*
* 注意：
* - 本类不主动抓相机图像；
* - 本类不主动调用 AprilTagPerception::update()；
* - 上层应保证当前 perception 缓存来自同一帧；
* - Screen Tag 和 Hand Tag 必须同时在当前帧可见。
*/
class TagRelativeTcpPose {
public:
enum class Status {
OK = 0,
NO_PERCEPTION,
INVALID_SCREEN_TAG_ID,
INVALID_HAND_TAG_ID,
SAME_TAG_ID,
SCREEN_TAG_NOT_FOUND,
HAND_TAG_NOT_FOUND,
INVALID_T_C_G,
INVALID_T_C_H,
INVALID_T_H_P,
INVALID_T_G_H,
INVALID_T_G_P,
INVALID_TCP_POSITION
};
public:
explicit TagRelativeTcpPose(
const std::shared_ptr<AprilTagPerception>& perception = nullptr);
/**
* @brief 设置 AprilTag 感知前端。
*
* 本类只读取感知缓存，不主动 update。
*/
void setPerception(
const std::shared_ptr<AprilTagPerception>& perception);
const std::shared_ptr<AprilTagPerception>& perception() const {
return perception_;
}
/**
* @brief 设置 Screen Tag ID。
*/
void setScreenTagId(int id);
/**
* @brief 设置 Hand Tag ID。
*/
void setHandTagId(int id);
int screenTagId() const {
return screen_tag_id_;
}
int handTagId() const {
return hand_tag_id_;
}
/**
* @brief 使用当前 AprilTagPerception 缓存计算 TCP 相对 Screen Tag 的位姿。
*
* 核心公式：
*
* T_G_P =
* inverse(T_C_G)
* * T_C_H
* * T_H_P
*
* @param T_H_P
* TCP(P) 相对于 Hand Tag(H) 的固定齐次变换。
*
* 坐标变换语义：
*
* p_H = T_H_P * p_P
*
* 即：
*
* ^H T_P
*
* @return 成功返回 true。
*/
bool update(
const Eigen::Matrix4d& T_H_P);
/**
* @brief 当前结果是否有效。
*/
bool valid() const {
return valid_;
}
/**
* @brief 最近一次 update() 的状态。
*/
Status lastStatus() const {
return last_status_;
}
static const char* statusToString(Status status);
/**
* @brief 当前 Screen Tag 在 Camera 中的位姿。
*
* ^C T_G
*/
const Eigen::Matrix4d& T_C_G() const {
return T_C_G_;
}
/**
* @brief 当前 Hand Tag 在 Camera 中的位姿。
*
* ^C T_H
*/
const Eigen::Matrix4d& T_C_H() const {
return T_C_H_;
}
/**
* @brief Hand Tag 相对于 Screen Tag 的位姿。
*
* ^G T_H
*/
const Eigen::Matrix4d& T_G_H() const {
return T_G_H_;
}
/**
* @brief TCP 相对于 Screen Tag 的完整 6DoF 位姿。
*
* ^G T_P
*/
const Eigen::Matrix4d& T_G_P() const {
return T_G_P_;
}
/**
* @brief TCP 原点在 Screen Tag 坐标系中的位置。
*
* P_P^G =
*
* [ x_P ]
* [ y_P ]
* [ z_P ]
*/
const Eigen::Vector3d& tcpPositionInScreenTag() const {
return p_G_P_;
}
/**
* @brief TCP 相对于 Screen Tag 的旋转矩阵。
*
* R_G_P
*/
Eigen::Matrix3d tcpRotationInScreenTag() const {
return T_G_P_.block<3, 3>(0, 0);
}
/**
* @brief 清空当前结果。
*/
void clear();
private:
/**
* @brief 检查 4x4 矩阵元素是否全部有限。
*/
static bool isFiniteTransform(
const Eigen::Matrix4d& T);
/**
* @brief 基础检查齐次矩阵最后一行。
*/
static bool hasValidHomogeneousBottomRow(
const Eigen::Matrix4d& T,
double tolerance = 1e-6);
/**
* @brief 判断一个矩阵是否可以作为基本齐次变换使用。
*
* 当前只检查：
* - 所有元素 finite
* - 最后一行约等于 [0 0 0 1]
*
* 暂时不强制检查 rotation orthonormal，
* 避免视觉估计中的微小数值误差导致误判。
*/
static bool isValidTransform(
const Eigen::Matrix4d& T);
private:
std::shared_ptr<AprilTagPerception> perception_{nullptr};
int screen_tag_id_{-1};
int hand_tag_id_{-1};
// 当前外部相机观测
Eigen::Matrix4d T_C_G_{
Eigen::Matrix4d::Identity()
};
Eigen::Matrix4d T_C_H_{
Eigen::Matrix4d::Identity()
};
// 相对变换
Eigen::Matrix4d T_G_H_{
Eigen::Matrix4d::Identity()
};
Eigen::Matrix4d T_G_P_{
Eigen::Matrix4d::Identity()
};
// TCP 原点在 Screen Tag 坐标系的位置
Eigen::Vector3d p_G_P_{
Eigen::Vector3d::Zero()
};
bool valid_{false};
Status last_status_{
Status::NO_PERCEPTION
};
};
} // namespace cmvr::perception
#endif // CMVR_PERCEPTION_TAG_RELATIVE_TCP_POSE_H

315
cmvr-es/algorithms/perception/apriltag/src/tag_relative_tcp_pose.cpp
View File

@ -1,315 +0,0 @@
#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h"
#include <cmath>
namespace cmvr::perception {
TagRelativeTcpPose::TagRelativeTcpPose(
const std::shared_ptr<AprilTagPerception>& perception)
: perception_(perception)
{
}
void TagRelativeTcpPose::setPerception(
const std::shared_ptr<AprilTagPerception>& perception)
{
perception_ = perception;
clear();
if (!perception_) {
last_status_ = Status::NO_PERCEPTION;
}
}
void TagRelativeTcpPose::setScreenTagId(
const int id)
{
screen_tag_id_ = id;
valid_ = false;
}
void TagRelativeTcpPose::setHandTagId(
const int id)
{
hand_tag_id_ = id;
valid_ = false;
}
bool TagRelativeTcpPose::update(
const Eigen::Matrix4d& T_H_P)
{
valid_ = false;
/*
* 1. 检查 perception
*/
if (!perception_) {
last_status_ = Status::NO_PERCEPTION;
return false;
}
/*
* 2. 检查 Tag ID
*/
if (screen_tag_id_ < 0) {
last_status_ = Status::INVALID_SCREEN_TAG_ID;
return false;
}
if (hand_tag_id_ < 0) {
last_status_ = Status::INVALID_HAND_TAG_ID;
return false;
}
if (screen_tag_id_ == hand_tag_id_) {
last_status_ = Status::SAME_TAG_ID;
return false;
}
/*
* 3. 检查外部传入的：
*
* ^H T_P
*
* Hand Tag -> TCP 固定标定矩阵。
*/
if (!isValidTransform(T_H_P)) {
last_status_ = Status::INVALID_T_H_P;
return false;
}
/*
* 4. 从 AprilTagPerception 当前缓存取 Screen Tag。
*
* AprilTagPerception 已经通过 ViSP / PnP 得到：
*
* ^C T_G
*/
const auto* screen_tag =
perception_->findTag(screen_tag_id_);
if (!screen_tag) {
last_status_ =
Status::SCREEN_TAG_NOT_FOUND;
return false;
}
/*
* 5. 取 Hand Tag：
*
* ^C T_H
*/
const auto* hand_tag =
perception_->findTag(hand_tag_id_);
if (!hand_tag) {
last_status_ =
Status::HAND_TAG_NOT_FOUND;
return false;
}
/*
* 6. 保存当前帧两个原始视觉变换。
*
* AprilTagPerception::Tag::T_c_t
*
* 定义是：
*
* Tag -> Camera
*
* 因此：
*
* screen tag:
*
* ^C T_G
*
* hand tag:
*
* ^C T_H
*/
T_C_G_ = screen_tag->T_c_t;
T_C_H_ = hand_tag->T_c_t;
if (!isValidTransform(T_C_G_)) {
last_status_ = Status::INVALID_T_C_G;
return false;
}
if (!isValidTransform(T_C_H_)) {
last_status_ = Status::INVALID_T_C_H;
return false;
}
/*
* 7. 求 Hand Tag 相对于 Screen Tag 的位姿。
*
* 已知：
*
* ^C T_G
* ^C T_H
*
* 因此：
*
* ^G T_H
*
* = (^C T_G)^-1 * ^C T_H
*/
T_G_H_ =
T_C_G_.inverse() *
T_C_H_;
if (!isValidTransform(T_G_H_)) {
last_status_ = Status::INVALID_T_G_H;
return false;
}
/*
* 8. 求 TCP 相对于 Screen Tag 的位姿。
*
* 已知：
*
* ^G T_H
* ^H T_P
*
* 因此：
*
* ^G T_P
*
* = ^G T_H * ^H T_P
*
* = (^C T_G)^-1
* * ^C T_H
* * ^H T_P
*/
T_G_P_ =
T_G_H_ *
T_H_P;
if (!isValidTransform(T_G_P_)) {
last_status_ = Status::INVALID_T_G_P;
return false;
}
/*
* 9. 提取 TCP 原点在 Screen Tag
* 坐标系 G 下的位置。
*
* P_P^G =
*
* [ x_P ]
* [ y_P ]
* [ z_P ]
*
*/
p_G_P_ =
T_G_P_.block<3, 1>(0, 3);
if (!p_G_P_.allFinite()) {
last_status_ =
Status::INVALID_TCP_POSITION;
return false;
}
/*
* 10. 当前帧结果有效。
*/
valid_ = true;
last_status_ = Status::OK;
return true;
}
void TagRelativeTcpPose::clear()
{
T_C_G_.setIdentity();
T_C_H_.setIdentity();
T_G_H_.setIdentity();
T_G_P_.setIdentity();
p_G_P_.setZero();
valid_ = false;
}
const char* TagRelativeTcpPose::statusToString(
const Status status)
{
switch (status) {
case Status::OK:
return "ok";
case Status::NO_PERCEPTION:
return "no_perception";
case Status::INVALID_SCREEN_TAG_ID:
return "invalid_screen_tag_id";
case Status::INVALID_HAND_TAG_ID:
return "invalid_hand_tag_id";
case Status::SAME_TAG_ID:
return "same_tag_id";
case Status::SCREEN_TAG_NOT_FOUND:
return "screen_tag_not_found";
case Status::HAND_TAG_NOT_FOUND:
return "hand_tag_not_found";
case Status::INVALID_T_C_G:
return "invalid_T_C_G";
case Status::INVALID_T_C_H:
return "invalid_T_C_H";
case Status::INVALID_T_H_P:
return "invalid_T_H_P";
case Status::INVALID_T_G_H:
return "invalid_T_G_H";
case Status::INVALID_T_G_P:
return "invalid_T_G_P";
case Status::INVALID_TCP_POSITION:
return "invalid_tcp_position";
default:
return "unknown";
}
}
bool TagRelativeTcpPose::isFiniteTransform(
const Eigen::Matrix4d& T)
{
return T.allFinite();
}
bool TagRelativeTcpPose::hasValidHomogeneousBottomRow(
const Eigen::Matrix4d& T,
const double tolerance)
{
return
std::abs(T(3, 0)) <= tolerance &&
std::abs(T(3, 1)) <= tolerance &&
std::abs(T(3, 2)) <= tolerance &&
std::abs(T(3, 3) - 1.0) <= tolerance;
}
bool TagRelativeTcpPose::isValidTransform(
const Eigen::Matrix4d& T)
{
if (!isFiniteTransform(T)) {
return false;
}
if (!hasValidHomogeneousBottomRow(T)) {
return false;
}
return true;
}
} // namespace cmvr::perception

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

@ -27,26 +27,6 @@ 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)
@ -122,10 +102,6 @@ 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();

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

@ -51,6 +51,7 @@ arm {
gain: 0.2
margin_ratio: 0.15
max_push: 0.25
weight: 0.05
}
}
}
@ -58,14 +59,6 @@ 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
@ -151,8 +144,6 @@ arm {
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 10
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}

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

@ -50,6 +50,7 @@ arm {
gain: 0.2
margin_ratio: 0.15
max_push: 0.25
weight: 2.0
}
}
}
@ -57,14 +58,6 @@ 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
@ -150,8 +143,6 @@ arm {
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 2.0
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}

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

@ -51,6 +51,7 @@ arm {
gain: 0.2
margin_ratio: 0.15
max_push: 0.25
weight: 2.0
}
}
}
@ -58,14 +59,6 @@ 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
@ -151,8 +144,6 @@ arm {
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 10
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}

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

@ -52,6 +52,7 @@ arm {
gain: 0.2
margin_ratio: 0.01
max_push: 0.02
weight: 0.05
}
}
}
@ -59,14 +60,6 @@ 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
@ -152,8 +145,6 @@ arm {
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 5
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}

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

@ -52,6 +52,7 @@ arm {
gain: 0.2
margin_ratio: 0.01
max_push: 0.02
weight: 0.05
}
}
}
@ -59,14 +60,6 @@ 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
@ -152,8 +145,6 @@ arm {
stop_command_velocity_norm: 1e-3
stop_measured_velocity_norm: 1e-2
stop_acceleration: 0.5
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
stop_timeout_s: 2.0
}
}
}

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

@ -92,7 +92,7 @@ camera {
}
consume_new_frame_only: false
viewer_pip {
enable: false
enable: true
left: -10
bottom: 10
width: 320
@ -101,36 +101,6 @@ 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/mujoco/right_arm_eye_to_hand_world.pb.txt
View File

@ -1,7 +0,0 @@
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
}

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

@ -5,43 +5,36 @@ device_manager {
devices {
id: "mujoco_world"
type: DEVICE_TYPE_MUJOCO_WORLD
config_file: "devices/mujoco/right_arm_eye_to_hand_world.pb.txt"
enable: true
config_file: "devices/mujoco/mujoco_world.pb.txt"
enable: false
}
devices {
id: "right_arm_mujoco_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/mujoco_motors.pb.txt"
enable: true
enable: false
}
devices {
id: "mujoco_right_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm_mujoco_qp.pb.txt"
enable: true
enable: false
}
devices {
id: "mujoco_viewer"
type: DEVICE_TYPE_MUJOCO_VIEWER
config_file: "devices/mujoco/mujoco_viewer.pb.txt"
enable: true
enable: false
}
devices {
id: "mujoco_hand_cam"
type: DEVICE_TYPE_CAMERA
config_file: "devices/camera/camera.pb.txt"
enable: true
}
devices {
id: "mujoco_external_touch_cam"
type: DEVICE_TYPE_CAMERA
config_file: "devices/camera/camera.pb.txt"
enable: true
enable: false
}
devices {
@ -60,12 +53,19 @@ device_manager {
devices {
id: "hand2"
id: "hand1"
type: DEVICE_TYPE_DEXHAND
config_file: "devices/dexhand/dexhand.pb.txt"
enable: false
enable: true
}
devices {
id: "hand2"
type: DEVICE_TYPE_DEXHAND
config_file: "devices/dexhand/dexhand.pb.txt"
enable: true
}
devices {
id: "paxini_tip_1"
type: DEVICE_TYPE_DEXHAND
@ -77,7 +77,7 @@ device_manager {
id: "mujoco_zero_touch_dexhand"
type: DEVICE_TYPE_DEXHAND
config_file: "devices/dexhand/dexhand.pb.txt"
enable: true
enable: false
}
devices {
@ -91,7 +91,7 @@ device_manager {
id: "right_arm_can_motors"
type: DEVICE_TYPE_MOTOR_SYSTEM
config_file: "devices/motor/ti5_motors.pb.txt"
enable: false
enable: true
}
devices {
@ -119,9 +119,16 @@ device_manager {
id: "right_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm.pb.txt"
enable: false
enable: true
}
devices {
id: "left_arm"
type: DEVICE_TYPE_ROBOT_ARM
config_file: "devices/arm/arm.pb.txt"
enable: false
}
devices {
id: "aubo_arm"
type: DEVICE_TYPE_ROBOT_ARM

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

@ -1,16 +1,10 @@
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 {
@ -25,56 +19,46 @@ 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 {
# 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
tags {
screen {
id: 1
size_m: 0.03
}
hand {
id: 0
size_m: 0.03
}
}
# 手部相机：用于点击目标点和手部目标跟踪。
hand_camera {
apriltag {
tag_size_m: 0.012
depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
}
}
alignment {
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 }
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 }
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 {
# 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 }
position_in_camera { x: -0.001 y: 0.08 z: 0.15 }
rotation_vector { x: 3.14159265358979323846 y: 0.0 z: 0.0 }
mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
}
error_threshold {
@ -108,7 +92,6 @@ 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
# TCP 后退目标距离，单位为米。
distance_m: 0.02
duration_s: 0.45
}
}

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

@ -1,16 +1,10 @@
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: "mujoco_right_arm"
dexhand_id: "mujoco_zero_touch_dexhand"
# 手部相机和外部相机的 DeviceManager ID。
camera_id: "mujoco_hand_cam"
external_camera_id: "mujoco_external_touch_cam"
}
initialization {
@ -23,73 +17,54 @@ 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.0
acceleration: 3.0
# 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
skip_position_tolerance_rad: 0.001
# 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
skip_velocity_tolerance_rad_s: 0.01
velocity: 2.8
acceleration: 20.0
}
perception {
# 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
tags {
screen {
id: 1
size_m: 0.03
}
hand {
id: 0
size_m: 0.03
}
}
# 手部相机：用于点击目标点和手部目标跟踪。
hand_camera {
apriltag {
tag_size_m: 0.12
depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
}
}
alignment {
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 }
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 }
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 {
# 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
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
}
error_threshold {
x: 0.005
y: 0.005
z: 0.005
z: 0.010
rx: 0.08726646259971647
ry: 0.08726646259971647
rz: 0.08726646259971647
@ -103,7 +78,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.02
max_distance_m: 0.12
}
tactile {
finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
@ -116,8 +91,7 @@ touch_screen_task {
retract {
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
acceleration: 5.0
duration_s: 2.0
}
}

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

@ -106,8 +106,6 @@ private:
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);
@ -132,13 +130,6 @@ 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};

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

@ -516,16 +516,6 @@ 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_);
}
@ -569,14 +559,9 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
}
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());
}
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,
@ -590,24 +575,6 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
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();
}
@ -671,9 +638,8 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
if (const auto stopped = safetyStopResult_("moveL")) {
return *stopped;
}
const auto cartesian_stop = stopCartesianMotionAndWait_();
if (!cartesian_stop.ok()) {
return cartesian_stop;
if (cartesian_velocity_controller_) {
cartesian_velocity_controller_->shutdown();
}
if (options.asynchronous) {
return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported");
@ -749,10 +715,7 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
Result MotorRobotArm::stopMotion()
{
const auto cartesian_stop = stopCartesianMotionAndWait_();
if (!cartesian_stop.ok()) {
return cartesian_stop;
}
stopL(0.0);
return stopJ(0.0);
}
@ -1000,142 +963,8 @@ 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;
@ -1248,10 +1077,6 @@ 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;
}

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

@ -3,22 +3,20 @@
#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{0.0F};
float cy{0.0F};
float fx{0.0F};
float fy{0.0F};
float coeffs[5]{};
float cx;
float cy;
float fx;
float fy;
float coeffs[5];
};
struct StreamFrameData
@ -66,26 +64,11 @@ 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,12 +7,9 @@
#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;
@ -30,23 +27,8 @@ 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,
@ -55,14 +37,6 @@ 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
View File

@ -1,25 +0,0 @@
#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,7 +1,6 @@
#include "devices/camera/common/include/camera_stream_encoder.h"
#include <chrono>
#include <cmath>
#include <ctime>
#include <iomanip>
#include <sstream>
@ -10,7 +9,6 @@
#include <opencv2/imgproc.hpp>
#include "common/base/logging/logger.h"
#include "devices/camera/abstract_camera.h"
namespace cmvr::device {
namespace {
@ -48,41 +46,6 @@ 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") {
@ -112,77 +75,6 @@ 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) {
@ -297,7 +189,6 @@ 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();
@ -314,27 +205,9 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
}
cv::Mat frame_to_encode = frame;
if (options.draw_timestamp || options.overlay.draw_coordinate_frames) {
if (options.draw_timestamp) {
frame_to_encode = frame.clone();
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);
}
}
drawTimeStamp(frame_to_encode);
}
const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode);
@ -421,14 +294,4 @@ 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

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

@ -19,13 +19,6 @@ 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;
@ -345,17 +338,12 @@ 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;
}
@ -365,6 +353,7 @@ 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;
@ -464,7 +453,6 @@ bool MujocoCamera::initOffscreen_()
return true;
}
std::lock_guard<std::mutex> glfw_lock(glfwInitMutex());
if (!glfwInit()) {
setError_("[MujocoCamera] glfwInit failed");
return false;

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

@ -772,18 +772,10 @@ 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,18 +504,10 @@ 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_;

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

@ -308,14 +308,12 @@ void DeviceManager::configure_mujoco_viewer_pip_()
auto viewer = getDevice<cmvr::MujocoViewerDevice>(viewer_id);
if (viewer && viewer->setPiPCameraConfig(camera_config)) {
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);
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);
});
break;
}

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

@ -35,9 +35,4 @@ 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
)

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

@ -5,11 +5,8 @@
#pragma once
#include <atomic>
#include <cstdint>
#include <deque>
#include <memory>
#include <mutex>
#include <string>
#include <thread>
#include <vector>
@ -55,14 +52,6 @@ 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 旧接口）
@ -71,16 +60,9 @@ 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,
@ -97,34 +79,9 @@ 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_;
@ -136,10 +93,31 @@ namespace cmvr {
std::unique_ptr<mujoco::Simulate> sim_;
std::thread sync_thread_;
std::deque<PiPCameraState> pip_cameras_;
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;
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; // 新增：帧序号
};
@ -161,20 +139,15 @@ 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_;
std::vector<config::MujocoCameraConfig> pip_camera_configs_;
config::MujocoCameraConfig pip_camera_config_;
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;
};

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

@ -53,6 +53,8 @@ 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>(
@ -70,7 +72,10 @@ namespace cmvr {
sync_thread_.join();
}
clearPiPCameras();
if (pip_scene_inited_) {
mjv_freeScene(&pip_scene_);
pip_scene_inited_ = false;
}
if (pip_render_data_ != nullptr) {
mj_deleteData(pip_render_data_);
pip_render_data_ = nullptr;
@ -115,12 +120,14 @@ namespace cmvr {
}
void MuJocoViewer::enablePiPCamera(const char *camera_name) {
clearPiPCameras();
PiPCameraState state;
state.name = camera_name ? camera_name : "";
mjv_defaultCamera(&state.camera);
mjv_defaultScene(&state.scene);
pip_cameras_.push_back(std::move(state));
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;
}
void MuJocoViewer::enablePiPCamera(const char *camera_name,
@ -138,66 +145,77 @@ namespace cmvr {
int display_height,
int render_width,
int render_height) {
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));
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;
}
void MuJocoViewer::disablePiPCamera() {
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();
pip_enabled_ = false;
}
void MuJocoViewer::renderPiP() {
if (!sim_ || pip_cameras_.empty()) return;
if (!pip_enabled_ || !sim_) return;
if (pip_camera_name_.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;
// Copy one visualization snapshot for all PiP cameras. Keep the
// simulation lock out of scene updates, GPU rendering, and readback.
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;
// Copy only the visualization state while synchronized with Simulate.
// Keep scene update, GPU rendering and pixel readback out of this lock
// so the world sync thread cannot hold the simulation mutex while
// waiting for the viewer.
{
std::unique_lock<std::recursive_mutex> lock(sim_->mtx, std::try_to_lock);
if (lock.owns_lock()) {
@ -215,164 +233,121 @@ namespace cmvr {
}
}
// If the sync thread owns the lock, use the last complete snapshot.
// The simulation lock is intentionally non-blocking. If the sync
// thread owns it, use the last complete snapshot so every back buffer
// still gets the PiP overlay; skip only until the first snapshot exists.
if (pip_render_data_ == nullptr || pip_render_data_model_ != render_model) {
return;
}
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_cam_.type = mjCAMERA_FIXED;
pip_cam_.fixedcamid = pip_camera_id_;
pip_cam_.trackbodyid = -1;
mjv_updateScene(render_model, pip_render_data_, &opt_, &pert_, &pip_cam_, mjCAT_ALL, &pip_scene_);
auto& context = sim_->platform_ui->mjr_context();
for (size_t pip_index = 0; pip_index < pip_cameras_.size(); ++pip_index) {
auto& pip = pip_cameras_[pip_index];
if (pip.name.empty()) continue;
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;
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;
}
}
mjrRect render_rect;
render_rect.left = 0;
render_rect.bottom = 0;
render_rect.width = render_width;
render_rect.height = render_height;
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));
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 {
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);
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));
mjr_readPixels(pip.rgb.data(), pip.depth.data(), render_rect, &context);
{
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));
// 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);
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);
}
// 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;
// 同时读 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);
}
}
if (rendered_offscreen) {
mjr_setBuffer(mjFB_WINDOW, &context);
mjr_render(display_rect, &pip.scene, &context);
// 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);
}
}
pip_rgb_width_ = w;
pip_rgb_height_ = h;
pip_rgb_valid_ = true;
++pip_frame_id_; // 新帧
}
}
}
@ -387,18 +362,12 @@ namespace cmvr {
if (!world_->model() || !world_->data()) {
mju_error("MuJocoViewer world has null model/data");
}
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) {
if (pip_enabled_ && pip_render_width_ > 0 && 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, max_pip_render_width);
model->vis.global.offheight = std::max(model->vis.global.offheight, max_pip_render_height);
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_);
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
@ -448,15 +417,7 @@ namespace cmvr {
uint64_t MuJocoViewer::getPiPCameraFrameId() const {
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
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;
return pip_frame_id_;
}
bool MuJocoViewer::getPiPCameraRGBD(std::vector<unsigned char> &rgb,
@ -465,35 +426,13 @@ namespace cmvr {
int &height,
uint64_t &frame_id) const {
std::lock_guard<std::mutex> lock(pip_rgb_mtx_);
if (pip_cameras_.empty()) return false;
const auto &pip = pip_cameras_.front();
if (!pip.rgb_valid || pip.rgb.empty()) return false;
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;
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;
rgb = pip_rgb_;
depth = pip_depth_;
width = pip_rgb_width_;
height = pip_rgb_height_;
frame_id = pip_frame_id_;
return true;
}
@ -548,7 +487,6 @@ namespace cmvr {
}
std::shared_ptr<simulate::MujocoWorld> world;
std::vector<config::MujocoCameraConfig> pip_camera_configs;
{
std::lock_guard<std::mutex> lock(mtx_);
if (running_) {
@ -559,7 +497,6 @@ namespace cmvr {
return false;
}
world = world_;
pip_camera_configs = pip_camera_configs_;
stop_requested_ = false;
running_ = true;
}
@ -580,30 +517,19 @@ namespace cmvr {
config_.camera_azimuth(),
config_.camera_elevation());
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(),
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(),
pip.left(),
pip.bottom(),
pip.width(),
pip.height(),
render.width(),
render.height());
first_pip_camera = false;
} else {
viewer->addPiPCamera(camera_config.camera_name().c_str(),
pip.left(),
pip.bottom(),
pip.width(),
pip.height(),
render.width(),
render.height());
viewer->enablePiPCamera(pip_camera_config_.camera_name().c_str());
}
}
@ -630,31 +556,19 @@ namespace cmvr {
if (camera_config.world_id() != config_.world_id()) {
return false;
}
if (camera_config.camera_name().empty()) {
return false;
}
std::lock_guard<std::mutex> lock(mtx_);
if (running_) {
CMVR_LOG(WARNING) << "[MujocoViewerDevice] cannot add PiP camera while viewer is running"
if (has_pip_camera_config_) {
CMVR_LOG(WARNING) << "[MujocoViewerDevice] PiP camera already configured, keep first"
<< ", 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();
<< ", current_camera=" << pip_camera_config_.camera_name()
<< ", ignored_camera=" << camera_config.camera_name();
return false;
}
pip_camera_configs_.push_back(camera_config);
CMVR_LOG(INFO) << "[MujocoViewerDevice] add PiP camera"
pip_camera_config_ = camera_config;
has_pip_camera_config_ = true;
CMVR_LOG(INFO) << "[MujocoViewerDevice] set PiP camera"
<< ", viewer_id=" << id_
<< ", camera=" << camera_config.camera_name()
<< ", world_id=" << camera_config.world_id();
@ -673,20 +587,6 @@ 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,21 +1,11 @@
#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"
@ -47,63 +37,6 @@ 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)
@ -130,67 +63,3 @@ 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();
}

57
cmvr-es/task/touch_screen_task/include/touch_screen_task.h
View File

@ -13,14 +13,13 @@
#include <Eigen/Dense>
#include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h"
#include "algorithms/controllers/pbvs/include/pbvs_controller.h"
#include "algorithms/perception/apriltag/include/apriltag_perception.h"
#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
#include "algorithms/perception/apriltag/include/tag_relative_tcp_pose.h"
#include "algorithms/controllers/ibvs/include/ibvs_controller.h"
#include "devices/camera/abstract_camera.h"
#include "devices/dexhand/abstract_dexhand.h"
#include "devices/arm/robot_arm.h"
#include "task/task.h"
#include "algorithms/perception/apriltag/include/apriltag_perception.h"
#include "algorithms/perception/apriltag/include/tag_relative_target_3d.h"
namespace cmvr::task {
@ -28,7 +27,7 @@ class TouchScreenTask : public Task {
public:
enum class Phase {
IDLE = 0, // 空闲，尚未开始任务。
ALIGNING, // 视觉对准阶段：持续 PBVS 对齐目标点。
ALIGNING, // 视觉对准阶段：持续 IBVS 对齐目标点。
ALIGN_REACHED, // 视觉对准已达到阈值，等待进入下一阶段。
TOUCHING, // 前进触控阶段：沿设定方向向屏幕推进。
DWELLING, // 已检测到接触，保持当前位置短暂停留。
@ -41,10 +40,11 @@ public:
IDLE = 0, // 空闲状态。
NOT_INITIALIZED, // 尚未调用 init() 完成初始化。
INVALID_CONFIG, // 配置非法，无法启动或应用参数。
CONTROL_JOINT_MISMATCH, // 控制关节顺序与 IK 链不一致。
ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。
ALIGN_WAITING_TRACK, // 对准阶段等待目标点跟踪恢复成功。
ALIGN_TARGET_SETUP_FAILED,// PBVS 目标位姿设置失败。
ALIGN_COMPUTE_FAILED, // 对准阶段 PBVS 计算失败。
ALIGN_TARGET_SETUP_FAILED,// 视觉目标设置失败，setTargetFromPointInTag 失败。
ALIGN_COMPUTE_FAILED, // 对准阶段 IBVS 或 IK 计算失败。
ALIGN_TIMEOUT, // 对准阶段超时仍未收敛。
ALIGNING, // 正在执行视觉对准。
ALIGN_REACHED, // 视觉对准完成。
@ -67,10 +67,6 @@ public:
bool init(const std::shared_ptr<device::RobotArm>& arm,
const std::shared_ptr<device::AbstractDexHand>& dexhand,
const std::shared_ptr<device::AbstractCamera>& camera);
bool init(const std::shared_ptr<device::RobotArm>& arm,
const std::shared_ptr<device::AbstractDexHand>& dexhand,
const std::shared_ptr<device::AbstractCamera>& camera,
const std::shared_ptr<device::AbstractCamera>& external_camera);
const std::string& id() const override { return id_; }
@ -96,15 +92,11 @@ public:
double lastTouchPressureSum() const;
int lastTouchNonzeroCount() const;
int lastActiveTagId() const;
Eigen::Vector3d lastAlignErrorScreenTag() const;
Eigen::Vector3d lastAlignErrorCamera() const;
const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
const std::shared_ptr<perception::AprilTagPerception>& externalPerception() const {
return external_perception_;
}
const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
const perception::TagRelativeTcpPose& tcpPoseTracker() const { return tcp_pose_tracker_; }
const PbvsController& pbvs() const { return pbvs_; }
const IbvsController& ibvs() const { return ibvs_; }
private:
using Clock = std::chrono::steady_clock;
@ -114,14 +106,18 @@ private:
bool startFromPixelUnlocked(int u, int v);
void stopUnlocked();
bool applyConfig();
bool validateControlJointNames() const;
bool stepAligning(double dt);
bool stepTouching();
bool stepDwelling();
bool stepRetracting();
void stopPbvsMotion();
bool readControlledJointPositions(std::vector<double>& q_out) const;
bool sendJointVelocity(const std::vector<double>& qdot) const;
bool sendZeroJointVelocity() const;
void hardStopIbvsMotion();
bool holdCurrentControlledPosition() const;
bool buildInitJointPositions(std::vector<double>& positions_out) const;
bool isAtInitPosition(const std::vector<double>& positions) const;
bool moveToInitPositionBeforeStartIfEnabled();
bool moveToInitPositionIfEnabled() const;
bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
@ -133,8 +129,6 @@ private:
void enterFailed(Status status);
bool updateTouchPressure();
void publishCoordinateOverlay();
void refreshCoordinateOverlay();
private:
mutable std::mutex mutex_;
@ -142,13 +136,10 @@ private:
std::shared_ptr<device::RobotArm> arm_{nullptr};
std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
std::shared_ptr<device::AbstractCamera> camera_{nullptr};
std::shared_ptr<device::AbstractCamera> external_camera_{nullptr};
std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
std::shared_ptr<perception::AprilTagPerception> external_perception_{nullptr};
perception::TagRelativeTarget3D tracker_;
perception::TagRelativeTcpPose tcp_pose_tracker_;
PbvsController pbvs_;
IbvsController ibvs_;
cmvr::config::TouchScreenTaskConfig config_{};
bool config_valid_{false};
@ -158,7 +149,7 @@ private:
bool initialized_{false};
bool target_locked_{false};
bool pbvs_target_initialized_{false};
bool ibvs_target_initialized_{false};
bool touch_command_started_{false};
bool retract_command_started_{false};
@ -166,31 +157,19 @@ private:
int target_v_{-1};
int align_stable_count_{0};
int align_debug_count_{0};
int pbvs_debug_count_{0};
int last_active_tag_id_{-1};
double last_touch_pressure_sum_{0.0};
int last_touch_nonzero_count_{0};
Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
double pbvs_command_acceleration_{0.25};
// Initialization MoveJ is skipped only when both position and velocity
// are within these configured limits.
double init_skip_position_tolerance_rad_{1e-3};
double init_skip_velocity_tolerance_rad_s_{1e-2};
Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
bool locked_target_rotation_valid_{false};
Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
bool touch_start_position_valid_{false};
Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
bool retract_start_position_valid_{false};
Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()};
bool have_last_T_B_G_{false};
Eigen::Matrix4d last_T_B_G_{Eigen::Matrix4d::Identity()};
double max_T_B_G_translation_delta_m_{0.0};
double max_T_B_G_rotation_delta_rad_{0.0};
Clock::time_point phase_start_time_{};
Clock::time_point last_coordinate_overlay_update_time_{};
Clock::time_point last_retract_log_time_{};
Status final_status_after_retract_{Status::DONE};
};

1159
cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
View File

File diff suppressed because it is too large Load Diff

392
cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp
View File

@ -1,7 +1,6 @@
#include "gtest/gtest.h"
#include <algorithm>
#include <atomic>
#include <array>
#include <chrono>
#include <cmath>
@ -23,11 +22,9 @@
#include "common/vision/image_projection.h"
#include "common/io/proto_file_io.h"
#include "cmvr/config/arm_config/arm_config.pb.h"
#include "cmvr/config/logger_config/logger_config.pb.h"
#include "cmvr/config/motor_config/motor_config.pb.h"
#include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h"
#include "devices/arm/robot_arm_factory.h"
#include "devices/camera/common/include/camera_stream_encoder.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"
@ -77,7 +74,6 @@ struct TagCenterPixel {
};
bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& camera,
const int target_tag_id,
const double tag_size_m,
const std::chrono::milliseconds timeout,
TagCenterPixel& pixel_out)
@ -107,9 +103,6 @@ bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& c
TagCenterPixel best_pixel;
for (const auto& tag : perception.tags()) {
if (tag.id != target_tag_id) {
continue;
}
const Eigen::Vector3d p_c_tag_center = tag.T_c_t.block<3, 1>(0, 3);
Eigen::Vector2d uv = Eigen::Vector2d::Zero();
if (!cmvr::ImageProcess::projectCameraPointToPixel(
@ -215,9 +208,9 @@ void run_touch_once(int u, int v) {
<< p_c_target.z() << "]"
<< ", nonzero_count=" << task->lastTouchNonzeroCount()
<< ", pressure_sum=" << task->lastTouchPressureSum()
<< ", err_G=[" << task->lastAlignErrorScreenTag().x() << ", "
<< task->lastAlignErrorScreenTag().y() << ", "
<< task->lastAlignErrorScreenTag().z() << "]\n";
<< ", err_c=[" << task->lastAlignErrorCamera().x() << ", "
<< task->lastAlignErrorCamera().y() << ", "
<< task->lastAlignErrorCamera().z() << "]\n";
if (task->lastStatus() == cmvr::task::TouchScreenTask::Status::ALIGN_REACHED) {
std::cout << "align reached, target_c=[" << p_c_target.x() << ", "
@ -271,384 +264,17 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
const auto& config = root_config.touch_screen_task();
EXPECT_TRUE(config.has_initialization());
EXPECT_TRUE(config.has_perception());
ASSERT_TRUE(config.perception().has_tags());
EXPECT_EQ(config.perception().tags().screen().id(), 1);
EXPECT_EQ(config.perception().tags().hand().id(), 0);
EXPECT_DOUBLE_EQ(config.perception().tags().screen().size_m(), 0.03);
EXPECT_DOUBLE_EQ(config.perception().tags().hand().size_m(), 0.03);
EXPECT_TRUE(config.perception().has_hand_camera());
const bool is_mujoco = std::string(file_name).find("mujoco") != std::string::npos;
EXPECT_EQ(config.devices().camera_id(),
is_mujoco ? "mujoco_hand_cam" : "right_hand_cam");
EXPECT_EQ(config.devices().external_camera_id(),
is_mujoco ? "mujoco_external_touch_cam" : "cam5");
EXPECT_TRUE(config.has_alignment());
ASSERT_TRUE(config.alignment().has_calibration());
EXPECT_TRUE(config.alignment().calibration().has_hand_tag_to_tcp());
ASSERT_TRUE(config.alignment().target().has_hand_orientation_g());
EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().rx(), 0.0);
EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().ry(), 0.0);
EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().rz(),
3.141592653589793);
ASSERT_TRUE(config.alignment().has_pbvs());
const auto& pbvs = config.alignment().pbvs();
EXPECT_DOUBLE_EQ(pbvs.position_gain().x(), 2.0);
EXPECT_DOUBLE_EQ(pbvs.rotation_gain().z(), 1.5);
EXPECT_DOUBLE_EQ(pbvs.vmax6().z(), 0.05);
EXPECT_DOUBLE_EQ(pbvs.amax6().rz(), 2.0);
EXPECT_DOUBLE_EQ(pbvs.twist_filter_alpha(), 1.0);
EXPECT_TRUE(config.alignment().has_ibvs());
EXPECT_TRUE(config.alignment().ibvs().has_camera_link());
EXPECT_TRUE(config.has_touch());
EXPECT_EQ(config.touch().motion_case(),
cmvr::config::TouchScreenTaskTouchConfig::kSpeedL);
EXPECT_TRUE(config.has_retract());
EXPECT_TRUE(config.retract().has_distance_m());
EXPECT_GT(config.retract().distance_m(), 0.0);
}
}
TEST(TouchScreenTaskTest, CoordinateFrameProjectionUsesBgrAxisColors) {
cv::Mat image = cv::Mat::zeros(240, 320, CV_8UC3);
cmvr::device::Rs2Intrinsics intrinsics{};
intrinsics.fx = 200.0F;
intrinsics.fy = 200.0F;
intrinsics.cx = 160.0F;
intrinsics.cy = 120.0F;
Eigen::Matrix4d T_C_Frame = Eigen::Matrix4d::Identity();
T_C_Frame(2, 3) = 1.0;
cmvr::device::drawCoordinateFrame(image,
T_C_Frame,
intrinsics,
0.2,
"G");
// X projects right in red and Y projects down in green for the camera
// convention used by the pinhole projection. Z is blue but projects onto
// the origin for this fronto-parallel pose.
const cv::Vec3b x_pixel = image.at<cv::Vec3b>(120, 180);
const cv::Vec3b y_pixel = image.at<cv::Vec3b>(140, 160);
EXPECT_GT(x_pixel[2], x_pixel[1]);
EXPECT_GT(x_pixel[2], x_pixel[0]);
EXPECT_GT(y_pixel[1], y_pixel[2]);
EXPECT_GT(y_pixel[1], y_pixel[0]);
}
TEST(TouchScreenTaskTest, RunEyeToHandTouchInMujoco) {
const auto project_root = findProjectRoot();
ASSERT_FALSE(project_root.empty());
cmvr::config::LoggerRootConfig logger_root;
const auto logger_config_path = project_root / "cmvr-es/config/logger/logger.pb.txt";
ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile(
logger_config_path.string(), &logger_root))
<< logger_config_path;
ASSERT_TRUE(cmvr::logging::initLogging(
logger_root.logger(), "touch_screen_task_test", project_root));
cmvr::ConfigHelper::setConfigRootFromFile(
(project_root / "cmvr-es/config/cmvr_es.pb.txt").string());
cmvr::task::TaskManager::destroyInstance();
cmvr::device::DeviceManager::destroyInstance();
// This test intentionally builds only the devices required by the
// MuJoCo Eye-to-Hand task. The task itself is still loaded from
// touch_screen_task_mujoco.pb.txt below.
cmvr::config::DeviceManagerConfig device_manager_config;
device_manager_config.set_name("touch_screen_eye_to_hand_mujoco_test");
device_manager_config.set_version("test");
auto add_device = [&](const char* id,
cmvr::config::DeviceConfigEntry::DeviceType type,
const char* config_file) {
auto* entry = device_manager_config.add_devices();
entry->set_id(id);
entry->set_type(type);
entry->set_config_file(config_file);
entry->set_enable(true);
};
add_device("mujoco_world",
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD,
"devices/mujoco/right_arm_eye_to_hand_world.pb.txt");
add_device("right_arm_mujoco_motors",
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM,
"devices/motor/mujoco_motors.pb.txt");
add_device("mujoco_right_arm",
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM,
"devices/arm/arm_mujoco.pb.txt");
add_device("mujoco_zero_touch_dexhand",
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND,
"devices/dexhand/dexhand.pb.txt");
add_device("mujoco_hand_cam",
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA,
"devices/camera/camera.pb.txt");
add_device("mujoco_external_touch_cam",
cmvr::config::DeviceConfigEntry::DEVICE_TYPE_CAMERA,
"devices/camera/camera.pb.txt");
auto& device_manager =
cmvr::device::DeviceManager::getInstance(device_manager_config);
auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
auto hand_camera = device_manager.getDevice<cmvr::device::AbstractCamera>(
"mujoco_hand_cam");
auto external_camera = device_manager.getDevice<cmvr::device::AbstractCamera>(
"mujoco_external_touch_cam");
ASSERT_NE(arm, nullptr);
ASSERT_NE(hand_camera, nullptr);
ASSERT_NE(external_camera, nullptr);
const auto hand_mujoco_camera =
std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(hand_camera);
const auto external_mujoco_camera =
std::dynamic_pointer_cast<cmvr::device::MujocoCamera>(external_camera);
ASSERT_NE(hand_mujoco_camera, nullptr);
ASSERT_NE(external_mujoco_camera, nullptr);
device_manager.start();
cv::Mat hand_camera_frame;
cmvr::device::Rs2Intrinsics hand_camera_intrinsics{};
hand_camera->getRGBImage(hand_camera_frame, hand_camera_intrinsics);
ASSERT_FALSE(hand_camera_frame.empty())
<< "mujoco_hand_cam did not produce a frame after DeviceManager::start()";
cv::Mat external_camera_frame;
cmvr::device::Rs2Intrinsics external_camera_intrinsics{};
external_camera->getRGBImage(external_camera_frame, external_camera_intrinsics);
ASSERT_FALSE(external_camera_frame.empty())
<< "mujoco_external_touch_cam did not produce a frame after DeviceManager::start()";
auto world = cmvr::device::MotorManager::mujocoWorldFor(
"right_arm_mujoco_motors");
ASSERT_NE(world, nullptr);
ASSERT_TRUE(world->isLoaded());
ASSERT_TRUE(world->isRunning());
cmvr::MuJocoViewer viewer(world);
ASSERT_NE(viewer.model(), nullptr);
ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "hand_cam"), 0);
ASSERT_GE(mj_name2id(viewer.model(), mjOBJ_CAMERA, "external_touch_cam"), 0);
viewer.setupCamera(2.5, -160.0, -25.0);
// PiP is display-only. Its visibility and layout follow camera.pb.txt;
// camera acquisition still uses each MujocoCamera's offscreen thread.
bool first_pip_camera = true;
const auto add_configured_pip = [&](const cmvr::device::MujocoCamera& camera) {
const auto& camera_config = camera.config();
const auto& pip = camera_config.viewer_pip();
if (!pip.enable()) {
return;
}
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.addPiPCamera(camera_config.camera_name().c_str(),
pip.left(),
pip.bottom(),
pip.width(),
pip.height(),
render.width(),
render.height());
}
};
add_configured_pip(*external_mujoco_camera);
add_configured_pip(*hand_mujoco_camera);
struct Outcome {
bool init_ok{false};
bool touch_ok{false};
bool align_reached{false};
bool retracting_seen{false};
bool finished{false};
cmvr::task::TouchScreenTask::Phase final_phase{
cmvr::task::TouchScreenTask::Phase::IDLE};
cmvr::task::TouchScreenTask::Status final_status{
cmvr::task::TouchScreenTask::Status::IDLE};
double max_pressure{0.0};
double speedl_command_norm{0.0};
int active_tag_id{-1};
std::string error;
} outcome;
// The viewer owns the lifetime of this test. Closing its window asks the
// worker to stop; a completed touch cycle must not close the viewer.
std::atomic<bool> stop_requested{false};
std::thread scenario([&] {
try {
const auto touch_config = loadMujocoTouchConfig(project_root);
if (touch_config.devices().arm_id() != "mujoco_right_arm" ||
touch_config.devices().camera_id() != "mujoco_hand_cam" ||
touch_config.devices().external_camera_id() !=
"mujoco_external_touch_cam") {
throw std::runtime_error(
"touch_screen_task_mujoco.pb.txt has unexpected device IDs");
}
cmvr::config::TaskManagerConfig task_manager_config;
auto* task_entry = task_manager_config.add_tasks();
task_entry->set_id(touch_config.id());
task_entry->set_type(
cmvr::config::TaskConfigEntry::TASK_TYPE_TOUCH_SCREEN);
task_entry->set_run_mode(
cmvr::config::TaskConfigEntry::TASK_RUN_MODE_PERIODIC_STEP);
task_entry->set_control_period_s(0.001);
task_entry->set_config_file(
"tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt");
task_entry->set_enable(true);
auto& task_manager =
cmvr::task::TaskManager::getInstance(task_manager_config);
auto task = task_manager.getTouchScreenTask(touch_config.id());
if (!task) {
throw std::runtime_error("TouchScreenTask not found: " +
touch_config.id());
}
outcome.init_ok =
task->lastStatus() !=
cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED;
if (!outcome.init_ok) {
throw std::runtime_error("TouchScreenTask initialization failed");
}
task_manager.startRunTask();
const auto& render = hand_mujoco_camera->config().render();
const int target_u = render.width() > 0 ? render.width() / 2 : 640;
const int target_v = render.height() > 0 ? render.height() / 2 : 360;
std::cout << "[TouchScreenTaskMujocoTest] touch target pixel: u="
<< target_u << ", v=" << target_v << std::endl;
const bool touch_started = task->touch(target_u, target_v);
if (!touch_started) {
throw std::runtime_error(
"task->touch failed, status=" +
std::string(cmvr::task::TouchScreenTask::statusToString(
task->lastStatus())));
}
outcome.touch_ok = true;
std::cout << "[TouchScreenTaskMujocoTest] touch started once"
<< std::endl;
const auto deadline =
std::chrono::steady_clock::now() + std::chrono::seconds(35);
int step_count = 0;
bool paused_at_alignment = false;
while (!stop_requested.load(std::memory_order_acquire) &&
task->isBusy() &&
std::chrono::steady_clock::now() < deadline) {
outcome.final_phase = task->phase();
outcome.final_status = task->lastStatus();
outcome.active_tag_id = task->lastActiveTagId();
outcome.max_pressure = std::max(
outcome.max_pressure, task->lastTouchPressureSum());
const auto speedl = arm->getSpeedLCommandTwistBase();
outcome.speedl_command_norm = std::max(
outcome.speedl_command_norm,
std::sqrt(speedl.vx * speedl.vx + speedl.vy * speedl.vy +
speedl.vz * speedl.vz));
const auto phase = task->phase();
if (task->lastStatus() ==
cmvr::task::TouchScreenTask::Status::ALIGN_REACHED ||
phase == cmvr::task::TouchScreenTask::Phase::TOUCHING ||
phase == cmvr::task::TouchScreenTask::Phase::DWELLING ||
phase == cmvr::task::TouchScreenTask::Phase::RETRACTING ||
phase == cmvr::task::TouchScreenTask::Phase::DONE) {
outcome.align_reached = true;
}
if (phase == cmvr::task::TouchScreenTask::Phase::RETRACTING) {
outcome.retracting_seen = true;
}
if (phase == cmvr::task::TouchScreenTask::Phase::ALIGN_REACHED &&
touch_config.alignment().pause_when_reached()) {
paused_at_alignment = true;
break;
}
if ((step_count++ % 20) == 0) {
std::cout << "[TouchScreenTaskMujocoTest] phase="
<< cmvr::task::TouchScreenTask::phaseToString(
task->phase())
<< ", status="
<< cmvr::task::TouchScreenTask::statusToString(
task->lastStatus())
<< ", active_tag=" << task->lastActiveTagId()
<< ", tcp_pose="
<< cmvr::perception::TagRelativeTcpPose::statusToString(
task->tcpPoseTracker().lastStatus())
<< ", pressure=" << task->lastTouchPressureSum()
<< std::endl;
}
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
if (stop_requested.load(std::memory_order_acquire)) {
task->stop();
} else if (task->isBusy() && !paused_at_alignment) {
task->stop();
throw std::runtime_error("touch scenario exceeded 35 seconds");
}
outcome.finished = task->isFinished();
outcome.final_phase = task->phase();
outcome.final_status = task->lastStatus();
std::cout << "[TouchScreenTaskMujocoTest] touch finished once, phase="
<< cmvr::task::TouchScreenTask::phaseToString(
outcome.final_phase)
<< "; viewer remains running" << std::endl;
// Keep the viewer and its camera frames alive after the task has
// run once. The main thread sets stop_requested when the window
// is closed.
while (!stop_requested.load(std::memory_order_acquire)) {
std::this_thread::sleep_for(std::chrono::milliseconds(100));
}
task->stop();
task_manager.stopRunTask();
std::cout << "[TouchScreenTaskMujocoTest] viewer is still running; "
"close the MuJoCo window to finish gtest." << std::endl;
} catch (const std::exception& error) {
try {
cmvr::task::TaskManager::getInstance().stopRunTask();
} catch (...) {
}
outcome.error = error.what();
stop_requested.store(true, std::memory_order_release);
std::cerr << "[TouchScreenTaskMujocoTest] scenario error: "
<< outcome.error
<< "; viewer remains open, close it manually to finish gtest."
<< std::endl;
}
});
viewer.setRunning(true);
viewer.run();
stop_requested.store(true, std::memory_order_release);
scenario.join();
cmvr::task::TaskManager::destroyInstance();
device_manager.stop();
cmvr::device::DeviceManager::destroyInstance();
EXPECT_TRUE(outcome.error.empty()) << outcome.error;
EXPECT_TRUE(outcome.init_ok);
EXPECT_TRUE(outcome.touch_ok);
EXPECT_TRUE(outcome.align_reached);
EXPECT_GT(outcome.speedl_command_norm, 0.005);
}
TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
const auto project_root = findProjectRoot();
ASSERT_FALSE(project_root.empty());
cmvr::ConfigHelper::setConfigRootFromFile(
@ -747,8 +373,7 @@ TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
std::thread scenario([&] {
try {
const auto touch_config = loadMujocoTouchConfig(project_root);
device_manager.registerDevice(
touch_config.devices().camera_id(), camera);
device_manager.registerDevice(touch_config.devices().camera_id(), camera);
cmvr::config::TaskManagerConfig task_manager_config;
auto* task_entry = task_manager_config.add_tasks();
@ -770,8 +395,7 @@ TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
TagCenterPixel target_pixel;
if (!detectTagCenterPixel(camera,
touch_config.perception().tags().screen().id(),
touch_config.perception().tags().screen().size_m(),
touch_config.perception().apriltag().tag_size_m(),
std::chrono::seconds(3),
target_pixel)) {
throw std::runtime_error("failed to detect MuJoCo AprilTag center pixel");

BIN
model/april_tag/tag36_11_00001.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 5.2 KiB

BIN
model/april_tag/tag36_11_00002_1000.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 5.2 KiB

BIN
model/april_tag/tag36_11_00003_1000.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 5.2 KiB

BIN
model/april_tag/tag36_11_00004_1000.png
View File

Binary file not shown.
Side by Side

Before

Width:  |  Height:  |  Size: 5.2 KiB

809
model/xiaoyan_description/right_arm_eye_to_hand.xml
View File

@ -1,809 +0,0 @@
<?xml version="1.0" encoding="UTF-8"?>
<mujoco model="right_arm_eye_to_hand">
<!-- =========================================================
Compiler
========================================================= -->
<compiler angle="radian"
meshdir="meshes/"/>
<option timestep="0.001"
gravity="0 0 -9.81"
integrator="implicitfast"/>
<default>
<joint damping="10"
armature="0.001"/>
</default>
<!-- =========================================================
Visual
========================================================= -->
<visual>
<global offwidth="1280"
offheight="720"/>
<map znear="0.02"
zfar="5"/>
</visual>
<!-- =========================================================
Assets
========================================================= -->
<asset>
<!-- ================= Right arm meshes ================= -->
<mesh name="PELVIS_S"
file="PELVIS_S.STL"/>
<mesh name="R_SHOULDER_P_S"
file="R_SHOULDER_P_S.STL"/>
<mesh name="R_SHOULDER_R_S"
file="R_SHOULDER_R_S.STL"/>
<mesh name="R_SHOULDER_Y_S"
file="R_SHOULDER_Y_S.STL"/>
<mesh name="R_ELBOW_R_S"
file="R_ELBOW_R_S.STL"/>
<mesh name="R_WRIST_P_S"
file="R_WRIST_P_S.STL"/>
<mesh name="R_WRIST_Y_S"
file="R_WRIST_Y_S.STL"/>
<mesh name="R_WRIST_R_S"
file="R_WRIST_R_S.STL"/>
<!-- ================= Skybox ================= -->
<texture name="skybox"
type="skybox"
builtin="gradient"
rgb1="0.4 0.6 0.8"
rgb2="0 0 0"
width="512"
height="512"/>
<!-- ================= Floor ================= -->
<texture name="grid"
type="2d"
builtin="checker"
width="512"
height="512"
rgb1="0.2 0.3 0.4"
rgb2="0.1 0.2 0.3"
mark="cross"
markrgb="0.8 0.8 0.8"/>
<material name="grid_floor"
texture="grid"
texrepeat="5 5"
rgba="1 1 1 1"
emission="0.9"
specular="0.5"
shininess="1"
reflectance="0.3"/>
<!-- ================= AprilTag =================
tag36h11 family
Hand Tag H:
ID = 0
texture = tag36_11.png
Screen Tag G:
ID = 1
texture = tag36_11_00001.png
Both texture planes are 37.5 mm x 37.5 mm. The PNG quiet zone
leaves a 30 mm x 30 mm detectable black tag region.
=========================================== -->
<!-- Hand Tag H : tag36h11 ID 0 -->
<texture name="hand_apriltag_tex"
type="2d"
file="../april_tag/tag36_11.png"/>
<!-- The +Z face is the visible face for external_touch_cam. -->
<material name="hand_apriltag_mat"
texture="hand_apriltag_tex"
texrepeat="1 1"
rgba="1 1 1 1"
emission="1"
specular="0"
shininess="0"
reflectance="0"/>
<!-- Screen Tag G : tag36h11 ID 1 -->
<texture name="screen_apriltag_tex"
type="2d"
file="../april_tag/tag36_11_00001.png"/>
<material name="screen_apriltag_mat"
texture="screen_apriltag_tex"
texrepeat="-1 -1"
rgba="1 1 1 1"
emission="1"
specular="0"
shininess="0"
reflectance="0"/>
<!-- ================= Screen material ================= -->
<material name="screen_material"
rgba="0.04 0.05 0.06 1"
specular="0.25"
shininess="0.4"/>
<material name="screen_frame_material"
rgba="0.15 0.15 0.15 1"
specular="0.2"
shininess="0.3"/>
</asset>
<!-- =========================================================
World
========================================================= -->
<worldbody>
<!-- =====================================================
Ground
===================================================== -->
<geom name="floor"
type="plane"
pos="0 0 0"
size="0 0 0.05"
material="grid_floor"
condim="3"
friction="1 0.005 0.0001"/>
<!-- =====================================================
Robot support
===================================================== -->
<geom name="robot_stand_visual"
size="0.05 0.6"
pos="0 0 0.6"
type="cylinder"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.25 0.25 0.28 1"/>
<!-- =====================================================
Fixed touch screen
screen center:
world = [0, -1.05, 1.25]
screen size:
width = 500 mm
height = 300 mm
Front surface points toward +Y,
i.e. toward robot.
Screen plane for Eye-to-Hand:
approximately y = -1.045
normal toward robot:
n_world = [0, 1, 0]
===================================================== -->
<body name="touch_screen"
pos="0.62 -0.2 0.95"
euler="0 0.1 1.57">
<!-- Back/frame -->
<geom name="screen_frame"
type="box"
size="0.265 0.012 0.165"
material="screen_frame_material"
contype="1"
conaffinity="1"/>
<!-- Actual screen glass -->
<geom name="screen_surface"
type="box"
pos="0 0.013 0"
size="0.25 0.002 0.15"
material="screen_material"
contype="1"
conaffinity="1"
friction="0.5 0.005 0.0001"/>
<!--
Touch surface center.
This is the point that should be used later
when defining the screen plane.
-->
<site name="screen_center"
pos="0 0.015 0"
size="0.006"
rgba="1 0 0 1"/>
<!-- Four visual reference points -->
<site name="screen_top_left"
pos="-0.25 0.015 0.15"
size="0.004"
rgba="1 1 0 1"/>
<site name="screen_top_right"
pos="0.25 0.015 0.15"
size="0.004"
rgba="1 1 0 0"/>
<site name="screen_bottom_left"
pos="-0.25 0.015 -0.15"
size="0.004"
rgba="1 1 0 1"/>
<site name="screen_bottom_right"
pos="0.25 0.015 -0.15"
size="0.004"
rgba="1 1 0 1"/>
<!-- =================================================
Screen AprilTag G : tag36h11 ID 1
Screen local frame:
- screen plane axes are local X and Z
- screen front normal is local +Y
The screen front surface is at local y = 0.015 m.
The tag is placed at the upper-right corner and
lifted by 0.1 mm to avoid z-fighting.
Texture plane size:
37.5 mm x 37.5 mm
Detectable black tag size:
30 mm x 30 mm
================================================= -->
<body name="SCREEN_APRILTAG"
pos="0.235 0.0151 0.135"
euler="-1.57079632679 0 0">
<geom name="SCREEN_APRILTAG_GEOM"
type="box"
size="0.01875 0.01875 0.0001"
material="screen_apriltag_mat"
rgba="1 1 1 1"
contype="0"
conaffinity="0"
group="2"/>
<!-- Non-rendered Screen Tag center Ground Truth -->
<site name="SCREEN_APRILTAG_SITE"
pos="0 0 0"
size="0.002"
rgba="0 1 0 0"/>
</body>
</body>
<!-- =====================================================
Fixed external Eye-to-Hand camera
IMPORTANT:
- The camera is placed slightly to the left of the robot and
aimed at the screen center, like a human viewing from the
left-front side.
- Screen front normal is local +Y.
- MuJoCo camera looks along its local -Z.
The optical axis is deliberately yawed toward the screen
center instead of being exactly fronto-parallel. This keeps
the Hand Tag visible while the arm moves in front of the
screen.
===================================================== -->
<body name="external_camera_mount"
pos="0.7 -0.2 0.9"
euler="0 0.4 1.57">
<geom name="external_camera_body"
type="box"
pos="0.18 0.55 0"
size="0.025 0.035 0.018"
rgba="0.1 0.1 0.1 1"
contype="0"
conaffinity="0"/>
<site name="external_touch_cam_site"
pos="0.18 0.55 0"
size="0.008"
rgba="1 0.2 0.2 1"/>
<camera name="external_touch_cam"
pos="0.18 0.55 0"
xyaxes="
-0.950 0.311 0
0 0 1
"
fovy="50"/>
</body>
<!-- =====================================================
Robot
===================================================== -->
<body name="PELVIS_S"
pos="0 0 1.2">
<!-- Pelvis visual -->
<geom pos="0 0 0"
quat="1 0 0 0"
type="mesh"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.698039 0.698039 0.698039 1"
mesh="PELVIS_S"/>
<!-- Pelvis collision -->
<geom pos="0 0 0"
quat="1 0 0 0"
type="mesh"
rgba="0.698039 0.698039 0.698039 1"
mesh="PELVIS_S"/>
<!-- =================================================
R_SHOULDER_P
================================================= -->
<body name="R_SHOULDER_P_S"
pos="0 -0.0945 0.042">
<inertial
pos="-0.00982259 -0.0704593 -1.1507e-06"
quat="0.706163 0.705933 0.0386962 0.0386741"
mass="0.880738"
diaginertia="0.000584874 0.000465648 0.000443849"/>
<joint name="R_SHOULDER_P"
pos="0 0 0"
axis="0 -1 0"
range="-3.14 3.14"
actuatorfrcrange="-120 120"/>
<geom type="mesh"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_P_S"/>
<geom type="mesh"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_P_S"/>
<!-- ===============================================
R_SHOULDER_R
=============================================== -->
<body name="R_SHOULDER_R_S"
pos="0.035 -0.0765 0">
<inertial
pos="-0.0346025 -0.0917393 1.86281e-08"
quat="0.609323 0.358823 -0.609303 0.358778"
mass="0.594788"
diaginertia="0.000414771 0.000407636 0.000294296"/>
<joint name="R_SHOULDER_R"
pos="0 0 0"
axis="1 0 0"
range="-0.78 1.57"
actuatorfrcrange="-120 120"/>
<geom type="mesh"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_R_S"/>
<geom type="mesh"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_R_S"/>
<!-- =============================================
R_SHOULDER_Y
============================================= -->
<body name="R_SHOULDER_Y_S"
pos="-0.035 -0.1475 0">
<inertial
pos="-0.00440977 -0.086362 -7.58792e-09"
quat="0.705001 0.704998 0.054559 0.0545441"
mass="0.563406"
diaginertia="0.000329815 0.000297341 0.000211019"/>
<joint name="R_SHOULDER_Y"
pos="0 0 0"
axis="0 -1 0"
range="-3.14 3.14"
actuatorfrcrange="-80 80"/>
<geom type="mesh"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_Y_S"/>
<geom type="mesh"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_SHOULDER_Y_S"/>
<!-- ===========================================
R_ELBOW_R
=========================================== -->
<body name="R_ELBOW_R_S"
pos="0.034 -0.1025 0">
<inertial
pos="-0.0335624 -0.06032 -2.97736e-07"
quat="0.674756 0.674714 -0.211333 -0.211667"
mass="0.393572"
diaginertia="0.000189078 0.000181042 0.000139034"/>
<joint name="R_ELBOW_R"
pos="0 0 0"
axis="1 0 0"
range="0 2.05"
actuatorfrcrange="-80 80"/>
<geom type="mesh"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_ELBOW_R_S"/>
<geom type="mesh"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_ELBOW_R_S"/>
<!-- =========================================
R_WRIST_P
========================================= -->
<body name="R_WRIST_P_S"
pos="-0.034 -0.0965 0">
<inertial
pos="-1.39657e-10 -0.0675973 0.0192006"
quat="0.530481 0.467537 -0.467537 0.530481"
mass="0.442332"
diaginertia="0.000489142 0.000476754 9.72811e-05"/>
<joint name="R_WRIST_P"
pos="0 0 0"
axis="0 -1 0"
range="-3.14 3.14"
actuatorfrcrange="-50 50"/>
<geom type="mesh"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.647059 0.619608 0.588235 1"
mesh="R_WRIST_P_S"/>
<geom type="mesh"
rgba="0.647059 0.619608 0.588235 1"
mesh="R_WRIST_P_S"/>
<!-- =======================================
R_WRIST_Y
======================================= -->
<body name="R_WRIST_Y_S"
pos="0 -0.1525 0.039">
<inertial
pos="-0.00464136 -5.06426e-10 -0.0341254"
quat="0.298107 0.641196 0.641196 0.298107"
mass="0.235738"
diaginertia="6.00636e-05 5.8497e-05 4.579e-05"/>
<joint name="R_WRIST_Y"
pos="0 0 0"
axis="0 0 1"
range="-0.78 0.78"
actuatorfrcrange="-50 50"/>
<geom type="mesh"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.647059 0.619608 0.588235 1"
mesh="R_WRIST_Y_S"/>
<geom type="mesh"
rgba="0.647059 0.619608 0.588235 1"
mesh="R_WRIST_Y_S"/>
<!-- =====================================
R_WRIST_R / hand
===================================== -->
<body name="R_WRIST_R_S"
pos="0.03 0 -0.039">
<inertial
pos="-0.0201642 -0.11075 -0.00598955"
quat="0.483404 0.529786 -0.463203 0.520663"
mass="0.504366"
diaginertia="0.00027189 0.000186086 0.000130629"/>
<joint name="R_WRIST_R"
pos="0 0 0"
axis="1 0 0"
range="-0.26 1.57"
actuatorfrcrange="-50 50"/>
<!-- ===============================
Wrist / hand mesh
=============================== -->
<geom type="mesh"
contype="0"
conaffinity="0"
group="1"
density="0"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_WRIST_R_S"/>
<geom type="mesh"
rgba="0.890196 0.890196 0.913725 1"
mesh="R_WRIST_R_S"/>
<!-- =================================================
Robot TCP
Same location as original:
R_FINGER_TIP_FIXED
================================================= -->
<site name="R_FINGER_TIP_SITE"
pos="0.00684256 -0.284077 0.00801525"
size="0.006"
rgba="0 1 1 1"/>
<!-- Hand-mounted camera copied from dual_arm.xml. -->
<camera name="hand_cam"
pos="-0.01212 -0.17655 0.07506"
quat="3.17467e-11 -3.17467e-11 0.707107 0.707107"
fovy="60"/>
<!-- fingertip visual -->
<geom name="R_FINGER_TIP_VISUAL"
type="sphere"
size="0.004"
pos="0.00684256 -0.284077 0.00801525"
contype="0"
conaffinity="0"
rgba="0 1 1 1"/>
<!-- fingertip collision
for later physical screen contact
-->
<geom name="R_FINGER_TIP_COLLISION"
type="sphere"
size="0.004"
pos="0.00684256 -0.284077 0.00801525"
contype="1"
conaffinity="1"
rgba="0 1 1 0.25"/>
<!-- =================================================
Hand AprilTag H : tag36h11 ID 0
3 cm AprilTag mounted on BACK OF HAND
Texture plane full width:
0.0375 m
Detectable black tag width:
0.030 m
MuJoCo box size is HALF-size:
0.01875 x 0.01875
thickness:
0.0006 m
half thickness:
0.0003
It is fixed rigidly to R_WRIST_R_S.
Current position is on upper/back side of hand,
slightly downstream from wrist camera area.
The tag front surface normal is local +Z.
================================================= -->
<body name="R_HAND_APRILTAG"
pos="0.00684256 -0.254077 0.00801525"
euler="-1.57079632679 0 0">
<geom name="R_HAND_APRILTAG_GEOM"
type="box"
size="0.01875 0.01875 0.0003"
material="hand_apriltag_mat"
rgba="1 1 1 1"
contype="0"
conaffinity="0"
group="2"/>
<!-- Non-rendered Hand Tag center Ground Truth -->
<site name="R_HAND_APRILTAG_SITE"
pos="0 0 0"
size="0.002"
rgba="1 0 1 0"/>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
</body>
<!-- =====================================================
Lighting
===================================================== -->
<light name="top_light"
mode="fixed"
directional="true"
diffuse="0.7 0.7 0.7"
specular="0.3 0.3 0.3"
pos="0 0 3"
dir="0 0 -1"/>
<!-- Add some front lighting for AprilTag -->
<light name="front_light"
mode="fixed"
directional="false"
diffuse="0.5 0.5 0.5"
specular="0.1 0.1 0.1"
pos="0.4 -0.5 2.0"
dir="-0.3 -0.5 -0.4"/>
</worldbody>
<!-- =========================================================
Actuators
Keep original RIGHT ARM actuator names.
This is important because existing:
mujoco_motors
mujoco_right_arm
uses these right-arm joints.
========================================================= -->
<actuator>
<position name="R_SHOULDER_P_pos"
joint="R_SHOULDER_P"
kp="2000"
ctrlrange="-3.14 3.14"/>
<position name="R_SHOULDER_R_pos"
joint="R_SHOULDER_R"
kp="2000"
ctrlrange="-0.78 1.57"/>
<position name="R_SHOULDER_Y_pos"
joint="R_SHOULDER_Y"
kp="2000"
ctrlrange="-3.14 3.14"/>
<position name="R_ELBOW_R_pos"
joint="R_ELBOW_R"
kp="1500"
ctrlrange="0 2.05"/>
<position name="R_WRIST_P_pos"
joint="R_WRIST_P"
kp="800"
ctrlrange="-3.14 3.14"/>
<position name="R_WRIST_Y_pos"
joint="R_WRIST_Y"
kp="800"
ctrlrange="-0.78 0.78"/>
<position name="R_WRIST_R_pos"
joint="R_WRIST_R"
kp="800"
ctrlrange="-0.26 1.57"/>
</actuator>
<!-- =========================================================
Optional initial pose
This pose keeps the right arm roughly facing the screen.
We can adjust this later after actually running Viewer.
========================================================= -->
<keyframe>
<key name="home"
qpos="
0
0
0
0
0
0
0
"/>
</keyframe>
</mujoco>

11
protos/cmvr/config/arm_config/arm_config.proto
View File

@ -68,8 +68,6 @@ message CartesianVelocityControllerConfig {
double stop_command_velocity_norm = 3;
double stop_measured_velocity_norm = 4;
double stop_acceleration = 5;
// 等待 Cartesian 速度运动停止的最长时间，单位为秒。
optional double stop_timeout_s = 6;
}
message ToppraJointMotionPlannerConfig {
@ -83,15 +81,6 @@ message MoveJConfig {
oneof algorithm {
ToppraJointMotionPlannerConfig toppra_joint_motion_planner = 1;
}
// MoveJ 轨迹发送完成后，等待关节实际状态稳定的最长时间，单位为秒。
optional double settle_timeout_s = 2;
// MoveJ 完成时允许的最大关节位置误差，单位为弧度。
optional double settle_position_tolerance_rad = 3;
// MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
optional double settle_velocity_tolerance_rad_s = 4;
// 位置和速度连续满足条件的采样次数。
optional int32 settle_stable_sample_count = 5;
}
message MoveLPlannerConfig {

1
protos/cmvr/config/joint_limits_config.proto
View File

@ -33,6 +33,7 @@ message JointLimitAvoidanceConfig {
double gain = 2;
double margin_ratio = 3;
double max_push = 4;
double weight = 5;
}
message JointLimitPolicyConfig {

19
protos/cmvr/config/touch_screen_algorithm_config.proto
View File

@ -11,10 +11,17 @@ message TouchScreenApriltagConfig {
optional TouchScreenTargetPointMethod target_point_method = 3;
}
message TouchScreenTaskPbvsConfig {
.cmvr.common.Vec3 position_gain = 1;
.cmvr.common.Vec3 rotation_gain = 2;
.cmvr.common.Vec6 vmax6 = 3;
.cmvr.common.Vec6 amax6 = 4;
optional double twist_filter_alpha = 5;
message TouchScreenIbvsConfig {
optional string camera_link = 1;
optional double lambda = 2;
optional double mu = 3;
optional double qdot_max = 4;
.cmvr.common.Vec6 vmax6 = 5;
.cmvr.common.Vec6 amax6 = 6;
optional double twist_filter_alpha = 7;
.cmvr.common.Mat3 r_camera_to_visp = 12;
.cmvr.common.Mat3 r_camera_to_urdf = 13;
repeated string control_joint_names = 14;
}

59
protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto
View File

@ -15,10 +15,9 @@ message TouchScreenTaskDevicesConfig {
reserved 1;
reserved "robot_id";
optional string camera_id = 3;
optional string arm_id = 4;
optional string external_camera_id = 5;
optional string dexhand_id = 2;
optional string camera_id = 3;
}
message TouchScreenTaskInitializationConfig {
@ -27,66 +26,27 @@ message TouchScreenTaskInitializationConfig {
repeated TouchScreenInitJointPoint joint_positions = 3;
optional double velocity = 4;
optional double acceleration = 5;
// 当前关节位置误差小于该值时可跳过初始化 MoveJ，单位为弧度。
optional double skip_position_tolerance_rad = 6;
// 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
optional double skip_velocity_tolerance_rad_s = 7;
}
message TouchScreenTaskTagConfig {
optional int32 id = 1;
optional double size_m = 2;
}
message TouchScreenTaskTagsConfig {
TouchScreenTaskTagConfig screen = 1;
TouchScreenTaskTagConfig hand = 2;
}
message TouchScreenTaskHandCameraConfig {
reserved 1;
reserved "camera_id";
optional TouchScreenDepthPolicy depth_policy = 2;
optional TouchScreenTargetPointMethod target_point_method = 3;
}
message TouchScreenTaskPerceptionConfig {
reserved 1, 2, 6;
reserved "apriltag", "external_apriltag", "external_camera";
TouchScreenTaskTagsConfig tags = 4;
TouchScreenTaskHandCameraConfig hand_camera = 5;
TouchScreenApriltagConfig apriltag = 1;
}
message TouchScreenAlignmentTargetConfig {
reserved 2;
reserved "rotation_vector";
// Target orientation of Hand Tag H relative to Screen Tag G.
// rx, ry and rz are fixed-axis (extrinsic) rotations about G.X, G.Y and G.Z,
// applied in that order, in radians.
.cmvr.common.Euler hand_orientation_G = 6;
.cmvr.common.Vec3 position_in_camera = 1;
.cmvr.common.Vec3 rotation_vector = 2;
optional TouchScreenAlignMode mode = 3;
.cmvr.common.Vec3 position_offset_G = 4;
.cmvr.common.Vec3 rotation_offset_G = 5;
}
message TouchScreenTaskAlignmentCalibrationConfig {
.cmvr.common.Mat4 hand_tag_to_tcp = 1;
reserved 2;
reserved "external_camera_to_base";
}
message TouchScreenTaskAlignmentConfig {
reserved 1;
reserved "kinematics";
TouchScreenIbvsConfig ibvs = 2;
TouchScreenAlignmentTargetConfig target = 3;
.cmvr.common.Vec6 error_threshold = 4;
optional int32 stable_frames = 5;
optional double timeout_s = 6;
optional bool pause_when_reached = 7;
TouchScreenTaskPbvsConfig pbvs = 8;
TouchScreenTaskAlignmentCalibrationConfig calibration = 9;
}
message TouchScreenTouchSpeedLConfig {
@ -124,10 +84,7 @@ message TouchScreenTaskTouchConfig {
message TouchScreenTaskRetractConfig {
.cmvr.common.Vec6 twist_tool = 1;
optional double acceleration = 2;
reserved 3;
reserved "duration_s";
// Distance traveled by the TCP before the retract motion stops, in meters.
optional double distance_m = 4;
optional double duration_s = 3;
}
message TouchScreenTaskConfig {
@ -138,10 +95,6 @@ message TouchScreenTaskConfig {
TouchScreenTaskTouchConfig touch = 5;
TouchScreenTaskRetractConfig retract = 6;
optional string id = 7;
// 是否在外部相机编码后的 gRPC 视频流中绘制坐标系，仅影响显示帧；未配置时默认开启。
optional bool debug_draw_coordinate_frames = 8;
// G/H 坐标轴长度，单位为米。
optional double debug_coordinate_axis_length_m = 9;
}
message TouchScreenTaskRootConfig {