diff --git a/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt b/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt index 120e91f9..3194deb1 100644 --- a/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt +++ b/cmvr-es/algorithms/controllers/arm_control/CMakeLists.txt @@ -11,3 +11,7 @@ target_link_libraries(arm_control add_library(cmvr_es::algorithms::arm_control ALIAS arm_control) install(TARGETS arm_control LIBRARY DESTINATION lib) + +add_executable(cartesian_velocity_controller_test src/cartesian_velocity_controller_test.cpp) +target_link_libraries(cartesian_velocity_controller_test PRIVATE + cmvr_es::algorithms::arm_control gtest gtest_main pthread) diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h index d8a728ed..cc6fc7ff 100644 --- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h +++ b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h @@ -46,6 +46,9 @@ public: double duration, FrameType frame); Result stop(std::optional acceleration = std::nullopt); + Result speedL(const CartesianVelocity& velocity, const SpeedLOptions& options, + double duration, FrameType frame); + SpeedLReference getReference() const; void shutdown(); bool busy() const { return busy_.load(); } @@ -77,6 +80,9 @@ private: CartesianVelocity target_twist_{}; FrameType target_frame_{FrameType::Base}; double target_acceleration_{0.25}; + SpeedLOptions target_options_{}; + SpeedLReference reference_{}; + CartesianVelocity command_twist_snapshot_{}; std::uint64_t command_version_{0}; std::atomic busy_{false}; }; diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp index 91ed55b3..073a7d47 100644 --- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp +++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp @@ -61,16 +61,26 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, const double duration, const FrameType frame) { - if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) { + SpeedLOptions options; + options.acceleration = acceleration; + return speedL(velocity, options, duration, frame); +} + +Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, + const SpeedLOptions& options, + const double duration, const FrameType frame) +{ + const double acceleration = options.acceleration; + if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || + !std::isfinite(acceleration) || acceleration <= 0.0 || + !std::isfinite(duration) || duration < 0.0 || !std::isfinite(twistNorm_(velocity)) || + (options.linear_jerk && (!std::isfinite(*options.linear_jerk) || *options.linear_jerk <= 0.0))) { return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input"); } if (!worker_ || !worker_->joinable()) { if (busy_.exchange(true)) { return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); } - } else { - // speedL is a streaming command: an existing worker may receive a new target. - busy_.store(true); } ensureWorkerStarted_(); @@ -81,7 +91,10 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity, target_twist_ = velocity; target_acceleration_ = acceleration; target_frame_ = frame; + target_options_ = options; + reference_ = {}; command_active_ = true; + busy_.store(true); command_version = ++command_version_; } cv_.notify_all(); @@ -143,10 +156,14 @@ void CartesianVelocityController::shutdown() CartesianVelocity CartesianVelocityController::getCommandTwistBase() const { - if (!planner_) { - return {}; - } - return planner_->getSpeedLCommandTwistBase(); + std::lock_guard lock(mutex_); + return command_twist_snapshot_; +} + +SpeedLReference CartesianVelocityController::getReference() const +{ + std::lock_guard lock(mutex_); + return reference_; } void CartesianVelocityController::ensureWorkerStarted_() @@ -167,6 +184,9 @@ void CartesianVelocityController::workerLoop_() CartesianVelocity target_twist; double acceleration = 0.25; FrameType target_frame = FrameType::Base; + SpeedLOptions options; + std::uint64_t applied_version = 0; + bool capture_reference = false; { std::unique_lock lock(mutex_); cv_.wait(lock, [&]() { @@ -195,9 +215,13 @@ void CartesianVelocityController::workerLoop_() target_twist = target_twist_; acceleration = target_acceleration_; target_frame = target_frame_; + options = target_options_; + options.acceleration = acceleration; + applied_version = command_version_; + capture_reference = options.capture_reference && !reference_.valid; } - if (!planner_->updateSpeedLAcceleration(acceleration)) { + if (!planner_->updateSpeedLLimits(options)) { if (twistNorm_(target_twist) < config_.stop_twist_norm) { abortCommand_(); break; @@ -221,6 +245,13 @@ void CartesianVelocityController::workerLoop_() } std::vector qd_cmd; + SpeedLReference reference; + if (capture_reference && + !planner_->captureSpeedLReference(q_now, target_twist, target_frame, reference)) { + CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] cannot capture motion reference"; + abortCommand_(); + break; + } if (!planner_->speedLStep(target_twist, dt, q_now, qd_now, qd_cmd, target_frame)) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] speedLStep failed, target_twist=[" << target_twist.vx << ", " << target_twist.vy << ", " @@ -249,15 +280,32 @@ void CartesianVelocityController::workerLoop_() continue; } + { + std::lock_guard lock(mutex_); + command_twist_snapshot_ = planner_->getSpeedLCommandTwistBase(); + if (capture_reference && command_version_ == applied_version) { + reference.command_version = applied_version; + reference_ = reference; + // Lock a captured Tool-frame translation in Base for the + // entire command, matching its distance reference axis. + target_twist_ = reference.target_base; + target_frame_ = FrameType::Base; + } + } + if (twistNorm_(target_twist) < config_.stop_twist_norm && velocityNorm_(qd_cmd) < config_.stop_command_velocity_norm && velocityNorm_(qd_now) < config_.stop_measured_velocity_norm) { { std::lock_guard lock(mutex_); + // A newer target may have arrived during planning/I/O. + if (command_version_ != applied_version) continue; command_active_ = false; + // Serialize the final zero and busy transition with new + // submissions, not only the version comparison. + sendZero_(); + busy_.store(false); } - sendZero_(); - busy_.store(false); break; } @@ -279,6 +327,8 @@ void CartesianVelocityController::requestStop_(const std::optional accel target_frame_ = FrameType::Base; target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration; + target_options_.acceleration = target_acceleration_; + target_options_.capture_reference = false; command_active_ = true; ++command_version_; } @@ -292,9 +342,9 @@ void CartesianVelocityController::abortCommand_() command_active_ = false; target_twist_ = {}; target_frame_ = FrameType::Base; + sendZero_(); + busy_.store(false); } - sendZero_(); - busy_.store(false); } void CartesianVelocityController::sendZero_() diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller_test.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller_test.cpp new file mode 100644 index 00000000..e9ee8c78 --- /dev/null +++ b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller_test.cpp @@ -0,0 +1,127 @@ +#include +#include "algorithms/controllers/arm_control/include/cartesian_velocity_controller.h" +#include +#include +#include +#include + +namespace cmvr::device { +namespace { +using namespace std::chrono_literals; +class Planner final : public CartesianMotionPlanner { +public: + bool configureSpeedL(const config::SpeedLPlannerConfig&, std::size_t) override { return true; } + bool configureMoveL(const config::MoveLPlannerConfig&) override { return true; } + bool planMoveL(const CartesianPose&, const std::vector&, const std::vector&, + double, double, double, FrameType, CartesianJointTrajectory&) override { return false; } + bool updateSpeedLAcceleration(double) override { return true; } + bool updateSpeedLLimits(const SpeedLOptions& options) override { + applied_jerk.store(options.linear_jerk.value_or(10.0)); return true; + } + bool captureSpeedLReference(const std::vector&, const CartesianVelocity& target, + FrameType, SpeedLReference& ref) override { + ref.valid = true; + ref.tcp_pose_base.y = .5; + ref.target_base.vx = target.vy; + return true; + } + bool speedLStep(const CartesianVelocity& v, double, const std::vector&, + const std::vector&, std::vector& out, FrameType) override { + current = v; out = {v.vy}; return true; + } + CartesianVelocity getSpeedLCommandTwistBase() const override { return current; } + CartesianVelocity current; + std::atomic applied_jerk{0.0}; +}; +TEST(CartesianVelocityController, CompletedStopCannotClearNewReversal) { + std::mutex mutex; + std::condition_variable cv; + bool forward_sent = false, block_zero = false, zero_entered = false; + bool release_zero = false, reverse_sent = false; + auto planner = std::make_shared(); + CartesianVelocityController controller({}, planner, 1, + [](auto& q, auto& qd) { q = {0}; qd = {0}; return true; }, + [&](const JointVelocityCommand& command, double) { + std::unique_lock lock(mutex); + forward_sent |= command.velocity[0] > 0; + reverse_sent |= command.velocity[0] < 0; + if (command.velocity[0] == 0 && block_zero && !zero_entered) { + zero_entered = true; cv.notify_all(); + cv.wait_for(lock, 2s, [&] { return release_zero; }); + } + cv.notify_all(); return Result::success(); + }); + CartesianVelocity v; v.vy = .08; + ASSERT_TRUE(controller.speedL(v, 3, 0, FrameType::Base).ok()); + { + std::unique_lock lock(mutex); + ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return forward_sent; })); + block_zero = true; + } + ASSERT_TRUE(controller.stop(3).ok()); + { + std::unique_lock lock(mutex); + ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return zero_entered; })); + } + v.vy = -.08; + ASSERT_TRUE(controller.speedL(v, 3, 0, FrameType::Base).ok()); + { + std::unique_lock lock(mutex); + release_zero = true; cv.notify_all(); + EXPECT_TRUE(cv.wait_for(lock, 1s, [&] { return reverse_sent; })); + } + EXPECT_TRUE(controller.busy()); + controller.shutdown(); +} +TEST(CartesianVelocityController, PublishesReferenceAfterSendAndKeepsCommandLimits) { + std::mutex mutex; + std::condition_variable cv; + bool entered = false, release = false; + auto planner = std::make_shared(); + CartesianVelocityController controller({}, planner, 1, + [](auto& q, auto& qd) { q = {0}; qd = {0}; return true; }, + [&](const JointVelocityCommand&, double) { + std::unique_lock lock(mutex); + if (!entered) { + entered = true; cv.notify_all(); + cv.wait_for(lock, 2s, [&] { return release; }); + } + return Result::success(); + }); + CartesianVelocity v; v.vy = -.08; + SpeedLOptions options; + options.acceleration = 3; + options.linear_jerk = 60; + options.capture_reference = true; + ASSERT_TRUE(controller.speedL(v, options, 0, FrameType::Tool).ok()); + { + std::unique_lock lock(mutex); + ASSERT_TRUE(cv.wait_for(lock, 1s, [&] { return entered; })); + EXPECT_FALSE(controller.getReference().valid); + release = true; cv.notify_all(); + } + const auto deadline = std::chrono::steady_clock::now() + 1s; + while (!controller.getReference().valid && std::chrono::steady_clock::now() < deadline) + std::this_thread::sleep_for(1ms); + const auto ref = controller.getReference(); + EXPECT_TRUE(ref.valid); + EXPECT_GT(ref.command_version, 0U); + EXPECT_DOUBLE_EQ(ref.tcp_pose_base.y, .5); + EXPECT_DOUBLE_EQ(ref.target_base.vx, -.08); + EXPECT_DOUBLE_EQ(planner->applied_jerk.load(), 60); + controller.shutdown(); +} +TEST(CartesianVelocityController, RejectsInvalidMotionLimitsBeforeStarting) { + auto planner = std::make_shared(); + CartesianVelocityController controller({}, planner, 1, + [](auto&, auto&) { return false; }, + [](const auto&, double) { return Result::success(); }); + SpeedLOptions options; + options.linear_jerk = std::numeric_limits::quiet_NaN(); + EXPECT_FALSE(controller.speedL({}, options, 0, FrameType::Base).ok()); + options.linear_jerk = -1; + EXPECT_FALSE(controller.speedL({}, options, 0, FrameType::Base).ok()); + EXPECT_FALSE(controller.busy()); +} +} +} diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt index 33026787..f5313688 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/arm_motion/CMakeLists.txt @@ -25,3 +25,11 @@ target_link_libraries(toppra_joint_motion_planner_test gtest gtest_main ) + +add_executable(pinocchio_speedl_limits_test + cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp +) +target_compile_definitions(pinocchio_speedl_limits_test PRIVATE + CMVR_TEST_SOURCE_DIR="${PROJECT_SOURCE_DIR}") +target_link_libraries(pinocchio_speedl_limits_test PRIVATE + cmvr_es::algorithms::arm_motion gtest gtest_main) diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h index 42a553c3..7cb782c5 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/cartesian_motion_planner.h @@ -44,6 +44,11 @@ public: FrameType frame) = 0; virtual bool updateSpeedLAcceleration(double acceleration) = 0; + virtual bool updateSpeedLLimits(const SpeedLOptions& options) { + return !options.linear_jerk && updateSpeedLAcceleration(options.acceleration); + } + virtual bool captureSpeedLReference(const std::vector&, + const CartesianVelocity&, FrameType, SpeedLReference&) { return false; } virtual CartesianVelocity getSpeedLCommandTwistBase() const = 0; }; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h index d0eddc48..b83848dc 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h @@ -37,6 +37,9 @@ public: FrameType frame) override; bool updateSpeedLAcceleration(double acceleration) override; + bool updateSpeedLLimits(const SpeedLOptions& options) override; + bool captureSpeedLReference(const std::vector& q, const CartesianVelocity& target, + FrameType frame, SpeedLReference& reference) override; CartesianVelocity getSpeedLCommandTwistBase() const override; private: @@ -96,6 +99,8 @@ private: Eigen::Vector3d speedl_line_start_tcp_base_{Eigen::Vector3d::Zero()}; Eigen::Vector3d speedl_line_direction_base_{Eigen::Vector3d::Zero()}; double speedl_applied_acceleration_{0.25}; + double speedl_applied_angular_acceleration_{0.25}; + double speedl_applied_linear_jerk_{-1.0}; bool speedl_line_check_active_{false}; bool speedl_line_deviation_warned_{false}; bool speedl_line_direction_warned_{false}; diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp index 08de05d0..7197ba65 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp @@ -121,6 +121,7 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne } cartesian_motion::configureTwistLimiterFromSpeedLConfig(twist_limiter_, speedl_config_); + speedl_applied_linear_jerk_ = -1.0; prev_qdot_command_.assign(dof, 0.0); speedl_command_twist_base_.setZero(); @@ -129,6 +130,7 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne speedl_line_direction_warned_ = false; speedl_stop_active_ = false; speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0); + speedl_applied_angular_acceleration_ = positiveOr(speedl_config_.angular_acceleration_max(), 5.0); speedl_configured_ = true; return true; } @@ -920,7 +922,8 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target speedl_stop_active_ = false; // 从静止开始一个新的 speedL command。 - if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { + if (!twist_limiter_.isMoving() && + speedl_command_twist_base_.squaredNorm() <= 1e-12) { twist_limiter_.initialize( Eigen::Matrix::Zero()); } @@ -1249,18 +1252,60 @@ std::string PinocchioCartesianMotionPlanner::describeJointLimitCandidates_( bool PinocchioCartesianMotionPlanner::updateSpeedLAcceleration(const double acceleration) { - if (!speedl_configured_ || acceleration <= 0.0) { + SpeedLOptions options; + options.acceleration = acceleration; + return updateSpeedLLimits(options); +} + +bool PinocchioCartesianMotionPlanner::updateSpeedLLimits(const SpeedLOptions& options) +{ + const double jerk_max = positiveOr(speedl_config_.linear_jerk_max(), 10.0); + const double requested_jerk = options.linear_jerk.value_or(jerk_max); + // Validate before clamping: invalid requests must not become valid limits. + if (!speedl_configured_ || !std::isfinite(options.acceleration) || options.acceleration <= 0.0 || + !std::isfinite(requested_jerk) || requested_jerk <= 0.0) { return false; } - if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { + const double acceleration = std::min(options.acceleration, + positiveOr(speedl_config_.linear_acceleration_max(), 5.0)); + const double angular_acceleration = std::min(options.acceleration, + positiveOr(speedl_config_.angular_acceleration_max(), 5.0)); + const double jerk = std::min(requested_jerk, jerk_max); + if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9 && + std::abs(speedl_applied_angular_acceleration_ - angular_acceleration) <= 1e-9 && + std::abs(speedl_applied_linear_jerk_ - jerk) <= 1e-9) { return true; } - cartesian_motion::updateTwistLimiterAcceleration( - twist_limiter_, - speedl_config_, - acceleration); + twist_limiter_.setLinearConstraints( + positiveOr(speedl_config_.linear_velocity_max(), .55), + acceleration, jerk); + twist_limiter_.setAngularConstraints( + positiveOr(speedl_config_.angular_velocity_max(), 1.0), + angular_acceleration, positiveOr(speedl_config_.angular_jerk_max(), 12.0)); speedl_applied_acceleration_ = acceleration; + speedl_applied_angular_acceleration_ = angular_acceleration; + speedl_applied_linear_jerk_ = jerk; + return true; +} + +bool PinocchioCartesianMotionPlanner::captureSpeedLReference( + const std::vector& q, const CartesianVelocity& target, + const FrameType frame, SpeedLReference& reference) +{ + Eigen::Matrix4d pose; + if (!solver_ || !solver_->fk(q, pose, true) || !pose.allFinite()) return false; + auto twist = common::math::velocityToVector(target); + if (frame == FrameType::Tool) { + const Eigen::Matrix3d rotation = pose.block<3, 3>(0, 0); + twist.head<3>() = rotation * twist.head<3>().eval(); + twist.tail<3>() = rotation * twist.tail<3>().eval(); + } else if (frame != FrameType::Base) { + return false; + } + reference.tcp_pose_base = common::math::matrixToPose(pose); + reference.target_base = common::math::vectorToVelocity(twist); + reference.valid = true; return true; } diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp new file mode 100644 index 00000000..1a7bc1e1 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/test/pinocchio_speedl_limits_test.cpp @@ -0,0 +1,157 @@ +#include + +#include +#include +#include +#include + +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_dls_ik_solver.h" +#include "algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/include/pinocchio_cartesian_motion_planner.h" +#include "common/io/proto_file_io.h" +#include "common/math/transform_math.h" + +namespace cmvr::device { +namespace { +constexpr double kDt = .001; +using Twist = Eigen::Matrix; + +// Exercise the production planner and real URDF kinematics, without motor I/O. +class PinocchioSpeedLLimits : public ::testing::Test { +protected: + void SetUp() override { + const auto root = std::filesystem::path(CMVR_TEST_SOURCE_DIR); + config::ArmRootConfig arms; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (root / "cmvr-es/config/devices/arm/arm.pb.txt").string(), &arms)); + ASSERT_GT(arms.arm().robot_arms_size(), 0); + auto ik = arms.arm().robot_arms(0).kinematics().pinocchio_dls_ik_solver(); + ik.set_urdf_path((root / "model/xiaoyan_description/dual_arm.urdf").string()); + solver_ = std::make_shared(ik); + ASSERT_TRUE(solver_->init()); + planner_ = std::make_unique(solver_); + limits_.set_linear_velocity_max(.2); + limits_.set_linear_acceleration_max(.4); + limits_.set_linear_jerk_max(2); + limits_.set_angular_velocity_max(1); + limits_.set_angular_acceleration_max(.8); + limits_.set_angular_jerk_max(4); + limits_.set_enforce_joint_acceleration_limits(false); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + } + + struct Peaks { double velocity{0}, acceleration{0}, jerk{0}; }; + Peaks sample(const CartesianVelocity& target, const SpeedLOptions& options, + bool angular = false, int steps = 2200, bool legacy_api = false) { + Peaks peaks; + Twist previous = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase()); + Twist previous_acceleration = Twist::Zero(); + for (int i = 0; i < steps; ++i) { + const bool updated = legacy_api + ? planner_->updateSpeedLAcceleration(options.acceleration) + : planner_->updateSpeedLLimits(options); + if (!updated || !planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)) { + ADD_FAILURE() << "Planning failed at sample " << i; + break; + } + const Twist velocity = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase()); + const Twist acceleration = (velocity - previous) / kDt; + const Twist jerk = (acceleration - previous_acceleration) / kDt; + const int offset = angular ? 3 : 0; + peaks.velocity = std::max(peaks.velocity, velocity.segment<3>(offset).norm()); + peaks.acceleration = std::max(peaks.acceleration, acceleration.segment<3>(offset).norm()); + peaks.jerk = std::max(peaks.jerk, jerk.segment<3>(offset).norm()); + previous = velocity; + previous_acceleration = acceleration; + } + return peaks; + } + + void expectPeaks(const Peaks& p, double velocity, double acceleration, double jerk) { + // These trajectories contain plateaus: limits must be reached, as well + // as obeyed, so an unintended smaller cap cannot pass the test. + EXPECT_NEAR(p.velocity, velocity, 1e-8); + EXPECT_NEAR(p.acceleration, acceleration, 1e-8); + EXPECT_NEAR(p.jerk, jerk, 1e-6); + } + + std::shared_ptr solver_; + std::unique_ptr planner_; + config::SpeedLPlannerConfig limits_; + const std::vector q_{.25, 1, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; + const std::vector qd_ = std::vector(7, 0); + std::vector command_; +}; + +TEST_F(PinocchioSpeedLLimits, ExcessiveRequestsRespectAllLinearLimits) { + SpeedLOptions options; + options.acceleration = 60; + options.linear_jerk = 60; + CartesianVelocity target; + target.vx = .6; + target.vy = .8; + expectPeaks(sample(target, options), .2, .4, 2); +} + +TEST_F(PinocchioSpeedLLimits, LowerRequestsRemainEffective) { + SpeedLOptions options; + options.acceleration = .15; + options.linear_jerk = .8; + CartesianVelocity target; + target.vy = 1; + expectPeaks(sample(target, options), .2, .15, .8); +} + +TEST_F(PinocchioSpeedLLimits, OmittedJerkRestoresArmLimit) { + SpeedLOptions options; + options.acceleration = .4; + options.linear_jerk = .3; + ASSERT_TRUE(planner_->updateSpeedLLimits(options)); + options.linear_jerk.reset(); + CartesianVelocity target; + target.vy = 1; + expectPeaks(sample(target, options), .2, .4, 2); +} + +TEST_F(PinocchioSpeedLLimits, LegacyAccelerationAndStopAreCapped) { + SpeedLOptions options; + options.acceleration = 60; + CartesianVelocity target; + target.vy = 1; + expectPeaks(sample(target, options, false, 1200, true), .2, .4, 2); + const auto stop = sample({}, options, false, 1200, true); + EXPECT_LE(stop.velocity, .2); + EXPECT_NEAR(stop.acceleration, .4, 1e-8); + EXPECT_NEAR(stop.jerk, 2, 1e-6); + EXPECT_NEAR(planner_->getSpeedLCommandTwistBase().vy, 0, 1e-12); +} + +TEST_F(PinocchioSpeedLLimits, AngularAccelerationHasItsOwnCapAndCache) { + limits_.set_linear_acceleration_max(.2); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + SpeedLOptions options; + options.acceleration = .3; + ASSERT_TRUE(planner_->updateSpeedLLimits(options)); + // Linear effective acceleration stays at .2, but angular must change. + options.acceleration = .6; + CartesianVelocity target; + target.wy = 2; + expectPeaks(sample(target, options, true), 1, .6, 4); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + options.acceleration = 60; + expectPeaks(sample(target, options, true), 1, .8, 4); +} + +TEST_F(PinocchioSpeedLLimits, InvalidRequestsAreRejectedBeforeClamping) { + for (double invalid : {0.0, -1.0, std::numeric_limits::infinity(), + std::numeric_limits::quiet_NaN()}) { + SpeedLOptions options; + options.acceleration = invalid; + EXPECT_FALSE(planner_->updateSpeedLLimits(options)); + EXPECT_FALSE(planner_->updateSpeedLAcceleration(invalid)); + options.acceleration = .3; + options.linear_jerk = invalid; + EXPECT_FALSE(planner_->updateSpeedLLimits(options)); + } +} +} // namespace +} // namespace cmvr::device diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h b/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h index 66ab7e88..97dde369 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h +++ b/cmvr-es/algorithms/motion_planner/arm_motion/common/include/twist_limiter_config.h @@ -1,6 +1,8 @@ #ifndef CMVR_ES_TWIST_LIMITER_CONFIG_H #define CMVR_ES_TWIST_LIMITER_CONFIG_H +#include + #include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" #include "cmvr/config/arm_config/arm_config.pb.h" #include "common/config/config_files.h" @@ -28,6 +30,8 @@ inline void configureTwistLimiterFromSpeedLConfig( ? config.linear_reverse_cos_threshold() : -0.8660254037844386, positiveOr(config.linear_reverse_switch_speed_threshold(), 1e-3)); + limiter.setContinuousLinearReversal(!config.has_continuous_linear_reversal() || + config.continuous_linear_reversal()); limiter.initialize(Eigen::Matrix::Zero()); } @@ -39,10 +43,10 @@ inline void updateTwistLimiterAcceleration( using cmvr::common::config::positiveOr; limiter.setLinearConstraints(positiveOr(config.linear_velocity_max(), 0.55), - acceleration, + std::min(acceleration, positiveOr(config.linear_acceleration_max(), 5.0)), positiveOr(config.linear_jerk_max(), 10.0)); limiter.setAngularConstraints(positiveOr(config.angular_velocity_max(), 1.0), - acceleration, + std::min(acceleration, positiveOr(config.angular_acceleration_max(), 5.0)), positiveOr(config.angular_jerk_max(), 12.0)); } diff --git a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt index 36881029..d815332b 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt @@ -20,6 +20,10 @@ target_link_libraries(base_motion PUBLIC ) add_library(cmvr_es::base_motion ALIAS base_motion) +add_executable(cartesian_twist_limiter_reversal_test + cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp) +target_link_libraries(cartesian_twist_limiter_reversal_test PRIVATE + cmvr_es::base_motion gtest gtest_main) install(TARGETS base_motion LIBRARY DESTINATION lib) add_executable(toppra_multi_waypoint_test diff --git a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h index 220389f0..f20e9ddf 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h +++ b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h @@ -16,7 +16,8 @@ enum class CartesianFrame * @brief 6维末端 twist 在线限幅器（基于 SCurveVelocityPlanner1D） * * 线速度部分： - * - 模长使用 SCurveVelocityPlanner1D 做 jerk-limited 速度规划 + * - 固定轴上的有符号速度使用 SCurveVelocityPlanner1D 做 jerk-limited 规划 + * - 同轴反向连续过零，保留加速度；可显式选择旧的停止后换向策略 * - 运动中锁定当前方向，不做方向插值 * - 若目标方向与当前方向不共线，则采用“先减速到0，再切方向”的 switch policy * @@ -67,6 +68,10 @@ public: void setLinearReverseSwitchPolicy(double cos_threshold, double switch_speed_threshold); + // Same-axis reversal uses a signed velocity without resetting acceleration + // at zero. Disable only to reproduce the legacy stop-then-switch policy. + void setContinuousLinearReversal(bool enabled) { continuous_linear_reversal_ = enabled; } + /** * @brief 初始化当前 twist 状态 * @@ -120,7 +125,7 @@ public: * * 作用： * - 更新当前执行方向 - * - 用测得模长与模长加速度同步两个 planner + * - 线速度使用固定轴投影（保留正负号），角速度使用模长 * - keep_target=true 时： * - 若测量状态仍贴着当前 profile，则保持当前 profile * - 否则由 planner 内部从测量状态重规划到当前目标 @@ -178,6 +183,7 @@ private: double linear_reverse_switch_speed_threshold_; double angular_switch_speed_threshold_; bool emergency_stop_active_; + bool continuous_linear_reversal_{true}; // 目标/当前状态 Twist target_twist_input_; diff --git a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp index bcf884b0..a6d5f9e4 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/src/cartesian_twist_limiter.cpp @@ -249,7 +249,8 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base, const Eigen::Vector3d v_meas = measured_twist_base.head<3>(); const Eigen::Vector3d w_meas = measured_twist_base.tail<3>(); - const double v_norm = v_meas.norm(); + const bool signed_linear = continuous_linear_reversal_ && current_linear_dir_base_.norm() > EPSILON; + const double v_norm = signed_linear ? v_meas.dot(current_linear_dir_base_) : v_meas.norm(); const double w_norm = w_meas.norm(); double v_acc = 0.0; @@ -273,7 +274,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base, last_measured_twist_base_ = measured_twist_base; has_measured_sync_ = true; - // 用测得模长/模长加速度同步 planner 当前状态。 + // 线速度按固定轴投影保留正负号；角速度仍按模长同步 planner。 // keep_target=true: // - 若测量值仍贴着当前 profile，则继续沿旧 profile 走 // - 否则从测量状态重规划到当前目标 @@ -289,7 +290,7 @@ void CartesianTwistLimiter::synchronize(const Twist& measured_twist_base, target_twist_base_.setZero(); } - if (v_norm > EPSILON) { + if (!signed_linear && v_norm > EPSILON) { current_linear_dir_base_ = v_meas / v_norm; last_target_linear_dir_base_ = current_linear_dir_base_; } @@ -464,7 +465,18 @@ CartesianTwistLimiter::update(double dt, const Eigen::Matrix3d& base_R_tool) const bool must_switch_axis = !same_axis || dir_dot < linear_reverse_cos_threshold_; - if (must_switch_axis && v_cur_norm > linear_reverse_switch_speed_threshold_) { + if (continuous_linear_reversal_ && same_axis) { + // Keep the axis fixed: the scalar profile carries the direction sign. + // Crossing zero is an interior point, with continuous acceleration. + linear_norm_planner_.setTargetVelocity(v_des.dot(current_linear_dir_base_)); + } else if (continuous_linear_reversal_ && + (std::abs(v_cur_norm) > EPSILON || + std::abs(linear_norm_planner_.getAcceleration()) > EPSILON || + linear_norm_planner_.hasActiveProfile())) { + // A different axis may only be adopted after the old profile settles. + linear_norm_planner_.setTargetVelocity(0.0); + } else if (!continuous_linear_reversal_ && must_switch_axis && + v_cur_norm > linear_reverse_switch_speed_threshold_) { linear_norm_planner_.setTargetVelocity(0.0); } else { current_linear_dir_base_ = v_target_dir; diff --git a/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp new file mode 100644 index 00000000..646f8d5b --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/test/cartesian_twist_limiter_reversal_test.cpp @@ -0,0 +1,97 @@ +#include +#include "algorithms/motion_planner/base_motion/cartesian_velocity/twist_limiter/include/cartesian_twist_limiter.h" + +namespace cmvr { +namespace { +using Twist = CartesianTwistLimiter::Twist; +constexpr double dt = .001; +Twist y(double v) { Twist t = Twist::Zero(); t.y() = v; return t; } +struct Motion { double peak{0}, reverse_time{-1}, return_time{-1}; }; +Motion reverse(bool continuous, int approach_ticks) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(.55, 5, 10); + limiter.setContinuousLinearReversal(continuous); + limiter.initialize(approach_ticks == 0 ? y(.08) : Twist::Zero()); + limiter.setTargetTwist(y(.08), CartesianFrame::Base); + for (int i = 0; i < approach_ticks; ++i) limiter.update(dt, Eigen::Matrix3d::Identity()); + limiter.setLinearConstraints(.55, 3, 10); + limiter.setTargetTwist(y(-.08), CartesianFrame::Base); + Motion result; + double position = 0; + for (int i = 1; i <= 1500; ++i) { + const auto v = limiter.update(dt, Eigen::Matrix3d::Identity()); + position += v.y() * dt; + result.peak = std::max(result.peak, position); + if (v.y() < 0 && result.reverse_time < 0) result.reverse_time = i * dt; + if (result.reverse_time > 0 && position <= 0 && result.return_time < 0) result.return_time = i * dt; + if (continuous) { + EXPECT_LE(limiter.getAccelerationBase().norm(), 3.0 + 1e-7); + EXPECT_LE(limiter.getJerkBase().norm(), 10.0 + 1e-6); + EXPECT_NEAR(v.x(), 0, 1e-12); + EXPECT_NEAR(v.z(), 0, 1e-12); + } + } + EXPECT_NEAR(limiter.getTwistBase().y(), -.08, 1e-9); + return result; +} +TEST(CartesianTwistReversal, ContinuousReversalReducesTimeAndForwardTravel) { + for (const int ticks : {0, 80, 120}) { + SCOPED_TRACE(ticks); + const auto legacy = reverse(false, ticks); + const auto continuous = reverse(true, ticks); + EXPECT_GT(continuous.reverse_time, 0); + EXPECT_LT(continuous.reverse_time, legacy.reverse_time); + EXPECT_LT(continuous.return_time, legacy.return_time); + EXPECT_LT(continuous.peak, legacy.peak); + } +} +TEST(CartesianTwistReversal, ExactZeroCrossingIsStillMovingAndKeepsAcceleration) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(1, 1, 4); + limiter.initialize(y(.02)); + limiter.setTargetTwist(y(-.02), CartesianFrame::Base); + for (int i = 0; i < 100; ++i) limiter.update(dt, Eigen::Matrix3d::Identity()); + EXPECT_NEAR(limiter.getTwistBase().y(), 0, 1e-12); + EXPECT_TRUE(limiter.isMoving()); + EXPECT_LT(limiter.getAccelerationBase().y(), -.39); + EXPECT_LT(limiter.update(dt, Eigen::Matrix3d::Identity()).y(), -.0003); +} +TEST(CartesianTwistReversal, StopFromEitherDirectionSettlesWithoutReversing) { + for (double v : {.08, -.08}) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(1, 3, 10); + limiter.initialize(y(v)); + limiter.stop(); + for (int i = 0; i < 1000; ++i) { + EXPECT_GE(limiter.update(dt, Eigen::Matrix3d::Identity()).y() * v, -1e-12); + } + EXPECT_FALSE(limiter.isMoving()); + EXPECT_NEAR(limiter.getTwistBase().norm(), 0, 1e-12); + } +} +TEST(CartesianTwistReversal, FeedbackPreservesNegativeVelocityOnLockedAxis) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(1, 3, 10); + limiter.initialize(y(.08)); + limiter.setTargetTwist(y(-.08), CartesianFrame::Base); + for (int i = 0; i < 400; ++i) limiter.update(dt, Eigen::Matrix3d::Identity()); + for (int i = 0; i < 10; ++i) { + limiter.synchronize(y(-.08), dt, true); + EXPECT_NEAR(limiter.update(dt, Eigen::Matrix3d::Identity()).y(), -.08, 1e-9); + } +} +TEST(CartesianTwistReversal, NonCollinearChangeStopsBeforeSwitchingAxis) { + CartesianTwistLimiter limiter; + limiter.setLinearConstraints(1, 3, 10); + limiter.initialize(y(.08)); + Twist x = Twist::Zero(); x.x() = .08; + limiter.setTargetTwist(x, CartesianFrame::Base); + for (int i = 0; i < 1000; ++i) { + const auto v = limiter.update(dt, Eigen::Matrix3d::Identity()); + EXPECT_FALSE(v.x() > 1e-9 && std::abs(v.y()) > 1e-9); + EXPECT_LE(limiter.getJerkBase().norm(), 10 + 1e-6); + } + EXPECT_NEAR(limiter.getTwistBase().x(), .08, 1e-9); +} +} +} diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 4728236c..67f642c8 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -2,6 +2,7 @@ #define CMVR_ES_ARM_TYPES_H #include +#include #include #include @@ -64,6 +65,24 @@ struct CartesianVelocity { double wz{0.0}; }; +struct SpeedLOptions { + // Requested acceleration, capped separately by the arm's linear/angular maxima. + double acceleration{0.5}; + // Requested linear jerk (m/s^3), capped by the arm's configured maximum. + // Unset uses that maximum. + std::optional linear_jerk; + // Capture the first applied command's measured TCP position and target + // direction in Base. Useful for distance-based motion without caller FK. + bool capture_reference{false}; +}; + +struct SpeedLReference { + bool valid{false}; + std::uint64_t command_version{0}; + CartesianPose tcp_pose_base{}; + CartesianVelocity target_base{}; +}; + struct CartesianWrench { double fx{0.0}; double fy{0.0}; diff --git a/cmvr-es/config/devices/arm/arm.pb.txt b/cmvr-es/config/devices/arm/arm.pb.txt index fb7173c5..59b24597 100644 --- a/cmvr-es/config/devices/arm/arm.pb.txt +++ b/cmvr-es/config/devices/arm/arm.pb.txt @@ -211,7 +211,6 @@ arm { gain: 0.2 margin_ratio: 0.15 max_push: 0.25 - weight: 0.05 } } } diff --git a/cmvr-es/config/devices/arm/arm_qp.pb.txt b/cmvr-es/config/devices/arm/arm_qp.pb.txt index 16532f9f..95ca92dd 100644 --- a/cmvr-es/config/devices/arm/arm_qp.pb.txt +++ b/cmvr-es/config/devices/arm/arm_qp.pb.txt @@ -109,9 +109,9 @@ arm { speed_l { pinocchio_cartesian_motion_planner { - linear_velocity_max: 0.55 - linear_acceleration_max: 5.0 - linear_jerk_max: 10.0 + linear_velocity_max: 0.8 + linear_acceleration_max: 10.0 + linear_jerk_max: 30.0 angular_velocity_max: 1.0 angular_acceleration_max: 5.0 angular_jerk_max: 12.0 diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt index 2d472a2d..43bacdd6 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task.pb.txt @@ -96,6 +96,7 @@ touch_screen_task { speed_l { twist_tool { x: 0.0 y: -0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } acceleration: 5.0 + linear_jerk: 10.0 max_distance_m: 0.03 } tactile { @@ -109,7 +110,8 @@ 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: 3.0 + acceleration: 5.0 + linear_jerk: 10.0 # m/s^3，约束接触后的减速和连续换向。 # TCP 后退目标距离，单位为米。 distance_m: 0.03 } diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt index c4878e59..5bacf38f 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt @@ -105,6 +105,7 @@ touch_screen_task { speed_l { twist_tool { x: 0.0 y: -0.04 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } acceleration: 6.0 + linear_jerk: 60.0 # m/s^3，接近阶段请求值，受机械臂 linear_jerk_max 限制。 max_distance_m: 0.02 } tactile { @@ -119,6 +120,7 @@ touch_screen_task { retract { twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } acceleration: 4.0 + linear_jerk: 60.0 # m/s^3，保持仿真机械臂原有的 jerk 上限。 # TCP 后退目标距离，单位为米。 distance_m: 0.05 } diff --git a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt index a194fa73..a3270cd1 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt +++ b/cmvr-es/devices/arm/motor_robot_arm/CMakeLists.txt @@ -19,6 +19,11 @@ target_link_libraries(motor_robot_arm add_library(cmvr_es::device::motor_robot_arm ALIAS motor_robot_arm) install(TARGETS motor_robot_arm LIBRARY DESTINATION lib) +add_executable(speedl_reversal_mujoco_test src/speedl_reversal_mujoco_test.cpp) +target_link_libraries(speedl_reversal_mujoco_test PRIVATE + cmvr_es::device::motor_robot_arm cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver cmvr_es::proto pthread) + add_executable(motor_robot_arm_mujoco_test src/motor_robot_arm_mujoco_test.cpp ) diff --git a/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md b/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md new file mode 100644 index 00000000..4423f169 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md @@ -0,0 +1,77 @@ +# speedL 连续换向与 MuJoCo 验证 + +同轴线速度换向使用固定轴上的有符号 S 曲线，从当前速度和加速度直接规划到反向目标。过零时保留加速度，不重置规划器。非同轴变向先停止再换轴；角速度仍使用原有策略。 + +`SpeedLPlannerConfig.continuous_linear_reversal` 默认 true；设为 false 可以复现旧的同轴停止后换向策略，用于同条件比较。 + +`MotorRobotArm::speedL(velocity, SpeedLOptions, duration, frame)` 可以为本条命令指定 `linear_jerk`（m/s³）。未指定时使用机器人配置的 jerk；普通 `speedL(velocity, acceleration, duration, frame)` 仍可直接使用。控制线程将速度、加速度、jerk 作为同一命令读取。正常停止的收尾操作检查命令版本，避免清除之后提交的运动命令。 + +机械臂的 `SpeedLPlannerConfig` 提供规划上限：速度保持按配置限幅；线加速度、角加速度分别取请求 `acceleration` 与各自配置上限的较小值；线性 jerk 取请求值与 `linear_jerk_max` 的较小值。省略 jerk 时使用配置上限。普通 `stopL` 的加速度也遵循此规则。未配置或无效的上限继续使用规划器默认值（线性 0.55/5/10，角向 1/5/12）。 + +例如机械臂配置加速度 5、jerk 10 时，回撤请求 60/60 实际按 5/10 规划，请求 3/6 则按 3/6 规划。任务参数不能提高机械臂上限。 + +接近阶段也可在 `touch.speed_l` 内配置 `linear_jerk`（m/s³），与 `retract.linear_jerk` 独立设置。两者都必须为有限正数，省略时使用机械臂上限；执行时都取请求值与上限的较小值。配置示例： + +```protobuf +touch { + speed_l { + twist_tool { x: 0 y: -0.08 z: 0 rx: 0 ry: 0 rz: 0 } + acceleration: 5.0 + linear_jerk: 10.0 + max_distance_m: 0.03 + } + # tactile 和 dwell_time_s 等其他必填项仍按任务配置填写。 +} +``` + +`capture_reference=true` 时，控制线程从第一拍关节状态计算 TCP 位置及 Base 下的目标方向，成功发送速度后发布 `SpeedLReference`；Tool 目标随后固定到这一 Base 方向。新命令提交时旧参考立即失效。尚不支持这些选项的机械臂后端明确返回不支持。 + +触屏任务在 `dwell_time_s=0` 时检测到阈值就提交回撤，随后才记录日志，跳过停止/停留中间目标。回撤距离为沿回撤方向的有符号位移，继续前压不会算成回撤。`retract.linear_jerk` 控制整个减速、过零及反向加速过程，并受机械臂上限限制。实机触控任务当前接近和回撤都请求 10 m/s³；DLS 机械臂配置上限为 10，QP 配置上限为 30，MuJoCo 配置上限为 60 m/s³，执行时取所加载配置与任务请求的较小值。 + +触屏任务的 `[RETRACT]`、`[RETRACT_DONE]` 日志包含 `max_forward_after_retract_mm`：以回撤首个控制周期锁存的实测 TCP 为起点，统计沿回撤反方向的最大正位移，单位毫米。每次回撤清零，后续后退不抵消已记录的峰值。统计复用任务每次回撤步骤的 FK 位置采样，不额外阻塞触觉触发后的命令提交。它是采样到的最大值，不包括触发到首个回撤控制周期之前的移动，也不包括非零 dwell 阶段的移动；短暂峰值可能落在两次任务采样之间。已取得回撤参考后发生失败时，`[RETRACT_FAILED]` 也会打印已记录的最大值。 + +## 实际仿真测试 + +测试创建 MuJoCo 世界、MuJoCo 电机组和 MotorRobotArm，不创建真实硬件或视觉任务。使用 `dual_arm.xml` 的位置执行器驱动动力学；没有通过直接修改关节状态模拟运动。 + +先 MoveJ 到已有运动测试姿态，然后调用 `speedL(vy=+0.08)`，运动过程中直接调用 `speedL(vy=-0.08)`。默认 Base 坐标系，`--tool-frame true` 可验证 Tool 坐标系。测量来自 MuJoCo `R_FINGER_TIP_SITE` 的位置和雅可比乘实际 qvel；不是规划速度的积分。 + +在仓库根目录运行（需要已配置 `cmake-build-debug`；可用 `CMVR_BUILD_DIR` 覆盖）： + +```bash +# 匀速阶段换向：接近加速度 5，回撤加速度 3，jerk 均为 10。 +./script/test_speedl_reversal_mujoco.sh --csv /tmp/reversal-new.csv + +# 同一新版本中选择旧换向策略，比较算法本身。 +./script/test_speedl_reversal_mujoco.sh --legacy true --csv /tmp/reversal-legacy.csv + +# 仍在加速时，命令 vy 首次达到 0.032 m/s 即反向。 +./script/test_speedl_reversal_mujoco.sh --trigger-speed 0.032 --csv /tmp/reversal-accelerating.csv + +# 上限仍为 10，回撤请求 60 将被限制为 10。 +./script/test_speedl_reversal_mujoco.sh --reverse-jerk 60 --reverse-acceleration 60 \ + --capture-reference true --csv /tmp/reversal-capped.csv + +# 将仿真机械臂 jerk 上限设为 60；接近请求 10，回撤请求 60。 +./script/test_speedl_reversal_mujoco.sh --jerk 60 --approach-jerk 10 \ + --reverse-jerk 60 --capture-reference true \ + --csv /tmp/reversal-jerk60.csv + +# Tool 坐标系和触屏任务使用的参数接口。 +./script/test_speedl_reversal_mujoco.sh --tool-frame true --jerk 60 --approach-jerk 10 --reverse-jerk 60 \ + --capture-reference true --csv /tmp/reversal-tool.csv +``` + +默认前进 500 ms 后换向；`--approach-ms` 可修改，`--trigger-speed` 非零时优先按命令速度触发。默认速度为 0.08 m/s，可用 `--speed` 修改；`--jerk` 设置仿真机械臂 jerk 上限，`--approach-jerk` 和 `--reverse-jerk` 分别设置接近、回撤请求（省略时使用上限）。`--reverse-acceleration` 设置回撤加速度请求，默认 3，机械臂上限为 5。输出同时标注请求值和限幅值。测试采样目标周期和仿真步长均为 1 ms；线程由操作系统调度，CSV 同时记录 wall time 和 simulation time。 + +`max_forward_mm` 是反向调用前采样点之后，实际 TCP 沿前进轴的最大正位移；`peak_at_ms` 是到达该最远点的时间；`actual_reverse_ms` 要求至少连续 5 次采样的轴向速度小于 -0.0001 m/s；`return_to_origin_ms` 是确认反向后返回调用时位置的时间。Tool 模式的 CSV 速度字段也表示沿锁定前进轴的投影。 + +测试要求换向前实际速度为正、之后产生反向速度并退回起点，且控制器未提前退出；使用起点锁存时还要求参考有效。失败返回非零。该自由空间实验不包含屏幕接触、触觉延迟或硅胶形变，不能把返回起点时间直接当作屏幕抬起事件时间。 + +## 回归测试 + +构建并运行 `cartesian_twist_limiter_reversal_test`、`s_curve_velocity_planner_stop_test` 和 `cartesian_velocity_controller_test`。覆盖匀速/加速中换向、速度与 jerk 连续性、恰好过零、普通停止、非同轴转向、负速度反馈、停止/回撤竞争、成功发送后才发布回撤参考及非法参数拒绝。 + +`pinocchio_speedl_limits_test` 使用真实 URDF 和生产规划器，采样输出速度并差分验证实际规划的速度、加速度、jerk 上限；覆盖超限请求、较小请求、省略 jerk、旧接口及停止、独立角加速度限幅和非法请求。测试不初始化电机。 + +运行时应优先加载当前构建的项目动态库，测试脚本已处理；不要把新可执行文件与 `output/lib` 的旧项目库混用。 diff --git a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h index d8e37615..bd3f5c8d 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h +++ b/cmvr-es/devices/arm/motor_robot_arm/include/motor_robot_arm.h @@ -62,6 +62,9 @@ public: double duration, FrameType frame = FrameType::Base) override; Result stopL(std::optional acceleration = std::nullopt) override; + Result speedL(const CartesianVelocity& velocity, const SpeedLOptions& options, + double duration, FrameType frame = FrameType::Base) override; + SpeedLReference getSpeedLReference() const override; Result stopMotion() override; Result startServoMode(const ServoOptions& options) override; diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp index 16757c8c..f19328b2 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm.cpp @@ -748,6 +748,21 @@ Result MotorRobotArm::stopL(const std::optional acceleration) return cartesian_velocity_controller_->stop(acceleration); } +Result MotorRobotArm::speedL(const CartesianVelocity& velocity, const SpeedLOptions& options, + const double duration, const FrameType frame) +{ + if (const auto stopped = safetyStopResult_("speedL")) return *stopped; + if (busy_.load() || !cartesian_velocity_controller_) { + return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy or velocity controller is unavailable"); + } + return cartesian_velocity_controller_->speedL(velocity, options, duration, frame); +} + +SpeedLReference MotorRobotArm::getSpeedLReference() const +{ + return cartesian_velocity_controller_ ? cartesian_velocity_controller_->getReference() : SpeedLReference{}; +} + Result MotorRobotArm::stopMotion() { const auto cartesian_stop = stopCartesianMotionAndWait_(); diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/speedl_reversal_mujoco_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/speedl_reversal_mujoco_test.cpp new file mode 100644 index 00000000..abc459b5 --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/speedl_reversal_mujoco_test.cpp @@ -0,0 +1,196 @@ +#include "arm/motor_robot_arm/include/motor_robot_arm.h" +#include "common/io/proto_file_io.h" +#include "devices/motor/manager/include/motor_manager.h" +#include "simulate/mujoco/mujoco_world/include/mujoco_world.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// Headless, actuator-driven MuJoCo experiment. Never creates real motor drivers. +namespace { +using namespace cmvr; +using namespace cmvr::device; +using Clock = std::chrono::steady_clock; + +template T readConfig(const std::filesystem::path& path) { + T config; + if (!ProtoMessageIo::getProtoFromAsciiFile(path.string(), &config)) { + throw std::runtime_error("Cannot read " + path.string()); + } + return config; +} +void require(const Result& result) { + if (!result.ok()) throw std::runtime_error(result.message); +} +struct Sample { + double sim_time; + Eigen::Vector3d position; + Eigen::Vector3d velocity; + Eigen::Matrix3d rotation; +}; +Sample sample(const std::shared_ptr& world, int site) { + std::lock_guard lock(world->mutex()); + const auto* model = world->model(); + const auto* data = world->data(); + std::vector jac(3 * model->nv); + mj_jacSite(model, data, jac.data(), nullptr, site); + Sample s{data->time, Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero(), Eigen::Matrix3d::Identity()}; + for (int axis = 0; axis < 3; ++axis) { + s.position[axis] = data->site_xpos[site * 3 + axis]; + for (int j = 0; j < 3; ++j) s.rotation(axis, j) = data->site_xmat[site * 9 + axis * 3 + j]; + for (int j = 0; j < model->nv; ++j) s.velocity[axis] += jac[axis * model->nv + j] * data->qvel[j]; + } + return s; +} +} + +int main(int argc, char** argv) { + try { + const auto root = std::filesystem::current_path(); + double approach_ms = 500.0, jerk = 10.0, speed = 0.08, reverse_jerk = 0.0, trigger_speed = 0.0; + double approach_jerk = 0.0, reverse_acceleration = 3.0; + bool legacy = false, capture = false, tool_frame = false; + std::string csv_path = "/tmp/speedl-reversal.csv"; + for (int i = 1; i < argc; ++i) { + const std::string arg = argv[i]; + if (++i >= argc) throw std::runtime_error("Missing value for " + arg); + if (arg == "--approach-ms") approach_ms = std::stod(argv[i]); + else if (arg == "--jerk") jerk = std::stod(argv[i]); + else if (arg == "--speed") speed = std::stod(argv[i]); + else if (arg == "--reverse-jerk") reverse_jerk = std::stod(argv[i]); + else if (arg == "--approach-jerk") approach_jerk = std::stod(argv[i]); + else if (arg == "--reverse-acceleration") reverse_acceleration = std::stod(argv[i]); + else if (arg == "--trigger-speed") trigger_speed = std::stod(argv[i]); + else if (arg == "--legacy") legacy = std::string(argv[i]) == "true"; + else if (arg == "--capture-reference") capture = std::string(argv[i]) == "true"; + else if (arg == "--tool-frame") tool_frame = std::string(argv[i]) == "true"; + else if (arg == "--csv") csv_path = argv[i]; + else throw std::runtime_error("Unknown option " + arg); + } + if (!(approach_ms > 0 && approach_ms <= 1000 && jerk > 0 && speed > 0 && speed <= .1 && + reverse_jerk >= 0 && trigger_speed >= 0 && trigger_speed <= speed && + approach_jerk >= 0 && reverse_acceleration > 0 && std::isfinite(jerk) && + std::isfinite(reverse_jerk) && std::isfinite(approach_jerk) && std::isfinite(reverse_acceleration))) { + throw std::runtime_error("Invalid experiment parameters"); + } + auto worlds = readConfig(root / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt"); + auto wc = worlds.worlds(0); + wc.set_model_path((root / "model/xiaoyan_description/dual_arm.xml").string()); + simulate::MujocoWorldDevice world_device(wc); + if (!world_device.init() || !world_device.start()) throw std::runtime_error("MuJoCo start failed"); + auto motors = readConfig(root / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt"); + auto manager = std::make_shared("right_arm_mujoco_motors", motors.motor(), "right_arm_mujoco_motors"); + manager->init(); + const auto world = world_device.world(); + const int site = mj_name2id(world->model(), mjOBJ_SITE, "R_FINGER_TIP_SITE"); + if (site < 0) throw std::runtime_error("TCP site not found"); + auto arms = readConfig(root / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt"); + auto ac = arms.arm().robot_arms(0); + ac.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path( + (root / "model/xiaoyan_description/dual_arm.urdf").string()); + auto* planner_config = ac.mutable_motion()->mutable_speed_l()->mutable_pinocchio_cartesian_motion_planner(); + // Match physical-arm limits instead of the simulation's faster defaults. + planner_config->set_linear_velocity_max(.55); + planner_config->set_linear_acceleration_max(5.0); + planner_config->set_linear_jerk_max(jerk); + planner_config->set_continuous_linear_reversal(!legacy); + MotorRobotArm arm(ac); + if (!arm.init()) throw std::runtime_error("Simulated arm initialization failed"); + MotionOptions move; + move.velocity = 1.0; + move.acceleration = 3.0; + require(arm.moveJ(JointPositionCommand{{.25, 1.0, M_PI/2, M_PI/2, -M_PI/2, 0, 0}}, move)); + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + std::ofstream csv(csv_path); + if (!csv) throw std::runtime_error("Cannot open CSV"); + csv << "wall_ms,sim_ms,after_reverse,x_m,y_m,z_m,vy_m_s,forward_displacement_m,command_vy_m_s\n" << std::setprecision(12); + CartesianVelocity forward; + forward.vy = speed; + const auto frame = tool_frame ? FrameType::Tool : FrameType::Base; + const Eigen::Vector3d axis = tool_frame ? Eigen::Vector3d(sample(world, site).rotation.col(1)) + : Eigen::Vector3d::UnitY(); + SpeedLOptions forward_options; + forward_options.acceleration = 5.0; + if (approach_jerk > 0) forward_options.linear_jerk = approach_jerk; + require(arm.speedL(forward, forward_options, 0.0, frame)); + const auto begin = Clock::now(); + auto next = begin; + auto reverse_time = begin; + Sample origin{}; + bool reversed = false; + bool reference_valid = !capture; + double peak = 0, peak_ms = 0, return_ms = -1, reverse_ms = -1; + int negative_samples = 0; + while (Clock::now() - begin < std::chrono::milliseconds(static_cast(approach_ms) + 1000)) { + auto now = Clock::now(); + auto s = sample(world, site); + const auto command = arm.getSpeedLCommandTwistBase(); + const double command_vy = Eigen::Vector3d(command.vx, command.vy, command.vz).dot(axis); + const double actual_vy = s.velocity.dot(axis); + const double wall_ms = std::chrono::duration(now - begin).count(); + if (!reversed && (trigger_speed > 0 ? command_vy >= trigger_speed : wall_ms >= approach_ms)) { + origin = s; + reverse_time = Clock::now(); + CartesianVelocity backward; + backward.vy = -speed; + SpeedLOptions options; + options.acceleration = reverse_acceleration; + options.capture_reference = capture; + if (reverse_jerk > 0) options.linear_jerk = reverse_jerk; + require(arm.speedL(backward, options, 0.0, frame)); + reversed = true; + } + const double elapsed = std::chrono::duration(now - reverse_time).count(); + const double displacement = reversed ? (s.position - origin.position).dot(axis) : 0.0; + if (reversed) { + if (capture) { + const auto ref = arm.getSpeedLReference(); + reference_valid |= ref.valid && Eigen::Vector3d(ref.target_base.vx, + ref.target_base.vy, ref.target_base.vz).dot(axis) < 0 && + std::isfinite(ref.tcp_pose_base.y) && ref.command_version != 0; + } + if (displacement > peak) { peak = displacement; peak_ms = elapsed; } + negative_samples = actual_vy < -1e-4 ? negative_samples + 1 : 0; + if (negative_samples >= 5 && reverse_ms < 0) reverse_ms = elapsed; + if (reverse_ms >= 0 && displacement <= 0 && return_ms < 0) return_ms = elapsed; + } + csv << wall_ms << ',' << s.sim_time * 1000 << ',' << reversed << ',' + << s.position.x() << ',' << s.position.y() << ',' << s.position.z() << ',' + << actual_vy << ',' << displacement << ',' << command_vy << '\n'; + next += std::chrono::milliseconds(1); + std::this_thread::sleep_until(next); + } + const bool active = arm.busy(); + require(arm.stopL(3.0)); + const auto stop_deadline = Clock::now() + std::chrono::seconds(3); + while (arm.busy() && Clock::now() < stop_deadline) std::this_thread::sleep_for(std::chrono::milliseconds(5)); + std::cout << std::fixed << std::setprecision(3) + << "REVERSAL legacy=" << legacy << " approach_ms=" << approach_ms + << " frame=" << (tool_frame ? "Tool" : "Base") + << " trigger_speed=" << trigger_speed << " speed_m_s=" << speed << " jerk_m_s3=" << jerk + << " approach_jerk_m_s3=" << std::min(approach_jerk > 0 ? approach_jerk : jerk, jerk) + << " requested_reverse_acceleration_m_s2=" << reverse_acceleration + << " reverse_acceleration_m_s2=" << std::min(reverse_acceleration, 5.0) + << " requested_reverse_jerk_m_s3=" << (reverse_jerk > 0 ? reverse_jerk : jerk) + << " reverse_jerk_m_s3=" << std::min(reverse_jerk > 0 ? reverse_jerk : jerk, jerk) + << " actual_vy_at_reverse=" << origin.velocity.dot(axis) + << " max_forward_mm=" << peak * 1000 << " peak_at_ms=" << peak_ms + << " actual_reverse_ms=" << reverse_ms << " return_to_origin_ms=" << return_ms + << " remained_active=" << active << " reference_valid=" << reference_valid << " csv=" << csv_path << '\n'; + arm.stop(); + manager->stop(); + world_device.stop(); + return active && reference_valid && reversed && origin.velocity.dot(axis) > .001 && reverse_ms > 0 && return_ms > 0 ? 0 : 1; + } catch (const std::exception& e) { + std::cerr << "Experiment failed: " << e.what() << '\n'; + return 1; + } +} diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index f3b927d2..e603386f 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -58,6 +58,16 @@ public: double duration, FrameType frame = FrameType::Base) = 0; virtual Result stopL(std::optional acceleration = std::nullopt) = 0; + virtual Result speedL(const CartesianVelocity& velocity, + const SpeedLOptions& options, + double duration, + FrameType frame = FrameType::Base) { + if (options.linear_jerk || options.capture_reference) { + return Result::failure(ArmErrorCode::UnsupportedCommand, "speedL options are not supported by this arm"); + } + return speedL(velocity, options.acceleration, duration, frame); + } + virtual SpeedLReference getSpeedLReference() const { return {}; } virtual Result stopMotion() = 0; virtual Result moveP(const CartesianPose& target, diff --git a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h index 665989d9..4e8decfc 100644 --- a/cmvr-es/task/touch_screen_task/include/touch_screen_task.h +++ b/cmvr-es/task/touch_screen_task/include/touch_screen_task.h @@ -200,6 +200,10 @@ private: Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()}; bool retract_start_position_valid_{false}; Eigen::Vector3d retract_start_position_base_{Eigen::Vector3d::Zero()}; + Eigen::Vector3d retract_direction_base_{Eigen::Vector3d::Zero()}; + // Sampled maximum TCP displacement opposite to the retract direction, + // relative to the first applied retract command's measured TCP position. + double max_forward_after_retract_m_{0.0}; bool have_last_T_B_G_{false}; Eigen::Matrix4d last_T_B_G_{Eigen::Matrix4d::Identity()}; double max_T_B_G_translation_delta_m_{0.0}; diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index 7ad183f4..8ddceaa3 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -988,6 +988,9 @@ 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 || + (retract.has_linear_jerk() && + (!std::isfinite(retract.linear_jerk()) || retract.linear_jerk() <= 0.0)) || + cmvr::common::math::toEigenVec6(retract.twist_tool()).head<3>().norm() <= 1e-9 || !std::isfinite(retract.distance_m()) || retract.distance_m() <= 0.0) { return false; } @@ -1058,6 +1061,8 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& return twist.allFinite() && std::isfinite(speed_l.acceleration()) && speed_l.acceleration() > 0.0 && + (!speed_l.has_linear_jerk() || + (std::isfinite(speed_l.linear_jerk()) && speed_l.linear_jerk() > 0.0)) && std::isfinite(speed_l.max_distance_m()) && speed_l.max_distance_m() >= 0.0; } @@ -1638,6 +1643,28 @@ bool TouchScreenTask::stepRetracting() { const auto now = Clock::now(); const double elapsed = std::chrono::duration(now - phase_start_time_).count(); const double retract_distance_m = config_.retract().distance_m(); + if (!retract_start_position_valid_) { + const auto reference = arm_->getSpeedLReference(); + if (!reference.valid) { + if (!arm_->busy() || elapsed > 1.0) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + return true; // First controller tick has not captured the origin yet. + } + retract_start_position_base_ << reference.tcp_pose_base.x, + reference.tcp_pose_base.y, + reference.tcp_pose_base.z; + retract_direction_base_ << reference.target_base.vx, + reference.target_base.vy, reference.target_base.vz; + if (!retract_start_position_base_.allFinite() || !retract_direction_base_.allFinite() || + retract_direction_base_.norm() <= 1e-9) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + retract_direction_base_.normalize(); + retract_start_position_valid_ = true; + } Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero(); if (!retract_start_position_valid_ || !readCurrentTouchPointPositionBase(current_position_base)) { @@ -1651,14 +1678,23 @@ bool TouchScreenTask::stepRetracting() { const Eigen::Vector3d delta_base = current_position_base - retract_start_position_base_; - const double traveled_distance_m = delta_base.norm(); + const double traveled_distance_m = delta_base.dot(retract_direction_base_); + // Keep the forward peak: subsequent backward travel must not cancel it. + // Reuse the existing measured-position sample, without delaying reversal. + max_forward_after_retract_m_ = + std::max(max_forward_after_retract_m_, -traveled_distance_m); if (traveled_distance_m < retract_distance_m) { + if (!arm_->busy()) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } if (std::chrono::duration(now - last_retract_log_time_).count() >= 0.2) { last_retract_log_time_ = now; const auto cmd_base = arm_ ? arm_->getSpeedLCommandTwistBase() : device::CartesianVelocity{}; CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT] elapsed=" << elapsed << ", distance=" << traveled_distance_m << "/" << retract_distance_m + << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0 << ", cmd_base=[" << cmd_base.vx << ", " << cmd_base.vy << ", " << cmd_base.vz << ", " << cmd_base.wx << ", " << cmd_base.wy << ", " @@ -1676,10 +1712,13 @@ bool TouchScreenTask::stepRetracting() { const bool have_final_delta = retract_start_position_valid_ && have_final_position; if (have_final_delta) { final_delta_base = final_position_base - retract_start_position_base_; + max_forward_after_retract_m_ = std::max(max_forward_after_retract_m_, + -final_delta_base.dot(retract_direction_base_)); } if (have_final_delta) { CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed << ", target_distance_m=" << retract_distance_m + << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0 << ", 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() @@ -1687,6 +1726,7 @@ bool TouchScreenTask::stepRetracting() { } else { CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_DONE] elapsed=" << elapsed << ", target_distance_m=" << retract_distance_m + << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0 << ", move_to_init=" << (config_.initialization().after_finish() ? 1 : 0) << ", final_tcp_delta_base=unavailable"; } @@ -1858,6 +1898,12 @@ bool TouchScreenTask::moveToInitPositionIfEnabled() const { } bool TouchScreenTask::handleTouchTriggered(const bool stop_forward_motion) { + if (config_.touch().dwell_time_s() <= 0.0) { + // Submit reversal in this tick, before any synchronous FK or logging. + if (!startRetractPhase(Phase::DONE, Status::DONE)) return false; + logTouchPressure(true); + return true; + } if (stop_forward_motion) { const auto result = arm_->stopL(); if (!result.ok()) { @@ -1899,9 +1945,12 @@ bool TouchScreenTask::startTouchPhase() { last_status_ = Status::ROBOT_STATE_FAILED; return false; } + device::SpeedLOptions options; + options.acceleration = speed_l.acceleration(); + if (speed_l.has_linear_jerk()) options.linear_jerk = speed_l.linear_jerk(); const auto result = arm_->speedL(toCartesianVelocity( cmvr::common::math::toEigenVec6(speed_l.twist_tool())), - speed_l.acceleration(), + options, 0.0, device::FrameType::Tool); if (!result.ok()) { @@ -1957,24 +2006,17 @@ 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; - } + retract_start_position_valid_ = false; + retract_direction_base_.setZero(); + max_forward_after_retract_m_ = 0.0; const auto retract_cmd = toCartesianVelocity( cmvr::common::math::toEigenVec6(retract.twist_tool())); - 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() - << ", distance_m=" << retract.distance_m() - << ", start_tcp_base=[" << retract_start_position_base_.x() << ", " - << retract_start_position_base_.y() << ", " - << retract_start_position_base_.z() << "]"; - + device::SpeedLOptions options; + options.acceleration = retract.acceleration(); + if (retract.has_linear_jerk()) options.linear_jerk = retract.linear_jerk(); + options.capture_reference = true; const auto result = arm_->speedL(retract_cmd, - retract.acceleration(), + options, 0.0, device::FrameType::Tool); if (!result.ok()) { @@ -1990,11 +2032,20 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, last_retract_log_time_ = phase_start_time_; retract_command_started_ = true; last_status_ = Status::RETRACTING; + CMVR_LOG(INFO) << "[TouchScreenTask][RETRACT_START] submitted twist_tool=[" + << retract_cmd.vx << ", " << retract_cmd.vy << ", " << retract_cmd.vz + << "], acceleration=" << retract.acceleration() + << ", linear_jerk=" << (retract.has_linear_jerk() ? retract.linear_jerk() : 0.0) + << ", distance_m=" << retract.distance_m(); return true; } void TouchScreenTask::enterFailed(const Status status) { stopPbvsMotion(); + if (phase_ == Phase::RETRACTING && retract_start_position_valid_) { + CMVR_LOG(WARNING) << "[TouchScreenTask][RETRACT_FAILED] status=" << statusToString(status) + << ", max_forward_after_retract_mm=" << max_forward_after_retract_m_ * 1000.0; + } phase_ = Phase::FAILED; setCoordinateOverlayEnabled(true); touch_command_started_ = false; diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp index 8e815968..7098140c 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp @@ -1,4 +1,5 @@ #include "gtest/gtest.h" +#include #include #include @@ -306,6 +307,45 @@ TEST(TouchScreenTaskTest, ConfigFilesUseStructuredSchema) { } } +TEST(TouchScreenTaskTest, TouchSpeedLJerkParsesFromTextConfig) { + cmvr::config::TouchScreenTaskRootConfig parsed; + ASSERT_TRUE(google::protobuf::TextFormat::ParseFromString( + "touch_screen_task { touch { speed_l { acceleration: 5 linear_jerk: 10 } } }", &parsed)); + ASSERT_TRUE(parsed.touch_screen_task().touch().speed_l().has_linear_jerk()); + EXPECT_DOUBLE_EQ(parsed.touch_screen_task().touch().speed_l().linear_jerk(), 10); + + const auto project_root = findProjectRoot(); + ASSERT_FALSE(project_root.empty()); + for (const auto* file_name : {"touch_screen_task.pb.txt", "touch_screen_task_mujoco.pb.txt"}) { + cmvr::config::TouchScreenTaskRootConfig root; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root / "cmvr-es/config/tasks/touch_screen_task" / file_name).string(), &root)); + cmvr::task::TouchScreenTask task(root.touch_screen_task()); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED) + << file_name; + } +} + +TEST(TouchScreenTaskTest, TouchSpeedLJerkIsOptionalAndMustBeFinitePositive) { + auto config = loadMujocoTouchConfig(findProjectRoot()); + config.mutable_touch()->mutable_speed_l()->clear_linear_jerk(); + { + cmvr::task::TouchScreenTask task(config); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED); + } + for (const double jerk : {.5, 10.0, 60.0}) { + config.mutable_touch()->mutable_speed_l()->set_linear_jerk(jerk); + cmvr::task::TouchScreenTask task(config); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED); + } + for (const double jerk : {0.0, -1.0, std::numeric_limits::infinity(), + std::numeric_limits::quiet_NaN()}) { + config.mutable_touch()->mutable_speed_l()->set_linear_jerk(jerk); + cmvr::task::TouchScreenTask task(config); + EXPECT_EQ(task.lastStatus(), cmvr::task::TouchScreenTask::Status::INVALID_CONFIG); + } +} + TEST(TouchScreenTaskTest, CoordinateFrameProjectionUsesBgrAxisColors) { cv::Mat image = cv::Mat::zeros(240, 320, CV_8UC3); cmvr::device::Rs2Intrinsics intrinsics{}; diff --git a/protos/cmvr/config/arm_config/arm_config.proto b/protos/cmvr/config/arm_config/arm_config.proto index 3db2c23a..5dfb704c 100644 --- a/protos/cmvr/config/arm_config/arm_config.proto +++ b/protos/cmvr/config/arm_config/arm_config.proto @@ -46,6 +46,11 @@ message VendorRobotArmBackendConfig { } message SpeedLPlannerConfig { + // Default true: preserve acceleration through same-axis velocity reversal. + optional bool continuous_linear_reversal = 21; + // Arm-level speedL limits. Command requests may lower, but not raise them. + // Linear units: m/s, m/s^2, m/s^3. Angular units: rad/s, rad/s^2, rad/s^3. + // Unset/nonpositive values use the planner defaults. double linear_velocity_max = 1; double linear_acceleration_max = 2; double linear_jerk_max = 3; diff --git a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto index e4b4c6ac..08446991 100644 --- a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto +++ b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto @@ -97,6 +97,9 @@ message TouchScreenTouchSpeedLConfig { .cmvr.common.Vec6 twist_tool = 1; optional double acceleration = 2; optional double max_distance_m = 3; + // Requested linear jerk during approach, m/s^3, capped by the arm's + // linear_jerk_max. Unset uses that arm limit. + optional double linear_jerk = 4; } message TouchScreenTouchMoveLConfig { @@ -129,11 +132,15 @@ message TouchScreenTaskTouchConfig { message TouchScreenTaskRetractConfig { .cmvr.common.Vec6 twist_tool = 1; + // Requested acceleration, capped by the arm's speedL acceleration limits. optional double acceleration = 2; reserved 3; reserved "duration_s"; - // Distance traveled by the TCP before the retract motion stops, in meters. + // Signed displacement along the retract direction before stopping, in meters. optional double distance_m = 4; + // Requested linear jerk for braking and reversal, m/s^3, capped by the arm's + // linear_jerk_max. Unset uses that arm limit. + optional double linear_jerk = 5; } message TouchScreenTaskConfig { diff --git a/script/test_speedl_reversal_mujoco.sh b/script/test_speedl_reversal_mujoco.sh new file mode 100755 index 00000000..e4b87d89 --- /dev/null +++ b/script/test_speedl_reversal_mujoco.sh @@ -0,0 +1,22 @@ +#!/usr/bin/env bash +set -euo pipefail + +# Headless MuJoCo only. Never initializes the physical robot. +repo_root="$(cd -- "$(dirname -- "${BASH_SOURCE[0]}")/.." && pwd)" +build_dir="${CMVR_BUILD_DIR:-${repo_root}/cmake-build-debug}" +cmake --build "$build_dir" --target speedl_reversal_mujoco_test -j 4 +cd "$repo_root" +python3 - "$build_dir" "$@" <<'PY' +from pathlib import Path +import os +import sys + +build = Path(sys.argv[1]).resolve() +# Prefer every freshly built project library over output/lib's installed copy. +paths = sorted({str(path.parent) for path in build.rglob('*.so')}) +paths.append(str(Path('output/lib').resolve())) +env = os.environ.copy() +env['LD_LIBRARY_PATH'] = ':'.join(paths + [env.get('LD_LIBRARY_PATH', '')]) +binary = build / 'cmvr-es/devices/arm/motor_robot_arm/speedl_reversal_mujoco_test' +os.execve(str(binary), [str(binary), *sys.argv[2:]], env) +PY