SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update controller

Browse Source
This commit is contained in:
linbo 2025-11-07 11:28:18 +08:00
parent 2065108bc2
commit a76a92aa9b
3 changed files with 86 additions and 70 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

97
src/devices/robot/controller/jointpositioncontroller.cpp
View File

@ -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<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);
}

53
src/devices/robot/humanoid_robot/humanoid_robot.cpp
View File

@ -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());
}

6
src/hardware/can/motor_protocol/motorprotocolmanager.cpp
View File

@ -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);
}