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 9d6f070e..d0eddc48 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 @@ -99,6 +99,11 @@ private: bool speedl_line_check_active_{false}; bool speedl_line_deviation_warned_{false}; bool speedl_line_direction_warned_{false}; + + // speedL 是否已经进入停止阶段。 + // 停止阶段不要每 1 ms 用 measured twist 重新点燃 Cartesian planner。 + bool speedl_stop_active_{false}; + bool speedl_configured_{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 160c2570..08de05d0 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 @@ -127,6 +127,7 @@ bool PinocchioCartesianMotionPlanner::configureSpeedL(const config::SpeedLPlanne speedl_line_check_active_ = false; speedl_line_deviation_warned_ = false; speedl_line_direction_warned_ = false; + speedl_stop_active_ = false; speedl_applied_acceleration_ = positiveOr(speedl_config_.linear_acceleration_max(), 5.0); speedl_configured_ = true; return true; @@ -894,15 +895,43 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target return false; } - const Eigen::Matrix target_twist = common::math::velocityToVector(target_velocity); - const bool is_stop_command = target_twist.squaredNorm() <= 1e-12; + const Eigen::Matrix target_twist = + common::math::velocityToVector(target_velocity); + + const bool is_stop_command = + target_twist.squaredNorm() <= 1e-12; + if (is_stop_command) { - twist_limiter_.synchronize(measured_twist_base, dt, true); - } else if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { - twist_limiter_.initialize(Eigen::Matrix::Zero()); + if (!speedl_stop_active_) { + // 只在 stop 边沿执行一次。 + // + // 非常重要： + // 不再调用 + // twist_limiter_.synchronize(measured_twist_base, dt, true); + // + // 停止应当从“上一拍已经发送出去的 command twist” + // 连续规划到 0，而不是每 1 ms 被 measured twist 重新点燃。 + speedl_stop_active_ = true; + + twist_limiter_.stop(); + } + } else { + // 收到新的非零 speedL，退出停止状态。 + speedl_stop_active_ = false; + + // 从静止开始一个新的 speedL command。 + if (speedl_command_twist_base_.squaredNorm() <= 1e-12) { + twist_limiter_.initialize( + Eigen::Matrix::Zero()); + } + + twist_limiter_.setTargetTwist( + target_twist, + common::math::toPlannerFrame(frame)); } - twist_limiter_.setTargetTwist(target_twist, common::math::toPlannerFrame(frame)); - speedl_command_twist_base_ = twist_limiter_.update(dt, base_R_tool); + + speedl_command_twist_base_ = + twist_limiter_.update(dt, base_R_tool); if (!updateAndValidateSpeedLLineDeviation_(q_measured, is_stop_command, speedl_command_twist_base_.head<3>())) { diff --git a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt index 62fb599c..36881029 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt +++ b/cmvr-es/algorithms/motion_planner/base_motion/CMakeLists.txt @@ -32,3 +32,14 @@ target_link_libraries(toppra_multi_waypoint_test gtest gtest_main ) + +add_executable(s_curve_velocity_planner_stop_test + motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp +) + +target_link_libraries(s_curve_velocity_planner_stop_test + PRIVATE + cmvr_es::base_motion + gtest + gtest_main +) 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 a4ecf10b..bcf884b0 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 @@ -201,7 +201,12 @@ void CartesianTwistLimiter::setTargetTwist(const Twist& target_twist, CartesianF void CartesianTwistLimiter::stop() { + // target_twist_input_.setZero(); target_twist_input_.setZero(); + target_twist_base_.setZero(); + + linear_norm_planner_.setTargetVelocity(0.0); + angular_norm_planner_.setTargetVelocity(0.0); } void CartesianTwistLimiter::emergencyStop(double emergency_acceleration, diff --git a/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp index b6748195..c058e673 100644 --- a/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp +++ b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/src/s_curve_velocity_planner.cpp @@ -114,13 +114,29 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, updateIsMovingFlag(); } - void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity, - double acceleration) +void SCurveVelocityPlanner1D::synchronizeAndReplan(double velocity, + double acceleration) { const double measured_velocity = clamp(velocity, -max_velocity_, max_velocity_); - const double measured_acceleration = + double measured_acceleration = clamp(acceleration, -max_acceleration_, max_acceleration_); + // When the target is zero this planner is also used for Cartesian speed + // magnitudes. A magnitude is non-negative, while the finite-difference + // derivative of the measured magnitude is signed. Feeding a large negative + // measured acceleration into a signed 1-D velocity planner can generate a + // profile that crosses through zero and becomes negative before returning to + // zero. The twist limiter then multiplies that negative "norm" by the + // current direction, which reverses and amplifies the Cartesian command. + // + // For feedback resynchronization during a stop, synchronize the measured + // speed only and restart the stop profile with zero scalar acceleration. + // This avoids noise-sensitive stop replans and preserves a non-overshooting + // deceleration profile for speed-magnitude users. + if (std::abs(state_.target_velocity) <= VELOCITY_THRESHOLD) { + measured_acceleration = 0.0; + } + if (!state_.has_active_profile && std::abs(measured_velocity - state_.target_velocity) <= VELOCITY_THRESHOLD) { state_.velocity = state_.target_velocity; @@ -128,7 +144,7 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, state_.jerk = 0.0; updateIsMovingFlag(); return; - } + } // 如果测量值已经基本落在当前采样状态上，就继续沿现有 profile 走。 // 否则每拍都从同一目标重规划，会把已经进入的 jerk phase 反复打断。 @@ -139,7 +155,7 @@ void SCurveVelocityPlanner1D::overwriteState(double velocity, state_.jerk = getJerkAtTime(active_profile_, state_.elapsed_time); updateIsMovingFlag(); return; - } + } state_.velocity = measured_velocity; state_.acceleration = measured_acceleration; diff --git a/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp new file mode 100644 index 00000000..12ba5e38 --- /dev/null +++ b/cmvr-es/algorithms/motion_planner/base_motion/motion_profile/s_curve/test/s_curve_velocity_planner_stop_test.cpp @@ -0,0 +1,77 @@ +#include + +#include + +#include "algorithms/motion_planner/base_motion/motion_profile/s_curve/include/s_curve_velocity_planner.h" + +namespace cmvr { +namespace { + +void finishActiveProfile(SCurveVelocityPlanner1D& planner, double dt) +{ + for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) { + planner.update(dt); + } + ASSERT_FALSE(planner.hasActiveProfile()); +} + +TEST(SCurveVelocityPlannerStopTest, FeedbackResyncDoesNotReverseSpeedMagnitude) +{ + SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0); + constexpr double kDt = 0.001; + constexpr double kInitialSpeed = 0.04; + + planner.initialize(kInitialSpeed, 0.0); + planner.setTargetVelocity(0.0); + finishActiveProfile(planner, kDt); + + // Reproduce the speedL stop feedback case: the command profile has already + // reached zero, but the measured TCP still has residual speed. A 1 kHz + // finite difference may report a large negative scalar acceleration. + planner.synchronizeAndReplan(kInitialSpeed, -3.0); + + double max_speed = planner.getVelocity(); + double min_speed = planner.getVelocity(); + for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) { + const double speed = planner.update(kDt); + max_speed = std::max(max_speed, speed); + min_speed = std::min(min_speed, speed); + } + + EXPECT_GE(min_speed, -1e-9); + EXPECT_LE(max_speed, kInitialSpeed + 1e-9); + EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9); +} + +TEST(SCurveVelocityPlannerStopTest, LargerMeasuredDecelerationDoesNotIncreaseStopSpeed) +{ + constexpr double kDt = 0.001; + constexpr double kInitialSpeed = 0.04; + const double measured_accelerations[] = {-0.5, -1.0, -2.0, -3.0}; + + for (const double measured_acceleration : measured_accelerations) { + SCurveVelocityPlanner1D planner(0.55, 3.0, 10.0); + planner.initialize(kInitialSpeed, 0.0); + planner.setTargetVelocity(0.0); + finishActiveProfile(planner, kDt); + + planner.synchronizeAndReplan(kInitialSpeed, measured_acceleration); + + double max_speed = planner.getVelocity(); + double min_speed = planner.getVelocity(); + for (int i = 0; i < 10000 && planner.hasActiveProfile(); ++i) { + const double speed = planner.update(kDt); + max_speed = std::max(max_speed, speed); + min_speed = std::min(min_speed, speed); + } + + EXPECT_GE(min_speed, -1e-9) << "measured_acceleration=" << measured_acceleration; + EXPECT_LE(max_speed, kInitialSpeed + 1e-9) + << "measured_acceleration=" << measured_acceleration; + EXPECT_NEAR(planner.getVelocity(), 0.0, 1e-9) + << "measured_acceleration=" << measured_acceleration; + } +} + +} // namespace +} // namespace cmvr diff --git a/cmvr-es/config/devices/arm/arm_qp.pb.txt b/cmvr-es/config/devices/arm/arm_qp.pb.txt index 30b9cb6a..2be40b68 100644 --- a/cmvr-es/config/devices/arm/arm_qp.pb.txt +++ b/cmvr-es/config/devices/arm/arm_qp.pb.txt @@ -137,9 +137,9 @@ arm { cartesian_velocity_feasibility_check { enable: true min_linear_speed_ratio: 0.2 - max_linear_direction_deviation_deg: 5.0 + max_linear_direction_deviation_deg: 70 min_angular_speed_ratio: 0.2 - max_angular_direction_deviation_deg: 5.0 + max_angular_direction_deviation_deg: 70 min_desired_linear_speed: 1e-4 min_desired_angular_speed: 1e-4 } @@ -151,7 +151,7 @@ arm { stop_twist_norm: 1e-9 stop_command_velocity_norm: 1e-3 stop_measured_velocity_norm: 1e-2 - stop_acceleration: 0.5 + stop_acceleration: 5 # 等待 Cartesian 速度运动停止的最长时间，单位为秒。 stop_timeout_s: 2.0 } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index 02667acf..48486297 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/cmvr-es/config/manager/device_manager.pb.txt @@ -118,7 +118,7 @@ device_manager { devices { id: "right_arm" type: DEVICE_TYPE_ROBOT_ARM - config_file: "devices/arm/arm.pb.txt" + config_file: "devices/arm/arm_qp.pb.txt" enable: false } diff --git a/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_speedl_stop_test.cpp b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_speedl_stop_test.cpp new file mode 100644 index 00000000..49b61bcf --- /dev/null +++ b/cmvr-es/devices/arm/motor_robot_arm/src/motor_robot_arm_speedl_stop_test.cpp @@ -0,0 +1,352 @@ +#include "arm/motor_robot_arm/include/motor_robot_arm.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include + +#include "algorithms/kinematics/ik_solver/ik_solver_factory.h" +#include "algorithms/kinematics/ik_solver/pinocchio/include/pinocchio_ik_base.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" + +namespace cmvr::device { +namespace { + +constexpr std::size_t kDof = 7; +constexpr std::array kJointNames = { + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", "R_ELBOW_R", + "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"}; +constexpr std::array kInitialJointPosition = { + -0.2423, 1.2929, 1.61, 1.58, -2.8792, 0.1150, -0.08}; +constexpr std::array kAccelerationSweep = { + 0.1, 0.3, 0.5, 1.0, 2.0, 3.0, 5.0}; +constexpr double kCommandVelocity = 0.04; +constexpr double kCommandDurationS = 2.0; +constexpr double kSamplePeriodS = 0.001; + +std::filesystem::path findProjectRoot() +{ + const std::filesystem::path marker = "model/xiaoyan_description/dual_arm.xml"; + auto search = [&](std::filesystem::path current) { + while (!current.empty()) { + if (std::filesystem::exists(current / marker)) { + return current; + } + const auto parent = current.parent_path(); + if (parent == current) { + break; + } + current = parent; + } + return std::filesystem::path{}; + }; + + auto root = search(std::filesystem::current_path()); + if (!root.empty()) { + return root; + } + return search(std::filesystem::path(__FILE__).parent_path()); +} + +void setKinematicsUrdfPath(config::RobotArmConfig& arm_config, + const std::string& urdf_path) +{ + switch (arm_config.kinematics().algorithm_case()) { + case config::ArmKinematicsConfig::kPinocchioDlsIkSolver: + arm_config.mutable_kinematics()->mutable_pinocchio_dls_ik_solver()->set_urdf_path(urdf_path); + break; + case config::ArmKinematicsConfig::kPinocchioQpIkSolver: + arm_config.mutable_kinematics()->mutable_pinocchio_qp_ik_solver()->set_urdf_path(urdf_path); + break; + default: + break; + } +} + +struct Sample { + double time_s{0.0}; + std::vector q; + std::vector qd; + CartesianVelocity commanded_twist; +}; + +struct Metrics { + double max_qd_before_stop{0.0}; + double max_qd_after_stop{0.0}; + double max_qdd{0.0}; + double max_tcp_linear_speed_before_stop{0.0}; + double max_tcp_linear_speed_after_stop{0.0}; + double max_tcp_linear_acceleration{0.0}; +}; + +double vectorNorm(const std::vector& value) +{ + double sum = 0.0; + for (const double item : value) { + sum += item * item; + } + return std::sqrt(sum); +} + +double linearSpeed(const Eigen::Matrix& twist) +{ + return twist.head<3>().norm(); +} + +std::string numberForFile(const double value) +{ + std::ostringstream stream; + stream << std::fixed << std::setprecision(3) << value; + auto result = stream.str(); + std::replace(result.begin(), result.end(), '.', '_'); + return result; +} + +void writeCsv(const std::filesystem::path& path, + const std::vector& samples, + const double stop_time_s, + const cmvr::PinocchioIKBase& solver, + Metrics& metrics) +{ + std::ofstream output(path); + ASSERT_TRUE(output.is_open()) << "failed to open CSV: " << path; + + output << "time_s,phase"; + for (const auto* name : kJointNames) { + output << "," << name << "_q"; + } + for (const auto* name : kJointNames) { + output << "," << name << "_qd"; + } + output << ",command_vx,command_vy,command_vz,command_wx,command_wy,command_wz" + << ",tcp_vx,tcp_vy,tcp_vz,tcp_wx,tcp_wy,tcp_wz,tcp_linear_speed,tcp_linear_acceleration"; + output << '\n'; + + Eigen::Matrix previous_tcp_twist = Eigen::Matrix::Zero(); + bool have_previous_tcp = false; + for (const auto& sample : samples) { + Eigen::Matrix tcp_twist = Eigen::Matrix::Zero(); + ASSERT_TRUE(solver.computeTwistBaseAtQ(sample.q, sample.qd, true, tcp_twist)); + const bool after_stop = stop_time_s >= 0.0 && sample.time_s >= stop_time_s; + const double qd_norm = vectorNorm(sample.qd); + const double tcp_speed = linearSpeed(tcp_twist); + double tcp_acceleration = 0.0; + if (have_previous_tcp) { + tcp_acceleration = (tcp_twist.head<3>() - previous_tcp_twist.head<3>()).norm() / + std::max(kSamplePeriodS, sample.time_s - + (samples[&sample - samples.data() - 1].time_s)); + } + previous_tcp_twist = tcp_twist; + have_previous_tcp = true; + + if (after_stop) { + metrics.max_qd_after_stop = std::max(metrics.max_qd_after_stop, qd_norm); + metrics.max_tcp_linear_speed_after_stop = + std::max(metrics.max_tcp_linear_speed_after_stop, tcp_speed); + } else { + metrics.max_qd_before_stop = std::max(metrics.max_qd_before_stop, qd_norm); + metrics.max_tcp_linear_speed_before_stop = + std::max(metrics.max_tcp_linear_speed_before_stop, tcp_speed); + } + metrics.max_tcp_linear_acceleration = + std::max(metrics.max_tcp_linear_acceleration, tcp_acceleration); + + if (&sample != samples.data()) { + const auto& previous = samples[&sample - samples.data() - 1]; + const double dt = std::max(kSamplePeriodS, sample.time_s - previous.time_s); + for (std::size_t i = 0; i < kDof; ++i) { + metrics.max_qdd = std::max(metrics.max_qdd, + std::abs(sample.qd[i] - previous.qd[i]) / dt); + } + } + + output << std::setprecision(9) << sample.time_s << ',' << (after_stop ? "stop" : "run"); + for (const double value : sample.q) { + output << ',' << value; + } + for (const double value : sample.qd) { + output << ',' << value; + } + output << ',' << sample.commanded_twist.vx + << ',' << sample.commanded_twist.vy + << ',' << sample.commanded_twist.vz + << ',' << sample.commanded_twist.wx + << ',' << sample.commanded_twist.wy + << ',' << sample.commanded_twist.wz; + for (Eigen::Index i = 0; i < 6; ++i) { + output << ',' << tcp_twist[i]; + } + output << ',' << tcp_speed << ',' << tcp_acceleration << '\n'; + } +} + +class MotorRobotArmSpeedLStopTest : public ::testing::Test { +protected: + void SetUp() override + { + project_root_ = findProjectRoot(); + ASSERT_FALSE(project_root_.empty()); + + config::MujocoWorldRootConfig world_root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/mujoco/mujoco_world.pb.txt").string(), + &world_root_config)); + ASSERT_GT(world_root_config.worlds_size(), 0); + auto world_config = world_root_config.worlds(0); + world_config.set_model_path( + (project_root_ / "model/xiaoyan_description/dual_arm.xml").string()); + world_device_ = std::make_shared(world_config); + ASSERT_TRUE(world_device_->init()); + ASSERT_TRUE(world_device_->start()); + + config::MotorRootConfig motor_root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/motor/mujoco_motors.pb.txt").string(), + &motor_root_config)); + motor_system_ = std::make_shared( + "right_arm_mujoco_motors", motor_root_config.motor(), "right_arm_mujoco_motors"); + ASSERT_TRUE(motor_system_->init()); + + config::ArmRootConfig root_config; + ASSERT_TRUE(ProtoMessageIo::getProtoFromAsciiFile( + (project_root_ / "cmvr-es/config/devices/arm/arm_mujoco.pb.txt").string(), + &root_config)); + ASSERT_GT(root_config.arm().robot_arms_size(), 0); + arm_config_ = root_config.arm().robot_arms(0); + setKinematicsUrdfPath( + arm_config_, (project_root_ / "model/xiaoyan_description/dual_arm.urdf").string()); + arm_ = std::make_unique(arm_config_); + ASSERT_TRUE(arm_->init()); + + analysis_solver_ = IKSolverFactory::create(arm_config_.kinematics()); + ASSERT_TRUE(analysis_solver_); + ASSERT_TRUE(analysis_solver_->init()); + analysis_pinocchio_solver_ = std::dynamic_pointer_cast(analysis_solver_); + ASSERT_TRUE(analysis_pinocchio_solver_); + } + + void TearDown() override + { + if (arm_) { + arm_->stop(); + } + if (motor_system_) { + motor_system_->stop(); + } + if (world_device_) { + world_device_->stop(); + } + } + + std::filesystem::path project_root_; + config::RobotArmConfig arm_config_; + std::shared_ptr world_device_; + std::shared_ptr motor_system_; + std::unique_ptr arm_; + std::shared_ptr analysis_solver_; + std::shared_ptr analysis_pinocchio_solver_; +}; + +TEST_F(MotorRobotArmSpeedLStopTest, SweepAccelerationAndDirection) +{ + ASSERT_TRUE(world_device_); + ASSERT_TRUE(world_device_->world()); + + const std::vector initial(kInitialJointPosition.begin(), kInitialJointPosition.end()); + MotionOptions move_options; + move_options.velocity = 2.0; + move_options.acceleration = 3.0; + ASSERT_TRUE(arm_->moveJ(JointPositionCommand{initial}, move_options).ok()); + + for (const double acceleration : kAccelerationSweep) { + for (const double direction : {1.0, -1.0}) { + ASSERT_FALSE(arm_->busy()); + std::vector samples; + samples.reserve(5000); + std::atomic stop_time_s{-1.0}; + Result command_result = Result::failure(ArmErrorCode::UnknownError, "not run"); + const auto start_time = std::chrono::steady_clock::now(); + + std::thread command_thread([&] { + CartesianVelocity velocity; + velocity.vx = direction * kCommandVelocity; + command_result = arm_->speedL(velocity, + acceleration, + kCommandDurationS, + FrameType::Base); + stop_time_s.store(std::chrono::duration( + std::chrono::steady_clock::now() - start_time).count()); + }); + + while (std::chrono::duration(std::chrono::steady_clock::now() - start_time).count() < 5.0) { + const double time_s = std::chrono::duration( + std::chrono::steady_clock::now() - start_time).count(); + const auto state = arm_->getJointState(); + ASSERT_EQ(state.position.size(), kDof); + ASSERT_EQ(state.velocity.size(), kDof); + samples.push_back(Sample{time_s, + state.position, + state.velocity, + arm_->getSpeedLCommandTwistBase()}); + + const double stop = stop_time_s.load(); + if (stop >= 0.0 && time_s > stop + 0.8 && !arm_->busy()) { + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + command_thread.join(); + + ASSERT_TRUE(command_result.ok()) << command_result.message; + const double stop = stop_time_s.load(); + ASSERT_GT(stop, 1.8); + ASSERT_LT(stop, 2.5); + ASSERT_FALSE(arm_->busy()); + + const auto csv_path = std::filesystem::path("/tmp") / + ("speedl_stop_acc_" + numberForFile(acceleration) + + "_" + (direction > 0.0 ? "pos" : "neg") + ".csv"); + Metrics metrics; + writeCsv(csv_path, samples, stop, *analysis_pinocchio_solver_, metrics); + std::cout << "[SpeedLStopTest] acceleration=" << acceleration + << ", direction=" << direction + << ", csv=" << csv_path + << ", max_qd_before=" << metrics.max_qd_before_stop + << ", max_qd_after=" << metrics.max_qd_after_stop + << ", max_qdd=" << metrics.max_qdd + << ", max_tcp_speed_before=" << metrics.max_tcp_linear_speed_before_stop + << ", max_tcp_speed_after=" << metrics.max_tcp_linear_speed_after_stop + << ", max_tcp_acceleration=" << metrics.max_tcp_linear_acceleration + << std::endl; + + // Stopping must not create a new velocity peak. A small tolerance + // allows one 1 ms feedback sample of transport jitter. + EXPECT_LE(metrics.max_qd_after_stop, + metrics.max_qd_before_stop + 0.25) + << "post-stop joint velocity peak for acceleration=" << acceleration + << ", direction=" << direction; + EXPECT_LE(metrics.max_tcp_linear_speed_after_stop, + metrics.max_tcp_linear_speed_before_stop + 0.01) + << "post-stop TCP velocity peak for acceleration=" << acceleration + << ", direction=" << direction; + } + } +} + +} // namespace +} // namespace cmvr::device