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

feat:add pbvs touch

Browse Source
This commit is contained in:
lgv 2026-09-15 10:42:56 +08:00
parent 6bfe01b854
commit 964b0457ca
32 changed files with 1256 additions and 196 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
MUJOCO_LOG.TXT
View File

@ -4,3 +4,9 @@ ERROR: could not create window
Fri Jul 24 15:40:37 2026 Fri Jul 24 15:40:37 2026
ERROR: could not create window 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

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

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

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

@ -29,6 +29,9 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController:
if (config.stop_acceleration <= 0.0) { if (config.stop_acceleration <= 0.0) {
config.stop_acceleration = defaults.stop_acceleration; 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; return config;
} }
@ -108,6 +111,12 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
if (!worker_ || !worker_->joinable()) { if (!worker_ || !worker_->joinable()) {
return Result::success(); 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); requestStop_(acceleration);
return Result::success(); return Result::success();
} }

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

@ -26,3 +26,14 @@ target_link_libraries(ik_solver PUBLIC
add_library(cmvr_es::ik_solver ALIAS ik_solver) add_library(cmvr_es::ik_solver ALIAS ik_solver)
install(TARGETS ik_solver LIBRARY DESTINATION lib) 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
)

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

@ -45,6 +45,11 @@ public:
std::vector<double>& qdot_out, std::vector<double>& qdot_out,
double qdot_abs_max = std::numeric_limits<double>::infinity()) const override; 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 setVelocityLimits(const Eigen::VectorXd &qd_max);
void setAccelerationLimits(const Eigen::VectorXd &qdd_max); void setAccelerationLimits(const Eigen::VectorXd &qdd_max);

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

