From a76a92aa9b15b627dd60e15b6f8e4e49c2e8ccca Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Fri, 7 Nov 2025 11:28:18 +0800 Subject: [PATCH] update controller --- .../controller/jointpositioncontroller.cpp | 97 +++++++++++++------ .../robot/humanoid_robot/humanoid_robot.cpp | 53 +++------- .../motor_protocol/motorprotocolmanager.cpp | 6 +- 3 files changed, 86 insertions(+), 70 deletions(-) diff --git a/src/devices/robot/controller/jointpositioncontroller.cpp b/src/devices/robot/controller/jointpositioncontroller.cpp index 444a236d..c186748c 100644 --- a/src/devices/robot/controller/jointpositioncontroller.cpp +++ b/src/devices/robot/controller/jointpositioncontroller.cpp @@ -16,43 +16,82 @@ JointPositionController::JointPositionController(const XmlNode& cfg):AbstractCon void JointPositionController::call(const Json::Value& json) { - if (state_ != ControllerState_Idle) - return; - - - //解析jason，执行算法 - /* - "params":{ - “canGroupId”:"leftArm", - "motors":[{"joint_name":""},{"joint_name":""}] - }, - "operate":{} - - **/ - std::string canGroupId; - std::string protocolType; - if (json.isMember("params")) + try { - const Json::Value& params = json["params"]; - if (params.isMember("canGroupId")) - canGroupId = params["canGroupId"].asString(); - if (params.isMember("motors")) + if (state_ != ControllerState_Idle) + return; + + + //解析json + /* + * 直接设置位置 + "params":{ + "canGroupId":["leftArm","rightArm"], + "motors":[{"joint_name":"","position":0.01,"velocity"：0.8}] + }, + **/ + std::string protocolType; + if (json.isMember("params")) { - for (int i = 0; i < params["motors"].size(); i++) + std::vector canGroupIds; + const Json::Value& params = json["params"]; + if (params.isMember("canGroupId")) { - std::string joint_name = params["motors"][i]["joint_name"].asString(); + for (const auto & canGroupId : params["canGroupId"]) + { + canGroupIds.emplace_back(canGroupId.asString()); + } + } + //先把所有电机信息都找到 + if (params.isMember("motors")) + { + for (int i = 0; i < params["motors"].size(); i++) + { + const Json::Value& motorJson = params["motors"][i]; + std::string joint_name = motorJson["joint_name"].asString(); + double pos = motorJson["position"].asDouble(); + double vel = motorJson["velocity"].asDouble(); + for (const auto& canGroupId : canGroupIds) + { + try + { + auto canGroup = HardWareManager::getInstance().getCanGroup(canGroupId); + auto motorInfo = canGroup->getMotorInfo(joint_name); + motorInfo.motorProtocol->setMode(motorInfo.node_id, msgs::RUN_MODE_PROFILE_POSITION); + motorInfo.motorProtocol->setQd(motorInfo.node_id, vel); + //考虑限位 + if (pos > motorInfo.limitQUb) + { + pos = motorInfo.limitQUb; + LOG(WARNING) << "JointPositionController[call]: Joint [" << joint_name + << "] target position exceeds upper limit! Current target: " << pos + << ", upper limit: " << motorInfo.limitQUb; + } + else if (pos < motorInfo.limitQLb) // Assume lower limit field exists (limitQLb) + { + pos = motorInfo.limitQLb; + LOG(WARNING) << "JointPositionController[call]: Joint [" << joint_name + << "] target position exceeds lower limit! Current target: " << pos + << ", lower limit: " << motorInfo.limitQLb; + } + motorInfo.motorProtocol->setQ(motorInfo.node_id, pos); + break; + } + catch (const std::exception& e) + { + continue; // 捕获异常，继续尝试下一个 canGroup + } + } + } } } - } - if (json.isMember("operate")) + } + catch (const std::exception& e) { - + LOG(ERROR)<<"[JointPositionController](call):" <getMotorInfo(joint_name); - motorInfo.motorProtocol->setQ(motorInfo.node_id,0.1); } diff --git a/src/devices/robot/humanoid_robot/humanoid_robot.cpp b/src/devices/robot/humanoid_robot/humanoid_robot.cpp index d5c6384e..d3a47110 100644 --- a/src/devices/robot/humanoid_robot/humanoid_robot.cpp +++ b/src/devices/robot/humanoid_robot/humanoid_robot.cpp @@ -316,7 +316,7 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double || state == ControlManagerState_EStop || state == ControlManagerState_MajorFault) { - throw runtime_error("Controller state error"); + throw runtime_error("Controller state error, current state:" + std::to_string(state)); } else if (state == ControlManagerState_Command) { @@ -326,7 +326,9 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double { activeController->stop(); } + //ComponentGroup的id要作为参数传入才能保证获取到对应的实例对象 auto group = controller_manager_->getComponentGroup("leftArm"); + //控住器实例由接口控制，比如当前MoveJ调用JointPositionCtrl去实现功能 if (!group.controllers_.count("JointPositionCtrl")) throw runtime_error("Controller not found"); auto controller = group.controllers_["JointPositionCtrl"]; @@ -334,48 +336,23 @@ void HumanoidRobot::moveJ(std::vector &cmd, double vel, double Json::Value callJson; Json::Value paramsJson; - Json::Value operateJson; + + //传入多个canGroupId + for (int i = 0; i < group.CanGroupIDs.size(); i++) + { + paramsJson["canGroupId"][i] = group.CanGroupIDs[i]; + } + for (int i = 0; i < cmd.size(); i++) + { + paramsJson["motors"][i]["joint_name"] = cmd[i].joint_name; + paramsJson["motors"][i]["position"] = cmd[i].rad; + paramsJson["motors"][i]["velocity"] = cmd[i].vel; + } //拆分cmd，构建json内容 - paramsJson["canGroupId"] = group.CanGroupIDs[0]; callJson["params"] = paramsJson; - callJson["operate"] = operateJson; controller->call(callJson); } - - 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()); } diff --git a/src/hardware/can/motor_protocol/motorprotocolmanager.cpp b/src/hardware/can/motor_protocol/motorprotocolmanager.cpp index db005d90..6972025a 100644 --- a/src/hardware/can/motor_protocol/motorprotocolmanager.cpp +++ b/src/hardware/can/motor_protocol/motorprotocolmanager.cpp @@ -51,7 +51,7 @@ std::shared_ptr MotorProtocolManager::getMotorProtocol(co { if (motor_protocols_.count(protocolType)) return motor_protocols_[protocolType]; - return nullptr; + throw runtime_error("Can't find motor protocol: " + protocolType); } uint8_t MotorProtocolManager::getNodeId(const std::string& joint_name) @@ -69,7 +69,7 @@ uint8_t MotorProtocolManager::getNodeId(const std::string& joint_name) } } } - return node_id; + throw runtime_error("Can't find motor nodeId: " + joint_name); } const MotorProtocolManager::MotorInfo& MotorProtocolManager::getMotorInfo(const std::string& joint_name) @@ -86,5 +86,5 @@ const MotorProtocolManager::MotorInfo& MotorProtocolManager::getMotorInfo(const } } - return MotorInfo(); + throw runtime_error("Can't find motor info: " + joint_name); } \ No newline at end of file