update controller

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)
+    try
-        return;
-
-
-    //解析jason，执行算法
-    /*
-        "params":{
-            “canGroupId”:"leftArm",
-            "motors":[{"joint_name":""},{"joint_name":""}]
-        },
-        "operate":{}
-
-    **/
-    std::string canGroupId;
-    std::string protocolType;
-    if (json.isMember("params"))
     {
-        const Json::Value& params = json["params"];
+        if (state_ != ControllerState_Idle)
-        if (params.isMember("canGroupId"))
+            return;
-            canGroupId = params["canGroupId"].asString();
+
-        if (params.isMember("motors"))
+
+        //解析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<std::string> 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):" <<e.what();
+        throw runtime_error(e.what());
     }
-    //最后计算好了通过下面的接口调用电机通信
-    std::string joint_name ="";
-    auto motorInfo = HardWareManager::getInstance().getCanGroup(canGroupId)->getMotorInfo(joint_name);
-    motorInfo.motorProtocol->setQ(motorInfo.node_id,0.1);
 
 }
 

src/devices/robot/humanoid_robot/humanoid_robot.cpp

@@ -316,7 +316,7 @@ void HumanoidRobot<DOF>::moveJ(std::vector<JointPoint> &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<DOF>::moveJ(std::vector<JointPoint> &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<DOF>::moveJ(std::vector<JointPoint> &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());
     }

src/hardware/can/motor_protocol/motorprotocolmanager.cpp

@@ -51,7 +51,7 @@ std::shared_ptr<AbstractMotorProtocol> 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);
 }