110 lines
4.1 KiB
C++
110 lines
4.1 KiB
C++
//
|
||
// Created by linbo on 2025/10/24.
|
||
//
|
||
|
||
#include "jointpositioncontroller.h"
|
||
#include "hardware/can/can_manager.h"
|
||
#include "hardware_manager/hardware_manager.h"
|
||
using namespace std;
|
||
using namespace cmvr::device;
|
||
using namespace cmvr::hardware;
|
||
|
||
JointPositionController::JointPositionController(const XmlNode& cfg):AbstractController(cfg)
|
||
{
|
||
defaultSpeed_ = cfg.getAttrDefault("defaultSpeed",0.5f);
|
||
defaultAcc_ = cfg.getAttrDefault("defaultAcc",0.5f);
|
||
}
|
||
|
||
void JointPositionController::call(const Json::Value& json)
|
||
{
|
||
try
|
||
{
|
||
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"))
|
||
{
|
||
std::vector<std::string> canGroupIds;
|
||
const Json::Value& params = json["params"];
|
||
if (params.isMember("canGroupId"))
|
||
{
|
||
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
|
||
}
|
||
}
|
||
}
|
||
}
|
||
}
|
||
state_ = ControllerState_Executing;
|
||
}
|
||
catch (const std::exception& e)
|
||
{
|
||
LOG(ERROR)<<"[JointPositionController](call):" <<e.what();
|
||
throw runtime_error(e.what());
|
||
}
|
||
|
||
}
|
||
|
||
void JointPositionController::interrupt()
|
||
{
|
||
state_ = ControllerState_Switching;
|
||
|
||
|
||
state_ = ControllerState_Idle;
|
||
}
|
||
|
||
void JointPositionController::stop()
|
||
{
|
||
state_ = ControllerState_Idle;
|
||
} |