@ -164,7 +164,9 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
const double margin_ratio = positiveOr(config.margin_ratio(), 0.08); const double margin_ratio = positiveOr(config.margin_ratio(), 0.08);
const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02); const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02);
Eigen::VectorXd limited = qdot; // 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;
for (Eigen::Index i = 0; i < q_chain.size(); ++i) { for (Eigen::Index i = 0; i < q_chain.size(); ++i) {
const double lower = joint_pos_lower_limits_[i]; const double lower = joint_pos_lower_limits_[i];
const double upper = joint_pos_upper_limits_[i]; const double upper = joint_pos_upper_limits_[i];
@ -174,21 +176,15 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
const double span = upper - lower; const double span = upper - lower;
const double margin = std::max(min_margin_rad, margin_ratio * span); const double margin = std::max(min_margin_rad, margin_ratio * span);
if (limited[i] < 0.0 && q_chain[i] < lower + margin) { if (qdot[i] < 0.0 && q_chain[i] < lower + margin) {
const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0); const double ratio = std::clamp((q_chain[i] - lower) / margin, 0.0, 1.0);
limited[i] *= ratio; scale = std::min(scale, ratio);
if (q_chain[i] <= lower) { } else if (qdot[i] > 0.0 && q_chain[i] > upper - margin) {
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); const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0);
limited[i] *= ratio; scale = std::min(scale, ratio);
if (q_chain[i] >= upper) {
limited[i] = std::min(0.0, limited[i]);
}
} }
} }
return limited; return scale * qdot;
} }
void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) { 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,11 +13,66 @@
#include <pinocchio/spatial/explog.hpp> #include <pinocchio/spatial/explog.hpp>
#include <algorithm> // std::clamp, std::max, std::min #include <algorithm> // std::clamp, std::max, std::min
#include <atomic>
#include <cmath> // std::sqrt #include <cmath> // std::sqrt
#include <cstdint>
#include <limits> #include <limits>
#include <sstream>
#include <unordered_map> #include <unordered_map>
#include <Eigen/SVD>
namespace cmvr { 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::Matrix4d;
using Eigen::VectorXd; using Eigen::VectorXd;
using Eigen::MatrixXd; using Eigen::MatrixXd;
@ -208,6 +263,24 @@ 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, bool PinocchioQpIKSolver::ik(const Matrix4d &target_pose,
std::vector<double> &joints_angle, std::vector<double> &joints_angle,
bool is_tcp) { bool is_tcp) {
@ -389,7 +462,7 @@ namespace cmvr {
const int dof = chain_v_dof_; const int dof = chain_v_dof_;
const auto& avoidance = jointLimitPolicy().avoidance(); const auto& avoidance = jointLimitPolicy().avoidance();
const bool use_joint_limit_avoidance = const bool use_joint_limit_avoidance =
!jointLimitsDisabled() && avoidance.enable() && avoidance.weight() > 0.0; !jointLimitsDisabled() && avoidance.enable() && avoidance.gain() > 0.0;
const int avoidance_rows = use_joint_limit_avoidance ? dof : 0; const int avoidance_rows = use_joint_limit_avoidance ? dof : 0;
MatrixXd cost(6 + dof + avoidance_rows, dof); MatrixXd cost(6 + dof + avoidance_rows, dof);
@ -406,7 +479,7 @@ namespace cmvr {
VectorXd upper(dof); VectorXd upper(dof);
const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof); const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof);
if (use_joint_limit_avoidance) { if (use_joint_limit_avoidance) {
const VectorXd qdot_avoid = const VectorXd qdot_avoid_raw =
cmvr::kinematics::computeJointLimitAvoidanceVelocity( cmvr::kinematics::computeJointLimitAvoidanceVelocity(
q_chain, q_chain,
joint_pos_lower_limits_, joint_pos_lower_limits_,
@ -415,10 +488,32 @@ namespace cmvr {
positiveOr(avoidance.gain(), 0.2), positiveOr(avoidance.gain(), 0.2),
positiveOr(avoidance.margin_ratio(), 0.15), positiveOr(avoidance.margin_ratio(), 0.15),
positiveOr(avoidance.max_push(), 0.25)); positiveOr(avoidance.max_push(), 0.25));
const double sqrt_weight = std::sqrt(positiveOr(avoidance.weight(), 0.05)); 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;
cost.middleRows(6 + dof, dof) = cost.middleRows(6 + dof, dof) =
sqrt_weight * MatrixXd::Identity(dof, dof); nullspace;
target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid; 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();
}
} }
for (int i = 0; i < dof; ++i) { for (int i = 0; i < dof; ++i) {
double limit = std::numeric_limits<double>::infinity(); double limit = std::numeric_limits<double>::infinity();
@ -452,6 +547,35 @@ 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; QPSolver solver;
@ -475,7 +599,6 @@ namespace cmvr {
if (qdot.size() != dof) { if (qdot.size() != dof) {
return false; return false;
} }
qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
qdot_out.assign(qdot.data(), qdot.data() + qdot.size()); qdot_out.assign(qdot.data(), qdot.data() + qdot.size());
return true; return true;
} }

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

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

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

@ -84,6 +84,10 @@ public:
// Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。 // Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()}; 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 { struct FrameCache {

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

@ -27,6 +27,26 @@ inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src)
return toEigenVec3(src, Eigen::Vector3d::Zero()); 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( inline Eigen::Matrix<double, 6, 1> toEigenVec6(
const cmvr::common::Vec6& src, const cmvr::common::Vec6& src,
Eigen::Matrix<double, 6, 1> defaults) Eigen::Matrix<double, 6, 1> defaults)
@ -102,6 +122,10 @@ inline bool hasVec3(const cmvr::common::Vec3& value) {
return value.has_x() && value.has_y() && value.has_z(); 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) { inline bool hasVec6(const cmvr::common::Vec6& value) {
return value.has_x() && value.has_y() && value.has_z() && return value.has_x() && value.has_y() && value.has_z() &&
value.has_rx() && value.has_ry() && value.has_rz(); value.has_rx() && value.has_ry() && value.has_rz();

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

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

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

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

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

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

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

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

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

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

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

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

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

@ -1,9 +1,14 @@
touch_screen_task { touch_screen_task {
id: "touch_screen" id: "touch_screen"
# 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
debug_draw_coordinate_frames: true
# G/H 坐标轴长度，单位为米。
debug_coordinate_axis_length_m: 0.02
devices { devices {
arm_id: "mujoco_right_arm" arm_id: "mujoco_right_arm"
dexhand_id: "mujoco_zero_touch_dexhand" dexhand_id: "mujoco_zero_touch_dexhand"
# 手部相机和外部相机的 DeviceManager ID。
camera_id: "mujoco_hand_cam" camera_id: "mujoco_hand_cam"
external_camera_id: "mujoco_external_touch_cam" external_camera_id: "mujoco_external_touch_cam"
} }
@ -18,30 +23,44 @@ touch_screen_task {
joint_positions { joint_name: "R_WRIST_P" rad: -2.8792 } 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_Y" rad: 0.1150 }
joint_positions { joint_name: "R_WRIST_R" rad: -0.08 } joint_positions { joint_name: "R_WRIST_R" rad: -0.08 }
velocity: 2.8 velocity: 2.0
acceleration: 20.0 acceleration: 3.0
# 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
skip_position_tolerance_rad: 0.001
# 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
skip_velocity_tolerance_rad_s: 0.01
} }
perception { perception {
apriltag { # 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
tag_size_m: 0.03 tags {
screen {
id: 1
size_m: 0.03
}
hand {
id: 0
size_m: 0.03
}
}
# 手部相机：用于点击目标点和手部目标跟踪。
hand_camera {
depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
} }
external_apriltag { }
tag_size_m: 0.03
hand_tag_id: 0 alignment {
# Hand Tag H -> TCP P after flipping the tag to face external_touch_cam. calibration {
t_h_p { # TCP P 相对于屏幕 Hand Tag H 的目标姿态
hand_tag_to_tcp {
m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0 m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
m10: 0.0 m11: 0.0 m12: -1.0 m13: 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 m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0 m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
} }
} }
}
alignment {
pbvs { pbvs {
position_gain { x: 2.0 y: 2.0 z: 1.5 } position_gain { x: 2.0 y: 2.0 z: 1.5 }
rotation_gain { x: 1.5 y: 1.5 z: 1.5 } rotation_gain { x: 1.5 y: 1.5 z: 1.5 }
@ -50,29 +69,27 @@ touch_screen_task {
twist_filter_alpha: 1.0 twist_filter_alpha: 1.0
} }
target { target {
rotation_vector { # Hand Tag H 相对于屏幕 Tag G 的目标姿态，单位为弧度。
x: 3.141592653589793 # rx、ry、rz 表示绕固定 G 坐标轴 X、Y、Z 依次旋转。
y: 0.0 # 旋转组合为 R_G_H = Rz(rz) * Ry(ry) * Rx(rx)。
z: 0.0 # PBVS 会结合上面的 T_H_P 将该目标转换为 TCP P 的目标姿态。
hand_orientation_G {
rx: 0.0
ry: 0.0
rz: 3.141592653589793
} }
# 点击目标点到 TCP 预对齐位置的偏移，表达在 G 坐标系，单位为米。
position_offset_G { position_offset_G {
x: 0.0 x: 0.0
y: 0.0 y: 0.0
z: 0.05 z: 0.05
} }
rotation_offset_G { mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
x: 0.0
y: 0.0
z: 0.0
}
mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
} }
error_threshold { error_threshold {
x: 0.005 x: 0.005
y: 0.005 y: 0.005
z: 0.010 z: 0.005
rx: 0.08726646259971647 rx: 0.08726646259971647
ry: 0.08726646259971647 ry: 0.08726646259971647
rz: 0.08726646259971647 rz: 0.08726646259971647
@ -99,7 +116,8 @@ touch_screen_task {
retract { retract {
twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
acceleration: 5.0 acceleration: 4.0
duration_s: 2.0 # TCP 后退目标距离，单位为米。
distance_m: 0.05
} }
} }

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

@ -106,6 +106,8 @@ private:
std::shared_ptr<AbstractMotor> getMotor_(const std::string& joint_name) const; std::shared_ptr<AbstractMotor> getMotor_(const std::string& joint_name) const;
bool readArmState_(std::vector<double>& q_now, std::vector<double>& qd_now) const; bool readArmState_(std::vector<double>& q_now, std::vector<double>& qd_now) const;
std::vector<double> readJointPosition_() const; std::vector<double> readJointPosition_() const;
Result stopCartesianMotionAndWait_();
Result waitForJointTarget_(const std::vector<double>& target) const;
bool configureAlgorithms_(); bool configureAlgorithms_();
bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory); bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory);
@ -130,6 +132,13 @@ private:
std::shared_ptr<CartesianMotionPlanner> cartesian_planner_{nullptr}; std::shared_ptr<CartesianMotionPlanner> cartesian_planner_{nullptr};
std::unique_ptr<CartesianVelocityController> cartesian_velocity_controller_{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_; mutable std::mutex mutex_;
std::atomic<bool> busy_{false}; std::atomic<bool> busy_{false};
double speed_scaling_{1.0}; double speed_scaling_{1.0};

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

@ -516,6 +516,16 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
if (!joint_planner_) { if (!joint_planner_) {
return Result::failure(ArmErrorCode::RobotNotReady, "joint planner is not initialized"); 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)) { if (busy_.exchange(true)) {
return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_); return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_);
} }
@ -559,9 +569,14 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch"); return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
} }
std::fill(command_velocity.begin(), command_velocity.end(), 0.0); std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
std::copy_n(sample.velocity.begin(), // A MoveJ is a rest-to-rest command. Do not let a non-zero numerical
std::min(sample.velocity.size(), command_velocity.size()), // endpoint velocity from an alternate planner keep the drive moving
command_velocity.begin()); // while the position-mode trajectory is being handed back to the arm.
if (k + 1 < samples.size()) {
std::copy_n(sample.velocity.begin(),
std::min(sample.velocity.size(), command_velocity.size()),
command_velocity.begin());
}
if (!motor_manager_->commandCyclicPositionsAtomic( if (!motor_manager_->commandCyclicPositionsAtomic(
motors, sample.position, command_velocity)) { motors, sample.position, command_velocity)) {
return Result::failure(ArmErrorCode::CommandFailed, return Result::failure(ArmErrorCode::CommandFailed,
@ -575,6 +590,24 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
std::chrono::duration<double>(next_t))); 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(); return Result::success();
} }
@ -638,8 +671,9 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
if (const auto stopped = safetyStopResult_("moveL")) { if (const auto stopped = safetyStopResult_("moveL")) {
return *stopped; return *stopped;
} }
if (cartesian_velocity_controller_) { const auto cartesian_stop = stopCartesianMotionAndWait_();
cartesian_velocity_controller_->shutdown(); if (!cartesian_stop.ok()) {
return cartesian_stop;
} }
if (options.asynchronous) { if (options.asynchronous) {
return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported"); return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported");
@ -715,7 +749,10 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
Result MotorRobotArm::stopMotion() Result MotorRobotArm::stopMotion()
{ {
stopL(0.0); const auto cartesian_stop = stopCartesianMotionAndWait_();
if (!cartesian_stop.ok()) {
return cartesian_stop;
}
return stopJ(0.0); return stopJ(0.0);
} }
@ -963,8 +1000,142 @@ std::vector<double> MotorRobotArm::readJointPosition_() const
return q_start; 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_() 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()); joint_planner_ = JointMotionPlannerFactory::create(cfg_.motion().move_j());
if (!joint_planner_) { if (!joint_planner_) {
return false; return false;
@ -1077,6 +1248,10 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController
result.stop_acceleration = result.stop_acceleration =
config.stop_acceleration() > 0.0 ? config.stop_acceleration() config.stop_acceleration() > 0.0 ? config.stop_acceleration()
: result.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; return result;
} }

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

@ -3,20 +3,22 @@
#pragma once #pragma once
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include <mutex>
#include "../abstract_device.h" #include "../abstract_device.h"
#include <Eigen/Core> #include <Eigen/Core>
#include "cmvr/config/camera_config/camera_config.pb.h" #include "cmvr/config/camera_config/camera_config.pb.h"
#include "devices/camera/common/include/camera_stream_overlay.h"
namespace cmvr::device { namespace cmvr::device {
enum CameraMode {PHOTO_MODE, VIDEO_MODE}; enum CameraMode {PHOTO_MODE, VIDEO_MODE};
struct Rs2Intrinsics struct Rs2Intrinsics
{ {
float cx; float cx{0.0F};
float cy; float cy{0.0F};
float fx; float fx{0.0F};
float fy; float fy{0.0F};
float coeffs[5]; float coeffs[5]{};
}; };
struct StreamFrameData struct StreamFrameData
@ -64,11 +66,26 @@ namespace cmvr::device {
return false; 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 bool startStreaming() {return true;}
virtual void stopStreaming() {} virtual void stopStreaming() {}
virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};} virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
protected: protected:
CameraState state_{}; CameraState state_{};
mutable std::mutex stream_overlay_mutex_;
CameraStreamOverlay stream_overlay_{};
void clear_error_() { void clear_error_() {
this->state_.is_error = false; this->state_.is_error = false;
this->state_.error_message.clear(); this->state_.error_message.clear();

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

@ -7,9 +7,12 @@
#include <opencv2/opencv.hpp> #include <opencv2/opencv.hpp>
#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h" #include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
#include "devices/camera/common/include/camera_stream_overlay.h"
namespace cmvr::device { namespace cmvr::device {
struct Rs2Intrinsics;
struct FfmpegEncoderInfo { struct FfmpegEncoderInfo {
std::string codec_name; std::string codec_name;
int width = 0; int width = 0;
@ -27,8 +30,23 @@ struct FfmpegEncoderInfo {
struct CameraStreamEncodeOptions { struct CameraStreamEncodeOptions {
bool draw_timestamp = false; 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 { class CameraStreamEncoder {
public: public:
static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder, static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
@ -37,6 +55,14 @@ public:
int height, int height,
int fps); 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, static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const cv::Mat& frame, const cv::Mat& frame,
std::vector<uint8_t>& encoded_frame, std::vector<uint8_t>& encoded_frame,

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

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

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

@ -1,6 +1,7 @@
#include "devices/camera/common/include/camera_stream_encoder.h" #include "devices/camera/common/include/camera_stream_encoder.h"
#include <chrono> #include <chrono>
#include <cmath>
#include <ctime> #include <ctime>
#include <iomanip> #include <iomanip>
#include <sstream> #include <sstream>
@ -9,6 +10,7 @@
#include <opencv2/imgproc.hpp> #include <opencv2/imgproc.hpp>
#include "common/base/logging/logger.h" #include "common/base/logging/logger.h"
#include "devices/camera/abstract_camera.h"
namespace cmvr::device { namespace cmvr::device {
namespace { namespace {
@ -46,6 +48,41 @@ void drawTimeStamp(cv::Mat& image)
cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness); 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) const AVCodec* findEncoder(const std::string& codec_name)
{ {
if (codec_name == "h264" || codec_name == "H264") { if (codec_name == "h264" || codec_name == "H264") {
@ -75,6 +112,77 @@ AVPixelFormat sourcePixelFormat(const cv::Mat& frame)
} // namespace } // 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() FfmpegEncoderInfo::~FfmpegEncoderInfo()
{ {
if (frame) { if (frame) {
@ -189,6 +297,7 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
const cv::Mat& frame, const cv::Mat& frame,
std::vector<uint8_t>& encoded_frame, std::vector<uint8_t>& encoded_frame,
bool& is_key, bool& is_key,
const Rs2Intrinsics& intrinsics,
const CameraStreamEncodeOptions& options) const CameraStreamEncodeOptions& options)
{ {
encoded_frame.clear(); encoded_frame.clear();
@ -205,9 +314,27 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
} }
cv::Mat frame_to_encode = frame; cv::Mat frame_to_encode = frame;
if (options.draw_timestamp) { if (options.draw_timestamp || options.overlay.draw_coordinate_frames) {
frame_to_encode = frame.clone(); frame_to_encode = frame.clone();
drawTimeStamp(frame_to_encode); if (options.draw_timestamp) {
drawTimeStamp(frame_to_encode);
}
if (options.overlay.draw_coordinate_frames) {
for (const auto& coordinate_frame : options.overlay.coordinate_frames) {
if (!coordinate_frame.valid) {
continue;
}
std::string label = coordinate_frame.label;
if (coordinate_frame.tag_id >= 0) {
label += " #" + std::to_string(coordinate_frame.tag_id);
}
drawCoordinateFrame(frame_to_encode,
coordinate_frame.T_C_Frame,
intrinsics,
options.overlay.coordinate_axis_length_m,
label);
}
}
} }
const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode); const AVPixelFormat src_pix_fmt = sourcePixelFormat(frame_to_encode);
@ -294,4 +421,14 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
return true; 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 } // namespace cmvr::device

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

@ -345,12 +345,17 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
} }
frame_data.rgbImage = color.clone(); frame_data.rgbImage = color.clone();
frame_data.intrinsics = intrinsics;
CameraStreamEncodeOptions encode_options; CameraStreamEncodeOptions encode_options;
encode_options.draw_timestamp = enable_stream_timestamp_; 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_, if (!CameraStreamEncoder::encode(rgb_encoder_,
color_to_encode, color_to_encode,
frame_data.rgbFrame, frame_data.rgbFrame,
frame_data.bKey, frame_data.bKey,
encode_intrinsics,
encode_options)) { encode_options)) {
return false; return false;
} }
@ -360,7 +365,6 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
const auto* depth_end = depth_begin + depth.total() * depth.elemSize(); const auto* depth_end = depth_begin + depth.total() * depth.elemSize();
frame_data.depthFrame.assign(depth_begin, depth_end); frame_data.depthFrame.assign(depth_begin, depth_end);
} }
frame_data.intrinsics = intrinsics;
frame_data.width = color_to_encode.cols; frame_data.width = color_to_encode.cols;
frame_data.height = color_to_encode.rows; frame_data.height = color_to_encode.rows;
frame_data.fps = fps; frame_data.fps = fps;

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

@ -772,10 +772,18 @@ void RealsenseCamera::streaming_worker_() {
} }
CameraStreamEncodeOptions encode_options; CameraStreamEncodeOptions encode_options;
encode_options.draw_timestamp = enable_stream_timestamp_; 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_, success = CameraStreamEncoder::encode(rgbEncoder_,
rgb_to_encode, rgb_to_encode,
frame_data.rgbFrame, frame_data.rgbFrame,
frame_data.bKey, frame_data.bKey,
encode_intrinsics,
encode_options); encode_options);
// 深度图编码 // 深度图编码
// success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey); // success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);

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

@ -504,10 +504,18 @@ void UVCCamera::streaming_worker_() {
} }
CameraStreamEncodeOptions encode_options; CameraStreamEncodeOptions encode_options;
encode_options.draw_timestamp = enable_stream_timestamp_; 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_, success = CameraStreamEncoder::encode(rgbEncoder_,
rgb_to_encode, rgb_to_encode,
frame_data.rgbFrame, frame_data.rgbFrame,
frame_data.bKey, frame_data.bKey,
encode_intrinsics,
encode_options); encode_options);
if (success) { if (success) {
frame_data.fps = fps_; frame_data.fps = fps_;

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

@ -121,6 +121,7 @@ private:
void stopPbvsMotion(); void stopPbvsMotion();
bool buildInitJointPositions(std::vector<double>& positions_out) const; bool buildInitJointPositions(std::vector<double>& positions_out) const;
bool isAtInitPosition(const std::vector<double>& positions) const;
bool moveToInitPositionBeforeStartIfEnabled(); bool moveToInitPositionBeforeStartIfEnabled();
bool moveToInitPositionIfEnabled() const; bool moveToInitPositionIfEnabled() const;
bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const; bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
@ -132,6 +133,8 @@ private:
void enterFailed(Status status); void enterFailed(Status status);
bool updateTouchPressure(); bool updateTouchPressure();
void publishCoordinateOverlay();
void refreshCoordinateOverlay();
private: private:
mutable std::mutex mutex_; mutable std::mutex mutex_;
@ -171,6 +174,10 @@ private:
Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()}; Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()}; Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
double pbvs_command_acceleration_{0.25}; 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};
bool locked_target_rotation_valid_{false}; bool locked_target_rotation_valid_{false};
Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()}; Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
bool touch_start_position_valid_{false}; bool touch_start_position_valid_{false};
@ -183,6 +190,7 @@ private:
double max_T_B_G_rotation_delta_rad_{0.0}; double max_T_B_G_rotation_delta_rad_{0.0};
Clock::time_point phase_start_time_{}; Clock::time_point phase_start_time_{};
Clock::time_point last_coordinate_overlay_update_time_{};
Clock::time_point last_retract_log_time_{}; Clock::time_point last_retract_log_time_{};
Status final_status_after_retract_{Status::DONE}; Status final_status_after_retract_{Status::DONE};
}; };

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

@ -6,6 +6,7 @@
#include <limits> #include <limits>
#include <sstream> #include <sstream>
#include <unordered_map> #include <unordered_map>
#include <utility>
#include <opencv2/imgcodecs.hpp> #include <opencv2/imgcodecs.hpp>
@ -20,6 +21,9 @@ namespace cmvr::task {
namespace { namespace {
// The initialization move is a safety repositioning step. Avoid replanning
// an already settled arm, while still requiring a stopped joint velocity so a
// moving arm is never mistaken for being at the initial pose.
device::CartesianVelocity toCartesianVelocity(const Eigen::Matrix<double, 6, 1>& twist) { device::CartesianVelocity toCartesianVelocity(const Eigen::Matrix<double, 6, 1>& twist) {
return {twist[0], twist[1], twist[2], twist[3], twist[4], twist[5]}; return {twist[0], twist[1], twist[2], twist[3], twist[4], twist[5]};
} }
@ -123,15 +127,19 @@ double tactileForceValue(const device::AbstractDexHand::TactilePoint& point,
return static_cast<double>(point.fz); return static_cast<double>(point.fz);
} }
Eigen::Matrix3d rotationFromTargetRotvec(const double rx, Eigen::Matrix3d rotationFromTargetEuler(const double rx,
const double ry, const double ry,
const double rz) { const double rz) {
const Eigen::Vector3d rotation_vector(rx, ry, rz); // The configured rotations are extrinsic rotations about the fixed G axes,
const double angle = rotation_vector.norm(); // applied in X -> Y -> Z order. With column-vector transforms this is
if (!std::isfinite(angle) || angle <= 1e-12) { // represented by left multiplication in reverse order.
return Eigen::Matrix3d::Identity(); const Eigen::Matrix3d R_x =
} Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()).toRotationMatrix();
return Eigen::AngleAxisd(angle, rotation_vector / angle).toRotationMatrix(); const Eigen::Matrix3d R_y =
Eigen::AngleAxisd(ry, Eigen::Vector3d::UnitY()).toRotationMatrix();
const Eigen::Matrix3d R_z =
Eigen::AngleAxisd(rz, Eigen::Vector3d::UnitZ()).toRotationMatrix();
return R_z * R_y * R_x;
} }
bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target, bool extractProjectedYawAboutTargetNormal(const Eigen::Matrix3d& R_target,
@ -282,10 +290,14 @@ bool TouchScreenTask::init() {
} }
const auto& devices = config_.devices(); const auto& devices = config_.devices();
const auto& perception_config = config_.perception();
if (!devices.has_arm_id() || devices.arm_id().empty() || if (!devices.has_arm_id() || devices.arm_id().empty() ||
!devices.has_dexhand_id() || devices.dexhand_id().empty() ||
!devices.has_camera_id() || devices.camera_id().empty() || !devices.has_camera_id() || devices.camera_id().empty() ||
!devices.has_external_camera_id() || devices.external_camera_id().empty() || !devices.has_external_camera_id() || devices.external_camera_id().empty() ||
!devices.has_dexhand_id() || devices.dexhand_id().empty()) { !perception_config.has_hand_camera() ||
!perception_config.hand_camera().has_depth_policy() ||
!perception_config.hand_camera().has_target_point_method()) {
last_status_ = Status::INVALID_CONFIG; last_status_ = Status::INVALID_CONFIG;
return false; return false;
} }
@ -293,6 +305,7 @@ bool TouchScreenTask::init() {
auto& dm = device::DeviceManager::getInstance(); auto& dm = device::DeviceManager::getInstance();
auto arm = dm.getDevice<device::RobotArm>(devices.arm_id()); auto arm = dm.getDevice<device::RobotArm>(devices.arm_id());
auto dexhand = dm.getDevice<device::AbstractDexHand>(devices.dexhand_id()); auto dexhand = dm.getDevice<device::AbstractDexHand>(devices.dexhand_id());
const auto& hand_camera_config = perception_config.hand_camera();
auto camera = dm.getDevice<device::AbstractCamera>(devices.camera_id()); auto camera = dm.getDevice<device::AbstractCamera>(devices.camera_id());
auto external_camera = auto external_camera =
dm.getDevice<device::AbstractCamera>(devices.external_camera_id()); dm.getDevice<device::AbstractCamera>(devices.external_camera_id());
@ -344,23 +357,30 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
return false; return false;
} }
const auto& perception_config = config_.perception();
const auto& tags = perception_config.tags();
const auto& hand_camera_config = perception_config.hand_camera();
perception_ = std::make_shared<perception::AprilTagPerception>(camera_); perception_ = std::make_shared<perception::AprilTagPerception>(camera_);
perception_->setTagSize(config_.perception().apriltag().tag_size_m()); perception_->setTagSize(tags.screen().size_m());
external_perception_ = external_perception_ =
std::make_shared<perception::AprilTagPerception>(external_camera_); std::make_shared<perception::AprilTagPerception>(external_camera_);
external_perception_->setTagSize( external_perception_->setTagSize(tags.screen().size_m());
config_.perception().external_apriltag().tag_size_m());
tracker_.setPerception(perception_); tracker_.setPerception(perception_);
tracker_.setTargetPointMethod(toTargetPointMethod( tracker_.setTargetPointMethod(toTargetPointMethod(
config_.perception().apriltag().target_point_method())); hand_camera_config.target_point_method()));
tcp_pose_tracker_.setPerception(external_perception_); tcp_pose_tracker_.setPerception(external_perception_);
tcp_pose_tracker_.setHandTagId( tcp_pose_tracker_.setScreenTagId(tags.screen().id());
config_.perception().external_apriltag().hand_tag_id()); tcp_pose_tracker_.setHandTagId(tags.hand().id());
// Publish the presentation settings immediately so a gRPC stream started
// before the first periodic task tick uses the configured behavior.
publishCoordinateOverlay();
initialized_ = applyConfig(); initialized_ = applyConfig();
if (initialized_) { if (initialized_) {
refreshCoordinateOverlay();
tracker_.clear(); tracker_.clear();
tracker_.resetActiveTagTracking(); tracker_.resetActiveTagTracking();
tcp_pose_tracker_.clear(); tcp_pose_tracker_.clear();
@ -385,6 +405,7 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
last_T_B_G_.setIdentity(); last_T_B_G_.setIdentity();
max_T_B_G_translation_delta_m_ = 0.0; max_T_B_G_translation_delta_m_ = 0.0;
max_T_B_G_rotation_delta_rad_ = 0.0; max_T_B_G_rotation_delta_rad_ = 0.0;
last_coordinate_overlay_update_time_ = Clock::time_point{};
last_status_ = Status::IDLE; last_status_ = Status::IDLE;
} else { } else {
last_status_ = Status::INVALID_CONFIG; last_status_ = Status::INVALID_CONFIG;
@ -479,6 +500,13 @@ bool TouchScreenTask::step(const double dt) {
// << std::endl; // << std::endl;
} }
// Keep the presentation snapshot alive outside ALIGNING as well. This
// path only updates the debug video overlay; it is not consumed by PBVS or
// any motion computation.
if (phase_ != Phase::ALIGNING) {
refreshCoordinateOverlay();
}
switch (phase_) { switch (phase_) {
case Phase::IDLE: case Phase::IDLE:
last_status_ = Status::IDLE; last_status_ = Status::IDLE;
@ -515,6 +543,75 @@ bool TouchScreenTask::step(const double dt) {
return false; return false;
} }
void TouchScreenTask::publishCoordinateOverlay()
{
if (!external_camera_) {
return;
}
device::CameraStreamOverlay overlay;
overlay.draw_coordinate_frames =
!config_.has_debug_draw_coordinate_frames() ||
config_.debug_draw_coordinate_frames();
overlay.coordinate_axis_length_m = 0.02;
if (config_.has_debug_coordinate_axis_length_m() &&
std::isfinite(config_.debug_coordinate_axis_length_m()) &&
config_.debug_coordinate_axis_length_m() > 0.0) {
overlay.coordinate_axis_length_m = config_.debug_coordinate_axis_length_m();
}
if (overlay.draw_coordinate_frames && external_perception_) {
const auto& tags = config_.perception().tags();
const int screen_tag_id = tags.screen().id();
const int hand_tag_id = tags.hand().id();
const auto* screen_tag = external_perception_->findTag(screen_tag_id);
if (screen_tag) {
device::CoordinateFrameOverlay frame;
frame.T_C_Frame = screen_tag->T_C_Tag();
frame.label = "G";
frame.tag_id = screen_tag->id;
frame.valid = true;
overlay.coordinate_frames.push_back(std::move(frame));
}
if (const auto* hand_tag = external_perception_->findTag(hand_tag_id)) {
device::CoordinateFrameOverlay frame;
frame.T_C_Frame = hand_tag->T_C_Tag();
frame.label = "H";
frame.tag_id = hand_tag->id;
frame.valid = true;
overlay.coordinate_frames.push_back(std::move(frame));
}
}
external_camera_->setStreamOverlay(overlay);
}
void TouchScreenTask::refreshCoordinateOverlay()
{
if (!external_perception_ || !external_camera_) {
return;
}
if (config_.has_debug_draw_coordinate_frames() &&
!config_.debug_draw_coordinate_frames()) {
return;
}
const auto now = Clock::now();
if (last_coordinate_overlay_update_time_.time_since_epoch().count() != 0 &&
std::chrono::duration<double>(now - last_coordinate_overlay_update_time_).count() <
(1.0 / 30.0)) {
return;
}
last_coordinate_overlay_update_time_ = now;
perception::AprilTagPerception::Options options;
options.depth_policy = perception::AprilTagPerception::DepthPolicy::NONE;
options.detect_tags = true;
options.fetch_encoded = false;
external_perception_->update(options);
publishCoordinateOverlay();
}
void TouchScreenTask::stop() { void TouchScreenTask::stop() {
std::lock_guard<std::mutex> lock(mutex_); std::lock_guard<std::mutex> lock(mutex_);
stopUnlocked(); stopUnlocked();
@ -699,9 +796,10 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
!config.initialization().has_velocity() || !config.initialization().has_velocity() ||
!config.initialization().has_acceleration() || !config.initialization().has_acceleration() ||
!config.has_perception() || !config.has_perception() ||
!config.perception().has_apriltag() || !config.perception().has_tags() ||
!config.perception().has_external_apriltag() || !config.perception().has_hand_camera() ||
!config.has_alignment() || !config.has_alignment() ||
!config.alignment().has_calibration() ||
!config.alignment().has_pbvs() || !config.alignment().has_pbvs() ||
!config.alignment().has_target() || !config.alignment().has_target() ||
!config.alignment().has_error_threshold() || !config.alignment().has_error_threshold() ||
@ -714,28 +812,33 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
!config.has_retract() || !config.has_retract() ||
!config.retract().has_twist_tool() || !config.retract().has_twist_tool() ||
!config.retract().has_acceleration() || !config.retract().has_acceleration() ||
!config.retract().has_duration_s()) { !config.retract().has_distance_m()) {
return false; return false;
} }
const auto& apriltag = config.perception().apriltag(); const auto& perception = config.perception();
const auto& external_apriltag = config.perception().external_apriltag(); const auto& tags = perception.tags();
const auto& hand_tag = tags.hand();
const auto& screen_tag = tags.screen();
const auto& hand_camera = perception.hand_camera();
const auto& alignment = config.alignment(); const auto& alignment = config.alignment();
const auto& calibration = alignment.calibration();
const auto& pbvs = alignment.pbvs(); const auto& pbvs = alignment.pbvs();
const auto& target = alignment.target(); const auto& target = alignment.target();
const auto& touch = config.touch(); const auto& touch = config.touch();
const auto& tactile = touch.tactile(); const auto& tactile = touch.tactile();
const auto& retract = config.retract(); const auto& retract = config.retract();
if (!apriltag.has_tag_size_m() || if (!screen_tag.has_id() ||
!apriltag.has_depth_policy() || !screen_tag.has_size_m() ||
!apriltag.has_target_point_method() || !hand_tag.has_id() ||
!external_apriltag.has_tag_size_m() || !hand_tag.has_size_m() ||
!external_apriltag.has_hand_tag_id() || !hand_camera.has_depth_policy() ||
!external_apriltag.has_t_h_p() || !hand_camera.has_target_point_method() ||
!hasMat4(external_apriltag.t_h_p()) || !calibration.has_hand_tag_to_tcp() ||
!target.has_rotation_vector() || !hasMat4(calibration.hand_tag_to_tcp()) ||
!hasVec3(target.rotation_vector()) || !target.has_hand_orientation_g() ||
!hasEuler(target.hand_orientation_g()) ||
!target.has_mode() || !target.has_mode() ||
!hasVec6(alignment.error_threshold()) || !hasVec6(alignment.error_threshold()) ||
!pbvs.has_position_gain() || !pbvs.has_position_gain() ||
@ -755,8 +858,8 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
return false; return false;
} }
const Eigen::Vector3d target_rotation = const Eigen::Vector3d target_orientation =
cmvr::common::math::toEigenVec3(alignment.target().rotation_vector()); cmvr::common::math::toEigenEuler(alignment.target().hand_orientation_g());
const Eigen::Vector3d position_offset_G = const Eigen::Vector3d position_offset_G =
target.has_position_offset_g() target.has_position_offset_g()
? cmvr::common::math::toEigenVec3(target.position_offset_g()) ? cmvr::common::math::toEigenVec3(target.position_offset_g())
@ -775,14 +878,17 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
cmvr::common::math::toEigenVec6(pbvs.vmax6()); cmvr::common::math::toEigenVec6(pbvs.vmax6());
const Eigen::Matrix<double, 6, 1> amax6 = const Eigen::Matrix<double, 6, 1> amax6 =
cmvr::common::math::toEigenVec6(pbvs.amax6()); cmvr::common::math::toEigenVec6(pbvs.amax6());
const Eigen::Matrix4d T_H_P = toEigenMat4(external_apriltag.t_h_p()); const Eigen::Matrix4d T_H_P = toEigenMat4(calibration.hand_tag_to_tcp());
if (!std::isfinite(apriltag.tag_size_m()) || apriltag.tag_size_m() <= 0.0 || if (screen_tag.id() < 0 || hand_tag.id() < 0 ||
!std::isfinite(external_apriltag.tag_size_m()) || screen_tag.id() == hand_tag.id() ||
external_apriltag.tag_size_m() <= 0.0 || !std::isfinite(screen_tag.size_m()) || screen_tag.size_m() <= 0.0 ||
external_apriltag.hand_tag_id() < 0 || !std::isfinite(hand_tag.size_m()) || hand_tag.size_m() <= 0.0 ||
std::abs(screen_tag.size_m() - hand_tag.size_m()) > 1e-12 ||
config.devices().camera_id().empty() ||
config.devices().external_camera_id().empty() ||
!isHomogeneousTransform(T_H_P) || !isHomogeneousTransform(T_H_P) ||
!target_rotation.allFinite() || !target_orientation.allFinite() ||
!position_offset_G.allFinite() || !position_offset_G.allFinite() ||
!rotation_offset_G.allFinite() || !rotation_offset_G.allFinite() ||
!position_gain.allFinite() || !position_gain.allFinite() ||
@ -797,7 +903,7 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
!std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 || !std::isfinite(touch.dwell_time_s()) || touch.dwell_time_s() < 0.0 ||
!cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() || !cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() ||
!std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 || !std::isfinite(retract.acceleration()) || retract.acceleration() <= 0.0 ||
!std::isfinite(retract.duration_s()) || retract.duration_s() < 0.0) { !std::isfinite(retract.distance_m()) || retract.distance_m() <= 0.0) {
return false; return false;
} }
for (int i = 0; i < 3; ++i) { for (int i = 0; i < 3; ++i) {
@ -834,6 +940,18 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
} }
} }
const auto& initialization = config.initialization();
if (initialization.has_skip_position_tolerance_rad() &&
(!std::isfinite(initialization.skip_position_tolerance_rad()) ||
initialization.skip_position_tolerance_rad() < 0.0)) {
return false;
}
if (initialization.has_skip_velocity_tolerance_rad_s() &&
(!std::isfinite(initialization.skip_velocity_tolerance_rad_s()) ||
initialization.skip_velocity_tolerance_rad_s() < 0.0)) {
return false;
}
std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions; std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
if (!appendRequestedTactileRegions(toFingerType(tactile.finger()), if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
toTactileRegion(tactile.region()), toTactileRegion(tactile.region()),
@ -899,6 +1017,15 @@ bool TouchScreenTask::applyConfig() {
return false; return false;
} }
init_skip_position_tolerance_rad_ =
config_.initialization().has_skip_position_tolerance_rad()
? config_.initialization().skip_position_tolerance_rad()
: 1e-3;
init_skip_velocity_tolerance_rad_s_ =
config_.initialization().has_skip_velocity_tolerance_rad_s()
? config_.initialization().skip_velocity_tolerance_rad_s()
: 1e-2;
if (dexhand_) { if (dexhand_) {
const auto& tactile = config_.touch().tactile(); const auto& tactile = config_.touch().tactile();
std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions; std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
@ -914,15 +1041,18 @@ bool TouchScreenTask::applyConfig() {
} }
} }
const auto& apriltag = config_.perception().apriltag(); const auto& perception = config_.perception();
const auto& external_apriltag = config_.perception().external_apriltag(); const auto& tags = perception.tags();
const auto& hand_camera_config = perception.hand_camera();
const auto& calibration = config_.alignment().calibration();
const auto& pbvs = config_.alignment().pbvs(); const auto& pbvs = config_.alignment().pbvs();
perception_->setTagSize(apriltag.tag_size_m()); perception_->setTagSize(tags.screen().size_m());
external_perception_->setTagSize(external_apriltag.tag_size_m()); external_perception_->setTagSize(tags.screen().size_m());
tracker_.setTargetPointMethod(toTargetPointMethod(apriltag.target_point_method())); tracker_.setTargetPointMethod(toTargetPointMethod(hand_camera_config.target_point_method()));
tcp_pose_tracker_.setPerception(external_perception_); tcp_pose_tracker_.setPerception(external_perception_);
tcp_pose_tracker_.setHandTagId(external_apriltag.hand_tag_id()); tcp_pose_tracker_.setScreenTagId(tags.screen().id());
T_H_P_ = toEigenMat4(external_apriltag.t_h_p()); tcp_pose_tracker_.setHandTagId(tags.hand().id());
T_H_P_ = toEigenMat4(calibration.hand_tag_to_tcp());
pbvs_.setPositionGain(cmvr::common::math::toEigenVec3(pbvs.position_gain())); pbvs_.setPositionGain(cmvr::common::math::toEigenVec3(pbvs.position_gain()));
pbvs_.setRotationGain(cmvr::common::math::toEigenVec3(pbvs.rotation_gain())); pbvs_.setRotationGain(cmvr::common::math::toEigenVec3(pbvs.rotation_gain()));
@ -950,16 +1080,17 @@ bool TouchScreenTask::applyConfig() {
<< ", arm_id=" << config_.devices().arm_id() << ", arm_id=" << config_.devices().arm_id()
<< ", hand_camera_id=" << config_.devices().camera_id() << ", hand_camera_id=" << config_.devices().camera_id()
<< ", external_camera_id=" << config_.devices().external_camera_id() << ", external_camera_id=" << config_.devices().external_camera_id()
<< ", hand_camera_tag_size_m=" << apriltag.tag_size_m() << ", hand_tag_size_m=" << tags.hand().size_m()
<< ", external_camera_tag_size_m=" << external_apriltag.tag_size_m() << ", screen_tag_size_m=" << tags.screen().size_m()
<< ", hand_tag_id=" << external_apriltag.hand_tag_id(); << ", hand_tag_id=" << tags.hand().id()
<< ", screen_tag_id=" << tags.screen().id();
return true; return true;
} }
bool TouchScreenTask::stepAligning(const double dt) { bool TouchScreenTask::stepAligning(const double dt) {
const auto& alignment = config_.alignment(); const auto& alignment = config_.alignment();
const auto target_rotation_vector = const auto target_orientation =
cmvr::common::math::toEigenVec3(alignment.target().rotation_vector()); cmvr::common::math::toEigenEuler(alignment.target().hand_orientation_g());
const auto& target = alignment.target(); const auto& target = alignment.target();
const Eigen::Vector3d position_offset_G = const Eigen::Vector3d position_offset_G =
target.has_position_offset_g() target.has_position_offset_g()
@ -986,7 +1117,7 @@ bool TouchScreenTask::stepAligning(const double dt) {
if (hand_camera_needed) { if (hand_camera_needed) {
perception::AprilTagPerception::Options hand_options; perception::AprilTagPerception::Options hand_options;
hand_options.depth_policy = hand_options.depth_policy =
toDepthPolicy(config_.perception().apriltag().depth_policy()); toDepthPolicy(config_.perception().hand_camera().depth_policy());
hand_options.detect_tags = true; hand_options.detect_tags = true;
hand_options.fetch_encoded = false; hand_options.fetch_encoded = false;
hand_perception_ok = perception_->update(hand_options); hand_perception_ok = perception_->update(hand_options);
@ -1048,7 +1179,8 @@ bool TouchScreenTask::stepAligning(const double dt) {
const int tag_id = tracker_.activeTagId(); const int tag_id = tracker_.activeTagId();
last_active_tag_id_ = tag_id; last_active_tag_id_ = tag_id;
if (tag_id < 0) { const int screen_tag_id = config_.perception().tags().screen().id();
if (tag_id != screen_tag_id) {
stopPbvsMotion(); stopPbvsMotion();
align_stable_count_ = 0; align_stable_count_ = 0;
last_status_ = Status::ALIGN_WAITING_TRACK; last_status_ = Status::ALIGN_WAITING_TRACK;
@ -1068,13 +1200,16 @@ bool TouchScreenTask::stepAligning(const double dt) {
external_options.detect_tags = true; external_options.detect_tags = true;
external_options.fetch_encoded = false; external_options.fetch_encoded = false;
if (!external_perception_->update(external_options)) { if (!external_perception_->update(external_options)) {
publishCoordinateOverlay();
stopPbvsMotion(); stopPbvsMotion();
align_stable_count_ = 0; align_stable_count_ = 0;
last_status_ = Status::ALIGN_WAITING_PERCEPTION; last_status_ = Status::ALIGN_WAITING_PERCEPTION;
return true; return true;
} }
tcp_pose_tracker_.setScreenTagId(tag_id); publishCoordinateOverlay();
tcp_pose_tracker_.setScreenTagId(screen_tag_id);
if (!tcp_pose_tracker_.update(T_H_P_)) { if (!tcp_pose_tracker_.update(T_H_P_)) {
if (align_debug_count_ == 0 && !external_perception_->color().empty()) { if (align_debug_count_ == 0 && !external_perception_->color().empty()) {
cv::imwrite("/tmp/cmvr_external_camera_perception.png", cv::imwrite("/tmp/cmvr_external_camera_perception.png",
@ -1104,9 +1239,14 @@ bool TouchScreenTask::stepAligning(const double dt) {
} }
const Eigen::Matrix4d& T_G_P = tcp_pose_tracker_.T_G_P(); const Eigen::Matrix4d& T_G_P = tcp_pose_tracker_.T_G_P();
Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(), const Eigen::Matrix3d R_G_H_target =
target_rotation_vector.y(), rotationFromTargetEuler(target_orientation.x(),
target_rotation_vector.z()); target_orientation.y(),
target_orientation.z());
// PBVS controls P, while hand_orientation_G configures H. Convert the
// configured target through the fixed hand-tag-to-TCP calibration.
Eigen::Matrix3d R_target =
R_G_H_target * T_H_P_.block<3, 3>(0, 0);
if (rotation_offset_G.norm() > 1e-12) { if (rotation_offset_G.norm() > 1e-12) {
const double offset_angle = rotation_offset_G.norm(); const double offset_angle = rotation_offset_G.norm();
R_target = Eigen::AngleAxisd( R_target = Eigen::AngleAxisd(
@ -1406,37 +1546,34 @@ bool TouchScreenTask::stepRetracting() {
const auto now = Clock::now(); const auto now = Clock::now();
const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count(); const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
const double retract_duration_s = config_.retract().duration_s(); const double retract_distance_m = config_.retract().distance_m();
if (elapsed < retract_duration_s) { Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
if (!retract_start_position_valid_ ||
!readCurrentTouchPointPositionBase(current_position_base)) {
try {
arm_->stopL();
} catch (...) {
}
enterFailed(Status::ROBOT_STATE_FAILED);
return false;
}
const Eigen::Vector3d delta_base =
current_position_base - retract_start_position_base_;
const double traveled_distance_m = delta_base.norm();
if (traveled_distance_m < retract_distance_m) {
if (std::chrono::duration<double>(now - last_retract_log_time_).count() >= 0.2) { if (std::chrono::duration<double>(now - last_retract_log_time_).count() >= 0.2) {
last_retract_log_time_ = now; last_retract_log_time_ = now;
const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{}; const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{};
Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
const bool have_current = readCurrentTouchPointPositionBase(current_position_base); << ", distance=" << traveled_distance_m
const bool have_delta = retract_start_position_valid_ && have_current; << "/" << retract_distance_m
Eigen::Vector3d delta_base = Eigen::Vector3d::Zero(); << ", cmd_base=[" << cmd_base.vx << ", "
if (have_delta) { << cmd_base.vy << ", " << cmd_base.vz << ", "
delta_base = current_position_base - retract_start_position_base_; << cmd_base.wx << ", " << cmd_base.wy << ", "
} << cmd_base.wz << "]"
if (have_delta) { << ", tcp_delta_base=[" << delta_base.x() << ", "
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed << delta_base.y() << ", " << delta_base.z() << "]";
<< "/" << retract_duration_s
<< ", cmd_base=[" << cmd_base.vx << ", "
<< cmd_base.vy << ", " << cmd_base.vz << ", "
<< cmd_base.wx << ", " << cmd_base.wy << ", "
<< cmd_base.wz << "]"
<< ", tcp_delta_base=[" << delta_base.x() << ", "
<< delta_base.y() << ", " << delta_base.z()
<< "], tcp_dist=" << delta_base.norm();
} else {
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
<< "/" << retract_duration_s
<< ", cmd_base=[" << cmd_base.vx << ", "
<< cmd_base.vy << ", " << cmd_base.vz << ", "
<< cmd_base.wx << ", " << cmd_base.wy << ", "
<< cmd_base.wz << "]"
<< ", tcp_delta_base=unavailable";
}
} }
last_status_ = Status::RETRACTING; last_status_ = Status::RETRACTING;
return true; return true;
@ -1451,19 +1588,23 @@ bool TouchScreenTask::stepRetracting() {
} }
if (have_final_delta) { if (have_final_delta) {
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
<< ", target_distance_m=" << retract_distance_m
<< ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0) << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
<< ", final_tcp_delta_base=[" << final_delta_base.x() << ", " << ", final_tcp_delta_base=[" << final_delta_base.x() << ", "
<< final_delta_base.y() << ", " << final_delta_base.z() << final_delta_base.y() << ", " << final_delta_base.z()
<< "], final_tcp_dist=" << final_delta_base.norm(); << "], final_tcp_dist=" << final_delta_base.norm();
} else { } else {
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
<< ", target_distance_m=" << retract_distance_m
<< ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0) << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
<< ", final_tcp_delta_base=unavailable"; << ", final_tcp_delta_base=unavailable";
} }
try { // Stop the retract worker synchronously before switching to the final
arm_->stopL(); // joint-position trajectory. stopL() only requests deceleration and can
} catch (...) { // return while the worker still owns the velocity-control mode.
const auto stop_result = arm_->stopMotion();
if (!stop_result.ok()) {
enterFailed(Status::ROBOT_COMMAND_FAILED); enterFailed(Status::ROBOT_COMMAND_FAILED);
return false; return false;
} }
@ -1545,6 +1686,27 @@ bool TouchScreenTask::buildInitJointPositions(std::vector<double>& positions_out
config_, arm_->getRobotModel().joint_names, positions_out); config_, arm_->getRobotModel().joint_names, positions_out);
} }
bool TouchScreenTask::isAtInitPosition(const std::vector<double>& positions) const {
if (!arm_ || arm_->busy()) {
return false;
}
const auto state = arm_->getJointState();
if (state.position.size() != positions.size() ||
state.velocity.size() != positions.size()) {
return false;
}
for (std::size_t i = 0; i < positions.size(); ++i) {
if (!std::isfinite(state.position[i]) || !std::isfinite(state.velocity[i]) ||
std::abs(state.position[i] - positions[i]) > init_skip_position_tolerance_rad_ ||
std::abs(state.velocity[i]) > init_skip_velocity_tolerance_rad_s_) {
return false;
}
}
return true;
}
bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() { bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() {
if (!config_.initialization().before_start()) { if (!config_.initialization().before_start()) {
return true; return true;
@ -1560,6 +1722,12 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() {
return false; return false;
} }
if (isAtInitPosition(init_positions)) {
CMVR_LOG(INFO) << "[TouchScreenTask] initial position already reached; skipping moveJ"
<< ", arm=" << id_;
return true;
}
device::JointPositionCommand init_cmd{init_positions}; device::JointPositionCommand init_cmd{init_positions};
device::MotionOptions options; device::MotionOptions options;
options.velocity = config_.initialization().velocity(); options.velocity = config_.initialization().velocity();
@ -1581,6 +1749,12 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const {
return false; return false;
} }
if (isAtInitPosition(init_positions)) {
CMVR_LOG(INFO) << "[TouchScreenTask] final initial position already reached; skipping moveJ"
<< ", arm=" << id_;
return true;
}
device::JointPositionCommand init_cmd{init_positions}; device::JointPositionCommand init_cmd{init_positions};
device::MotionOptions options; device::MotionOptions options;
options.velocity = config_.initialization().velocity(); options.velocity = config_.initialization().velocity();
@ -1688,25 +1862,20 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
const auto& retract = config_.retract(); const auto& retract = config_.retract();
retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_); retract_start_position_valid_ = readCurrentTouchPointPositionBase(retract_start_position_base_);
if (!retract_start_position_valid_) {
CMVR_LOG(ERROR) << "[TouchScreenTask][RETRACT_START] cannot read TCP start pose";
return false;
}
const auto retract_cmd = toCartesianVelocity( const auto retract_cmd = toCartesianVelocity(
cmvr::common::math::toEigenVec6(retract.twist_tool())); cmvr::common::math::toEigenVec6(retract.twist_tool()));
if (retract_start_position_valid_) { CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=[" << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
<< retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz << ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
<< ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz << "], acceleration=" << retract.acceleration()
<< "], acceleration=" << retract.acceleration() << ", distance_m=" << retract.distance_m()
<< ", duration_s=" << retract.duration_s() << ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
<< ", start_tcp_base=[" << retract_start_position_base_.x() << ", " << retract_start_position_base_.y() << ", "
<< retract_start_position_base_.y() << ", " << retract_start_position_base_.z() << "]";
<< retract_start_position_base_.z() << "]";
} else {
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] twist_tool=["
<< retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz
<< ", " << retract_cmd.wx << ", " << retract_cmd.wy << ", " << retract_cmd.wz
<< "], acceleration=" << retract.acceleration()
<< ", duration_s=" << retract.duration_s()
<< ", start_tcp_base=unavailable";
}
const auto result = arm_->speedL(retract_cmd, const auto result = arm_->speedL(retract_cmd,
retract.acceleration(), retract.acceleration(),

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

@ -27,6 +27,7 @@
#include "cmvr/config/motor_config/motor_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 "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h"
#include "devices/arm/robot_arm_factory.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/camera/mujoco_camera/include/mujoco_camera.h"
#include "devices/motor/manager/include/motor_manager.h" #include "devices/motor/manager/include/motor_manager.h"
#include "manager/device_manager/include/device_manager.h" #include "manager/device_manager/include/device_manager.h"
@ -76,6 +77,7 @@ struct TagCenterPixel {
}; };
bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& camera, bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& camera,
const int target_tag_id,
const double tag_size_m, const double tag_size_m,
const std::chrono::milliseconds timeout, const std::chrono::milliseconds timeout,
TagCenterPixel& pixel_out) TagCenterPixel& pixel_out)
@ -105,6 +107,9 @@ bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& c
TagCenterPixel best_pixel; TagCenterPixel best_pixel;
for (const auto& tag : perception.tags()) { 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); const Eigen::Vector3d p_c_tag_center = tag.T_c_t.block<3, 1>(0, 3);
Eigen::Vector2d uv = Eigen::Vector2d::Zero(); Eigen::Vector2d uv = Eigen::Vector2d::Zero();
if (!cmvr::ImageProcess::projectCameraPointToPixel( if (!cmvr::ImageProcess::projectCameraPointToPixel(
@ -266,8 +271,25 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
const auto& config = root_config.touch_screen_task(); const auto& config = root_config.touch_screen_task();
EXPECT_TRUE(config.has_initialization()); EXPECT_TRUE(config.has_initialization());
EXPECT_TRUE(config.has_perception()); EXPECT_TRUE(config.has_perception());
EXPECT_TRUE(config.perception().has_external_apriltag()); 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()); 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()); ASSERT_TRUE(config.alignment().has_pbvs());
const auto& pbvs = config.alignment().pbvs(); const auto& pbvs = config.alignment().pbvs();
EXPECT_DOUBLE_EQ(pbvs.position_gain().x(), 2.0); EXPECT_DOUBLE_EQ(pbvs.position_gain().x(), 2.0);
@ -279,9 +301,38 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
EXPECT_EQ(config.touch().motion_case(), EXPECT_EQ(config.touch().motion_case(),
cmvr::config::TouchScreenTaskTouchConfig::kSpeedL); cmvr::config::TouchScreenTaskTouchConfig::kSpeedL);
EXPECT_TRUE(config.has_retract()); 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) { TEST(TouchScreenTaskTest, RunEyeToHandTouchInMujoco) {
const auto project_root = findProjectRoot(); const auto project_root = findProjectRoot();
ASSERT_FALSE(project_root.empty()); ASSERT_FALSE(project_root.empty());
@ -696,7 +747,8 @@ TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
std::thread scenario([&] { std::thread scenario([&] {
try { try {
const auto touch_config = loadMujocoTouchConfig(project_root); 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; cmvr::config::TaskManagerConfig task_manager_config;
auto* task_entry = task_manager_config.add_tasks(); auto* task_entry = task_manager_config.add_tasks();
@ -718,7 +770,8 @@ TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
TagCenterPixel target_pixel; TagCenterPixel target_pixel;
if (!detectTagCenterPixel(camera, if (!detectTagCenterPixel(camera,
touch_config.perception().apriltag().tag_size_m(), touch_config.perception().tags().screen().id(),
touch_config.perception().tags().screen().size_m(),
std::chrono::seconds(3), std::chrono::seconds(3),
target_pixel)) { target_pixel)) {
throw std::runtime_error("failed to detect MuJoCo AprilTag center pixel"); throw std::runtime_error("failed to detect MuJoCo AprilTag center pixel");

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

@ -68,6 +68,8 @@ message CartesianVelocityControllerConfig {
double stop_command_velocity_norm = 3; double stop_command_velocity_norm = 3;
double stop_measured_velocity_norm = 4; double stop_measured_velocity_norm = 4;
double stop_acceleration = 5; double stop_acceleration = 5;
// 等待 Cartesian 速度运动停止的最长时间，单位为秒。
optional double stop_timeout_s = 6;
} }
message ToppraJointMotionPlannerConfig { message ToppraJointMotionPlannerConfig {
@ -81,6 +83,15 @@ message MoveJConfig {
oneof algorithm { oneof algorithm {
ToppraJointMotionPlannerConfig toppra_joint_motion_planner = 1; 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 { message MoveLPlannerConfig {

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

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

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

@ -15,10 +15,10 @@ message TouchScreenTaskDevicesConfig {
reserved 1; reserved 1;
reserved "robot_id"; reserved "robot_id";
optional string arm_id = 4;
optional string dexhand_id = 2;
optional string camera_id = 3; optional string camera_id = 3;
optional string arm_id = 4;
optional string external_camera_id = 5; optional string external_camera_id = 5;
optional string dexhand_id = 2;
} }
message TouchScreenTaskInitializationConfig { message TouchScreenTaskInitializationConfig {
@ -27,26 +27,56 @@ message TouchScreenTaskInitializationConfig {
repeated TouchScreenInitJointPoint joint_positions = 3; repeated TouchScreenInitJointPoint joint_positions = 3;
optional double velocity = 4; optional double velocity = 4;
optional double acceleration = 5; optional double acceleration = 5;
// 当前关节位置误差小于该值时可跳过初始化 MoveJ，单位为弧度。
optional double skip_position_tolerance_rad = 6;
// 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
optional double skip_velocity_tolerance_rad_s = 7;
} }
message TouchScreenTaskExternalApriltagConfig { message TouchScreenTaskTagConfig {
optional double tag_size_m = 1; optional int32 id = 1;
optional int32 hand_tag_id = 2; optional double size_m = 2;
.cmvr.common.Mat4 t_h_p = 3; }
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 { message TouchScreenTaskPerceptionConfig {
TouchScreenApriltagConfig apriltag = 1; reserved 1, 2, 6;
TouchScreenTaskExternalApriltagConfig external_apriltag = 2; reserved "apriltag", "external_apriltag", "external_camera";
TouchScreenTaskTagsConfig tags = 4;
TouchScreenTaskHandCameraConfig hand_camera = 5;
} }
message TouchScreenAlignmentTargetConfig { message TouchScreenAlignmentTargetConfig {
.cmvr.common.Vec3 rotation_vector = 2; 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;
optional TouchScreenAlignMode mode = 3; optional TouchScreenAlignMode mode = 3;
.cmvr.common.Vec3 position_offset_G = 4; .cmvr.common.Vec3 position_offset_G = 4;
.cmvr.common.Vec3 rotation_offset_G = 5; .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 { message TouchScreenTaskAlignmentConfig {
reserved 1; reserved 1;
reserved "kinematics"; reserved "kinematics";
@ -56,6 +86,7 @@ message TouchScreenTaskAlignmentConfig {
optional double timeout_s = 6; optional double timeout_s = 6;
optional bool pause_when_reached = 7; optional bool pause_when_reached = 7;
TouchScreenTaskPbvsConfig pbvs = 8; TouchScreenTaskPbvsConfig pbvs = 8;
TouchScreenTaskAlignmentCalibrationConfig calibration = 9;
} }
message TouchScreenTouchSpeedLConfig { message TouchScreenTouchSpeedLConfig {
@ -93,7 +124,10 @@ message TouchScreenTaskTouchConfig {
message TouchScreenTaskRetractConfig { message TouchScreenTaskRetractConfig {
.cmvr.common.Vec6 twist_tool = 1; .cmvr.common.Vec6 twist_tool = 1;
optional double acceleration = 2; optional double acceleration = 2;
optional double duration_s = 3; reserved 3;
reserved "duration_s";
// Distance traveled by the TCP before the retract motion stops, in meters.
optional double distance_m = 4;
} }
message TouchScreenTaskConfig { message TouchScreenTaskConfig {
@ -104,6 +138,10 @@ message TouchScreenTaskConfig {
TouchScreenTaskTouchConfig touch = 5; TouchScreenTaskTouchConfig touch = 5;
TouchScreenTaskRetractConfig retract = 6; TouchScreenTaskRetractConfig retract = 6;
optional string id = 7; optional string id = 7;
// 是否在外部相机编码后的 gRPC 视频流中绘制坐标系，仅影响显示帧；未配置时默认开启。
optional bool debug_draw_coordinate_frames = 8;
// G/H 坐标轴长度，单位为米。
optional double debug_coordinate_axis_length_m = 9;
} }
message TouchScreenTaskRootConfig { message TouchScreenTaskRootConfig {