From 34e349ce2a22b09a934f46237866be9f7196cd97 Mon Sep 17 00:00:00 2001 From: lgv Date: Wed, 4 Mar 2026 10:50:58 +0800 Subject: [PATCH] feat: css and csp test pass && add speedJ --- cmvr-es/common/config/cabin_robot.xml | 12 +-- cmvr-es/devices/motor/motor_manager_test.cpp | 9 ++- .../canopen/protocol/ti5_motor_tpdo1.cpp | 3 + .../canopen/ti5_motor_canopen_protocol.cpp | 20 ++--- .../canopen/ti5_motor_canopen_protocol.h | 6 +- cmvr-es/devices/motor/ti5_motor/ti5_motor.h | 11 ++- cmvr-es/devices/robot/abstract_robot.h | 3 +- .../humanoid_robot/include/humanoid_robot.h | 2 + .../humanoid_robot/src/humanoid_robot.cpp | 27 ++++++- .../src/humanoid_robot_test.cpp | 81 ++++++++++--------- 10 files changed, 108 insertions(+), 66 deletions(-) diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml index 826862ee..fa833a13 100644 --- a/cmvr-es/common/config/cabin_robot.xml +++ b/cmvr-es/common/config/cabin_robot.xml @@ -50,12 +50,12 @@ - - - - - - + + + + + + diff --git a/cmvr-es/devices/motor/motor_manager_test.cpp b/cmvr-es/devices/motor/motor_manager_test.cpp index 28018830..ea3e6769 100644 --- a/cmvr-es/devices/motor/motor_manager_test.cpp +++ b/cmvr-es/devices/motor/motor_manager_test.cpp @@ -73,11 +73,11 @@ TEST(MotorMangerTest,MyTest) { - motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY); - motor_3->setTarget(-0.5); - std::this_thread::sleep_for(std::chrono::milliseconds(5000)); + // motor_3->setMode(RUN_MODE_CYCLIC_SYNC_VELOCITY); + // motor_3->setTarget(-0.5); + // std::this_thread::sleep_for(std::chrono::milliseconds(5000)); motor_3->setMode(RUN_MODE_CYCLIC_SYNC_POSITION); - motor_3->setTarget(-3.14,1.6); + // motor_3->setTarget(0,1.6); // std::this_thread::sleep_for(std::chrono::milliseconds(5000)); // @@ -100,6 +100,7 @@ TEST(MotorMangerTest,MyTest) { while (true) { + auto mode = motor_3->getMode(); auto q = motor_3->getQ(); auto qd = motor_3->getQd(); std::cout << q << ", " << qd << std::endl; diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp index 10565b12..a119df4b 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp @@ -31,5 +31,8 @@ void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot } + LOG(INFO) << " Motor ID " << int(this->node_id_) << " mode = " << motor_status->run_mode() ; + + } diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp index 099ff914..b4a2b544 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.cpp @@ -104,10 +104,10 @@ void Ti5MotorCanopenProtocol::seedSdoRequest(uint8_t node_id, CommandSpecifier c void Ti5MotorCanopenProtocol::setQ(uint8_t node_id, double angle_rad) { auto cmd = (angle_rad * RADTODEG) / 360.0 * GearRatio * 65536.0; - switch (cur_mode_[node_id]) { - case RUN_MODE_CYCLIC_SYNC_POSITION: - setCSPTargetPosByPdo(node_id, static_cast(cmd)); - break; + switch (getMode(node_id)) { + // case RUN_MODE_CYCLIC_SYNC_POSITION: + // setCSPTargetPosByPdo(node_id, static_cast(cmd)); + // break; case RUN_MODE_PROFILE_POSITION: // setPPTargetPosByPdo(node_id, static_cast(cmd)); setPPTargetPosBySdo(node_id, static_cast(cmd)); @@ -179,10 +179,8 @@ void Ti5MotorCanopenProtocol::setCSPTargetPosByPdo(uint8_t node_id, int32_t pos) void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { - cur_mode_[node_id] = mode; - // 1 : 先设置模式 - auto data = static_cast(mode); - seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data); + // cur_mode_[node_id] = mode; + // 2 : 状态机步进 —— Shutdown（0x06） @@ -193,6 +191,10 @@ void Ti5MotorCanopenProtocol::setMode(uint8_t node_id, msgs::RunMode mode) { // configRPDO1(node_id, false); // configRPDO2(node_id, false); + // 1 : 先设置模式 + auto data = static_cast(mode); + seedSdoRequest(node_id, CS_WRITE_ONE_BYTE, OPERATION_MODE_6060, SUB_INDEX_0, data); + // 3 : 状态机步进 —— Switch On & Enable Operation（0x0F） cw.switch_on = 1; @@ -482,7 +484,7 @@ bool Ti5MotorCanopenProtocol::reachedTargetQ(uint8_t node_id) { void Ti5MotorCanopenProtocol::setQd(uint8_t node_id, double qd) { auto speed = ((qd * RADTODEG) * GearRatio * 100.0) / 360.0; - switch (cur_mode_[node_id]) { + switch (getMode(node_id)) { case msgs::RUN_MODE_CYCLIC_SYNC_POSITION: case msgs::RUN_MODE_PROFILE_POSITION: { auto it = last_Qd_.find(node_id); diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h index e0b3ac88..2abec2cc 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h +++ b/cmvr-es/devices/motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h @@ -62,10 +62,10 @@ namespace cmvr { } msgs::RunMode getMode(uint8_t node_id) override { - // auto feed_mode = GetRobotDetail()->motors().at(node_id).run_mode(); + return GetRobotDetail()->motors().at(node_id).run_mode(); // cur_mode_[node_id] = feed_mode; // return feed_mode; - return cur_mode_[node_id]; + // return cur_mode_[node_id]; } @@ -75,7 +75,7 @@ namespace cmvr { std::shared_ptr can_client_{nullptr}; // key node_id - std::unordered_map cur_mode_{}; + // std::unordered_map cur_mode_{}; std::unordered_map last_Qd_{}; std::unordered_map last_Qdd_{}; std::shared_ptr > can_sender_{nullptr}; diff --git a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h index f28d20b5..cfc9bde9 100644 --- a/cmvr-es/devices/motor/ti5_motor/ti5_motor.h +++ b/cmvr-es/devices/motor/ti5_motor/ti5_motor.h @@ -35,12 +35,15 @@ namespace cmvr { canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_RESET_COMMUNICATION); // canopen_protocol->torqueOff(node_id_); canopen_protocol->configPdo(node_id_); - canopen_protocol->configProfile(node_id_,4000,8000,8000); + // canopen_protocol->configProfile(node_id_,4000,8000,8000); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL); canopen_protocol->seedNmtRequest(node_id_,msgs::NMT_START_REMOTE_NODE); - canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06); - canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F); - canopen_protocol->setLimitQd(node_id_,6.0); + canopen_protocol->setMode(node_id_,msgs::RUN_MODE_CYCLIC_SYNC_POSITION); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x06,15); + // canopen_protocol->seedSdoRequest(node_id_, msgs::CS_WRITE_TWO_BYTES, msgs::CONTROL_WORD_6040, msgs::SUB_INDEX_0, 0x0F,15); + canopen_protocol->setLimitQd(node_id_,5.0); + canopen_protocol->setLimitQdd(node_id_,10.0,-10.0); + // canopen_protocol->torqueOff(node_id_); } } diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index dc39eb2f..50315346 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -84,7 +84,8 @@ namespace cmvr::device{ * @param acc 主导轴最大加速度，单位 rad/s^2。 */ virtual void moveJ(std::vector &cmd, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); } - + virtual void speedJ(std::vector &cmd) { throw std::runtime_error("Not implemented"); } + virtual void speedJ(double vel) { throw std::runtime_error("Not implemented"); } /** * @brief 末端位姿目标的关节空间运动。 * @details 典型实现为先做 IK 求解关节目标，再按 `moveJ` 语义执行（关节空间规划与同步）。 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 b52fc584..740176e7 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -79,6 +79,8 @@ namespace cmvr::device{ /* robot basic command*/ void eStop() override; void moveJ(std::vector &cmd, double vel = 0.5, double acc = 0.1) override; + void speedJ(std::vector &cmd) override; + void speedJ(double vel) override; void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override; void moveJ_IK(const std::string &base_link, const std::vector &targets, double vel, double acc) override ; void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override; 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 d04d5e6a..e745d671 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -216,6 +216,28 @@ void HumanoidRobot::eStop() { // } + +template +void HumanoidRobot::speedJ(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 +void HumanoidRobot::speedJ(double vel) { + for (const auto &motor: motor_manager_->motorsMap()) { + motor.second->setTarget(vel); + } +} template void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { if (cmd.empty()) return; @@ -341,7 +363,7 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); } - motor->setQd(vel); + // motor->setQd(vel); motors.push_back(motor); } @@ -353,7 +375,8 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double while (k < K) { const auto &s = samples[k]; for (size_t i = 0; i < motors.size(); ++i) { - motors[i]->setQ(s.q[i]); + // motors[i]->setQ(s.q[i]); + motors[i]->setTarget(s.q[i],s.qd[i]); } ++k; 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 6b82dda7..e408d51d 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 @@ -220,6 +220,43 @@ TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) { } + +TEST(HumanoidRobotTest,speedJTest) { + const XmlNode config("/home/lgv/cmvr/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 = dmgr.getDevice("hc01"); + std::vector cmd{}; + cmd = { + // {"R_SHOULDER_P", 0.0}, + // {"R_SHOULDER_R", 0.0}, + // {"R_SHOULDER_Y", 0.0}, + // {"R_ELBOW_R", 0.0}, + // {"R_WRIST_P", 0.0}, + // {"R_WRIST_Y", 0.0}, + {"R_WRIST_R", 0.2}, + }; + robot->speedJ(cmd); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + // robot->speedJ(0); + // + std::vector cmd1{}; + cmd1 = { + // {"R_SHOULDER_P", 0.0}, + // {"R_SHOULDER_R", 0.0}, + // {"R_SHOULDER_Y", 0.0}, + // {"R_ELBOW_R", 0.0}, + // {"R_WRIST_P", 0.0}, + // {"R_WRIST_Y", 0.0}, + {"R_WRIST_R", 0}, + }; + robot->moveJ(cmd1,2.0,10.0); + // robot->eStop(); + + // +} 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"; @@ -319,19 +356,6 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { << "Failed to extract right-arm 7 joints from robot state"; ibvs_controller.reset(q_now); - auto motor_manager = cmvr::MotorsInfo::getInstance()->getMotorManager(); - ASSERT_TRUE(motor_manager != nullptr) << "MotorManager is null"; - std::vector> right_arm_motors; - right_arm_motors.reserve(kRightArmJointNames.size()); - for (const auto* name : kRightArmJointNames) { - auto motor = motor_manager->getMotor(name); - ASSERT_TRUE(motor != nullptr) << "getMotor failed for " << name; - if (motor->getMode() !=cmvr::msgs::RUN_MODE_PROFILE_VELOCITY) { - motor->setMode(cmvr::msgs::RUN_MODE_PROFILE_VELOCITY); - } - motor->setQd(0.0); - right_arm_motors.push_back(motor); - } const int max_steps = 30000; const int log_every = 1; @@ -367,23 +391,10 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { std::vector qdot_next; const bool ok = ibvs_controller.compute(q_now, qdot_next); - if (!ok || qdot_next.size() != kRightArmJointNames.size()) { - for (auto& motor : right_arm_motors) { - motor->setQd(0.0); - } - std::fill(qdot_cmd_prev.begin(), qdot_cmd_prev.end(), 0.0); - ++fail_steps; - if ((step % log_every) == 0) { - std::cout << "[IBVS_REAL] step=" << step - << " compute failed: " - << cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus()) - << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); - continue; - } - std::vector qd_send(kRightArmJointNames.size(), 0.0); + + + std::vector qd_send; for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { qdot_lpf[i] = qdot_lpf_alpha * qdot_lpf[i] + (1.0 - qdot_lpf_alpha) * qdot_next[i]; double v = applySoftJointLimitVelocity(q_now[i], qdot_lpf[i], @@ -396,12 +407,10 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { v = std::clamp(v, qdot_cmd_prev[i] - dv_max, qdot_cmd_prev[i] + dv_max); v = std::clamp(v, -qdot_max_send, qdot_max_send); qdot_cmd_prev[i] = v; - qd_send[i] = v; + qd_send.push_back({kRightArmJointNames[i],v}); } - for (size_t i = 0; i < right_arm_motors.size(); ++i) { - right_arm_motors[i]->setQd(qd_send[i]); - } + robot->speedJ(qd_send); ++ok_steps; if ((step % log_every) == 0) { @@ -415,7 +424,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { << std::endl; std::cout << "qd_cmd: " ; for (const auto& it : qd_send) { - std::cout << it << " "; + std::cout << it.vel << " "; } std::cout << std::endl; } @@ -428,9 +437,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { << " fail_steps=" << fail_steps << std::endl; - for (auto& motor : right_arm_motors) { - motor->setQd(0.0); - } + robot->speedJ(0); EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps."; }