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 index e9ee8c78..daf2ed72 100644 --- 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 @@ -16,6 +16,7 @@ public: double, double, double, FrameType, CartesianJointTrajectory&) override { return false; } bool updateSpeedLAcceleration(double) override { return true; } bool updateSpeedLLimits(const SpeedLOptions& options) override { + applied_reversal.store(options.continuous_linear_reversal.value_or(false)); applied_jerk.store(options.linear_jerk.value_or(10.0)); return true; } bool captureSpeedLReference(const std::vector&, const CartesianVelocity& target, @@ -32,6 +33,7 @@ public: CartesianVelocity getSpeedLCommandTwistBase() const override { return current; } CartesianVelocity current; std::atomic applied_jerk{0.0}; + std::atomic applied_reversal{false}; }; TEST(CartesianVelocityController, CompletedStopCannotClearNewReversal) { std::mutex mutex; @@ -92,6 +94,7 @@ TEST(CartesianVelocityController, PublishesReferenceAfterSendAndKeepsCommandLimi SpeedLOptions options; options.acceleration = 3; options.linear_jerk = 60; + options.continuous_linear_reversal = true; options.capture_reference = true; ASSERT_TRUE(controller.speedL(v, options, 0, FrameType::Tool).ok()); { @@ -109,6 +112,7 @@ TEST(CartesianVelocityController, PublishesReferenceAfterSendAndKeepsCommandLimi 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); + EXPECT_TRUE(planner->applied_reversal.load()); controller.shutdown(); } TEST(CartesianVelocityController, RejectsInvalidMotionLimitsBeforeStarting) { 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 7cb782c5..816854f0 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 @@ -45,7 +45,8 @@ public: virtual bool updateSpeedLAcceleration(double acceleration) = 0; virtual bool updateSpeedLLimits(const SpeedLOptions& options) { - return !options.linear_jerk && updateSpeedLAcceleration(options.acceleration); + return !options.linear_jerk && !options.continuous_linear_reversal.value_or(false) && + updateSpeedLAcceleration(options.acceleration); } virtual bool captureSpeedLReference(const std::vector&, const CartesianVelocity&, FrameType, SpeedLReference&) { return 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 7197ba65..661b771a 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 @@ -922,8 +922,8 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target speedl_stop_active_ = false; // 从静止开始一个新的 speedL command。 - if (!twist_limiter_.isMoving() && - speedl_command_twist_base_.squaredNorm() <= 1e-12) { + if (speedl_command_twist_base_.squaredNorm() <= 1e-12 && + (!twist_limiter_.continuousLinearReversalEnabled() || !twist_limiter_.isMoving())) { twist_limiter_.initialize( Eigen::Matrix::Zero()); } @@ -1271,6 +1271,10 @@ bool PinocchioCartesianMotionPlanner::updateSpeedLLimits(const SpeedLOptions& op 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); + // Apply the command's motion policy even when acceleration/jerk are + // unchanged. PBVS streams directions; contact retraction locks an axis. + twist_limiter_.setContinuousLinearReversal(options.continuous_linear_reversal.value_or( + !speedl_config_.has_continuous_linear_reversal() || speedl_config_.continuous_linear_reversal())); 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) { 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 index 1a7bc1e1..2454b6c6 100644 --- 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 @@ -153,5 +153,61 @@ TEST_F(PinocchioSpeedLLimits, InvalidRequestsAreRejectedBeforeClamping) { EXPECT_FALSE(planner_->updateSpeedLLimits(options)); } } + +TEST_F(PinocchioSpeedLLimits, StreamingAlignmentOverridesGlobalReversalPolicy) { + for (const double jerk : {10.0, 30.0}) { + SCOPED_TRACE(jerk); + limits_.set_continuous_linear_reversal(true); + limits_.set_linear_acceleration_max(5); + limits_.set_linear_jerk_max(jerk); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + SpeedLOptions options; + options.acceleration = .5; + CartesianVelocity target; + target.vy = .02; + sample(target, options, false, 300); + ASSERT_NEAR(planner_->getSpeedLCommandTwistBase().vy, .02, 1e-10); + + // Same acceleration/jerk: the policy override must bypass their cache. + options.continuous_linear_reversal = false; + double min_speed = .02; + double max_error = 0; + for (int i = 0; i < 2000; ++i) { + const double angle = (i / 20) * .01; + target.vx = .02 * std::sin(angle); + target.vy = .02 * std::cos(angle); + ASSERT_TRUE(planner_->updateSpeedLLimits(options)); + ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)); + const Twist actual = common::math::velocityToVector(planner_->getSpeedLCommandTwistBase()); + min_speed = std::min(min_speed, actual.head<3>().norm()); + max_error = std::max(max_error, (actual - common::math::velocityToVector(target)).norm()); + } + EXPECT_NEAR(min_speed, .02, 1e-10); + EXPECT_LT(max_error, 1e-10); + } +} + +TEST_F(PinocchioSpeedLLimits, ContactReversalStillCrossesZeroAfterStreamingAlignment) { + limits_.set_linear_acceleration_max(1); + limits_.set_linear_jerk_max(4); + ASSERT_TRUE(planner_->configureSpeedL(limits_, q_.size())); + SpeedLOptions options; + options.acceleration = 1; + options.continuous_linear_reversal = false; + CartesianVelocity target; + target.vy = .02; + sample(target, options, false, 300); + ASSERT_NEAR(planner_->getSpeedLCommandTwistBase().vy, .02, 1e-10); + + options.continuous_linear_reversal = true; + target.vy = -.02; + for (int i = 0; i < 100; ++i) { + ASSERT_TRUE(planner_->updateSpeedLLimits(options)); + ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)); + } + EXPECT_NEAR(planner_->getSpeedLCommandTwistBase().vy, 0, 1e-10); + ASSERT_TRUE(planner_->speedLStep(target, kDt, q_, qd_, command_, FrameType::Base)); + EXPECT_LT(planner_->getSpeedLCommandTwistBase().vy, -.0003); +} } // namespace } // namespace cmvr::device 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 f20e9ddf..864f1902 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 @@ -69,8 +69,9 @@ public: 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; } + // at zero. Disable for streaming direction tracking (e.g. visual alignment). + void setContinuousLinearReversal(bool enabled); + bool continuousLinearReversalEnabled() const { return continuous_linear_reversal_; } /** * @brief 初始化当前 twist 状态 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 a6d5f9e4..8167c59a 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 @@ -108,6 +108,25 @@ void CartesianTwistLimiter::setLinearReverseSwitchPolicy(double cos_threshold, linear_reverse_switch_speed_threshold_ = std::max(0.0, switch_speed_threshold); } +void CartesianTwistLimiter::setContinuousLinearReversal(bool enabled) +{ + if (continuous_linear_reversal_ == enabled) return; + + if (!enabled) { + const double velocity = linear_norm_planner_.getVelocity(); + const double acceleration = linear_norm_planner_.getAcceleration(); + // The streaming policy stores a speed magnitude and a physical direction. + // Convert a negative signed state without reversing its physical motion or + // dropping its acceleration when a command changes policy mid-retraction. + if (velocity < 0.0 || (std::abs(velocity) <= EPSILON && acceleration < 0.0)) { + current_linear_dir_base_ = -current_linear_dir_base_; + linear_norm_planner_.overwriteState(-velocity, -acceleration, false); + prev_measured_linear_norm_ = -prev_measured_linear_norm_; + } + } + continuous_linear_reversal_ = enabled; +} + void CartesianTwistLimiter::initialize(const Twist& initial_twist) { restoreNominalConstraints(); 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 index 646f8d5b..18ba446f 100644 --- 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 @@ -93,5 +93,26 @@ TEST(CartesianTwistReversal, NonCollinearChangeStopsBeforeSwitchingAxis) { } EXPECT_NEAR(limiter.getTwistBase().x(), .08, 1e-9); } + +TEST(CartesianTwistReversal, SwitchingToStreamingPreservesNegativePhysicalVelocity) { + for (const int reverse_ticks : {150, 250, 500}) { + SCOPED_TRACE(reverse_ticks); + CartesianTwistLimiter reference; + reference.setLinearConstraints(1, 3, 10); + reference.initialize(y(.08)); + reference.setTargetTwist(y(-.08), CartesianFrame::Base); + for (int i = 0; i < reverse_ticks; ++i) reference.update(dt, Eigen::Matrix3d::Identity()); + ASSERT_LT(reference.getTwistBase().y(), 0); + auto streaming = reference; + streaming.setContinuousLinearReversal(false); + EXPECT_NEAR((streaming.getTwistBase() - reference.getTwistBase()).norm(), 0, 1e-12); + for (int i = 0; i < 500; ++i) { + const auto expected = reference.update(dt, Eigen::Matrix3d::Identity()); + const auto actual = streaming.update(dt, Eigen::Matrix3d::Identity()); + EXPECT_NEAR((actual - expected).norm(), 0, 1e-9); + EXPECT_LE(streaming.getJerkBase().norm(), 10 + 1e-6); + } + } +} } } diff --git a/cmvr-es/common/types/arm/arm_types.h b/cmvr-es/common/types/arm/arm_types.h index 67f642c8..35b201ea 100644 --- a/cmvr-es/common/types/arm/arm_types.h +++ b/cmvr-es/common/types/arm/arm_types.h @@ -71,6 +71,9 @@ struct SpeedLOptions { // Requested linear jerk (m/s^3), capped by the arm's configured maximum. // Unset uses that maximum. std::optional linear_jerk; + // Unset follows the arm config. False preserves the original streaming + // direction-following policy used by PBVS; true enables fixed-axis reversal. + std::optional continuous_linear_reversal; // 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}; diff --git a/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md b/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md index 4423f169..561872b9 100644 --- a/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md +++ b/cmvr-es/devices/arm/motor_robot_arm/SPEEDL_REVERSAL.md @@ -1,8 +1,8 @@ # speedL 连续换向与 MuJoCo 验证 -同轴线速度换向使用固定轴上的有符号 S 曲线,从当前速度和加速度直接规划到反向目标。过零时保留加速度,不重置规划器。非同轴变向先停止再换轴;角速度仍使用原有策略。 +触控接近和回撤的同轴线速度换向使用固定轴上的有符号 S 曲线,从当前速度和加速度直接规划到反向目标。过零时保留加速度,不重置规划器。该模式下非同轴变向先停止再换轴;角速度仍使用原有策略。 -`SpeedLPlannerConfig.continuous_linear_reversal` 默认 true;设为 false 可以复现旧的同轴停止后换向策略,用于同条件比较。 +`SpeedLPlannerConfig.continuous_linear_reversal` 默认 true;`SpeedLOptions.continuous_linear_reversal` 可按命令覆盖。触控任务的 PBVS 对齐明确设为 false,保持原有的小角度方向跟随和低速换轴行为;接近及回撤设为 true。固定轴模式不适合方向不断变化的视觉对齐:小方向变化会累积到换轴阈值,导致反复停走。设为 false 也可用于与旧换向策略做同条件比较。运动中退出固定轴模式时会保留实际运动方向和加速度。 `MotorRobotArm::speedL(velocity, SpeedLOptions, duration, frame)` 可以为本条命令指定 `linear_jerk`(m/s³)。未指定时使用机器人配置的 jerk;普通 `speedL(velocity, acceleration, duration, frame)` 仍可直接使用。控制线程将速度、加速度、jerk 作为同一命令读取。正常停止的收尾操作检查命令版本,避免清除之后提交的运动命令。 @@ -74,4 +74,6 @@ touch { `pinocchio_speedl_limits_test` 使用真实 URDF 和生产规划器,采样输出速度并差分验证实际规划的速度、加速度、jerk 上限;覆盖超限请求、较小请求、省略 jerk、旧接口及停止、独立角加速度限幅和非法请求。测试不初始化电机。 +对齐回归覆盖每 20 ms 小幅更新速度方向:显式关闭固定轴模式后,20 mm/s 的命令不会因方向变化反复降到零;同时验证对齐后启用连续换向仍能平滑过零,以及反向运动中退出固定轴模式不会翻转实际速度。 + 运行时应优先加载当前构建的项目动态库,测试脚本已处理;不要把新可执行文件与 `output/lib` 的旧项目库混用。 diff --git a/cmvr-es/devices/arm/robot_arm.h b/cmvr-es/devices/arm/robot_arm.h index e603386f..7cdf7293 100644 --- a/cmvr-es/devices/arm/robot_arm.h +++ b/cmvr-es/devices/arm/robot_arm.h @@ -62,7 +62,8 @@ public: const SpeedLOptions& options, double duration, FrameType frame = FrameType::Base) { - if (options.linear_jerk || options.capture_reference) { + if (options.linear_jerk || options.capture_reference || + options.continuous_linear_reversal.value_or(false)) { return Result::failure(ArmErrorCode::UnsupportedCommand, "speedL options are not supported by this arm"); } return speedL(velocity, options.acceleration, duration, frame); 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 8ddceaa3..87e573c8 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 @@ -1525,14 +1525,15 @@ bool TouchScreenTask::stepAligning(const double dt) { return true; } + device::SpeedLOptions alignment_options; + alignment_options.acceleration = pbvs_command_acceleration_; + // PBVS changes direction on each visual update. Keep the original + // direction-following policy instead of repeatedly braking to change axes. + alignment_options.continuous_linear_reversal = false; const auto speed_result = arm_->speedL(twist_B, - pbvs_command_acceleration_, + alignment_options, 0.0, device::FrameType::Base); - // const auto speed_result = arm_->speedL({0,0,0,0,0,0}, - // pbvs_command_acceleration_, - // 0.0, - // device::FrameType::Base); if (!speed_result.ok()) { CMVR_LOG(ERROR) << "[TouchScreenTask][ALIGNING] speedL failed: " << speed_result.message; @@ -1948,6 +1949,7 @@ bool TouchScreenTask::startTouchPhase() { device::SpeedLOptions options; options.acceleration = speed_l.acceleration(); if (speed_l.has_linear_jerk()) options.linear_jerk = speed_l.linear_jerk(); + options.continuous_linear_reversal = true; const auto result = arm_->speedL(toCartesianVelocity( cmvr::common::math::toEigenVec6(speed_l.twist_tool())), options, @@ -2014,6 +2016,7 @@ bool TouchScreenTask::startRetractPhase(const Phase next_phase_after_retract, device::SpeedLOptions options; options.acceleration = retract.acceleration(); if (retract.has_linear_jerk()) options.linear_jerk = retract.linear_jerk(); + options.continuous_linear_reversal = true; options.capture_reference = true; const auto result = arm_->speedL(retract_cmd, options,