From aeae44b8a4a2f70fff5db8ecee0de15ccd2ac0bc Mon Sep 17 00:00:00 2001 From: lgv Date: Wed, 12 Nov 2025 18:57:09 +0800 Subject: [PATCH] test:movej --- .../robot/humanoid_robot/humanoid_robot.cpp | 247 +++++++++--------- 1 file changed, 123 insertions(+), 124 deletions(-) diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index 25692aaa..23bc269c 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -173,138 +173,137 @@ void HumanoidRobot::eStop() { } } -// template -// void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { -// try { -// for (const auto &j: cmd) { -// auto motor = motor_manager_->getMotor(j.joint_name); -// if (motor != nullptr) { -// // PPM 模式下 这个实际速度会超30% 左右 -// motor->setQd(vel); -// -// if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { -// motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); -// } -// motor->setQ(j.rad); -// } -// } -// -// //3. wait for completion -// bool completion = true; -// do { -// completion = true; -// for (const auto &j: cmd) { -// auto motor = motor_manager_->getMotor(j.joint_name); -// if (motor != nullptr) { -// if (!motor->reachedTargetQ()) { -// completion = false; -// break; -// } -// } -// } -// // 4. while waiting, check flash_cmd_, if it is true, set it false then exit -// if (flash_cmd_.load()) { -// flash_cmd_.store(false); -// return; -// } -// std::this_thread::sleep_for(std::chrono::milliseconds(2)); -// } while (!completion); -// } catch (exception &e) { -// throw runtime_error(e.what()); -// } -// } - - template void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { - if (cmd.empty()) return; + try { + for (const auto &j: cmd) { + auto motor = motor_manager_->getMotor(j.joint_name); + if (motor != nullptr) { + // PPM 模式下 这个实际速度会超30% 左右 + motor->setQd(vel); - std::unordered_map cur_joints_angle; - cur_joints_angle.reserve(cmd.size()); - - std::vector > motors; - motors.reserve(cmd.size()); - - for (auto &jp: cmd) { - auto motor = motor_manager_->getMotor(jp.joint_name); - motors.push_back(motor); - // 切 CSP - if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { - motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); - } - // 为 map 预填 key - cur_joints_angle.emplace(jp.joint_name, 0.0); - } - - getJointQ(cur_joints_angle); - - std::vector q_start, q_goal; - q_start.reserve(cmd.size()); - q_goal.reserve(cmd.size()); - for (auto &jp: cmd) { - auto it = cur_joints_angle.find(jp.joint_name); - if (it == cur_joints_angle.end()) { - LOG(ERROR) << "missing current angle for " << jp.joint_name; - return; - } - q_start.push_back(it->second); // 当前读到的弧度 - q_goal.push_back(jp.rad); // 目标弧度 - } - if (q_start.size() != q_goal.size()) return; - - - TrajPtr traj; - joint_space_planner_->setPathType(PathType::Quintic); - joint_space_planner_->setSymmetricLimits(std::vector(cmd.size(), vel), - std::vector(cmd.size(), acc)); - - - bool ok = joint_space_planner_->plan(q_start, q_goal, traj); - if (!ok) { - LOG(ERROR) << "planner plan() failed"; - return; - } - - - // 10ms - const double dt = 0.01; - auto samples = joint_space_planner_->sampleTrajectory(traj, dt); - if (samples.size() < 2) { - LOG(ERROR) << "trajectory too short"; - return; - } - - const auto t0 = std::chrono::steady_clock::now(); - - // k=1 开始 - size_t k = 0; - const size_t K = samples.size(); - - while (k < K) { - // ——根据墙钟计算“应当在的拍次”，用于追赶—— - auto now = std::chrono::steady_clock::now(); - double elapsed = std::chrono::duration(now - t0).count(); - size_t k_should = static_cast(std::floor(elapsed / dt)); // 向下取整更保守 - if (k_should >= K) k_should = K - 1; - if (k < k_should) k = k_should; // 追赶，避免越拖越晚 - - // ——下发第 k 个采样点—— - const auto &s = samples[k]; - // 这里最好在进入循环前做过维度检查：s.q/qd 与 motors.size() 相等 - for (size_t i = 0; i < motors.size(); ++i) { - motors[i]->setQd(s.qd[i]); - motors[i]->setQ(s.q[i]); + if (motor->getMode() != msgs::RUN_MODE_PROFILE_POSITION) { + motor->setMode(msgs::RUN_MODE_PROFILE_POSITION); + } + motor->setQ(j.rad); + } } - // ——睡到下一拍 (k+1)*dt—— - ++k; // 先递增，下一拍时间点明确 - if (k < K) { - auto next_t = t0 + std::chrono::duration(k * dt); - std::this_thread::sleep_until(next_t); - } + //3. wait for completion + bool completion = true; + do { + completion = true; + for (const auto &j: cmd) { + auto motor = motor_manager_->getMotor(j.joint_name); + if (motor != nullptr) { + if (!motor->reachedTargetQ()) { + completion = false; + break; + } + } + } + // 4. while waiting, check flash_cmd_, if it is true, set it false then exit + if (flash_cmd_.load()) { + flash_cmd_.store(false); + return; + } + std::this_thread::sleep_for(std::chrono::milliseconds(2)); + } while (!completion); + } catch (exception &e) { + throw runtime_error(e.what()); } } + +// template +// void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { +// if (cmd.empty()) return; +// +// std::unordered_map cur_joints_angle; +// cur_joints_angle.reserve(cmd.size()); +// +// std::vector > motors; +// motors.reserve(cmd.size()); +// +// for (auto &jp: cmd) { +// auto motor = motor_manager_->getMotor(jp.joint_name); +// motors.push_back(motor); +// // 切 CSP +// if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { +// motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); +// } +// // 为 map 预填 key +// cur_joints_angle.emplace(jp.joint_name, 0.0); +// } +// +// getJointQ(cur_joints_angle); +// +// std::vector q_start, q_goal; +// q_start.reserve(cmd.size()); +// q_goal.reserve(cmd.size()); +// for (auto &jp: cmd) { +// auto it = cur_joints_angle.find(jp.joint_name); +// if (it == cur_joints_angle.end()) { +// LOG(ERROR) << "missing current angle for " << jp.joint_name; +// return; +// } +// q_start.push_back(it->second); // 当前读到的弧度 +// q_goal.push_back(jp.rad); // 目标弧度 +// } +// if (q_start.size() != q_goal.size()) return; +// +// +// TrajPtr traj; +// joint_space_planner_->setPathType(PathType::Quintic); +// joint_space_planner_->setSymmetricLimits(std::vector(cmd.size(), vel), +// std::vector(cmd.size(), acc)); +// +// +// bool ok = joint_space_planner_->plan(q_start, q_goal, traj); +// if (!ok) { +// LOG(ERROR) << "planner plan() failed"; +// return; +// } +// +// +// // 10ms +// const double dt = 0.01; +// auto samples = joint_space_planner_->sampleTrajectory(traj, dt); +// if (samples.size() < 2) { +// LOG(ERROR) << "trajectory too short"; +// return; +// } +// +// const auto t0 = std::chrono::steady_clock::now(); +// +// // k=0 开始 +// size_t k = 0; +// const size_t K = samples.size(); +// +// while (k < K) { +// // // ——根据墙钟计算“应当在的拍次”，用于追赶—— +// // auto now = std::chrono::steady_clock::now(); +// // double elapsed = std::chrono::duration(now - t0).count(); +// // size_t k_should = static_cast(std::floor(elapsed / dt)); // 向下取整更保守 +// // if (k_should >= K) k_should = K - 1; +// // if (k < k_should) k = k_should; // 追赶，避免越拖越晚 +// +// // ——下发第 k 个采样点—— +// const auto &s = samples[k]; +// +// for (size_t i = 0; i < motors.size(); ++i) { +// motors[i]->setQd(s.qd[i]); +// motors[i]->setQ(s.q[i]); +// } +// +// ++k; +// if (k < K) { +// auto next_t = t0 + std::chrono::duration(k * dt); +// std::this_thread::sleep_until(next_t); +// } +// } +// } + template void HumanoidRobot::calibrateZeroQ(const std::string &joint_name) { auto motor = motor_manager_->getMotor(joint_name);