feat:add pbvs touch
This commit is contained in:
parent
6bfe01b854
commit
964b0457ca
@ -4,3 +4,9 @@ ERROR: could not create window
|
||||
Fri Jul 24 15:40:37 2026
|
||||
ERROR: could not create window
|
||||
|
||||
Fri Sep 11 13:10:38 2026
|
||||
ERROR: could not initialize GLFW
|
||||
|
||||
Fri Sep 11 14:13:09 2026
|
||||
ERROR: could not initialize GLFW
|
||||
|
||||
|
||||
@ -25,6 +25,7 @@ public:
|
||||
double stop_command_velocity_norm{1e-3};
|
||||
double stop_measured_velocity_norm{1e-2};
|
||||
double stop_acceleration{0.5};
|
||||
double stop_timeout_s{2.0};
|
||||
};
|
||||
|
||||
using ReadStateCallback = std::function<bool(std::vector<double>& q, std::vector<double>& qd)>;
|
||||
@ -48,6 +49,7 @@ public:
|
||||
void shutdown();
|
||||
|
||||
bool busy() const { return busy_.load(); }
|
||||
double stopTimeoutS() const { return config_.stop_timeout_s; }
|
||||
CartesianVelocity getCommandTwistBase() const;
|
||||
|
||||
private:
|
||||
|
||||
@ -29,6 +29,9 @@ CartesianVelocityController::Config normalizeConfig(CartesianVelocityController:
|
||||
if (config.stop_acceleration <= 0.0) {
|
||||
config.stop_acceleration = defaults.stop_acceleration;
|
||||
}
|
||||
if (!std::isfinite(config.stop_timeout_s) || config.stop_timeout_s <= 0.0) {
|
||||
config.stop_timeout_s = defaults.stop_timeout_s;
|
||||
}
|
||||
return config;
|
||||
}
|
||||
|
||||
@ -108,6 +111,12 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
|
||||
if (!worker_ || !worker_->joinable()) {
|
||||
return Result::success();
|
||||
}
|
||||
// A completed speedL command leaves the worker thread joinable but idle.
|
||||
// Do not turn that idle worker into a new command just because a caller
|
||||
// requests a stop during a task transition.
|
||||
if (!busy_.load()) {
|
||||
return Result::success();
|
||||
}
|
||||
requestStop_(acceleration);
|
||||
return Result::success();
|
||||
}
|
||||
|
||||
@ -26,3 +26,14 @@ target_link_libraries(ik_solver PUBLIC
|
||||
add_library(cmvr_es::ik_solver ALIAS ik_solver)
|
||||
|
||||
install(TARGETS ik_solver LIBRARY DESTINATION lib)
|
||||
|
||||
add_executable(pinocchio_qp_ik_solver_test
|
||||
pinocchio/src/pinocchio_qp_ik_solver_test.cpp
|
||||
)
|
||||
|
||||
target_link_libraries(pinocchio_qp_ik_solver_test PRIVATE
|
||||
cmvr_es::ik_solver
|
||||
cmvr_es::proto
|
||||
gtest
|
||||
gtest_main
|
||||
)
|
||||
|
||||
@ -45,6 +45,11 @@ public:
|
||||
std::vector<double>& qdot_out,
|
||||
double qdot_abs_max = std::numeric_limits<double>::infinity()) const override;
|
||||
|
||||
// Projects a secondary joint velocity into the Cartesian task null space.
|
||||
static Eigen::VectorXd projectJointLimitAvoidanceToNullspace(
|
||||
const Eigen::MatrixXd& jacobian_base,
|
||||
const Eigen::VectorXd& qdot_avoid);
|
||||
|
||||
/// 如你有更严格的速度 / 加速度限位，可以覆盖默认值
|
||||
void setVelocityLimits(const Eigen::VectorXd &qd_max);
|
||||
void setAccelerationLimits(const Eigen::VectorXd &qdd_max);
|
||||
|
||||
@ -164,7 +164,9 @@ Eigen::VectorXd PinocchioIKBase::applyJointSoftLimitsToVelocity(
|
||||
|
||||
const double margin_ratio = positiveOr(config.margin_ratio(), 0.08);
|
||||
const double min_margin_rad = positiveOr(config.min_margin_rad(), 0.02);
|
||||
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) {
|
||||
const double lower = joint_pos_lower_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 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);
|
||||
limited[i] *= ratio;
|
||||
if (q_chain[i] <= lower) {
|
||||
limited[i] = std::max(0.0, limited[i]);
|
||||
}
|
||||
} else if (limited[i] > 0.0 && q_chain[i] > upper - margin) {
|
||||
scale = std::min(scale, ratio);
|
||||
} else if (qdot[i] > 0.0 && q_chain[i] > upper - margin) {
|
||||
const double ratio = std::clamp((upper - q_chain[i]) / margin, 0.0, 1.0);
|
||||
limited[i] *= ratio;
|
||||
if (q_chain[i] >= upper) {
|
||||
limited[i] = std::min(0.0, limited[i]);
|
||||
scale = std::min(scale, ratio);
|
||||
}
|
||||
}
|
||||
}
|
||||
return limited;
|
||||
return scale * qdot;
|
||||
}
|
||||
|
||||
void PinocchioIKBase::updateKinematics(const Eigen::VectorXd& q_full) {
|
||||
|
||||
@ -13,11 +13,66 @@
|
||||
#include <pinocchio/spatial/explog.hpp>
|
||||
|
||||
#include <algorithm> // std::clamp, std::max, std::min
|
||||
#include <atomic>
|
||||
#include <cmath> // std::sqrt
|
||||
#include <cstdint>
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <unordered_map>
|
||||
#include <Eigen/SVD>
|
||||
|
||||
namespace cmvr {
|
||||
namespace {
|
||||
|
||||
Eigen::MatrixXd moorePenrosePseudoInverse(const Eigen::MatrixXd& matrix)
|
||||
{
|
||||
if (matrix.rows() == 0 || matrix.cols() == 0 || !matrix.allFinite()) {
|
||||
return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
|
||||
}
|
||||
|
||||
Eigen::JacobiSVD<Eigen::MatrixXd> svd(
|
||||
matrix, Eigen::ComputeFullU | Eigen::ComputeFullV);
|
||||
if (svd.info() != Eigen::Success) {
|
||||
return Eigen::MatrixXd::Zero(matrix.cols(), matrix.rows());
|
||||
}
|
||||
|
||||
const Eigen::VectorXd singular_values = svd.singularValues();
|
||||
const double max_singular = singular_values.size() > 0
|
||||
? singular_values.maxCoeff()
|
||||
: 0.0;
|
||||
const double tolerance =
|
||||
std::numeric_limits<double>::epsilon() *
|
||||
static_cast<double>(std::max(matrix.rows(), matrix.cols())) *
|
||||
std::max(1.0, max_singular);
|
||||
Eigen::VectorXd inverse_singular = singular_values;
|
||||
for (Eigen::Index i = 0; i < inverse_singular.size(); ++i) {
|
||||
inverse_singular[i] = singular_values[i] > tolerance
|
||||
? 1.0 / singular_values[i]
|
||||
: 0.0;
|
||||
}
|
||||
|
||||
const Eigen::Index rank_dimension = singular_values.size();
|
||||
return svd.matrixV().leftCols(rank_dimension) *
|
||||
inverse_singular.asDiagonal() *
|
||||
svd.matrixU().leftCols(rank_dimension).transpose();
|
||||
}
|
||||
|
||||
std::string vectorToString(const Eigen::VectorXd& value)
|
||||
{
|
||||
std::ostringstream stream;
|
||||
stream << '[';
|
||||
for (Eigen::Index i = 0; i < value.size(); ++i) {
|
||||
if (i > 0) {
|
||||
stream << ' ';
|
||||
}
|
||||
stream << value[i];
|
||||
}
|
||||
stream << ']';
|
||||
return stream.str();
|
||||
}
|
||||
|
||||
} // namespace
|
||||
|
||||
using Eigen::Matrix4d;
|
||||
using Eigen::VectorXd;
|
||||
using Eigen::MatrixXd;
|
||||
@ -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,
|
||||
std::vector<double> &joints_angle,
|
||||
bool is_tcp) {
|
||||
@ -389,7 +462,7 @@ namespace cmvr {
|
||||
const int dof = chain_v_dof_;
|
||||
const auto& avoidance = jointLimitPolicy().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;
|
||||
|
||||
MatrixXd cost(6 + dof + avoidance_rows, dof);
|
||||
@ -406,7 +479,7 @@ namespace cmvr {
|
||||
VectorXd upper(dof);
|
||||
const Eigen::Map<const VectorXd> q_chain(q_chain_std.data(), dof);
|
||||
if (use_joint_limit_avoidance) {
|
||||
const VectorXd qdot_avoid =
|
||||
const VectorXd qdot_avoid_raw =
|
||||
cmvr::kinematics::computeJointLimitAvoidanceVelocity(
|
||||
q_chain,
|
||||
joint_pos_lower_limits_,
|
||||
@ -415,10 +488,32 @@ namespace cmvr {
|
||||
positiveOr(avoidance.gain(), 0.2),
|
||||
positiveOr(avoidance.margin_ratio(), 0.15),
|
||||
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) =
|
||||
sqrt_weight * MatrixXd::Identity(dof, dof);
|
||||
target.segment(6 + dof, dof) = sqrt_weight * qdot_avoid;
|
||||
nullspace;
|
||||
target.segment(6 + dof, dof) = qdot_avoid_null;
|
||||
|
||||
static std::atomic<std::uint64_t> avoidance_debug_counter{0};
|
||||
const auto debug_index =
|
||||
avoidance_debug_counter.fetch_add(1, std::memory_order_relaxed);
|
||||
if (debug_index % 1000 == 0) {
|
||||
CMVR_LOG(DEBUG)
|
||||
<< "[PinocchioQpIKSolver][JOINT_LIMIT_AVOIDANCE]"
|
||||
<< " qdot_avoid_raw=" << vectorToString(qdot_avoid_raw)
|
||||
<< " qdot_avoid_null=" << vectorToString(qdot_avoid_null)
|
||||
<< " norm(J*qdot_avoid_null)="
|
||||
<< (jacobian_base * qdot_avoid_null).norm();
|
||||
}
|
||||
}
|
||||
for (int i = 0; i < dof; ++i) {
|
||||
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;
|
||||
@ -475,7 +599,6 @@ namespace cmvr {
|
||||
if (qdot.size() != dof) {
|
||||
return false;
|
||||
}
|
||||
qdot = applyJointSoftLimitsToVelocity(q_chain, qdot);
|
||||
qdot_out.assign(qdot.data(), qdot.data() + qdot.size());
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -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
|
||||
@ -84,6 +84,10 @@ public:
|
||||
|
||||
// Eigen 表示的齐次变换 `T_c_t`：将 tag 坐标系 `t` 中的点变换到相机坐标系 `c`。
|
||||
Eigen::Matrix4d T_c_t{Eigen::Matrix4d::Identity()};
|
||||
|
||||
// Semantic alias used by visualization and downstream consumers:
|
||||
// `T_C_Tag` maps points in this tag frame into camera frame C.
|
||||
const Eigen::Matrix4d& T_C_Tag() const { return T_c_t; }
|
||||
};
|
||||
|
||||
struct FrameCache {
|
||||
|
||||
@ -27,6 +27,26 @@ inline Eigen::Vector3d toEigenVec3(const cmvr::common::Vec3& src)
|
||||
return toEigenVec3(src, Eigen::Vector3d::Zero());
|
||||
}
|
||||
|
||||
inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src,
|
||||
Eigen::Vector3d defaults)
|
||||
{
|
||||
if (src.has_rx()) {
|
||||
defaults.x() = src.rx();
|
||||
}
|
||||
if (src.has_ry()) {
|
||||
defaults.y() = src.ry();
|
||||
}
|
||||
if (src.has_rz()) {
|
||||
defaults.z() = src.rz();
|
||||
}
|
||||
return defaults;
|
||||
}
|
||||
|
||||
inline Eigen::Vector3d toEigenEuler(const cmvr::common::Euler& src)
|
||||
{
|
||||
return toEigenEuler(src, Eigen::Vector3d::Zero());
|
||||
}
|
||||
|
||||
inline Eigen::Matrix<double, 6, 1> toEigenVec6(
|
||||
const cmvr::common::Vec6& src,
|
||||
Eigen::Matrix<double, 6, 1> defaults)
|
||||
@ -102,6 +122,10 @@ inline bool hasVec3(const cmvr::common::Vec3& value) {
|
||||
return value.has_x() && value.has_y() && value.has_z();
|
||||
}
|
||||
|
||||
inline bool hasEuler(const cmvr::common::Euler& value) {
|
||||
return value.has_rx() && value.has_ry() && value.has_rz();
|
||||
}
|
||||
|
||||
inline bool hasVec6(const cmvr::common::Vec6& value) {
|
||||
return value.has_x() && value.has_y() && value.has_z() &&
|
||||
value.has_rx() && value.has_ry() && value.has_rz();
|
||||
|
||||
@ -51,7 +51,6 @@ arm {
|
||||
gain: 0.2
|
||||
margin_ratio: 0.15
|
||||
max_push: 0.25
|
||||
weight: 0.05
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -59,6 +58,14 @@ arm {
|
||||
|
||||
motion {
|
||||
move_j {
|
||||
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
|
||||
settle_timeout_s: 2.0
|
||||
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
|
||||
settle_position_tolerance_rad: 0.002
|
||||
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
|
||||
settle_velocity_tolerance_rad_s: 0.02
|
||||
# 位置和速度连续满足条件的采样次数。
|
||||
settle_stable_sample_count: 3
|
||||
toppra_joint_motion_planner {
|
||||
path_type: TOPPRA_PATH_TYPE_QUINTIC
|
||||
sample_period_s: 0.001
|
||||
@ -144,6 +151,8 @@ arm {
|
||||
stop_command_velocity_norm: 1e-3
|
||||
stop_measured_velocity_norm: 1e-2
|
||||
stop_acceleration: 10
|
||||
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
|
||||
stop_timeout_s: 2.0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -50,7 +50,6 @@ arm {
|
||||
gain: 0.2
|
||||
margin_ratio: 0.15
|
||||
max_push: 0.25
|
||||
weight: 2.0
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -58,6 +57,14 @@ arm {
|
||||
|
||||
motion {
|
||||
move_j {
|
||||
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
|
||||
settle_timeout_s: 2.0
|
||||
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
|
||||
settle_position_tolerance_rad: 0.002
|
||||
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
|
||||
settle_velocity_tolerance_rad_s: 0.02
|
||||
# 位置和速度连续满足条件的采样次数。
|
||||
settle_stable_sample_count: 3
|
||||
toppra_joint_motion_planner {
|
||||
path_type: TOPPRA_PATH_TYPE_QUINTIC
|
||||
sample_period_s: 0.001
|
||||
@ -143,6 +150,8 @@ arm {
|
||||
stop_command_velocity_norm: 1e-3
|
||||
stop_measured_velocity_norm: 1e-2
|
||||
stop_acceleration: 2.0
|
||||
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
|
||||
stop_timeout_s: 2.0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -51,7 +51,6 @@ arm {
|
||||
gain: 0.2
|
||||
margin_ratio: 0.15
|
||||
max_push: 0.25
|
||||
weight: 2.0
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -59,6 +58,14 @@ arm {
|
||||
|
||||
motion {
|
||||
move_j {
|
||||
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
|
||||
settle_timeout_s: 2.0
|
||||
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
|
||||
settle_position_tolerance_rad: 0.002
|
||||
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
|
||||
settle_velocity_tolerance_rad_s: 0.02
|
||||
# 位置和速度连续满足条件的采样次数。
|
||||
settle_stable_sample_count: 3
|
||||
toppra_joint_motion_planner {
|
||||
path_type: TOPPRA_PATH_TYPE_QUINTIC
|
||||
sample_period_s: 0.001
|
||||
@ -144,6 +151,8 @@ arm {
|
||||
stop_command_velocity_norm: 1e-3
|
||||
stop_measured_velocity_norm: 1e-2
|
||||
stop_acceleration: 10
|
||||
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
|
||||
stop_timeout_s: 2.0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -52,7 +52,6 @@ arm {
|
||||
gain: 0.2
|
||||
margin_ratio: 0.01
|
||||
max_push: 0.02
|
||||
weight: 0.05
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -60,6 +59,14 @@ arm {
|
||||
|
||||
motion {
|
||||
move_j {
|
||||
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
|
||||
settle_timeout_s: 2.0
|
||||
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
|
||||
settle_position_tolerance_rad: 0.002
|
||||
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
|
||||
settle_velocity_tolerance_rad_s: 0.02
|
||||
# 位置和速度连续满足条件的采样次数。
|
||||
settle_stable_sample_count: 3
|
||||
toppra_joint_motion_planner {
|
||||
path_type: TOPPRA_PATH_TYPE_QUINTIC
|
||||
sample_period_s: 0.001
|
||||
@ -145,6 +152,8 @@ arm {
|
||||
stop_command_velocity_norm: 1e-3
|
||||
stop_measured_velocity_norm: 1e-2
|
||||
stop_acceleration: 5
|
||||
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
|
||||
stop_timeout_s: 2.0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -52,7 +52,6 @@ arm {
|
||||
gain: 0.2
|
||||
margin_ratio: 0.01
|
||||
max_push: 0.02
|
||||
weight: 0.05
|
||||
}
|
||||
}
|
||||
}
|
||||
@ -60,6 +59,14 @@ arm {
|
||||
|
||||
motion {
|
||||
move_j {
|
||||
# MoveJ 轨迹完成后等待关节稳定的最长时间，单位为秒。
|
||||
settle_timeout_s: 2.0
|
||||
# MoveJ 完成时允许的最大关节位置误差，单位为弧度。
|
||||
settle_position_tolerance_rad: 0.002
|
||||
# MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
|
||||
settle_velocity_tolerance_rad_s: 0.02
|
||||
# 位置和速度连续满足条件的采样次数。
|
||||
settle_stable_sample_count: 3
|
||||
toppra_joint_motion_planner {
|
||||
path_type: TOPPRA_PATH_TYPE_QUINTIC
|
||||
sample_period_s: 0.001
|
||||
@ -145,6 +152,8 @@ arm {
|
||||
stop_command_velocity_norm: 1e-3
|
||||
stop_measured_velocity_norm: 1e-2
|
||||
stop_acceleration: 0.5
|
||||
# 等待 Cartesian 速度运动停止的最长时间，单位为秒。
|
||||
stop_timeout_s: 2.0
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,9 +1,14 @@
|
||||
touch_screen_task {
|
||||
id: "touch_screen"
|
||||
# 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
|
||||
debug_draw_coordinate_frames: true
|
||||
# G/H 坐标轴长度，单位为米。
|
||||
debug_coordinate_axis_length_m: 0.02
|
||||
|
||||
devices {
|
||||
arm_id: "right_arm"
|
||||
dexhand_id: "paxini_tip_1"
|
||||
# 手部相机和外部相机的 DeviceManager ID。
|
||||
camera_id: "right_hand_cam"
|
||||
external_camera_id: "cam5"
|
||||
}
|
||||
@ -20,27 +25,41 @@ touch_screen_task {
|
||||
joint_positions { joint_name: "R_WRIST_R" rad: 0.1297 }
|
||||
velocity: 1.0
|
||||
acceleration: 2.0
|
||||
# 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
|
||||
skip_position_tolerance_rad: 0.001
|
||||
# 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
|
||||
skip_velocity_tolerance_rad_s: 0.01
|
||||
}
|
||||
|
||||
perception {
|
||||
apriltag {
|
||||
tag_size_m: 0.012
|
||||
# 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
|
||||
tags {
|
||||
screen {
|
||||
id: 1
|
||||
size_m: 0.03
|
||||
}
|
||||
hand {
|
||||
id: 0
|
||||
size_m: 0.03
|
||||
}
|
||||
}
|
||||
# 手部相机：用于点击目标点和手部目标跟踪。
|
||||
hand_camera {
|
||||
depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
|
||||
target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
|
||||
}
|
||||
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 {
|
||||
calibration {
|
||||
# TCP P 相对于屏幕 Hand Tag H 的目标姿态
|
||||
hand_tag_to_tcp {
|
||||
m00: 1.0 m01: 0.0 m02: 0.0 m03: 0.0
|
||||
m10: 0.0 m11: 0.0 m12: -1.0 m13: 0.0
|
||||
m20: 0.0 m21: 1.0 m22: 0.0 m23: -0.03
|
||||
m30: 0.0 m31: 0.0 m32: 0.0 m33: 1.0
|
||||
}
|
||||
}
|
||||
pbvs {
|
||||
position_gain { x: 2.0 y: 2.0 z: 1.5 }
|
||||
rotation_gain { x: 1.5 y: 1.5 z: 1.5 }
|
||||
@ -49,7 +68,13 @@ touch_screen_task {
|
||||
twist_filter_alpha: 1.0
|
||||
}
|
||||
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
|
||||
}
|
||||
error_threshold {
|
||||
@ -83,6 +108,7 @@ touch_screen_task {
|
||||
retract {
|
||||
twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 8.0
|
||||
duration_s: 0.45
|
||||
# TCP 后退目标距离，单位为米。
|
||||
distance_m: 0.02
|
||||
}
|
||||
}
|
||||
|
||||
@ -1,9 +1,14 @@
|
||||
touch_screen_task {
|
||||
id: "touch_screen"
|
||||
# 是否在外部相机编码后的 gRPC 视频流中绘制 G/H 坐标系，仅影响显示帧；未配置时默认开启。
|
||||
debug_draw_coordinate_frames: true
|
||||
# G/H 坐标轴长度，单位为米。
|
||||
debug_coordinate_axis_length_m: 0.02
|
||||
|
||||
devices {
|
||||
arm_id: "mujoco_right_arm"
|
||||
dexhand_id: "mujoco_zero_touch_dexhand"
|
||||
# 手部相机和外部相机的 DeviceManager ID。
|
||||
camera_id: "mujoco_hand_cam"
|
||||
external_camera_id: "mujoco_external_touch_cam"
|
||||
}
|
||||
@ -18,30 +23,44 @@ touch_screen_task {
|
||||
joint_positions { joint_name: "R_WRIST_P" rad: -2.8792 }
|
||||
joint_positions { joint_name: "R_WRIST_Y" rad: 0.1150 }
|
||||
joint_positions { joint_name: "R_WRIST_R" rad: -0.08 }
|
||||
velocity: 2.8
|
||||
acceleration: 20.0
|
||||
velocity: 2.0
|
||||
acceleration: 3.0
|
||||
# 当前关节位置误差小于该值时跳过初始化 MoveJ，单位为弧度。
|
||||
skip_position_tolerance_rad: 0.001
|
||||
# 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
|
||||
skip_velocity_tolerance_rad_s: 0.01
|
||||
}
|
||||
|
||||
perception {
|
||||
apriltag {
|
||||
tag_size_m: 0.03
|
||||
# 屏幕 Tag G 与手部 Tag H 的 ID 和物理边长，单位为米。
|
||||
tags {
|
||||
screen {
|
||||
id: 1
|
||||
size_m: 0.03
|
||||
}
|
||||
hand {
|
||||
id: 0
|
||||
size_m: 0.03
|
||||
}
|
||||
}
|
||||
# 手部相机：用于点击目标点和手部目标跟踪。
|
||||
hand_camera {
|
||||
|
||||
depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE
|
||||
target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE
|
||||
}
|
||||
external_apriltag {
|
||||
tag_size_m: 0.03
|
||||
hand_tag_id: 0
|
||||
# Hand Tag H -> TCP P after flipping the tag to face external_touch_cam.
|
||||
t_h_p {
|
||||
}
|
||||
|
||||
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
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
alignment {
|
||||
pbvs {
|
||||
position_gain { x: 2.0 y: 2.0 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
|
||||
}
|
||||
target {
|
||||
rotation_vector {
|
||||
x: 3.141592653589793
|
||||
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
|
||||
}
|
||||
rotation_offset_G {
|
||||
x: 0.0
|
||||
y: 0.0
|
||||
z: 0.0
|
||||
}
|
||||
|
||||
mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
|
||||
|
||||
mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
|
||||
}
|
||||
error_threshold {
|
||||
x: 0.005
|
||||
y: 0.005
|
||||
z: 0.010
|
||||
z: 0.005
|
||||
rx: 0.08726646259971647
|
||||
ry: 0.08726646259971647
|
||||
rz: 0.08726646259971647
|
||||
@ -99,7 +116,8 @@ touch_screen_task {
|
||||
|
||||
retract {
|
||||
twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
|
||||
acceleration: 5.0
|
||||
duration_s: 2.0
|
||||
acceleration: 4.0
|
||||
# TCP 后退目标距离，单位为米。
|
||||
distance_m: 0.05
|
||||
}
|
||||
}
|
||||
|
||||
@ -106,6 +106,8 @@ private:
|
||||
std::shared_ptr<AbstractMotor> getMotor_(const std::string& joint_name) const;
|
||||
bool readArmState_(std::vector<double>& q_now, std::vector<double>& qd_now) const;
|
||||
std::vector<double> readJointPosition_() const;
|
||||
Result stopCartesianMotionAndWait_();
|
||||
Result waitForJointTarget_(const std::vector<double>& target) const;
|
||||
|
||||
bool configureAlgorithms_();
|
||||
bool executeMoveLTrajectory_(const CartesianJointTrajectory& trajectory);
|
||||
@ -130,6 +132,13 @@ private:
|
||||
std::shared_ptr<CartesianMotionPlanner> cartesian_planner_{nullptr};
|
||||
std::unique_ptr<CartesianVelocityController> cartesian_velocity_controller_{nullptr};
|
||||
|
||||
// MoveJ post-trajectory settling criteria. These defaults preserve the
|
||||
// historical behavior when the optional arm configuration fields are absent.
|
||||
double move_j_settle_timeout_s_{2.0};
|
||||
double move_j_position_tolerance_rad_{2e-3};
|
||||
double move_j_velocity_tolerance_rad_s_{2e-2};
|
||||
int move_j_stable_sample_count_{3};
|
||||
|
||||
mutable std::mutex mutex_;
|
||||
std::atomic<bool> busy_{false};
|
||||
double speed_scaling_{1.0};
|
||||
|
||||
@ -516,6 +516,16 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
|
||||
if (!joint_planner_) {
|
||||
return Result::failure(ArmErrorCode::RobotNotReady, "joint planner is not initialized");
|
||||
}
|
||||
|
||||
// speedL runs in a worker thread and sends speedJ commands. A moveJ
|
||||
// trajectory writes cyclic-position commands directly, so allowing both
|
||||
// paths to run concurrently can overwrite the motor mode/target and cause
|
||||
// a short surge at the transition. Finish the Cartesian worker before
|
||||
// reading the planning start state.
|
||||
const auto cartesian_stop = stopCartesianMotionAndWait_();
|
||||
if (!cartesian_stop.ok()) {
|
||||
return cartesian_stop;
|
||||
}
|
||||
if (busy_.exchange(true)) {
|
||||
return Result::failure(ArmErrorCode::RobotNotReady, "[MotorRobotArm] arm is busy: " + id_);
|
||||
}
|
||||
@ -559,9 +569,14 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
|
||||
return Result::failure(ArmErrorCode::CommandFailed, "moveJ sample size mismatch");
|
||||
}
|
||||
std::fill(command_velocity.begin(), command_velocity.end(), 0.0);
|
||||
// A MoveJ is a rest-to-rest command. Do not let a non-zero numerical
|
||||
// endpoint velocity from an alternate planner keep the drive moving
|
||||
// while the position-mode trajectory is being handed back to the arm.
|
||||
if (k + 1 < samples.size()) {
|
||||
std::copy_n(sample.velocity.begin(),
|
||||
std::min(sample.velocity.size(), command_velocity.size()),
|
||||
command_velocity.begin());
|
||||
}
|
||||
if (!motor_manager_->commandCyclicPositionsAtomic(
|
||||
motors, sample.position, command_velocity)) {
|
||||
return Result::failure(ArmErrorCode::CommandFailed,
|
||||
@ -575,6 +590,24 @@ Result MotorRobotArm::moveJ(const JointPositionCommand& target, const MotionOpti
|
||||
std::chrono::duration<double>(next_t)));
|
||||
}
|
||||
}
|
||||
|
||||
// Repeat the final position with zero velocity before checking feedback.
|
||||
// This seeds the position-mode target after a velocity-to-position switch
|
||||
// and prevents a stale final velocity command from producing a short
|
||||
// motion spike at the destination.
|
||||
if (!motor_manager_->commandCyclicPositionsAtomic(
|
||||
motors, target.position, std::vector<double>(motors.size(), 0.0))) {
|
||||
return Result::failure(ArmErrorCode::CommandFailed,
|
||||
"failed to hold final moveJ target");
|
||||
}
|
||||
// Allow one servo cycle to consume the explicit hold command before
|
||||
// evaluating feedback-based settling criteria.
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
|
||||
const auto settle_result = waitForJointTarget_(target.position);
|
||||
if (!settle_result.ok()) {
|
||||
return settle_result;
|
||||
}
|
||||
return Result::success();
|
||||
}
|
||||
|
||||
@ -638,8 +671,9 @@ Result MotorRobotArm::moveL(const CartesianPose& target,
|
||||
if (const auto stopped = safetyStopResult_("moveL")) {
|
||||
return *stopped;
|
||||
}
|
||||
if (cartesian_velocity_controller_) {
|
||||
cartesian_velocity_controller_->shutdown();
|
||||
const auto cartesian_stop = stopCartesianMotionAndWait_();
|
||||
if (!cartesian_stop.ok()) {
|
||||
return cartesian_stop;
|
||||
}
|
||||
if (options.asynchronous) {
|
||||
return Result::failure(ArmErrorCode::UnsupportedCommand, "moveL asynchronous=true is not supported");
|
||||
@ -715,7 +749,10 @@ Result MotorRobotArm::stopL(const std::optional<double> acceleration)
|
||||
|
||||
Result MotorRobotArm::stopMotion()
|
||||
{
|
||||
stopL(0.0);
|
||||
const auto cartesian_stop = stopCartesianMotionAndWait_();
|
||||
if (!cartesian_stop.ok()) {
|
||||
return cartesian_stop;
|
||||
}
|
||||
return stopJ(0.0);
|
||||
}
|
||||
|
||||
@ -963,8 +1000,142 @@ std::vector<double> MotorRobotArm::readJointPosition_() const
|
||||
return q_start;
|
||||
}
|
||||
|
||||
Result MotorRobotArm::stopCartesianMotionAndWait_()
|
||||
{
|
||||
if (!cartesian_velocity_controller_) {
|
||||
return Result::success();
|
||||
}
|
||||
|
||||
if (cartesian_velocity_controller_->busy()) {
|
||||
const auto stop_result = cartesian_velocity_controller_->stop();
|
||||
if (!stop_result.ok()) {
|
||||
return stop_result;
|
||||
}
|
||||
|
||||
const auto deadline = std::chrono::steady_clock::now() +
|
||||
std::chrono::duration<double>(
|
||||
cartesian_velocity_controller_->stopTimeoutS());
|
||||
while (cartesian_velocity_controller_->busy() &&
|
||||
std::chrono::steady_clock::now() < deadline) {
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
if (cartesian_velocity_controller_->busy()) {
|
||||
// Do not start a position trajectory while the worker can still
|
||||
// issue velocity commands. Shutdown joins it and sends one final
|
||||
// zero-velocity command before reporting the timeout.
|
||||
cartesian_velocity_controller_->shutdown();
|
||||
return Result::failure(
|
||||
ArmErrorCode::Timeout,
|
||||
"timed out waiting for Cartesian velocity motion to stop");
|
||||
}
|
||||
}
|
||||
|
||||
// The worker may be idle but still joinable. Joining it here removes any
|
||||
// last command/worker race before the next motion mode is selected.
|
||||
cartesian_velocity_controller_->shutdown();
|
||||
return Result::success();
|
||||
}
|
||||
|
||||
Result MotorRobotArm::waitForJointTarget_(const std::vector<double>& target) const
|
||||
{
|
||||
if (target.size() != joint_names_.size()) {
|
||||
return Result::failure(ArmErrorCode::InvalidArgument,
|
||||
"moveJ target size mismatch while settling");
|
||||
}
|
||||
|
||||
const auto deadline = std::chrono::steady_clock::now() +
|
||||
std::chrono::duration<double>(move_j_settle_timeout_s_);
|
||||
int stable_samples = 0;
|
||||
while (std::chrono::steady_clock::now() < deadline) {
|
||||
if (const auto stopped = safetyStopResult_("moveJ", true)) {
|
||||
return *stopped;
|
||||
}
|
||||
|
||||
bool settled = true;
|
||||
for (std::size_t i = 0; i < joint_names_.size(); ++i) {
|
||||
const auto motor = getMotor_(joint_names_[i]);
|
||||
if (!motor) {
|
||||
return Result::failure(
|
||||
ArmErrorCode::RobotNotReady,
|
||||
"motor not found while waiting for moveJ target: " + joint_names_[i]);
|
||||
}
|
||||
const double position = motor->getQ();
|
||||
const double velocity = motor->getQd();
|
||||
if (!std::isfinite(position) || !std::isfinite(velocity) ||
|
||||
std::abs(position - target[i]) > move_j_position_tolerance_rad_ ||
|
||||
std::abs(velocity) > move_j_velocity_tolerance_rad_s_) {
|
||||
settled = false;
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
stable_samples = settled ? stable_samples + 1 : 0;
|
||||
if (stable_samples >= move_j_stable_sample_count_) {
|
||||
return Result::success();
|
||||
}
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(1));
|
||||
}
|
||||
|
||||
CMVR_LOG(ERROR) << "[MotorRobotArm] moveJ target did not settle before timeout: " << id_;
|
||||
return Result::failure(ArmErrorCode::Timeout,
|
||||
"timed out waiting for moveJ target to settle");
|
||||
}
|
||||
|
||||
bool MotorRobotArm::configureAlgorithms_()
|
||||
{
|
||||
// Optional settling fields are read once during initialization so every
|
||||
// MoveJ command uses one consistent set of safety criteria.
|
||||
move_j_settle_timeout_s_ = 2.0;
|
||||
move_j_position_tolerance_rad_ = 2e-3;
|
||||
move_j_velocity_tolerance_rad_s_ = 2e-2;
|
||||
move_j_stable_sample_count_ = 3;
|
||||
const auto& move_j_config = cfg_.motion().move_j();
|
||||
if (move_j_config.has_settle_timeout_s()) {
|
||||
const double value = move_j_config.settle_timeout_s();
|
||||
if (!std::isfinite(value) || value <= 0.0) {
|
||||
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_timeout_s: " << value;
|
||||
return false;
|
||||
}
|
||||
move_j_settle_timeout_s_ = value;
|
||||
}
|
||||
if (move_j_config.has_settle_position_tolerance_rad()) {
|
||||
const double value = move_j_config.settle_position_tolerance_rad();
|
||||
if (!std::isfinite(value) || value < 0.0) {
|
||||
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_position_tolerance_rad: "
|
||||
<< value;
|
||||
return false;
|
||||
}
|
||||
move_j_position_tolerance_rad_ = value;
|
||||
}
|
||||
if (move_j_config.has_settle_velocity_tolerance_rad_s()) {
|
||||
const double value = move_j_config.settle_velocity_tolerance_rad_s();
|
||||
if (!std::isfinite(value) || value < 0.0) {
|
||||
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_velocity_tolerance_rad_s: "
|
||||
<< value;
|
||||
return false;
|
||||
}
|
||||
move_j_velocity_tolerance_rad_s_ = value;
|
||||
}
|
||||
if (move_j_config.has_settle_stable_sample_count()) {
|
||||
const int value = move_j_config.settle_stable_sample_count();
|
||||
if (value < 1) {
|
||||
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid moveJ settle_stable_sample_count: "
|
||||
<< value;
|
||||
return false;
|
||||
}
|
||||
move_j_stable_sample_count_ = value;
|
||||
}
|
||||
|
||||
const auto& cartesian_controller_config =
|
||||
cfg_.motion().speed_l().speed_l_controller().cartesian_velocity_controller();
|
||||
if (cartesian_controller_config.has_stop_timeout_s() &&
|
||||
(!std::isfinite(cartesian_controller_config.stop_timeout_s()) ||
|
||||
cartesian_controller_config.stop_timeout_s() <= 0.0)) {
|
||||
CMVR_LOG(ERROR) << "[MotorRobotArm] invalid Cartesian stop_timeout_s: "
|
||||
<< cartesian_controller_config.stop_timeout_s();
|
||||
return false;
|
||||
}
|
||||
|
||||
joint_planner_ = JointMotionPlannerFactory::create(cfg_.motion().move_j());
|
||||
if (!joint_planner_) {
|
||||
return false;
|
||||
@ -1077,6 +1248,10 @@ CartesianVelocityController::Config MotorRobotArm::toCartesianVelocityController
|
||||
result.stop_acceleration =
|
||||
config.stop_acceleration() > 0.0 ? config.stop_acceleration()
|
||||
: result.stop_acceleration;
|
||||
if (config.has_stop_timeout_s() && std::isfinite(config.stop_timeout_s()) &&
|
||||
config.stop_timeout_s() > 0.0) {
|
||||
result.stop_timeout_s = config.stop_timeout_s();
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
@ -3,20 +3,22 @@
|
||||
#pragma once
|
||||
|
||||
#include <opencv2/opencv.hpp>
|
||||
#include <mutex>
|
||||
#include "../abstract_device.h"
|
||||
#include <Eigen/Core>
|
||||
#include "cmvr/config/camera_config/camera_config.pb.h"
|
||||
#include "devices/camera/common/include/camera_stream_overlay.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
enum CameraMode {PHOTO_MODE, VIDEO_MODE};
|
||||
|
||||
struct Rs2Intrinsics
|
||||
{
|
||||
float cx;
|
||||
float cy;
|
||||
float fx;
|
||||
float fy;
|
||||
float coeffs[5];
|
||||
float cx{0.0F};
|
||||
float cy{0.0F};
|
||||
float fx{0.0F};
|
||||
float fy{0.0F};
|
||||
float coeffs[5]{};
|
||||
|
||||
};
|
||||
struct StreamFrameData
|
||||
@ -64,11 +66,26 @@ namespace cmvr::device {
|
||||
return false;
|
||||
}
|
||||
|
||||
// Video overlay is a presentation-only snapshot. It is deliberately
|
||||
// kept on the camera so the encoding thread can consume it without
|
||||
// coupling the camera to AprilTag or task code.
|
||||
void setStreamOverlay(const CameraStreamOverlay& overlay) {
|
||||
std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
|
||||
stream_overlay_ = overlay;
|
||||
}
|
||||
|
||||
CameraStreamOverlay streamOverlay() const {
|
||||
std::lock_guard<std::mutex> lock(stream_overlay_mutex_);
|
||||
return stream_overlay_;
|
||||
}
|
||||
|
||||
virtual bool startStreaming() {return true;}
|
||||
virtual void stopStreaming() {}
|
||||
virtual Eigen::Vector3f get3DPointFromPixel(int u, int v) {return {0,0,0};}
|
||||
protected:
|
||||
CameraState state_{};
|
||||
mutable std::mutex stream_overlay_mutex_;
|
||||
CameraStreamOverlay stream_overlay_{};
|
||||
void clear_error_() {
|
||||
this->state_.is_error = false;
|
||||
this->state_.error_message.clear();
|
||||
|
||||
@ -7,9 +7,12 @@
|
||||
#include <opencv2/opencv.hpp>
|
||||
|
||||
#include "speaker/ffmpeg_speaker/include/ffmpeg_ptr.h"
|
||||
#include "devices/camera/common/include/camera_stream_overlay.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
|
||||
struct Rs2Intrinsics;
|
||||
|
||||
struct FfmpegEncoderInfo {
|
||||
std::string codec_name;
|
||||
int width = 0;
|
||||
@ -27,8 +30,23 @@ struct FfmpegEncoderInfo {
|
||||
|
||||
struct CameraStreamEncodeOptions {
|
||||
bool draw_timestamp = false;
|
||||
CameraStreamOverlay overlay;
|
||||
};
|
||||
|
||||
// Draw a frame whose pose is expressed as ^C T_Frame onto a BGR/BGRA image.
|
||||
// The image is modified in place and no camera/perception state is touched.
|
||||
void drawCoordinateFrame(cv::Mat& image,
|
||||
const Eigen::Matrix4d& T_C_Frame,
|
||||
const Rs2Intrinsics& intrinsics,
|
||||
double axis_length_m,
|
||||
const std::string& label);
|
||||
|
||||
Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
|
||||
int source_width,
|
||||
int source_height,
|
||||
int target_width,
|
||||
int target_height);
|
||||
|
||||
class CameraStreamEncoder {
|
||||
public:
|
||||
static bool init(std::shared_ptr<FfmpegEncoderInfo>& encoder,
|
||||
@ -37,6 +55,14 @@ public:
|
||||
int height,
|
||||
int fps);
|
||||
|
||||
static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
|
||||
const cv::Mat& frame,
|
||||
std::vector<uint8_t>& encoded_frame,
|
||||
bool& is_key,
|
||||
const Rs2Intrinsics& intrinsics,
|
||||
const CameraStreamEncodeOptions& options = {});
|
||||
|
||||
// Compatibility overload for callers that only need timestamp drawing.
|
||||
static bool encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
|
||||
const cv::Mat& frame,
|
||||
std::vector<uint8_t>& encoded_frame,
|
||||
|
||||
@ -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
|
||||
@ -1,6 +1,7 @@
|
||||
#include "devices/camera/common/include/camera_stream_encoder.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <ctime>
|
||||
#include <iomanip>
|
||||
#include <sstream>
|
||||
@ -9,6 +10,7 @@
|
||||
#include <opencv2/imgproc.hpp>
|
||||
|
||||
#include "common/base/logging/logger.h"
|
||||
#include "devices/camera/abstract_camera.h"
|
||||
|
||||
namespace cmvr::device {
|
||||
namespace {
|
||||
@ -46,6 +48,41 @@ void drawTimeStamp(cv::Mat& image)
|
||||
cv::putText(image, time_str, text_pos, font_face, font_scale, cv::Scalar(255, 255, 255), thickness);
|
||||
}
|
||||
|
||||
bool projectPoint(const Eigen::Vector3d& point,
|
||||
const Rs2Intrinsics& intrinsics,
|
||||
cv::Point& pixel)
|
||||
{
|
||||
if (!point.allFinite() || point.z() <= 1e-9 ||
|
||||
!std::isfinite(intrinsics.fx) || !std::isfinite(intrinsics.fy) ||
|
||||
!std::isfinite(intrinsics.cx) || !std::isfinite(intrinsics.cy) ||
|
||||
intrinsics.fx <= 0.0f || intrinsics.fy <= 0.0f) {
|
||||
return false;
|
||||
}
|
||||
const double u = static_cast<double>(intrinsics.fx) * point.x() / point.z() +
|
||||
static_cast<double>(intrinsics.cx);
|
||||
const double v = static_cast<double>(intrinsics.fy) * point.y() / point.z() +
|
||||
static_cast<double>(intrinsics.cy);
|
||||
if (!std::isfinite(u) || !std::isfinite(v)) {
|
||||
return false;
|
||||
}
|
||||
pixel = cv::Point(cvRound(u), cvRound(v));
|
||||
return true;
|
||||
}
|
||||
|
||||
void drawOutlinedText(cv::Mat& image,
|
||||
const std::string& text,
|
||||
const cv::Point& origin,
|
||||
const cv::Scalar& color)
|
||||
{
|
||||
constexpr int font_face = cv::FONT_HERSHEY_SIMPLEX;
|
||||
constexpr double font_scale = 0.55;
|
||||
constexpr int thickness = 1;
|
||||
cv::putText(image, text, origin, font_face, font_scale,
|
||||
cv::Scalar(0, 0, 0), thickness + 2, cv::LINE_AA);
|
||||
cv::putText(image, text, origin, font_face, font_scale,
|
||||
color, thickness, cv::LINE_AA);
|
||||
}
|
||||
|
||||
const AVCodec* findEncoder(const std::string& codec_name)
|
||||
{
|
||||
if (codec_name == "h264" || codec_name == "H264") {
|
||||
@ -75,6 +112,77 @@ AVPixelFormat sourcePixelFormat(const cv::Mat& frame)
|
||||
|
||||
} // namespace
|
||||
|
||||
void drawCoordinateFrame(cv::Mat& image,
|
||||
const Eigen::Matrix4d& T_C_Frame,
|
||||
const Rs2Intrinsics& intrinsics,
|
||||
const double axis_length_m,
|
||||
const std::string& label)
|
||||
{
|
||||
if (image.empty() || image.channels() < 3 || !T_C_Frame.allFinite() ||
|
||||
!std::isfinite(axis_length_m) || axis_length_m <= 0.0) {
|
||||
return;
|
||||
}
|
||||
|
||||
const Eigen::Vector4d origin_h(0.0, 0.0, 0.0, 1.0);
|
||||
const Eigen::Vector4d x_h(axis_length_m, 0.0, 0.0, 1.0);
|
||||
const Eigen::Vector4d y_h(0.0, axis_length_m, 0.0, 1.0);
|
||||
const Eigen::Vector4d z_h(0.0, 0.0, axis_length_m, 1.0);
|
||||
const Eigen::Vector3d origin = (T_C_Frame * origin_h).head<3>();
|
||||
const Eigen::Vector3d x = (T_C_Frame * x_h).head<3>();
|
||||
const Eigen::Vector3d y = (T_C_Frame * y_h).head<3>();
|
||||
const Eigen::Vector3d z = (T_C_Frame * z_h).head<3>();
|
||||
|
||||
cv::Point origin_px;
|
||||
if (!projectPoint(origin, intrinsics, origin_px)) {
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Point x_px;
|
||||
cv::Point y_px;
|
||||
cv::Point z_px;
|
||||
constexpr int thickness = 2;
|
||||
if (projectPoint(x, intrinsics, x_px)) {
|
||||
cv::arrowedLine(image, origin_px, x_px, cv::Scalar(0, 0, 255), thickness,
|
||||
cv::LINE_AA, 0, 0.15);
|
||||
drawOutlinedText(image, "X", x_px + cv::Point(4, -4), cv::Scalar(0, 0, 255));
|
||||
}
|
||||
if (projectPoint(y, intrinsics, y_px)) {
|
||||
cv::arrowedLine(image, origin_px, y_px, cv::Scalar(0, 255, 0), thickness,
|
||||
cv::LINE_AA, 0, 0.15);
|
||||
drawOutlinedText(image, "Y", y_px + cv::Point(4, -4), cv::Scalar(0, 255, 0));
|
||||
}
|
||||
if (projectPoint(z, intrinsics, z_px)) {
|
||||
cv::arrowedLine(image, origin_px, z_px, cv::Scalar(255, 0, 0), thickness,
|
||||
cv::LINE_AA, 0, 0.15);
|
||||
drawOutlinedText(image, "Z", z_px + cv::Point(4, -4), cv::Scalar(255, 0, 0));
|
||||
}
|
||||
|
||||
cv::drawMarker(image, origin_px, cv::Scalar(255, 255, 255), cv::MARKER_CROSS, 9, 1,
|
||||
cv::LINE_AA);
|
||||
if (!label.empty()) {
|
||||
drawOutlinedText(image, label, origin_px + cv::Point(7, -7),
|
||||
cv::Scalar(255, 255, 255));
|
||||
}
|
||||
}
|
||||
|
||||
Rs2Intrinsics scaleIntrinsics(const Rs2Intrinsics& intrinsics,
|
||||
const int source_width,
|
||||
const int source_height,
|
||||
const int target_width,
|
||||
const int target_height)
|
||||
{
|
||||
Rs2Intrinsics scaled = intrinsics;
|
||||
if (source_width > 0 && source_height > 0 && target_width > 0 && target_height > 0) {
|
||||
const float sx = static_cast<float>(target_width) / static_cast<float>(source_width);
|
||||
const float sy = static_cast<float>(target_height) / static_cast<float>(source_height);
|
||||
scaled.fx *= sx;
|
||||
scaled.cx *= sx;
|
||||
scaled.fy *= sy;
|
||||
scaled.cy *= sy;
|
||||
}
|
||||
return scaled;
|
||||
}
|
||||
|
||||
FfmpegEncoderInfo::~FfmpegEncoderInfo()
|
||||
{
|
||||
if (frame) {
|
||||
@ -189,6 +297,7 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
|
||||
const cv::Mat& frame,
|
||||
std::vector<uint8_t>& encoded_frame,
|
||||
bool& is_key,
|
||||
const Rs2Intrinsics& intrinsics,
|
||||
const CameraStreamEncodeOptions& options)
|
||||
{
|
||||
encoded_frame.clear();
|
||||
@ -205,10 +314,28 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
|
||||
}
|
||||
|
||||
cv::Mat frame_to_encode = frame;
|
||||
if (options.draw_timestamp) {
|
||||
if (options.draw_timestamp || options.overlay.draw_coordinate_frames) {
|
||||
frame_to_encode = frame.clone();
|
||||
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);
|
||||
if (src_pix_fmt == AV_PIX_FMT_NONE) {
|
||||
@ -294,4 +421,14 @@ bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
|
||||
return true;
|
||||
}
|
||||
|
||||
bool CameraStreamEncoder::encode(std::shared_ptr<FfmpegEncoderInfo>& encoder,
|
||||
const cv::Mat& frame,
|
||||
std::vector<uint8_t>& encoded_frame,
|
||||
bool& is_key,
|
||||
const CameraStreamEncodeOptions& options)
|
||||
{
|
||||
Rs2Intrinsics intrinsics{};
|
||||
return encode(encoder, frame, encoded_frame, is_key, intrinsics, options);
|
||||
}
|
||||
|
||||
} // namespace cmvr::device
|
||||
|
||||
@ -345,12 +345,17 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
|
||||
}
|
||||
|
||||
frame_data.rgbImage = color.clone();
|
||||
frame_data.intrinsics = intrinsics;
|
||||
CameraStreamEncodeOptions encode_options;
|
||||
encode_options.draw_timestamp = enable_stream_timestamp_;
|
||||
encode_options.overlay = streamOverlay();
|
||||
const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
|
||||
intrinsics, color.cols, color.rows, color_to_encode.cols, color_to_encode.rows);
|
||||
if (!CameraStreamEncoder::encode(rgb_encoder_,
|
||||
color_to_encode,
|
||||
frame_data.rgbFrame,
|
||||
frame_data.bKey,
|
||||
encode_intrinsics,
|
||||
encode_options)) {
|
||||
return false;
|
||||
}
|
||||
@ -360,7 +365,6 @@ bool MujocoCamera::getLatestEncodedFrame(StreamFrameData& frame_data, size_t& ne
|
||||
const auto* depth_end = depth_begin + depth.total() * depth.elemSize();
|
||||
frame_data.depthFrame.assign(depth_begin, depth_end);
|
||||
}
|
||||
frame_data.intrinsics = intrinsics;
|
||||
frame_data.width = color_to_encode.cols;
|
||||
frame_data.height = color_to_encode.rows;
|
||||
frame_data.fps = fps;
|
||||
|
||||
@ -772,10 +772,18 @@ void RealsenseCamera::streaming_worker_() {
|
||||
}
|
||||
CameraStreamEncodeOptions encode_options;
|
||||
encode_options.draw_timestamp = enable_stream_timestamp_;
|
||||
encode_options.overlay = streamOverlay();
|
||||
const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
|
||||
frame_data.intrinsics,
|
||||
frame_data.rgbImage.cols,
|
||||
frame_data.rgbImage.rows,
|
||||
rgb_to_encode.cols,
|
||||
rgb_to_encode.rows);
|
||||
success = CameraStreamEncoder::encode(rgbEncoder_,
|
||||
rgb_to_encode,
|
||||
frame_data.rgbFrame,
|
||||
frame_data.bKey,
|
||||
encode_intrinsics,
|
||||
encode_options);
|
||||
// 深度图编码
|
||||
// success = encodeFrameWithEncoder(depthEncoder_, frame_data.depthImage, frame_data.depthFrame, frame_data.depthKey);
|
||||
|
||||
@ -504,10 +504,18 @@ void UVCCamera::streaming_worker_() {
|
||||
}
|
||||
CameraStreamEncodeOptions encode_options;
|
||||
encode_options.draw_timestamp = enable_stream_timestamp_;
|
||||
encode_options.overlay = streamOverlay();
|
||||
const Rs2Intrinsics encode_intrinsics = scaleIntrinsics(
|
||||
frame_data.intrinsics,
|
||||
frame_data.rgbImage.cols,
|
||||
frame_data.rgbImage.rows,
|
||||
rgb_to_encode.cols,
|
||||
rgb_to_encode.rows);
|
||||
success = CameraStreamEncoder::encode(rgbEncoder_,
|
||||
rgb_to_encode,
|
||||
frame_data.rgbFrame,
|
||||
frame_data.bKey,
|
||||
encode_intrinsics,
|
||||
encode_options);
|
||||
if (success) {
|
||||
frame_data.fps = fps_;
|
||||
|
||||
@ -121,6 +121,7 @@ private:
|
||||
|
||||
void stopPbvsMotion();
|
||||
bool buildInitJointPositions(std::vector<double>& positions_out) const;
|
||||
bool isAtInitPosition(const std::vector<double>& positions) const;
|
||||
bool moveToInitPositionBeforeStartIfEnabled();
|
||||
bool moveToInitPositionIfEnabled() const;
|
||||
bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
|
||||
@ -132,6 +133,8 @@ private:
|
||||
void enterFailed(Status status);
|
||||
|
||||
bool updateTouchPressure();
|
||||
void publishCoordinateOverlay();
|
||||
void refreshCoordinateOverlay();
|
||||
|
||||
private:
|
||||
mutable std::mutex mutex_;
|
||||
@ -171,6 +174,10 @@ private:
|
||||
Eigen::Vector3d last_align_error_screen_tag_{Eigen::Vector3d::Zero()};
|
||||
Eigen::Matrix4d T_H_P_{Eigen::Matrix4d::Identity()};
|
||||
double pbvs_command_acceleration_{0.25};
|
||||
// Initialization MoveJ is skipped only when both position and velocity
|
||||
// are within these configured limits.
|
||||
double init_skip_position_tolerance_rad_{1e-3};
|
||||
double init_skip_velocity_tolerance_rad_s_{1e-2};
|
||||
bool locked_target_rotation_valid_{false};
|
||||
Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
|
||||
bool touch_start_position_valid_{false};
|
||||
@ -183,6 +190,7 @@ private:
|
||||
double max_T_B_G_rotation_delta_rad_{0.0};
|
||||
|
||||
Clock::time_point phase_start_time_{};
|
||||
Clock::time_point last_coordinate_overlay_update_time_{};
|
||||
Clock::time_point last_retract_log_time_{};
|
||||
Status final_status_after_retract_{Status::DONE};
|
||||
};
|
||||
|
||||
@ -6,6 +6,7 @@
|
||||
#include <limits>
|
||||
#include <sstream>
|
||||
#include <unordered_map>
|
||||
#include <utility>
|
||||
|
||||
#include <opencv2/imgcodecs.hpp>
|
||||
|
||||
@ -20,6 +21,9 @@ namespace cmvr::task {
|
||||
|
||||
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) {
|
||||
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);
|
||||
}
|
||||
|
||||
Eigen::Matrix3d rotationFromTargetRotvec(const double rx,
|
||||
Eigen::Matrix3d rotationFromTargetEuler(const double rx,
|
||||
const double ry,
|
||||
const double rz) {
|
||||
const Eigen::Vector3d rotation_vector(rx, ry, rz);
|
||||
const double angle = rotation_vector.norm();
|
||||
if (!std::isfinite(angle) || angle <= 1e-12) {
|
||||
return Eigen::Matrix3d::Identity();
|
||||
}
|
||||
return Eigen::AngleAxisd(angle, rotation_vector / angle).toRotationMatrix();
|
||||
// The configured rotations are extrinsic rotations about the fixed G axes,
|
||||
// applied in X -> Y -> Z order. With column-vector transforms this is
|
||||
// represented by left multiplication in reverse order.
|
||||
const Eigen::Matrix3d R_x =
|
||||
Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()).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,
|
||||
@ -282,10 +290,14 @@ bool TouchScreenTask::init() {
|
||||
}
|
||||
|
||||
const auto& devices = config_.devices();
|
||||
const auto& perception_config = config_.perception();
|
||||
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_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;
|
||||
return false;
|
||||
}
|
||||
@ -293,6 +305,7 @@ bool TouchScreenTask::init() {
|
||||
auto& dm = device::DeviceManager::getInstance();
|
||||
auto arm = dm.getDevice<device::RobotArm>(devices.arm_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 external_camera =
|
||||
dm.getDevice<device::AbstractCamera>(devices.external_camera_id());
|
||||
@ -344,23 +357,30 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
|
||||
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_->setTagSize(config_.perception().apriltag().tag_size_m());
|
||||
perception_->setTagSize(tags.screen().size_m());
|
||||
external_perception_ =
|
||||
std::make_shared<perception::AprilTagPerception>(external_camera_);
|
||||
external_perception_->setTagSize(
|
||||
config_.perception().external_apriltag().tag_size_m());
|
||||
external_perception_->setTagSize(tags.screen().size_m());
|
||||
|
||||
tracker_.setPerception(perception_);
|
||||
tracker_.setTargetPointMethod(toTargetPointMethod(
|
||||
config_.perception().apriltag().target_point_method()));
|
||||
hand_camera_config.target_point_method()));
|
||||
|
||||
tcp_pose_tracker_.setPerception(external_perception_);
|
||||
tcp_pose_tracker_.setHandTagId(
|
||||
config_.perception().external_apriltag().hand_tag_id());
|
||||
tcp_pose_tracker_.setScreenTagId(tags.screen().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();
|
||||
if (initialized_) {
|
||||
refreshCoordinateOverlay();
|
||||
tracker_.clear();
|
||||
tracker_.resetActiveTagTracking();
|
||||
tcp_pose_tracker_.clear();
|
||||
@ -385,6 +405,7 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
|
||||
last_T_B_G_.setIdentity();
|
||||
max_T_B_G_translation_delta_m_ = 0.0;
|
||||
max_T_B_G_rotation_delta_rad_ = 0.0;
|
||||
last_coordinate_overlay_update_time_ = Clock::time_point{};
|
||||
last_status_ = Status::IDLE;
|
||||
} else {
|
||||
last_status_ = Status::INVALID_CONFIG;
|
||||
@ -479,6 +500,13 @@ bool TouchScreenTask::step(const double dt) {
|
||||
// << 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_) {
|
||||
case Phase::IDLE:
|
||||
last_status_ = Status::IDLE;
|
||||
@ -515,6 +543,75 @@ bool TouchScreenTask::step(const double dt) {
|
||||
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() {
|
||||
std::lock_guard<std::mutex> lock(mutex_);
|
||||
stopUnlocked();
|
||||
@ -699,9 +796,10 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
|
||||
!config.initialization().has_velocity() ||
|
||||
!config.initialization().has_acceleration() ||
|
||||
!config.has_perception() ||
|
||||
!config.perception().has_apriltag() ||
|
||||
!config.perception().has_external_apriltag() ||
|
||||
!config.perception().has_tags() ||
|
||||
!config.perception().has_hand_camera() ||
|
||||
!config.has_alignment() ||
|
||||
!config.alignment().has_calibration() ||
|
||||
!config.alignment().has_pbvs() ||
|
||||
!config.alignment().has_target() ||
|
||||
!config.alignment().has_error_threshold() ||
|
||||
@ -714,28 +812,33 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
|
||||
!config.has_retract() ||
|
||||
!config.retract().has_twist_tool() ||
|
||||
!config.retract().has_acceleration() ||
|
||||
!config.retract().has_duration_s()) {
|
||||
!config.retract().has_distance_m()) {
|
||||
return false;
|
||||
}
|
||||
|
||||
const auto& apriltag = config.perception().apriltag();
|
||||
const auto& external_apriltag = config.perception().external_apriltag();
|
||||
const auto& perception = config.perception();
|
||||
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& calibration = alignment.calibration();
|
||||
const auto& pbvs = alignment.pbvs();
|
||||
const auto& target = alignment.target();
|
||||
const auto& touch = config.touch();
|
||||
const auto& tactile = touch.tactile();
|
||||
const auto& retract = config.retract();
|
||||
|
||||
if (!apriltag.has_tag_size_m() ||
|
||||
!apriltag.has_depth_policy() ||
|
||||
!apriltag.has_target_point_method() ||
|
||||
!external_apriltag.has_tag_size_m() ||
|
||||
!external_apriltag.has_hand_tag_id() ||
|
||||
!external_apriltag.has_t_h_p() ||
|
||||
!hasMat4(external_apriltag.t_h_p()) ||
|
||||
!target.has_rotation_vector() ||
|
||||
!hasVec3(target.rotation_vector()) ||
|
||||
if (!screen_tag.has_id() ||
|
||||
!screen_tag.has_size_m() ||
|
||||
!hand_tag.has_id() ||
|
||||
!hand_tag.has_size_m() ||
|
||||
!hand_camera.has_depth_policy() ||
|
||||
!hand_camera.has_target_point_method() ||
|
||||
!calibration.has_hand_tag_to_tcp() ||
|
||||
!hasMat4(calibration.hand_tag_to_tcp()) ||
|
||||
!target.has_hand_orientation_g() ||
|
||||
!hasEuler(target.hand_orientation_g()) ||
|
||||
!target.has_mode() ||
|
||||
!hasVec6(alignment.error_threshold()) ||
|
||||
!pbvs.has_position_gain() ||
|
||||
@ -755,8 +858,8 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
|
||||
return false;
|
||||
}
|
||||
|
||||
const Eigen::Vector3d target_rotation =
|
||||
cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
|
||||
const Eigen::Vector3d target_orientation =
|
||||
cmvr::common::math::toEigenEuler(alignment.target().hand_orientation_g());
|
||||
const Eigen::Vector3d position_offset_G =
|
||||
target.has_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());
|
||||
const Eigen::Matrix<double, 6, 1> 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 ||
|
||||
!std::isfinite(external_apriltag.tag_size_m()) ||
|
||||
external_apriltag.tag_size_m() <= 0.0 ||
|
||||
external_apriltag.hand_tag_id() < 0 ||
|
||||
if (screen_tag.id() < 0 || hand_tag.id() < 0 ||
|
||||
screen_tag.id() == hand_tag.id() ||
|
||||
!std::isfinite(screen_tag.size_m()) || screen_tag.size_m() <= 0.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) ||
|
||||
!target_rotation.allFinite() ||
|
||||
!target_orientation.allFinite() ||
|
||||
!position_offset_G.allFinite() ||
|
||||
!rotation_offset_G.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 ||
|
||||
!cmvr::common::math::toEigenVec6(retract.twist_tool()).allFinite() ||
|
||||
!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;
|
||||
}
|
||||
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;
|
||||
if (!appendRequestedTactileRegions(toFingerType(tactile.finger()),
|
||||
toTactileRegion(tactile.region()),
|
||||
@ -899,6 +1017,15 @@ bool TouchScreenTask::applyConfig() {
|
||||
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_) {
|
||||
const auto& tactile = config_.touch().tactile();
|
||||
std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
|
||||
@ -914,15 +1041,18 @@ bool TouchScreenTask::applyConfig() {
|
||||
}
|
||||
}
|
||||
|
||||
const auto& apriltag = config_.perception().apriltag();
|
||||
const auto& external_apriltag = config_.perception().external_apriltag();
|
||||
const auto& perception = config_.perception();
|
||||
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();
|
||||
perception_->setTagSize(apriltag.tag_size_m());
|
||||
external_perception_->setTagSize(external_apriltag.tag_size_m());
|
||||
tracker_.setTargetPointMethod(toTargetPointMethod(apriltag.target_point_method()));
|
||||
perception_->setTagSize(tags.screen().size_m());
|
||||
external_perception_->setTagSize(tags.screen().size_m());
|
||||
tracker_.setTargetPointMethod(toTargetPointMethod(hand_camera_config.target_point_method()));
|
||||
tcp_pose_tracker_.setPerception(external_perception_);
|
||||
tcp_pose_tracker_.setHandTagId(external_apriltag.hand_tag_id());
|
||||
T_H_P_ = toEigenMat4(external_apriltag.t_h_p());
|
||||
tcp_pose_tracker_.setScreenTagId(tags.screen().id());
|
||||
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_.setRotationGain(cmvr::common::math::toEigenVec3(pbvs.rotation_gain()));
|
||||
@ -950,16 +1080,17 @@ bool TouchScreenTask::applyConfig() {
|
||||
<< ", arm_id=" << config_.devices().arm_id()
|
||||
<< ", hand_camera_id=" << config_.devices().camera_id()
|
||||
<< ", external_camera_id=" << config_.devices().external_camera_id()
|
||||
<< ", hand_camera_tag_size_m=" << apriltag.tag_size_m()
|
||||
<< ", external_camera_tag_size_m=" << external_apriltag.tag_size_m()
|
||||
<< ", hand_tag_id=" << external_apriltag.hand_tag_id();
|
||||
<< ", hand_tag_size_m=" << tags.hand().size_m()
|
||||
<< ", screen_tag_size_m=" << tags.screen().size_m()
|
||||
<< ", hand_tag_id=" << tags.hand().id()
|
||||
<< ", screen_tag_id=" << tags.screen().id();
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TouchScreenTask::stepAligning(const double dt) {
|
||||
const auto& alignment = config_.alignment();
|
||||
const auto target_rotation_vector =
|
||||
cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
|
||||
const auto target_orientation =
|
||||
cmvr::common::math::toEigenEuler(alignment.target().hand_orientation_g());
|
||||
const auto& target = alignment.target();
|
||||
const Eigen::Vector3d position_offset_G =
|
||||
target.has_position_offset_g()
|
||||
@ -986,7 +1117,7 @@ bool TouchScreenTask::stepAligning(const double dt) {
|
||||
if (hand_camera_needed) {
|
||||
perception::AprilTagPerception::Options hand_options;
|
||||
hand_options.depth_policy =
|
||||
toDepthPolicy(config_.perception().apriltag().depth_policy());
|
||||
toDepthPolicy(config_.perception().hand_camera().depth_policy());
|
||||
hand_options.detect_tags = true;
|
||||
hand_options.fetch_encoded = false;
|
||||
hand_perception_ok = perception_->update(hand_options);
|
||||
@ -1048,7 +1179,8 @@ bool TouchScreenTask::stepAligning(const double dt) {
|
||||
|
||||
const int tag_id = tracker_.activeTagId();
|
||||
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();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
@ -1068,13 +1200,16 @@ bool TouchScreenTask::stepAligning(const double dt) {
|
||||
external_options.detect_tags = true;
|
||||
external_options.fetch_encoded = false;
|
||||
if (!external_perception_->update(external_options)) {
|
||||
publishCoordinateOverlay();
|
||||
stopPbvsMotion();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_PERCEPTION;
|
||||
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 (align_debug_count_ == 0 && !external_perception_->color().empty()) {
|
||||
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();
|
||||
Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(),
|
||||
target_rotation_vector.y(),
|
||||
target_rotation_vector.z());
|
||||
const Eigen::Matrix3d R_G_H_target =
|
||||
rotationFromTargetEuler(target_orientation.x(),
|
||||
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) {
|
||||
const double offset_angle = rotation_offset_G.norm();
|
||||
R_target = Eigen::AngleAxisd(
|
||||
@ -1406,37 +1546,34 @@ bool TouchScreenTask::stepRetracting() {
|
||||
|
||||
const auto now = Clock::now();
|
||||
const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
|
||||
const double retract_duration_s = config_.retract().duration_s();
|
||||
if (elapsed < retract_duration_s) {
|
||||
const double retract_distance_m = config_.retract().distance_m();
|
||||
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) {
|
||||
last_retract_log_time_ = now;
|
||||
const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{};
|
||||
Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
|
||||
const bool have_current = readCurrentTouchPointPositionBase(current_position_base);
|
||||
const bool have_delta = retract_start_position_valid_ && have_current;
|
||||
Eigen::Vector3d delta_base = Eigen::Vector3d::Zero();
|
||||
if (have_delta) {
|
||||
delta_base = current_position_base - retract_start_position_base_;
|
||||
}
|
||||
if (have_delta) {
|
||||
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed
|
||||
<< "/" << retract_duration_s
|
||||
<< ", distance=" << traveled_distance_m
|
||||
<< "/" << retract_distance_m
|
||||
<< ", 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";
|
||||
}
|
||||
<< delta_base.y() << ", " << delta_base.z() << "]";
|
||||
}
|
||||
last_status_ = Status::RETRACTING;
|
||||
return true;
|
||||
@ -1451,19 +1588,23 @@ bool TouchScreenTask::stepRetracting() {
|
||||
}
|
||||
if (have_final_delta) {
|
||||
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
|
||||
<< ", target_distance_m=" << retract_distance_m
|
||||
<< ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
|
||||
<< ", final_tcp_delta_base=[" << final_delta_base.x() << ", "
|
||||
<< final_delta_base.y() << ", " << final_delta_base.z()
|
||||
<< "], final_tcp_dist=" << final_delta_base.norm();
|
||||
} else {
|
||||
CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed
|
||||
<< ", target_distance_m=" << retract_distance_m
|
||||
<< ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0)
|
||||
<< ", final_tcp_delta_base=unavailable";
|
||||
}
|
||||
|
||||
try {
|
||||
arm_->stopL();
|
||||
} catch (...) {
|
||||
// Stop the retract worker synchronously before switching to the final
|
||||
// joint-position trajectory. stopL() only requests deceleration and can
|
||||
// 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);
|
||||
return false;
|
||||
}
|
||||
@ -1545,6 +1686,27 @@ bool TouchScreenTask::buildInitJointPositions(std::vector<double>& 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() {
|
||||
if (!config_.initialization().before_start()) {
|
||||
return true;
|
||||
@ -1560,6 +1722,12 @@ bool TouchScreenTask::moveToInitPositionBeforeStartIfEnabled() {
|
||||
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::MotionOptions options;
|
||||
options.velocity = config_.initialization().velocity();
|
||||
@ -1581,6 +1749,12 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const {
|
||||
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::MotionOptions options;
|
||||
options.velocity = config_.initialization().velocity();
|
||||
@ -1688,25 +1862,20 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract,
|
||||
|
||||
const auto& retract = config_.retract();
|
||||
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(
|
||||
cmvr::common::math::toEigenVec6(retract.twist_tool()));
|
||||
if (retract_start_position_valid_) {
|
||||
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()
|
||||
<< ", distance_m=" << retract.distance_m()
|
||||
<< ", start_tcp_base=[" << retract_start_position_base_.x() << ", "
|
||||
<< retract_start_position_base_.y() << ", "
|
||||
<< 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,
|
||||
retract.acceleration(),
|
||||
|
||||
@ -27,6 +27,7 @@
|
||||
#include "cmvr/config/motor_config/motor_config.pb.h"
|
||||
#include "cmvr/config/touch_screen_task_config/touch_screen_task_config.pb.h"
|
||||
#include "devices/arm/robot_arm_factory.h"
|
||||
#include "devices/camera/common/include/camera_stream_encoder.h"
|
||||
#include "devices/camera/mujoco_camera/include/mujoco_camera.h"
|
||||
#include "devices/motor/manager/include/motor_manager.h"
|
||||
#include "manager/device_manager/include/device_manager.h"
|
||||
@ -76,6 +77,7 @@ struct TagCenterPixel {
|
||||
};
|
||||
|
||||
bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& camera,
|
||||
const int target_tag_id,
|
||||
const double tag_size_m,
|
||||
const std::chrono::milliseconds timeout,
|
||||
TagCenterPixel& pixel_out)
|
||||
@ -105,6 +107,9 @@ bool detectTagCenterPixel(const std::shared_ptr<cmvr::device::AbstractCamera>& c
|
||||
TagCenterPixel best_pixel;
|
||||
|
||||
for (const auto& tag : perception.tags()) {
|
||||
if (tag.id != target_tag_id) {
|
||||
continue;
|
||||
}
|
||||
const Eigen::Vector3d p_c_tag_center = tag.T_c_t.block<3, 1>(0, 3);
|
||||
Eigen::Vector2d uv = Eigen::Vector2d::Zero();
|
||||
if (!cmvr::ImageProcess::projectCameraPointToPixel(
|
||||
@ -266,8 +271,25 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
|
||||
const auto& config = root_config.touch_screen_task();
|
||||
EXPECT_TRUE(config.has_initialization());
|
||||
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());
|
||||
ASSERT_TRUE(config.alignment().has_calibration());
|
||||
EXPECT_TRUE(config.alignment().calibration().has_hand_tag_to_tcp());
|
||||
ASSERT_TRUE(config.alignment().target().has_hand_orientation_g());
|
||||
EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().rx(), 0.0);
|
||||
EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().ry(), 0.0);
|
||||
EXPECT_DOUBLE_EQ(config.alignment().target().hand_orientation_g().rz(),
|
||||
3.141592653589793);
|
||||
ASSERT_TRUE(config.alignment().has_pbvs());
|
||||
const auto& pbvs = config.alignment().pbvs();
|
||||
EXPECT_DOUBLE_EQ(pbvs.position_gain().x(), 2.0);
|
||||
@ -279,9 +301,38 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) {
|
||||
EXPECT_EQ(config.touch().motion_case(),
|
||||
cmvr::config::TouchScreenTaskTouchConfig::kSpeedL);
|
||||
EXPECT_TRUE(config.has_retract());
|
||||
EXPECT_TRUE(config.retract().has_distance_m());
|
||||
EXPECT_GT(config.retract().distance_m(), 0.0);
|
||||
}
|
||||
}
|
||||
|
||||
TEST(TouchScreenTaskTest, CoordinateFrameProjectionUsesBgrAxisColors) {
|
||||
cv::Mat image = cv::Mat::zeros(240, 320, CV_8UC3);
|
||||
cmvr::device::Rs2Intrinsics intrinsics{};
|
||||
intrinsics.fx = 200.0F;
|
||||
intrinsics.fy = 200.0F;
|
||||
intrinsics.cx = 160.0F;
|
||||
intrinsics.cy = 120.0F;
|
||||
|
||||
Eigen::Matrix4d T_C_Frame = Eigen::Matrix4d::Identity();
|
||||
T_C_Frame(2, 3) = 1.0;
|
||||
cmvr::device::drawCoordinateFrame(image,
|
||||
T_C_Frame,
|
||||
intrinsics,
|
||||
0.2,
|
||||
"G");
|
||||
|
||||
// X projects right in red and Y projects down in green for the camera
|
||||
// convention used by the pinhole projection. Z is blue but projects onto
|
||||
// the origin for this fronto-parallel pose.
|
||||
const cv::Vec3b x_pixel = image.at<cv::Vec3b>(120, 180);
|
||||
const cv::Vec3b y_pixel = image.at<cv::Vec3b>(140, 160);
|
||||
EXPECT_GT(x_pixel[2], x_pixel[1]);
|
||||
EXPECT_GT(x_pixel[2], x_pixel[0]);
|
||||
EXPECT_GT(y_pixel[1], y_pixel[2]);
|
||||
EXPECT_GT(y_pixel[1], y_pixel[0]);
|
||||
}
|
||||
|
||||
TEST(TouchScreenTaskTest, RunEyeToHandTouchInMujoco) {
|
||||
const auto project_root = findProjectRoot();
|
||||
ASSERT_FALSE(project_root.empty());
|
||||
@ -696,7 +747,8 @@ TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
|
||||
std::thread scenario([&] {
|
||||
try {
|
||||
const auto touch_config = loadMujocoTouchConfig(project_root);
|
||||
device_manager.registerDevice(touch_config.devices().camera_id(), camera);
|
||||
device_manager.registerDevice(
|
||||
touch_config.devices().camera_id(), camera);
|
||||
|
||||
cmvr::config::TaskManagerConfig task_manager_config;
|
||||
auto* task_entry = task_manager_config.add_tasks();
|
||||
@ -718,7 +770,8 @@ TEST(TouchScreenTaskTest, DISABLED_RunTouchOnceInMujoco) {
|
||||
|
||||
TagCenterPixel target_pixel;
|
||||
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),
|
||||
target_pixel)) {
|
||||
throw std::runtime_error("failed to detect MuJoCo AprilTag center pixel");
|
||||
|
||||
@ -68,6 +68,8 @@ message CartesianVelocityControllerConfig {
|
||||
double stop_command_velocity_norm = 3;
|
||||
double stop_measured_velocity_norm = 4;
|
||||
double stop_acceleration = 5;
|
||||
// 等待 Cartesian 速度运动停止的最长时间，单位为秒。
|
||||
optional double stop_timeout_s = 6;
|
||||
}
|
||||
|
||||
message ToppraJointMotionPlannerConfig {
|
||||
@ -81,6 +83,15 @@ message MoveJConfig {
|
||||
oneof algorithm {
|
||||
ToppraJointMotionPlannerConfig toppra_joint_motion_planner = 1;
|
||||
}
|
||||
|
||||
// MoveJ 轨迹发送完成后，等待关节实际状态稳定的最长时间，单位为秒。
|
||||
optional double settle_timeout_s = 2;
|
||||
// MoveJ 完成时允许的最大关节位置误差，单位为弧度。
|
||||
optional double settle_position_tolerance_rad = 3;
|
||||
// MoveJ 完成时允许的最大关节速度，单位为弧度/秒。
|
||||
optional double settle_velocity_tolerance_rad_s = 4;
|
||||
// 位置和速度连续满足条件的采样次数。
|
||||
optional int32 settle_stable_sample_count = 5;
|
||||
}
|
||||
|
||||
message MoveLPlannerConfig {
|
||||
|
||||
@ -33,7 +33,6 @@ message JointLimitAvoidanceConfig {
|
||||
double gain = 2;
|
||||
double margin_ratio = 3;
|
||||
double max_push = 4;
|
||||
double weight = 5;
|
||||
}
|
||||
|
||||
message JointLimitPolicyConfig {
|
||||
|
||||
@ -15,10 +15,10 @@ message TouchScreenTaskDevicesConfig {
|
||||
reserved 1;
|
||||
reserved "robot_id";
|
||||
|
||||
optional string arm_id = 4;
|
||||
optional string dexhand_id = 2;
|
||||
optional string camera_id = 3;
|
||||
optional string arm_id = 4;
|
||||
optional string external_camera_id = 5;
|
||||
optional string dexhand_id = 2;
|
||||
}
|
||||
|
||||
message TouchScreenTaskInitializationConfig {
|
||||
@ -27,26 +27,56 @@ message TouchScreenTaskInitializationConfig {
|
||||
repeated TouchScreenInitJointPoint joint_positions = 3;
|
||||
optional double velocity = 4;
|
||||
optional double acceleration = 5;
|
||||
// 当前关节位置误差小于该值时可跳过初始化 MoveJ，单位为弧度。
|
||||
optional double skip_position_tolerance_rad = 6;
|
||||
// 当前关节速度小于该值时才允许跳过初始化 MoveJ，单位为弧度/秒。
|
||||
optional double skip_velocity_tolerance_rad_s = 7;
|
||||
}
|
||||
|
||||
message TouchScreenTaskExternalApriltagConfig {
|
||||
optional double tag_size_m = 1;
|
||||
optional int32 hand_tag_id = 2;
|
||||
.cmvr.common.Mat4 t_h_p = 3;
|
||||
message TouchScreenTaskTagConfig {
|
||||
optional int32 id = 1;
|
||||
optional double size_m = 2;
|
||||
}
|
||||
|
||||
message TouchScreenTaskTagsConfig {
|
||||
TouchScreenTaskTagConfig screen = 1;
|
||||
TouchScreenTaskTagConfig hand = 2;
|
||||
}
|
||||
|
||||
message TouchScreenTaskHandCameraConfig {
|
||||
reserved 1;
|
||||
reserved "camera_id";
|
||||
optional TouchScreenDepthPolicy depth_policy = 2;
|
||||
optional TouchScreenTargetPointMethod target_point_method = 3;
|
||||
}
|
||||
|
||||
message TouchScreenTaskPerceptionConfig {
|
||||
TouchScreenApriltagConfig apriltag = 1;
|
||||
TouchScreenTaskExternalApriltagConfig external_apriltag = 2;
|
||||
reserved 1, 2, 6;
|
||||
reserved "apriltag", "external_apriltag", "external_camera";
|
||||
|
||||
TouchScreenTaskTagsConfig tags = 4;
|
||||
TouchScreenTaskHandCameraConfig hand_camera = 5;
|
||||
}
|
||||
|
||||
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;
|
||||
.cmvr.common.Vec3 position_offset_G = 4;
|
||||
.cmvr.common.Vec3 rotation_offset_G = 5;
|
||||
}
|
||||
|
||||
message TouchScreenTaskAlignmentCalibrationConfig {
|
||||
.cmvr.common.Mat4 hand_tag_to_tcp = 1;
|
||||
reserved 2;
|
||||
reserved "external_camera_to_base";
|
||||
}
|
||||
|
||||
message TouchScreenTaskAlignmentConfig {
|
||||
reserved 1;
|
||||
reserved "kinematics";
|
||||
@ -56,6 +86,7 @@ message TouchScreenTaskAlignmentConfig {
|
||||
optional double timeout_s = 6;
|
||||
optional bool pause_when_reached = 7;
|
||||
TouchScreenTaskPbvsConfig pbvs = 8;
|
||||
TouchScreenTaskAlignmentCalibrationConfig calibration = 9;
|
||||
}
|
||||
|
||||
message TouchScreenTouchSpeedLConfig {
|
||||
@ -93,7 +124,10 @@ message TouchScreenTaskTouchConfig {
|
||||
message TouchScreenTaskRetractConfig {
|
||||
.cmvr.common.Vec6 twist_tool = 1;
|
||||
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 {
|
||||
@ -104,6 +138,10 @@ message TouchScreenTaskConfig {
|
||||
TouchScreenTaskTouchConfig touch = 5;
|
||||
TouchScreenTaskRetractConfig retract = 6;
|
||||
optional string id = 7;
|
||||
// 是否在外部相机编码后的 gRPC 视频流中绘制坐标系，仅影响显示帧；未配置时默认开启。
|
||||
optional bool debug_draw_coordinate_frames = 8;
|
||||
// G/H 坐标轴长度，单位为米。
|
||||
optional double debug_coordinate_axis_length_m = 9;
|
||||
}
|
||||
|
||||
message TouchScreenTaskRootConfig {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user