diff --git a/config/cabin_robot.xml b/config/cabin_robot.xml index d3c4ef93..6300a070 100644 --- a/config/cabin_robot.xml +++ b/config/cabin_robot.xml @@ -20,9 +20,9 @@ - - - + + + @@ -49,17 +49,17 @@ - - - - - - + + + + + + - + - + diff --git a/src/devices/motor/ti5_motor/ti5_motor.h b/src/devices/motor/ti5_motor/ti5_motor.h index 5e21788b..f28d20b5 100644 --- a/src/devices/motor/ti5_motor/ti5_motor.h +++ b/src/devices/motor/ti5_motor/ti5_motor.h @@ -31,6 +31,9 @@ namespace cmvr { } if (protocol_->comm_proto == MotorProtocolInterface::CommProto::CANOPEN ) { auto canopen_protocol = std::dynamic_pointer_cast(protocol_); + // torqueOff(node_id_); + 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->seedNmtRequest(node_id_,msgs::NMT_ENTER_PRE_OPERATIONAL); @@ -38,6 +41,7 @@ namespace cmvr { 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->torqueOff(node_id_); } } }; diff --git a/src/devices/robot/humanoid_robot/include/humanoid_robot.h b/src/devices/robot/humanoid_robot/include/humanoid_robot.h index 73ba85cc..b52fc584 100644 --- a/src/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/src/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -194,6 +194,11 @@ namespace cmvr::device{ std::shared_ptr joint_space_planner_{nullptr}; + // 每个电机组的锁和执行状态 + std::mutex left_arm_mutex_, right_arm_mutex_, head_mutex_, waist_mutex_; + std::atomic is_left_arm_busy_{false}, is_right_arm_busy_{false}; + std::atomic is_head_busy_{false}, is_waist_busy_{false}; + }; diff --git a/src/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 7075c9ed..d04d5e6a 100644 --- a/src/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -220,11 +220,67 @@ template void HumanoidRobot::moveJ(std::vector &cmd, double vel, double acc) { if (cmd.empty()) return; - for (auto c: cmd) { - LOG(INFO) << c.joint_name << c.rad << endl; - LOG(INFO) << vel << " "<< acc << endl; + LOG(INFO) << "vel : " << vel << "acc : " << acc << endl; + for (const auto &js: cmd) { + LOG(INFO) << js.joint_name << " , "<< js.rad << endl; } + // 获取命令中的第一个电机，判断其所属电机组 + auto &first_jp = cmd.at(0); + std::string group = ""; + + if (first_jp.joint_name.find("L_") != std::string::npos) { + group = "L_"; + } else if (first_jp.joint_name.find("R_") != std::string::npos) { + group = "R_"; + } else if (first_jp.joint_name.find("HEAD_") != std::string::npos) { + group = "HEAD_"; + } else if (first_jp.joint_name.find("WAIST_") != std::string::npos) { + group = "WAIST_"; + } else { + LOG(ERROR) << "Unknown joint group for " << first_jp.joint_name; + return; + } + + // 检查所有电机是否属于同一组 + for (auto &jp : cmd) { + if (jp.joint_name.find(group) == std::string::npos) { + LOG(ERROR) << "Mixed motor groups detected, skipping command."; + return; // 发现不属于同一组，直接返回 + } + } + + // 获取电机组的互斥锁和忙碌状态 + std::mutex* group_mutex = nullptr; + std::atomic* is_busy_flag = nullptr; + + // 根据电机组选择对应的mutex和忙碌标志 + if (group == "L_") { + group_mutex = &left_arm_mutex_; + is_busy_flag = &is_left_arm_busy_; + } else if (group == "R_") { + group_mutex = &right_arm_mutex_; + is_busy_flag = &is_right_arm_busy_; + } else if (group == "HEAD_") { + group_mutex = &head_mutex_; + is_busy_flag = &is_head_busy_; + } else if (group == "WAIST_") { + group_mutex = &waist_mutex_; + is_busy_flag = &is_waist_busy_; + } + + // 检查电机组是否已忙碌 + bool is_busy = is_busy_flag->load(); + if (is_busy) { + LOG(ERROR) << " Another thread is already controlling this motor group :" << group << ", skipping command."; + return; + } + + // 设置为忙碌 + is_busy_flag->store(true); + + // 使用 mutex 锁住当前电机组，保证只有一个线程在控制 + std::lock_guard lock(*group_mutex); // 1. 读当前关节角 std::unordered_map cur_joints_angle; @@ -265,7 +321,6 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double return; } - // 3. 采样 const double dt = 0.001; auto samples = joint_space_planner_->sampleTrajectory(traj, dt); @@ -274,9 +329,8 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double return; } - // 4. 切模式、设速度 - std::vector > motors; + std::vector> motors; motors.reserve(cmd.size()); for (auto &jp: cmd) { auto motor = motor_manager_->getMotor(jp.joint_name); @@ -291,13 +345,11 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double motors.push_back(motor); } - // 5. 下发命令 const auto t0 = std::chrono::steady_clock::now(); size_t k = 1; const size_t K = samples.size(); - while (k < K) { const auto &s = samples[k]; for (size_t i = 0; i < motors.size(); ++i) { @@ -321,9 +373,15 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double } LOG(INFO) << jp.joint_name << " pos err = " << std::abs(it->second - jp.rad); } + + // 结束后恢复状态 + is_busy_flag->store(false); } + + + template void HumanoidRobot::calibrateZeroQ(const std::string &joint_name) { auto motor = motor_manager_->getMotor(joint_name);