From a16a1083634a53ea92f1ad2daa90f16c265ce218 Mon Sep 17 00:00:00 2001 From: lgv Date: Thu, 12 Mar 2026 11:14:25 +0800 Subject: [PATCH] test:add speedL test --- cmvr-es/devices/robot/abstract_robot.h | 4 +- .../robot/humanoid_robot/CMakeLists.txt | 1 + .../humanoid_robot/include/humanoid_robot.h | 28 +- .../humanoid_robot/src/humanoid_robot.cpp | 934 +++++++----------- .../src/humanoid_robot_test.cpp | 274 ++++- .../include/pinocchio_dls_ik_solver.h | 30 +- .../ik_solver/src/pinocchio_dls_ik_solver.cpp | 133 ++- cmvr-es/ik_solver/src/srs_ik_test.cpp | 6 +- cmvr-es/planner/CMakeLists.txt | 2 +- .../grpc/src/grpc_humanoid_robot_service.cpp | 2 +- 10 files changed, 786 insertions(+), 628 deletions(-) diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 50315346..09c5d62b 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -108,11 +108,11 @@ namespace cmvr::device{ virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); } virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); } + virtual bool speedL(const std::vector &xd, double acceleration = 0.25, double time = 0.0) { throw std::runtime_error("Not implemented"); } + virtual void stopSpeedL() { throw std::runtime_error("Not implemented"); } virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); } - virtual void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); } - virtual void followJointTrajectory(std::vector> &traj, double dt) { throw std::runtime_error("Not implemented"); } virtual void followJointTrajectory(std::vector> &traj, double dt) { throw std::runtime_error("Not implemented"); } diff --git a/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt b/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt index d3b3e717..76bae1b3 100644 --- a/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt +++ b/cmvr-es/devices/robot/humanoid_robot/CMakeLists.txt @@ -38,6 +38,7 @@ target_link_libraries(humanoid_robot_test gtest_main pthread glog + matplot cmvr_es::proto ${OpenCV_LIBS} ccd diff --git a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h index 740176e7..0f881368 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -22,6 +22,7 @@ #include #include #include +#include #include #include #include @@ -33,6 +34,7 @@ #include "planner/joint_space_planner/include/joint_space_planner_creator.h" #include "planner/joint_space_planner/include/joint_space_planner.h" +#include "ik_solver/include/pinocchio_dls_ik_solver.h" namespace cmvr::device{ @@ -91,10 +93,13 @@ namespace cmvr::device{ void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override; - void speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) override; - void followJointTrajectory(std::vector> &traj, double dt) override; - void followPoseTrajectory(std::string &base_link, std::vector> &targets, double dt) override; + bool speedL(const std::vector &xd, double acceleration = 0.25, double time = 0.0) override; + void stopSpeedL() override; + Eigen::Matrix getSpeedLCommandTwistBase(); + + + void servoJ(std::vector &joints, double dt) override; void servoJ(std::vector &joints, double vel, double dt) override; @@ -111,6 +116,11 @@ namespace cmvr::device{ int exec_CSP_cmd_(std::vector &ids, std::vector &pos, int dt); int exec_CSV_cmd_(std::vector &ids, std::vector &vel, int dt); int exec_CSC_cmd_(std::vector &ids, std::vector &cur, int dt); + void sendJointVelocityCommand_(const std::vector& cmd); + bool updateSpeedLAccelerationConfig_(double acceleration); + void ensureSpeedLWorkerStarted_(); + void stopSpeedLWorker_(); + void speedLWorkerLoop_(); @@ -195,6 +205,18 @@ namespace cmvr::device{ std::shared_ptr motor_manager_{nullptr}; std::shared_ptr joint_space_planner_{nullptr}; + std::shared_ptr ik_solver_{nullptr}; + PinocchioDlsIKSolver::SpeedLConfig speedl_config_{}; + std::unique_ptr speedl_thread_; + std::mutex speedl_mutex_; + std::condition_variable speedl_cv_; + std::atomic speedl_stop_requested_{false}; + bool speedl_command_active_{false}; + Eigen::Matrix speedl_target_twist_{Eigen::Matrix::Zero()}; + Eigen::Matrix speedl_last_command_twist_base_{Eigen::Matrix::Zero()}; + double speedl_target_acceleration_{0.25}; + double speedl_applied_acceleration_{0.25}; + std::uint64_t speedl_command_version_{0}; // 每个电机组的锁和执行状态 std::mutex left_arm_mutex_, right_arm_mutex_, head_mutex_, waist_mutex_; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 08e4631a..6055f3c1 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -56,12 +56,329 @@ void HumanoidRobot::init() { joint_space_planner_->setSymmetricLimits(std::vector(7, 1.0), std::vector(7, 1.0)); + + + ik_solver_ = std::make_shared("/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + "PELVIS_S", + "R_WRIST_R_S", + "R_FINGER_TIP_FIXED"); + if (!ik_solver_->init()) { + throw runtime_error("HumanoidRobot speedL init failed: PinocchioDlsIKSolver::init() failed"); + } + + speedl_config_.linear_velocity_max = 0.55; + speedl_config_.linear_acceleration_max = 0.80; + speedl_config_.linear_jerk_max = 3.30; + speedl_config_.angular_velocity_max = 1.00; + speedl_config_.angular_acceleration_max = 3.00; + speedl_config_.angular_jerk_max = 12.0; + speedl_config_.joint_acceleration_max = std::vector(7, 8.0); + speedl_config_.linear_target_replan_threshold = 1e-4; + speedl_config_.angular_target_replan_threshold = 1e-4; + speedl_config_.linear_reverse_cos_threshold = -0.8660254037844386; + speedl_config_.linear_reverse_switch_speed_threshold = 1e-3; + if (!ik_solver_->configureSpeedL(speedl_config_)) { + throw runtime_error("HumanoidRobot speedL init failed: configureSpeedL() failed"); + } + speedl_applied_acceleration_ = speedl_config_.linear_acceleration_max; rsm_.store(ROBOT_ESTOP); } + +template +bool HumanoidRobot::speedL(const std::vector& xd, double acceleration, double time) +{ + if (!ik_solver_) { + throw runtime_error("speedL failed: ik_solver_ is not initialized"); + } + if (xd.size() != 6) { + throw runtime_error("speedL failed: xd size must be 6"); + } + if (acceleration <= 0.0) { + throw runtime_error("speedL failed: acceleration must be positive"); + } + if ((!speedl_thread_ || !speedl_thread_->joinable()) && + is_right_arm_busy_.exchange(true)) { + throw runtime_error("speedL failed: right arm is busy"); + } + + ensureSpeedLWorkerStarted_(); + + Eigen::Matrix target_twist = Eigen::Matrix::Zero(); + std::uint64_t command_version = 0; + for (int i = 0; i < 6; ++i) { + target_twist[i] = xd[static_cast(i)]; + } + + { + std::lock_guard lock(speedl_mutex_); + speedl_target_twist_ = target_twist; + speedl_target_acceleration_ = acceleration; + speedl_command_active_ = true; + command_version = ++speedl_command_version_; + } + speedl_cv_.notify_all(); + + if (time > 0.0) { + std::this_thread::sleep_for(std::chrono::duration(time)); + bool should_stop = false; + { + std::lock_guard lock(speedl_mutex_); + if (speedl_command_version_ == command_version) { + speedl_target_twist_.setZero(); + speedl_command_active_ = true; + ++speedl_command_version_; + should_stop = true; + } + } + if (should_stop) { + speedl_cv_.notify_all(); + } + } + return true; +} + +template +void HumanoidRobot::stopSpeedL() +{ + if (!speedl_thread_ || !speedl_thread_->joinable()) { + return; + } + + { + std::lock_guard lock(speedl_mutex_); + speedl_target_twist_.setZero(); + speedl_command_active_ = true; + ++speedl_command_version_; + } + LOG(INFO) << "HumanoidRobot stopSpeedL requested"; + speedl_cv_.notify_all(); +} + +template +Eigen::Matrix HumanoidRobot::getSpeedLCommandTwistBase() +{ + std::lock_guard lock(speedl_mutex_); + return speedl_last_command_twist_base_; +} + +template +void HumanoidRobot::sendJointVelocityCommand_(const std::vector& cmd) { + for (const auto& j : cmd) { + auto motor = motor_manager_->getMotor(j.joint_name); + if (motor != nullptr) { + if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) { + motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY); + } + motor->setTarget(j.vel); + } + } +} + +template +bool HumanoidRobot::updateSpeedLAccelerationConfig_(double acceleration) { + if (std::abs(speedl_applied_acceleration_ - acceleration) <= 1e-9) { + return true; + } + + auto config = speedl_config_; + config.linear_acceleration_max = acceleration; + config.angular_acceleration_max = acceleration; + if (!ik_solver_->configureSpeedL(config)) { + return false; + } + + speedl_config_ = config; + speedl_applied_acceleration_ = acceleration; + return true; +} + +template +void HumanoidRobot::ensureSpeedLWorkerStarted_() { + if (speedl_thread_ && speedl_thread_->joinable()) { + return; + } + speedl_stop_requested_.store(false); + LOG(INFO) << "HumanoidRobot speedL worker starting"; + speedl_thread_ = std::make_unique(&HumanoidRobot::speedLWorkerLoop_, this); +} + +template +void HumanoidRobot::stopSpeedLWorker_() { + if (!speedl_thread_ || !speedl_thread_->joinable()) { + return; + } + + { + std::lock_guard lock(speedl_mutex_); + speedl_stop_requested_.store(true); + speedl_command_active_ = false; + speedl_target_twist_.setZero(); + speedl_last_command_twist_base_.setZero(); + } + speedl_cv_.notify_all(); + speedl_thread_->join(); + LOG(INFO) << "HumanoidRobot speedL worker stopped"; + speedl_thread_.reset(); + speedl_stop_requested_.store(false); + speedl_applied_acceleration_ = speedl_config_.linear_acceleration_max; + is_right_arm_busy_.store(false); +} + +template +void HumanoidRobot::speedLWorkerLoop_() { + static const std::vector kRightArmJointNames = { + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", + "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R" + }; + const double dt = 1.0 / static_cast(std::max(upd_freq_, 1)); + auto next_tick = std::chrono::steady_clock::now(); + + auto read_right_arm_state = [&](std::vector& q_now, + std::vector& qd_now) -> bool { + std::vector states; + getJointsState(states); + std::unordered_map state_map; + state_map.reserve(states.size()); + for (const auto& js : states) { + state_map[js.name] = js; + } + + q_now.resize(kRightArmJointNames.size()); + qd_now.resize(kRightArmJointNames.size()); + for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { + const auto it = state_map.find(kRightArmJointNames[i]); + if (it == state_map.end()) { + return false; + } + q_now[i] = it->second.position; + qd_now[i] = it->second.velocity; + } + return true; + }; + + auto send_zero = [&]() { + std::vector zero_cmd; + zero_cmd.reserve(kRightArmJointNames.size()); + for (const auto& name : kRightArmJointNames) { + zero_cmd.push_back({name, 0.0}); + } + sendJointVelocityCommand_(zero_cmd); + }; + + LOG(INFO) << "HumanoidRobot speedL worker loop entered"; + + while (true) { + Eigen::Matrix target_twist = Eigen::Matrix::Zero(); + double acceleration = 0.25; + { + std::unique_lock lock(speedl_mutex_); + speedl_cv_.wait(lock, [&]() { + return speedl_stop_requested_.load() || speedl_command_active_; + }); + if (speedl_stop_requested_.load()) { + LOG(INFO) << "HumanoidRobot speedL worker loop received stop request"; + break; + } + target_twist = speedl_target_twist_; + acceleration = speedl_target_acceleration_; + } + + next_tick = std::chrono::steady_clock::now(); + while (true) { + { + std::lock_guard lock(speedl_mutex_); + if (speedl_stop_requested_.load()) { + LOG(INFO) << "HumanoidRobot speedL worker loop stopping during active command"; + send_zero(); + is_right_arm_busy_.store(false); + return; + } + if (!speedl_command_active_) { + break; + } + target_twist = speedl_target_twist_; + acceleration = speedl_target_acceleration_; + } + + if (!updateSpeedLAccelerationConfig_(acceleration)) { + LOG(ERROR) << "speedL worker: configureSpeedL() failed"; + send_zero(); + is_right_arm_busy_.store(false); + return; + } + + std::vector q_now; + std::vector qd_now; + if (!read_right_arm_state(q_now, qd_now)) { + LOG(ERROR) << "speedL worker: failed to read right arm joint state"; + send_zero(); + is_right_arm_busy_.store(false); + return; + } + + std::vector qd_cmd; + if (!ik_solver_->speedLStep(target_twist, + dt, + q_now, + qd_now, + qd_cmd, + cmvr::CartesianFrame::Tool, + true)) { + LOG(ERROR) << "speedL worker: speedLStep() failed"; + send_zero(); + is_right_arm_busy_.store(false); + return; + } + + std::vector qd_send; + qd_send.reserve(kRightArmJointNames.size()); + for (size_t i = 0; i < kRightArmJointNames.size() && i < qd_cmd.size(); ++i) { + qd_send.push_back({kRightArmJointNames[i], qd_cmd[i]}); + } + { + std::lock_guard lock(speedl_mutex_); + speedl_last_command_twist_base_ = ik_solver_->getSpeedLCommandTwistBase(); + } + sendJointVelocityCommand_(qd_send); + + double qd_cmd_norm = 0.0; + double qd_meas_norm = 0.0; + for (double v : qd_cmd) { + qd_cmd_norm += v * v; + } + for (double v : qd_now) { + qd_meas_norm += v * v; + } + qd_cmd_norm = std::sqrt(qd_cmd_norm); + qd_meas_norm = std::sqrt(qd_meas_norm); + + if (target_twist.norm() < 1e-9 && qd_cmd_norm < 1e-3 && qd_meas_norm < 1e-2) { + { + std::lock_guard lock(speedl_mutex_); + speedl_command_active_ = false; + speedl_last_command_twist_base_.setZero(); + } + send_zero(); + is_right_arm_busy_.store(false); + break; + } + + next_tick += std::chrono::duration_cast( + std::chrono::duration(dt)); + std::this_thread::sleep_until(next_tick); + } + } + + send_zero(); + is_right_arm_busy_.store(false); + LOG(INFO) << "HumanoidRobot speedL worker loop exited"; +} + template void HumanoidRobot::torqueOff() { try { + stopSpeedLWorker_(); for (const auto &pair: motor_manager_->motorsMap()) { if (pair.second->jointName() != "WAIST_Y" && pair.second->jointName() != "WAIST_P") pair.second->torqueOff(); @@ -75,6 +392,7 @@ void HumanoidRobot::torqueOff() { template HumanoidRobot::~HumanoidRobot() { // TODO: close can interfaces + stopSpeedLWorker_(); upd_timer_->stop(); // this->torqueOff(); } @@ -149,23 +467,27 @@ void HumanoidRobot::getState(RobotState &state) { template void HumanoidRobot::torqueOn() { + stopSpeedLWorker_(); eStop(); } template void HumanoidRobot::torqueOn(const std::string &joint_name) { + stopSpeedLWorker_(); auto motor = motor_manager_->getMotor(joint_name); motor->brake(); } template void HumanoidRobot::torqueOff(const std::string &joint_name) { + stopSpeedLWorker_(); auto motor = motor_manager_->getMotor(joint_name); motor->torqueOff(); } template void HumanoidRobot::eStop() { + stopSpeedLWorker_(); CSP_buffer_->clear(); CSV_buffer_->clear(); CSC_buffer_->clear(); @@ -219,6 +541,7 @@ void HumanoidRobot::eStop() { template void HumanoidRobot::speedJ(std::vector &cmd) { + stopSpeedLWorker_(); for (const auto &j: cmd) { auto motor = motor_manager_->getMotor(j.joint_name); @@ -234,12 +557,14 @@ void HumanoidRobot::speedJ(std::vector &cmd) { template void HumanoidRobot::speedJ(double vel) { + stopSpeedLWorker_(); for (const auto &motor: motor_manager_->motorsMap()) { motor.second->setTarget(vel); } } template void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { + stopSpeedLWorker_(); if (cmd.empty()) return; LOG(INFO) << "vel : " << vel << "acc : " << acc << endl; @@ -407,6 +732,7 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double template void HumanoidRobot::calibrateZeroQ(const std::string &joint_name) { + stopSpeedLWorker_(); auto motor = motor_manager_->getMotor(joint_name); motor->calibrateZeroQ(); } @@ -415,6 +741,7 @@ template void HumanoidRobot::moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) { try { + stopSpeedLWorker_(); // update m_state_ Eigen::Vector q_init; auto q_map = getJointQ(); @@ -499,6 +826,7 @@ void HumanoidRobot::moveJ_IK(const std::string &base_link, const std::vecto double vel, double acc) { try { + stopSpeedLWorker_(); // update m_state_ Eigen::Vector q_init; auto q_map = getJointQ(); @@ -565,6 +893,7 @@ template void HumanoidRobot::moveL(std::string &base_link, std::vector &targets, double vel, double acc) { try { + stopSpeedLWorker_(); // 获取当前关节状态 Eigen::Vector q_init; auto q_map = getJointQ(); @@ -739,6 +1068,7 @@ void HumanoidRobot::moveL(std::string &base_link, std::vector void HumanoidRobot::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) { try { + stopSpeedLWorker_(); // 获取电机控制对象 // 这里的控制函数需要根据你的实际实现来进行填充 // 获取目标关节的电机 @@ -770,609 +1100,11 @@ void HumanoidRobot::speedJ(std::string &joint_name, RobotJointIndexDirectio } } -template -void HumanoidRobot::speedL(RobotCartesian cart, RobotJointIndexDirection dir, double vel, double acc) { - if (vel <= 0 || acc <= 0) { - throw std::runtime_error("speedL: vel and acc must be positive"); - } - - try { - const double CONTROL_PERIOD = 1.0 / 100; // 控制周期保持不变 - const size_t MAX_QUEUE_SIZE = 10; // 队列最大缓存的轨迹点数量，防止内存溢出 - - // 定义基座和末端执行器链接 - std::string base_link = "PELVIS_S"; - std::string ee_link = toolFrame_; - - msgs::Pose3d current_pose = fk(base_link, ee_link); - - // 初始化当前位姿矩阵 - Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity(); - T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix( - current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz() - ); - T_current(0, 3) = current_pose.position().x(); - T_current(1, 3) = current_pose.position().y(); - T_current(2, 3) = current_pose.position().z(); - - // 获取目标笛卡尔速度方向 - Eigen::Vector3d direction = Eigen::Vector3d::Zero(); - bool is_rotation = false; - - // 根据方向设置笛卡尔速度 - switch (dir) { - case RobotJointIndexDirection::X_POSITIVE: - direction.x() = 1.0; - break; - case RobotJointIndexDirection::X_NEGATIVE: - direction.x() = -1.0; - break; - case RobotJointIndexDirection::Y_POSITIVE: - direction.y() = 1.0; - break; - case RobotJointIndexDirection::Y_NEGATIVE: - direction.y() = -1.0; - break; - case RobotJointIndexDirection::Z_POSITIVE: - direction.z() = 1.0; - break; - case RobotJointIndexDirection::Z_NEGATIVE: - direction.z() = -1.0; - break; - case RobotJointIndexDirection::ROTATE_X: - direction.x() = 1.0; - is_rotation = true; - break; - case RobotJointIndexDirection::ROTATE_Y: - direction.y() = 1.0; - is_rotation = true; - break; - case RobotJointIndexDirection::ROTATE_Z: - direction.z() = 1.0; - is_rotation = true; - break; - case RobotJointIndexDirection::FORWARD: - if (cart == RobotCartesian::X) direction.x() = 1.0; - else if (cart == RobotCartesian::Y) direction.y() = 1.0; - else if (cart == RobotCartesian::Z) direction.z() = 1.0; - break; - case RobotJointIndexDirection::BACKWARD: - if (cart == RobotCartesian::X) direction.x() = -1.0; - else if (cart == RobotCartesian::Y) direction.y() = -1.0; - else if (cart == RobotCartesian::Z) direction.z() = -1.0; - break; - default: - throw std::runtime_error("speedL: unknown direction"); - } - - // 计算末端执行器的总运动时间和轨迹点数量 - double move_time = calculateMoveTime(direction.norm(), vel, acc); - size_t num_points = std::max(2ul, static_cast(ceil(move_time / CONTROL_PERIOD))); - - // 生成S曲线速度规划的时间点和距离比例（主线程预计算） - std::vector time_points; - std::vector distance_ratios; - generateSTrapezoidalProfile(direction.norm(), vel, acc, move_time, num_points, - time_points, distance_ratios); - - LOG(INFO) << "speedL: Planning trajectory - points=" << num_points - << ", total distance=" << direction.norm() << "m, move time=" << move_time << "s"; - - // 创建轨迹队列及同步机制 - std::queue, Eigen::Vector > > trajectory_queue; - std::mutex queue_mutex; - std::condition_variable queue_cv; - std::atomic planning_completed{false}; // 规划是否完成 - std::atomic execution_failed{false}; // 执行是否失败 - std::atomic planned_points{0}; // 已规划的点数 - std::atomic executed_points{0}; // 已执行的点数 - - // 获取当前关节位置（初始点） - auto q_map_current = getJointQ(); - Eigen::Vector q_current; - for (int i = 0; i < DOF; ++i) { - q_current[i] = q_map_current[joint_names_[i]]; - } - - // 先将初始点加入队列 - { - std::lock_guard lock(queue_mutex); - trajectory_queue.push({q_current, Eigen::Vector::Zero()}); - planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量 - } - - // 启动控制执行子线程（先启动子线程） - std::thread control_thread([&]() { - try { - auto start_time = std::chrono::high_resolution_clock::now(); - LOG(INFO) << "控制执行线程已启动"; - - // 循环条件使用load()读取原子变量 - while (!planning_completed.load() || !trajectory_queue.empty() && !execution_failed.load()) { - // 从队列中获取轨迹点 - std::pair, Eigen::Vector > point; - bool has_point = false; - - { - std::unique_lock lock(queue_mutex); - // 等待队列中有数据或规划完成，使用load()读取原子变量 - if (queue_cv.wait_for(lock, std::chrono::milliseconds(500), - [&] { - return !trajectory_queue.empty() || planning_completed.load() || - execution_failed.load(); - })) { - if (!trajectory_queue.empty()) { - point = trajectory_queue.front(); - trajectory_queue.pop(); - has_point = true; - executed_points.store(executed_points.load() + 1); // 使用store和load操作原子变量 - } - } else { - // 超时，可能规划线程出现问题 - LOG(WARNING) << "控制线程等待轨迹点超时"; - execution_failed.store(true); // 使用store设置原子变量 - break; - } - } - - if (has_point) { - // 计算当前点的期望执行时间，确保按时间规划执行 - auto current_time = std::chrono::high_resolution_clock::now(); - std::chrono::duration elapsed = current_time - start_time; - double expected_time = (executed_points.load() - 1) * CONTROL_PERIOD; // 使用load读取原子变量 - - // 如果执行过快，等待到期望时间 - if (elapsed.count() < expected_time) { - std::this_thread::sleep_for(std::chrono::duration(expected_time - elapsed.count())); - } - - // 更新关节命令 - m_state_->SetQ(point.first); - m_robot_->ComputeForwardKinematics(m_state_); - - // 发送关节命令 - for (int j = 0; j < DOF; ++j) { - auto motor = motor_manager_->getMotor(joint_names_[j]); - if (motor != nullptr) { - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - } - motor->setQd(point.second[j]); - motor->setQ(point.first[j]); - - LOG(INFO) << "执行点 " << executed_points.load() // 使用load读取原子变量 - << ": joint[" << joint_names_[j] << "] = " - << point.first[j]; - } - } - } - } - - if (execution_failed.load()) { - // 使用load读取原子变量 - LOG(ERROR) << "控制执行线程异常退出"; - rsm_.store(ROBOT_ERROR); - } else { - LOG(INFO) << "控制执行线程完成，共执行 " << executed_points.load() // 使用load读取原子变量 - << " 个轨迹点"; - rsm_.store(ROBOT_READY); - } - } catch (const std::exception &e) { - LOG(ERROR) << "控制线程错误: " << e.what(); - execution_failed.store(true); // 使用store设置原子变量 - rsm_.store(ROBOT_ERROR); - } - }); - - // 主线程开始进行IK逆解和轨迹点规划（边规划边放入队列） - try { - Eigen::Vector prev_q = q_current; // 上一个关节位置 - double prev_time = 0.0; - - // 生成并规划轨迹点（从1开始，因为0已经作为初始点） - for (size_t i = 1; i <= num_points; ++i) { - // 检查执行线程是否失败，如果失败则停止规划，使用load读取原子变量 - if (execution_failed.load()) { - LOG(WARNING) << "执行线程失败，停止轨迹规划"; - break; - } - - // 生成笛卡尔空间轨迹点 - double s = distance_ratios[i]; - Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity(); - - if (is_rotation) { - // 旋转运动 - T_interp.block<3, 1>(0, 3) = T_current.block<3, 1>(0, 3); - double angle = s * direction.norm(); - Eigen::AngleAxisd rotation(angle, direction.normalized()); - Eigen::Matrix3d R_current = T_current.block<3, 3>(0, 0); - Eigen::Matrix3d R_interp = rotation * R_current; - T_interp.block<3, 3>(0, 0) = R_interp; - } else { - // 平移运动 - T_interp.block<3, 3>(0, 0) = T_current.block<3, 3>(0, 0); - T_interp(0, 3) = T_current(0, 3) + s * direction.x(); - T_interp(1, 3) = T_current(1, 3) + s * direction.y(); - T_interp(2, 3) = T_current(2, 3) + s * direction.z(); - } - - // IK逆解计算 - cmvr::ctrl::PoseTarget current_target; - current_target.T_target = T_interp; - current_target.link_name = ee_link; - current_target.w_posrot = 0.5; - current_target.weight = 1.0; - - Eigen::Vector q_next; - bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD, - ctrl::CartesianController::Mode::Position, - q_next, 10000, 1e-6); - - if (!ok) { - LOG(WARNING) << "轨迹点 " << i << " IK求解失败，停止规划"; - execution_failed.store(true); // 使用store设置原子变量 - break; - } - - // 计算时间差和关节速度 - double dt = time_points[i] - prev_time; - Eigen::Vector q_vel; - if (dt > 0) { - q_vel = (q_next - prev_q) / dt; - } else { - q_vel = Eigen::Vector::Zero(); - } - - // 将计算好的轨迹点放入队列，如果队列满了则等待 - { - std::unique_lock lock(queue_mutex); - // 等待队列有空间，使用load读取原子变量 - queue_cv.wait(lock, [&] { - return trajectory_queue.size() < MAX_QUEUE_SIZE || execution_failed.load(); - }); - - if (execution_failed.load()) { - // 使用load读取原子变量 - break; - } - - trajectory_queue.push({q_next, q_vel}); - planned_points.store(planned_points.load() + 1); // 使用store和load操作原子变量 - prev_q = q_next; - prev_time = time_points[i]; - - LOG(INFO) << "规划点 " << i << " 已加入队列，当前队列大小: " << trajectory_queue.size(); - } - queue_cv.notify_one(); // 通知控制线程有新数据 - - // 简单的速率控制，避免规划过快，使用load读取原子变量 - if (planned_points.load() - executed_points.load() > MAX_QUEUE_SIZE / 2) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - } - } catch (const std::exception &e) { - LOG(ERROR) << "轨迹规划错误: " << e.what(); - execution_failed.store(true); // 使用store设置原子变量 - } - - // 规划完成，通知控制线程，使用store设置原子变量 - planning_completed.store(true); - queue_cv.notify_one(); - LOG(INFO) << "轨迹规划完成，共规划 " << planned_points.load() // 使用load读取原子变量 - << " 个轨迹点"; - - // 等待控制线程完成 - if (control_thread.joinable()) { - control_thread.join(); - } - - if (execution_failed.load()) { - // 使用load读取原子变量 - LOG(ERROR) << "speedL执行失败"; - rsm_.store(ROBOT_ERROR); - throw std::runtime_error("speedL execution failed"); - } else { - LOG(INFO) << "speedL轨迹执行成功完成"; - } - } catch (const std::exception &e) { - LOG(ERROR) << "speedL失败: " << e.what(); - rsm_.store(ROBOT_ERROR); - throw std::runtime_error(std::string("speedL error: ") + e.what()); - } -} - - -template -void HumanoidRobot::followJointTrajectory(std::vector > &traj, double dt) { - try { - // 2. 轨迹合法性检查 - if (!check_joint_traj_(traj, dt)) { - throw runtime_error("followJointTrajectory: invalid trajectory"); - } - - // 3. 初始化：切换电机模式为CSP，清空缓冲，更新状态机 - std::lock_guard exec_lock(exec_mtx_); // 防止多线程指令冲突 - CSP_buffer_->clear(); // 清空CSP模式缓冲 - rsm_.store(ROBOT_RUNNING); - - // 3.1 预配置所有电机为CSP模式（避免轨迹执行中切换模式导致延迟） - for (const auto &motor_pair: motor_manager_->motorsMap()) { - auto motor = motor_pair.second; - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - LOG(INFO) << "Motor " << motor->jointName() << " switched to CSP mode"; - } - } - - // 4. 轨迹执行：使用定时器按dt间隔发送轨迹点 - std::shared_ptr traj_timer = std::make_shared(); - std::atomic waypoint_idx(0); // 当前执行的轨迹点索引（原子变量防线程竞争） - std::atomic traj_completed(false); // 轨迹是否完成 - - // 4.1 定时器回调：发送当前轨迹点 - traj_timer->start( - std::chrono::nanoseconds(static_cast(dt * 1e9)), // dt转换为纳秒 - [this, &traj, &waypoint_idx, &traj_completed, traj_timer]() { - // 检查轨迹中断（外部指令触发） - if (flash_cmd_.load()) { - flash_cmd_.store(false); - traj_timer->stop(); - traj_completed.store(true); - rsm_.store(ROBOT_ESTOP); - LOG(INFO) << "followJointTrajectory: interrupted by external command"; - return; - } - - // 检查轨迹是否完成 - size_t current_idx = waypoint_idx.load(); - if (current_idx >= traj.size()) { - traj_timer->stop(); - traj_completed.store(true); - rsm_.store(ROBOT_ESTOP); - LOG(INFO) << "followJointTrajectory: trajectory completed"; - return; - } - - // 4.2 发送当前轨迹点的关节指令 - const auto ¤t_waypoint = traj[current_idx]; - for (const auto &joint: current_waypoint) { - auto motor = motor_manager_->getMotor(joint.joint_name); - if (motor) { - // 优先使用轨迹点中的速度，若无则用默认速度（0.5 rad/s） - double target_vel = (joint.vel > 0) ? joint.vel : 0.5; - motor->setQd(target_vel); // 设置关节速度 - motor->setQ(joint.rad); // 设置关节目标位置 - LOG(INFO) << "Joint " << joint.joint_name - << " -> pos=" << joint.rad << " rad, vel=" << target_vel << " rad/s"; - } - } - - // 4.3 推进轨迹点索引 - waypoint_idx.fetch_add(1); - } - ); - - // 4.4 等待轨迹完成或中断 - while (!traj_completed.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用 - } - } catch (std::exception &e) { - // 异常处理：停止轨迹，重置状态机 - rsm_.store(ROBOT_ESTOP); - LOG(ERROR) << "followJointTrajectory failed: " << e.what(); - throw runtime_error("followJointTrajectory error: " + std::string(e.what())); - } -} - -template -void HumanoidRobot::followPoseTrajectory(std::string &base_link, - std::vector > &targets, double dt) { - try { - if (targets.empty()) { - throw runtime_error("followPoseTrajectory: pose trajectory is empty"); - } - if (dt <= 0 || dt > 0.1) { - throw runtime_error("followPoseTrajectory: invalid dt=" + std::to_string(dt) + " (must be 0 < dt ≤ 0.1)"); - } - // // 检查基座链接有效性（依赖机器人模型接口） - // int base_link_idx = m_robot_->GetLinkIdx(base_link); - // if (base_link_idx == -1) { - // throw runtime_error("followPoseTrajectory: invalid base link: " + base_link); - // } - - - // 2. 关键步骤1：获取当前关节状态，解算“轨迹第一个点”的关节配置（作为后续IK基准） - std::lock_guard exec_lock(exec_mtx_); - CSP_buffer_->clear(); // 清空CSP缓冲，避免指令冲突 - - // 2.1 获取当前关节角度（初始化机器人状态） - Eigen::Vector q_current; - auto q_map_current = getJointQ(); - q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"], - q_map_current["L_ELBOW_R"], - q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"], - q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"], - q_map_current["R_ELBOW_R"], - q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"]; - m_state_->SetQ(q_current); - m_robot_->ComputeForwardKinematics(m_state_); // 更新当前正运动学状态 - - - // 2.2 解算“轨迹第一个点”的关节配置（q_first，作为后续所有IK的初始值） - const auto &first_pose_targets = targets[0]; // 轨迹第一个点的位姿目标 - Eigen::Vector q_first; // 轨迹第一个点的关节配置（IK基准） - bool ik_first_ok = m_cctrl_->compute( - m_state_, // 当前机器人状态（作为IK初始值） - base_link, // 基座链接 - first_pose_targets, // 第一个点的位姿目标 - dt, // 控制周期（用于速度限制） - ctrl::CartesianController::Mode::Position, // 位置控制模式 - q_first, // 输出：第一个点的关节配置 - 10000, // IK最大迭代次数（确保精度） - 1e-6 // IK位置精度（1mm/0.001°） - ); - if (!ik_first_ok) { - throw runtime_error("followPoseTrajectory: IK failed for the FIRST waypoint (unreachable target)"); - } - LOG(INFO) << "followPoseTrajectory: first waypoint IK solved successfully, q_first=" << q_first.transpose(); - - - // 3. 关键步骤2：从当前位置移动到“轨迹第一个点”（过渡运动） - rsm_.store(ROBOT_RUNNING); // 切换状态为运行中 - LOG(INFO) << "followPoseTrajectory: moving from current position to first waypoint..."; - - // 3.1 配置电机为CSP模式（用于过渡运动和后续轨迹） - for (const auto &motor_pair: motor_manager_->motorsMap()) { - auto motor = motor_pair.second; - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - LOG(INFO) << "followPoseTrajectory: motor " << motor->jointName() << " switched to CSP mode"; - } - } - - // 3.2 执行“当前→第一个点”的过渡运动（匀速逼近，避免冲击） - const double TRANSITION_VEL = 0.5; // 过渡运动速度（1rad/s，可根据需求调整） - bool transition_completed = false; - auto transition_start_time = std::chrono::high_resolution_clock::now(); - - while (!transition_completed && !flash_cmd_.load()) { - // 3.2.1 计算当前应到达的关节位置（匀速插值） - auto now = std::chrono::high_resolution_clock::now(); - double elapsed = std::chrono::duration(now - transition_start_time).count(); - Eigen::Vector q_transition = q_current + (q_first - q_current) * std::min( - elapsed * TRANSITION_VEL / (q_first - q_current).norm(), 1.0); - - // 3.2.2 发送过渡运动关节指令 - for (size_t i = 0; i < DOF; ++i) { - const std::string &joint_name = joint_names_[i]; - auto motor = motor_manager_->getMotor(joint_name); - if (motor) { - motor->setQd(TRANSITION_VEL); // 过渡运动速度 - motor->setQ(q_transition[i]); // 当前过渡位置 - } - } - - // 3.2.3 检查过渡运动是否完成（所有关节到达目标） - transition_completed = true; - for (size_t i = 0; i < DOF; ++i) { - const std::string &joint_name = joint_names_[i]; - auto motor = motor_manager_->getMotor(joint_name); - if (motor && !motor->reachedTargetQ()) { - // 精度阈值：0.0001rad（≈0.0057°） - transition_completed = false; - break; - } - } - - // 3.2.4 控制过渡运动频率（与后续轨迹一致） - std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast(dt * 1e9))); - } - - // 3.2.5 过渡运动中断处理 - if (flash_cmd_.load()) { - flash_cmd_.store(false); - rsm_.store(ROBOT_ESTOP); - throw runtime_error("followPoseTrajectory: transition to first waypoint interrupted"); - } - LOG(INFO) << "followPoseTrajectory: reached first waypoint, start trajectory execution"; - - - // 4. 关键步骤3：执行轨迹（所有点的IK均以q_first为初始值） - std::shared_ptr traj_timer = std::make_shared(); - std::atomic waypoint_idx(0); // 当前执行的轨迹点索引（从0开始，即第一个点） - std::atomic traj_completed(false); // 轨迹是否完成 - Eigen::Vector last_valid_q = q_first; // 上一次有效的关节配置（容错用） - - // 4.1 初始化机器人状态为第一个点（确保轨迹起始状态正确） - m_state_->SetQ(q_first); - m_robot_->ComputeForwardKinematics(m_state_); - - // 4.2 定时器回调：按dt间隔解算IK并发送指令（IK初始值固定为q_first） - traj_timer->start( - std::chrono::nanoseconds(static_cast(dt * 1e9)), // 定时器周期=控制周期dt - [this, &base_link, &targets, &waypoint_idx, &traj_completed, &last_valid_q, &q_first, traj_timer, dt]() { - // 4.2.1 检查外部中断 - if (flash_cmd_.load()) { - flash_cmd_.store(false); - traj_timer->stop(); - traj_completed.store(true); - rsm_.store(ROBOT_ESTOP); - LOG(INFO) << "followPoseTrajectory: trajectory interrupted by external command"; - return; - } - - // 4.2.2 检查轨迹是否完成 - size_t current_idx = waypoint_idx.load(); - if (current_idx >= targets.size()) { - traj_timer->stop(); - traj_completed.store(true); - rsm_.store(ROBOT_ESTOP); - LOG(INFO) << "followPoseTrajectory: trajectory executed completely"; - return; - } - - // 4.2.3 解算当前轨迹点的IK（关键：初始值固定为q_first） - const auto ¤t_pose_targets = targets[current_idx]; - Eigen::Vector q_cmd; // 当前点的关节目标 - - // 临时更新机器人状态为q_first（确保IK初始值固定） - m_state_->SetQ(q_first); - m_robot_->ComputeForwardKinematics(m_state_); - - bool ik_ok = m_cctrl_->compute( - m_state_, // IK初始值：固定为q_first - base_link, // 基座链接 - current_pose_targets, // 当前点的位姿目标 - dt, // 控制周期 - ctrl::CartesianController::Mode::Position, - q_cmd, // 输出：当前点的关节配置 - 5000, // 减少迭代次数（平衡精度与速度） - 5e-4 // IK精度：0.5mm/0.028°（轨迹执行可适当放宽） - ); - - // 4.2.4 IK容错：失败时使用上一次有效配置 - if (!ik_ok) { - LOG(WARNING) << "followPoseTrajectory: IK failed at waypoint " << current_idx - << ", use last valid config (q_last_valid=" << last_valid_q.transpose() << ")"; - q_cmd = last_valid_q; - } else { - last_valid_q = q_cmd; // 更新有效配置 - } - - // 4.2.5 发送当前点的关节指令（固定速度，可根据需求调整） - const double TRAJ_VEL = 1.0; // 轨迹执行速度（1rad/s） - for (size_t i = 0; i < DOF; ++i) { - const std::string &joint_name = joint_names_[i]; - auto motor = motor_manager_->getMotor(joint_name); - if (motor) { - motor->setQd(TRAJ_VEL); // 轨迹执行速度 - motor->setQ(q_cmd[i]); // 关节目标位置 - LOG(INFO) << "followPoseTrajectory: waypoint " << current_idx - << ", joint " << joint_name << " -> pos=" << q_cmd[i] << " rad"; - } - } - - // 4.2.6 推进轨迹点索引 - waypoint_idx.fetch_add(1); - } - ); - - // 4.3 等待轨迹执行完成 - while (!traj_completed.load()) { - std::this_thread::sleep_for(std::chrono::milliseconds(1)); // 降低CPU占用 - } - } catch (std::exception &e) { - // 异常处理：重置状态机，确保机器人安全 - rsm_.store(ROBOT_ESTOP); - LOG(ERROR) << "followPoseTrajectory failed: " << e.what(); - throw runtime_error("followPoseTrajectory error: " + std::string(e.what())); - } -} template void HumanoidRobot::servoJ(std::vector &joints, double dt) { + stopSpeedLWorker_(); for (const auto &j: joints) { auto motor = motor_manager_->getMotor(j.joint_name); if (motor != nullptr) { @@ -1388,6 +1120,7 @@ void HumanoidRobot::servoJ(std::vector &joints, double dt) { template void HumanoidRobot::servoJ(std::vector &joints, double vel, double dt) { + stopSpeedLWorker_(); for (const auto &j: joints) { auto motor = motor_manager_->getMotor(j.joint_name); if (motor != nullptr) { @@ -1404,6 +1137,7 @@ template void HumanoidRobot::servoJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose, double vel, double acc) { try { + stopSpeedLWorker_(); // update m_state_ Eigen::Vector q_init; auto q_map = getJointQ(); @@ -1479,6 +1213,7 @@ void HumanoidRobot::servoDeltaJ(const std::string &base_link, const std::st template void HumanoidRobot::servoL(std::string &base_link, std::vector &targets, double dt) { try { + stopSpeedLWorker_(); Eigen::Vector q_cmd; bool ok = m_cctrl_->compute(m_state_, base_link, targets, 1, ctrl::CartesianController::Mode::Position, q_cmd, 60, 1e-4); @@ -1761,6 +1496,7 @@ void HumanoidRobot::moveDeltaL(const std::string &base_link, const std::str template void HumanoidRobot::moveL(const std::string &base_link, const std::string &ee_link, msgs::Pose3d target_pose, double vel, double acc) { + stopSpeedLWorker_(); if (vel <= 0 || acc <= 0) { throw std::runtime_error("moveL: vel and acc must be positive"); } diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index 7a93496a..ddf892f2 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -18,10 +18,12 @@ #include "gtest/gtest.h" #include +#include #include "../../../../device_manager/include/device_manager.h" #include #include "controller/include/ibvs_controller.h" +#include "ik_solver/include/pinocchio_dls_ik_solver.h" #include "cmvr/msgs/can_card_parameter.grpc.pb.h" #include "../include/humanoid_robot.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" @@ -257,6 +259,277 @@ TEST(HumanoidRobotTest,speedJTest) { // } + +TEST(HumanoidRobotTest, speedLSmokeTest) { + const XmlNode config("/home/lgv/cmvr/0-workspace/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); + ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; + auto dmgr_cfg = config.getChild("DeviceManager"); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + + auto robot_abs = dmgr.getDevice("hc01"); + auto robot = std::dynamic_pointer_cast>(robot_abs); + ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>"; + + const std::vector twist_pos = {0.03, 0.0, 0.0, 0.0, 0.0, 0.0}; + const std::vector twist_neg = {-0.03, 0.0, 0.0, 0.0, 0.0, 0.0}; + const double acceleration = 0.20; + const double segment_time = 2.0; + const double settle_time = 1.0; + const double sample_dt = 0.02; + + std::vector t_trace; + std::vector target_vx_trace; + std::vector command_vx_trace; + std::vector command_vy_trace; + std::vector command_vz_trace; + std::vector command_speed_trace; + + double t_now = 0.0; + auto sample_phase = [&](const std::vector& target_twist, + double duration, + const char* phase) { + LOG(INFO) << "speedLSmokeTest phase: " << phase; + ASSERT_NO_THROW(robot->speedL(target_twist, acceleration, 0.0)); + const int steps = static_cast(std::ceil(duration / sample_dt)); + for (int i = 0; i < steps; ++i) { + const Eigen::Matrix cmd_twist = robot->getSpeedLCommandTwistBase(); + t_trace.push_back(t_now); + target_vx_trace.push_back(target_twist[0]); + command_vx_trace.push_back(cmd_twist[0]); + command_vy_trace.push_back(cmd_twist[1]); + command_vz_trace.push_back(cmd_twist[2]); + command_speed_trace.push_back(cmd_twist.head<3>().norm()); + std::this_thread::sleep_for(std::chrono::duration(sample_dt)); + t_now += sample_dt; + } + }; + + sample_phase(twist_pos, segment_time, "+X"); + sample_phase(twist_neg, segment_time, "-X"); + + LOG(INFO) << "speedLSmokeTest phase: stop"; + ASSERT_NO_THROW(robot->stopSpeedL()); + const int settle_steps = static_cast(std::ceil(settle_time / sample_dt)); + for (int i = 0; i < settle_steps; ++i) { + const Eigen::Matrix cmd_twist = robot->getSpeedLCommandTwistBase(); + t_trace.push_back(t_now); + target_vx_trace.push_back(0.0); + command_vx_trace.push_back(cmd_twist[0]); + command_vy_trace.push_back(cmd_twist[1]); + command_vz_trace.push_back(cmd_twist[2]); + command_speed_trace.push_back(cmd_twist.head<3>().norm()); + std::this_thread::sleep_for(std::chrono::duration(sample_dt)); + t_now += sample_dt; + } + + ASSERT_FALSE(t_trace.empty()); + + using namespace matplot; + auto fig = figure(true); + fig->size(1600, 1000); + fig->font_size(16); + + auto ax1 = subplot(2, 1, 0); + hold(ax1, true); + auto l_target = plot(ax1, t_trace, target_vx_trace, "k--"); + l_target->line_width(2.0f); + auto l_cmd_x = plot(ax1, t_trace, command_vx_trace, "r-"); + l_cmd_x->line_width(2.0f); + auto l_cmd_y = plot(ax1, t_trace, command_vy_trace, "g-"); + l_cmd_y->line_width(2.0f); + auto l_cmd_z = plot(ax1, t_trace, command_vz_trace, "b-"); + l_cmd_z->line_width(2.0f); + title(ax1, "speedL target vx vs command vxyz"); + xlabel(ax1, "time [s]"); + ylabel(ax1, "linear cmd [m/s]"); + legend(ax1, {"target vx", "command vx", "command vy", "command vz"}); + grid(ax1, true); + + auto ax2 = subplot(2, 1, 1); + hold(ax2, true); + auto l_norm = plot(ax2, t_trace, command_speed_trace, "m-"); + l_norm->line_width(2.0f); + title(ax2, "speedL command speed norm"); + xlabel(ax2, "time [s]"); + ylabel(ax2, "norm [m/s]"); + legend(ax2, {"||command v||"}); + grid(ax2, true); + + show(fig); +} + +TEST(HumanoidRobotTest, SpeedLOpenLoopPlannerPlot) { + cmvr::PinocchioDlsIKSolver solver( + "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + "PELVIS_S", + "R_WRIST_R_S", + "R_FINGER_TIP_FIXED"); + ASSERT_TRUE(solver.init()) << "PinocchioDlsIKSolver init failed"; + + cmvr::PinocchioDlsIKSolver::SpeedLConfig speedl_config; + speedl_config.linear_velocity_max = 0.55; + speedl_config.linear_acceleration_max = 0.80; + speedl_config.linear_jerk_max = 3.30; + speedl_config.angular_velocity_max = 1.00; + speedl_config.angular_acceleration_max = 3.00; + speedl_config.angular_jerk_max = 12.0; + speedl_config.joint_acceleration_max = std::vector(7, 8.0); + speedl_config.linear_target_replan_threshold = 1e-4; + speedl_config.angular_target_replan_threshold = 1e-4; + speedl_config.linear_reverse_cos_threshold = -0.8660254037844386; + speedl_config.linear_reverse_switch_speed_threshold = 1e-3; + ASSERT_TRUE(solver.configureSpeedL(speedl_config)) << "configureSpeedL failed"; + + std::vector q_init = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; + solver.update_joints_state(q_init); + + const double dt = 0.002; + const int log_every = 50; + const double segment_time = 1.5; + const double stop_time = 1.0; + const double settle_time = 1.0; + const double linear_speed_cmd = 0.1; + const double z_speed_cmd = 0.10; + const double total_time = 2.0 * segment_time + stop_time; + const int active_steps = static_cast(std::ceil(total_time / dt)); + const int settle_steps = static_cast(std::ceil(settle_time / dt)); + const int total_steps = active_steps + settle_steps; + + int ok_steps = 0; + std::vector t_trace; + std::vector target_vy_trace; + std::vector target_vz_trace; + std::vector command_vx_trace; + std::vector command_vy_trace; + std::vector command_vz_trace; + std::vector> qd_cmd_trace(kRightArmJointNames.size()); + t_trace.reserve(total_steps); + target_vy_trace.reserve(total_steps); + target_vz_trace.reserve(total_steps); + command_vx_trace.reserve(total_steps); + command_vy_trace.reserve(total_steps); + command_vz_trace.reserve(total_steps); + for (auto& v : qd_cmd_trace) v.reserve(total_steps); + + for (int step = 0; step < total_steps; ++step) { + const double t = static_cast(step) * dt; + + Eigen::Matrix target_twist = Eigen::Matrix::Zero(); + if (t < segment_time) { + target_twist[1] = linear_speed_cmd; + target_twist[2] = z_speed_cmd; + + } else if (t < 2.0 * segment_time) { + target_twist[1] = -linear_speed_cmd; + target_twist[2] = -z_speed_cmd; + } else if (t < total_time) { + target_twist.setZero(); + } else { + target_twist.setZero(); + } + + std::vector qd_cmd; + ASSERT_TRUE(solver.speedLStep(target_twist, + dt, + qd_cmd, + cmvr::CartesianFrame::Base, + true)) + << "speedLStep failed at step " << step; + + t_trace.push_back(t); + target_vy_trace.push_back(target_twist[1]); + target_vz_trace.push_back(target_twist[2]); + command_vx_trace.push_back(solver.getSpeedLCommandTwistBase()[0]); + command_vy_trace.push_back(solver.getSpeedLCommandTwistBase()[1]); + command_vz_trace.push_back(solver.getSpeedLCommandTwistBase()[2]); + for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { + qd_cmd_trace[i].push_back(i < qd_cmd.size() ? qd_cmd[i] : 0.0); + } + + ++ok_steps; + + if ((step % log_every) == 0) { + std::cout << "[SPEEDL_OPEN_LOOP] step=" << step + << " t=" << t + << " target_vy=" << target_twist[1] + << " target_vz=" << target_twist[2] + << " phase=" + << (t < segment_time ? "pos" : + (t < 2.0 * segment_time ? "neg" : + (t < total_time ? "stop" : "settle"))) + << " cmd_qd="; + for (const auto& v : qd_cmd) { + std::cout << v << " "; + } + std::cout << std::endl; + } + } + + if (!t_trace.empty()) { + using namespace matplot; + auto fig = figure(true); + fig->size(1600, 1200); + fig->font_size(16); + + auto ax1 = subplot(3, 1, 0); + hold(ax1, true); + auto l_target_y = plot(ax1, t_trace, target_vy_trace, "k--"); + l_target_y->line_width(2.0f); + auto l_target_z = plot(ax1, t_trace, target_vz_trace, "c--"); + l_target_z->line_width(2.0f); + auto l_cmd_x = plot(ax1, t_trace, command_vx_trace, "r-"); + l_cmd_x->line_width(2.0f); + auto l_cmd_y = plot(ax1, t_trace, command_vy_trace, "g-"); + l_cmd_y->line_width(2.0f); + auto l_cmd_z = plot(ax1, t_trace, command_vz_trace, "b-"); + l_cmd_z->line_width(2.0f); + title(ax1, "speedL target vy/vz vs command vxyz"); + xlabel(ax1, "time [s]"); + ylabel(ax1, "linear cmd [m/s]"); + legend(ax1, {"target vy", "target vz", "command vx", "command vy", "command vz"}); + grid(ax1, true); + + auto ax2 = subplot(3, 1, 1); + hold(ax2, true); + std::vector joint_labels; + joint_labels.reserve(kRightArmJointNames.size()); + for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { + auto line = plot(ax2, t_trace, qd_cmd_trace[i]); + line->line_width(1.8f); + joint_labels.emplace_back(kRightArmJointNames[i]); + } + title(ax2, "solver qd_cmd"); + xlabel(ax2, "time [s]"); + ylabel(ax2, "joint vel [rad/s]"); + legend(ax2, joint_labels); + grid(ax2, true); + + auto ax3 = subplot(3, 1, 2); + hold(ax3, true); + std::vector qd_norm_trace; + qd_norm_trace.reserve(t_trace.size()); + for (size_t k = 0; k < t_trace.size(); ++k) { + double norm = 0.0; + for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { + const double v = qd_cmd_trace[i][k]; + norm += v * v; + } + qd_norm_trace.push_back(std::sqrt(norm)); + } + auto qd_norm_line = plot(ax3, t_trace, qd_norm_trace, "m-"); + qd_norm_line->line_width(2.0f); + title(ax3, "solver qd_cmd norm"); + xlabel(ax3, "time [s]"); + ylabel(ax3, "norm [rad/s]"); + legend(ax3, {"||qd_cmd||"}); + grid(ax3, true); + + show(fig); + } + + EXPECT_GT(ok_steps, 0) << "No successful open-loop speedL steps."; +} + TEST(HumanoidRobotTest,IBVSWithRealRobot) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; @@ -440,4 +713,3 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps."; } - diff --git a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h index 70800170..35c9d013 100644 --- a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h +++ b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h @@ -9,7 +9,7 @@ #include #include -#include "cartesian_space_planner/include/cartesian_twist_limiter.h" +#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h" namespace cmvr { @@ -39,6 +39,9 @@ public: std::vector joint_velocity_max; ///< size=chain_v_dof_，为空则仅用 URDF limit std::vector joint_acceleration_max; ///< size=chain_v_dof_，为空则不做 joint accel 限制 + bool enable_joint_soft_limit_velocity{true}; + double joint_soft_limit_margin{0.05}; + double joint_hard_limit_margin{0.03}; double linear_target_replan_threshold{1e-4}; double angular_target_replan_threshold{1e-4}; @@ -101,7 +104,11 @@ public: /** * @brief 配置 speedL task-space / joint-space 限幅参数。 * - * 调用后会重置 speedL 内部运行态，但保留本次配置。 + * 仅更新 speedL 配置，不主动重置当前运行态。 + * + * 使用注意： + * 1) 运行中再次调用本接口时，当前 target / planner state / 上一拍 qdot 缓存会被保留。 + * 2) 如果你需要“重新配置并从静止状态重新开始”，请先调用 resetSpeedL()，再调用 configureSpeedL()。 */ bool configureSpeedL(const SpeedLConfig& config); @@ -154,7 +161,9 @@ public: * 1) 只有在能拿到真实 q、但暂时拿不到真实 qd 时再用这个版本。 * 2) 这里的 qdot_measured 是 speedl_prev_qdot_cmd_ 的近似值；下游存在饱和、延迟或丢包时， * 这个近似会偏乐观。 - * 3) 一旦能拿到真实 qdot，应切回带 q_measured/qdot_measured 的闭环版本。 + * 3) 为避免把伪造的 qdot_measured 再同步回 task-space planner，这个版本不会执行 + * CartesianTwistLimiter::synchronize(...)。 + * 4) 一旦能拿到真实 qdot，应切回带 q_measured/qdot_measured 的闭环版本。 */ bool speedLStep(const Eigen::Matrix& target_twist, double dt, @@ -174,7 +183,8 @@ public: * 2) 该版本依赖内部预测状态，会随时间累计漂移；不适合长时间连续运行。 * 3) 仅当 chain_dof_ == chain_v_dof_ 时才允许使用，因为内部使用简单的 q += qdot * dt * 欧拉推进。 - * 4) 一旦恢复真实 q 或 qd，请立即切回上面两个 overload。 + * 4) 该版本同样不会执行 CartesianTwistLimiter::synchronize(...)。 + * 5) 一旦恢复真实 q 或 qd，请立即切回上面两个 overload。 */ bool speedLStep(const Eigen::Matrix& target_twist, double dt, @@ -235,11 +245,22 @@ private: Eigen::VectorXd* q_full_out = nullptr); Eigen::VectorXd applyJointVelocityLimits(const Eigen::VectorXd& qdot_des) const; + Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain, + const Eigen::VectorXd& qdot_des); Eigen::VectorXd applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des, const Eigen::VectorXd& qdot_reference, double dt) const; + bool speedLStepImpl(const Eigen::Matrix& target_twist, + double dt, + const std::vector& q_measured, + const std::vector& qdot_measured, + std::vector& qdot_cmd, + CartesianFrame input_frame, + bool is_tcp, + bool enable_twist_sync); + private: int chain_q_start_{0}; int chain_dof_{0}; @@ -273,6 +294,7 @@ private: Eigen::Matrix speedl_twist_measured_base_{Eigen::Matrix::Zero()}; Eigen::Matrix speedl_twist_command_base_{Eigen::Matrix::Zero()}; Eigen::Matrix speedl_twist_executed_base_{Eigen::Matrix::Zero()}; + bool speedl_joint_soft_limit_active_{false}; }; } // namespace cmvr diff --git a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp index 8503333c..191668be 100644 --- a/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp +++ b/cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp @@ -15,6 +15,7 @@ #include #include #include +#include namespace cmvr { @@ -709,12 +710,6 @@ bool PinocchioDlsIKSolver::configureSpeedL(const SpeedLConfig& config) speedl_twist_limiter_.setLinearReverseSwitchPolicy(config.linear_reverse_cos_threshold, config.linear_reverse_switch_speed_threshold); - speedl_twist_limiter_.reset(); - speedl_prev_qdot_cmd_.assign(chain_v_dof_, 0.0); - speedl_twist_measured_base_.setZero(); - speedl_twist_command_base_.setZero(); - speedl_twist_executed_base_.setZero(); - speedl_configured_ = true; return true; } @@ -726,6 +721,7 @@ void PinocchioDlsIKSolver::resetSpeedL() speedl_twist_measured_base_.setZero(); speedl_twist_command_base_.setZero(); speedl_twist_executed_base_.setZero(); + speedl_joint_soft_limit_active_ = false; } bool PinocchioDlsIKSolver::resolveSpeedLEeFrame(bool is_tcp, pinocchio::FrameIndex& ee_id) const @@ -847,6 +843,89 @@ Eigen::VectorXd PinocchioDlsIKSolver::applyJointVelocityLimits(const Eigen::Vect return gamma * qdot_des; } +Eigen::VectorXd PinocchioDlsIKSolver::applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain, + const Eigen::VectorXd& qdot_des) +{ + if (!speedl_config_.enable_joint_soft_limit_velocity || + q_chain.size() != chain_dof_ || + qdot_des.size() != chain_v_dof_ || + chain_dof_ != chain_v_dof_ || + joint_pos_lower_limits_.size() != chain_dof_ || + joint_pos_upper_limits_.size() != chain_dof_) { + return qdot_des; + } + + const double hard_margin = std::max(1e-4, speedl_config_.joint_hard_limit_margin); + const double soft_margin = std::max(hard_margin + 1e-4, speedl_config_.joint_soft_limit_margin); + + auto apply_scalar = [&](double q, + double v, + double q_min, + double q_max) -> double { + if (q_max <= q_min) { + return 0.0; + } + + if (v < 0.0) { + const double q_hard = q_min + hard_margin; + const double q_soft = q_min + soft_margin; + if (q <= q_hard) { + return 0.0; + } + if (q < q_soft) { + const double s = std::clamp((q - q_hard) / (q_soft - q_hard), 0.0, 1.0); + return v * s; + } + } else if (v > 0.0) { + const double q_hard = q_max - hard_margin; + const double q_soft = q_max - soft_margin; + if (q >= q_hard) { + return 0.0; + } + if (q > q_soft) { + const double s = std::clamp((q_hard - q) / (q_hard - q_soft), 0.0, 1.0); + return v * s; + } + } + return v; + }; + + Eigen::VectorXd qdot_limited = qdot_des; + bool clamped_any = false; + std::ostringstream oss; + + for (int i = 0; i < chain_v_dof_; ++i) { + const double v_before = qdot_des[i]; + const double v_after = apply_scalar(q_chain[i], + v_before, + joint_pos_lower_limits_[i], + joint_pos_upper_limits_[i]); + qdot_limited[i] = v_after; + + if (std::abs(v_after - v_before) > 1e-9) { + clamped_any = true; + if (oss.tellp() > 0) { + oss << " | "; + } + const std::string joint_name = + (i < static_cast(chain_joint_names_.size())) ? chain_joint_names_[i] : ("joint_" + std::to_string(i)); + oss << joint_name + << " q=" << q_chain[i] + << " v:" << v_before << "->" << v_after; + } + } + + if (clamped_any && !speedl_joint_soft_limit_active_) { + std::cerr << "[PinocchioDlsIKSolver] speedL soft joint-limit velocity clamp active: " + << oss.str() << "\n"; + } else if (!clamped_any && speedl_joint_soft_limit_active_) { + std::cerr << "[PinocchioDlsIKSolver] speedL soft joint-limit velocity clamp released\n"; + } + speedl_joint_soft_limit_active_ = clamped_any; + + return qdot_limited; +} + Eigen::VectorXd PinocchioDlsIKSolver::applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des, const Eigen::VectorXd& qdot_reference, double dt) const @@ -885,6 +964,25 @@ bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix& target_tw std::vector& qdot_cmd, CartesianFrame input_frame, bool is_tcp) +{ + return speedLStepImpl(target_twist, + dt, + q_measured, + qdot_measured, + qdot_cmd, + input_frame, + is_tcp, + true); +} + +bool PinocchioDlsIKSolver::speedLStepImpl(const Eigen::Matrix& target_twist, + double dt, + const std::vector& q_measured, + const std::vector& qdot_measured, + std::vector& qdot_cmd, + CartesianFrame input_frame, + bool is_tcp, + bool enable_twist_sync) { if (!initialized_) { std::cerr << "[PinocchioDlsIKSolver] speedLStep failed: solver not initialized\n"; @@ -928,8 +1026,9 @@ bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix& target_tw return false; } - // 1) 用 measured twist 同步 task-space limiter - speedl_twist_limiter_.synchronize(speedl_twist_measured_base_, dt, true); + if (enable_twist_sync) { + speedl_twist_limiter_.synchronize(speedl_twist_measured_base_, dt, true); + } // 2) 设置目标 twist speedl_twist_limiter_.setTargetTwist(target_twist, input_frame); @@ -965,6 +1064,9 @@ bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix& target_tw // 5) joint velocity limit（整体缩放） qdot_des = applyJointVelocityLimits(qdot_des); + // 5.1) joint soft position-limit velocity clamp（逐轴压缩靠近限位且继续往外的速度） + qdot_des = applyJointSoftLimitVelocity(q_chain, qdot_des); + // 6) joint acceleration limit（相对 measured qdot 整体缩放） const Eigen::Map qdot_meas_vec(qdot_measured.data(), chain_v_dof_); qdot_des = applyJointAccelerationLimits(qdot_des, qdot_meas_vec, dt); @@ -1012,13 +1114,14 @@ bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix& target_tw qdot_measured = speedl_prev_qdot_cmd_; } - return speedLStep(target_twist, - dt, - q_measured, - qdot_measured, - qdot_cmd, - input_frame, - is_tcp); + return speedLStepImpl(target_twist, + dt, + q_measured, + qdot_measured, + qdot_cmd, + input_frame, + is_tcp, + false); } bool PinocchioDlsIKSolver::speedLStep(const Eigen::Matrix& target_twist, diff --git a/cmvr-es/ik_solver/src/srs_ik_test.cpp b/cmvr-es/ik_solver/src/srs_ik_test.cpp index 4ee03407..854d46b3 100644 --- a/cmvr-es/ik_solver/src/srs_ik_test.cpp +++ b/cmvr-es/ik_solver/src/srs_ik_test.cpp @@ -1534,9 +1534,11 @@ TEST(SRS_IK_TEST, SPEEDL_RUN_MUJOCO) { Eigen::Matrix target_twist = Eigen::Matrix::Zero(); if (t < segment_time) { - target_twist[5] = -linear_speed_cmd; + target_twist[1] = linear_speed_cmd; + target_twist[2] = 0.1; } else if (t < 2.0 * segment_time) { - target_twist[5] = linear_speed_cmd; + target_twist[1] = -linear_speed_cmd; + target_twist[2] = -0.1; } else { target_twist.setZero(); } diff --git a/cmvr-es/planner/CMakeLists.txt b/cmvr-es/planner/CMakeLists.txt index 65a400c5..0e6384b3 100644 --- a/cmvr-es/planner/CMakeLists.txt +++ b/cmvr-es/planner/CMakeLists.txt @@ -1,6 +1,6 @@ -add_library(planner STATIC +add_library(planner SHARED joint_space_planner/src/joint_space_planner.cpp joint_space_planner/src/joint_space_planner_creator.cpp joint_space_planner/src/toppra_bspline.cpp diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp index a87180b5..f121f1dd 100644 --- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp @@ -143,7 +143,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::speedL(grpc::ServerContext* context, auto cart = static_cast(request->cart()); robot->setToolFrame(ee_link); - robot->speedL(cart,dir,vel,acc); + // robot->speedL(cart,dir,vel,acc); }catch (const std::exception& e) { response->mutable_header()->set_success(false); response->mutable_header()->set_error_message(e.what());