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

add grpc servoJ function

Browse Source
This commit is contained in:
linbo 2026-03-04 17:07:05 +08:00
parent 0fc5e271e8
commit 17043684fe
5 changed files with 50 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp
View File

@ -1379,8 +1379,9 @@ void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
} }
motor->setQd(j.vel); motor->setTarget(j.rad,j.vel);
motor->setQ(j.rad); // motor->setQd(j.vel);
// motor->setQ(j.rad);
} }
} }
rsm_.store(ROBOT_READY); rsm_.store(ROBOT_READY);
@ -1394,8 +1395,9 @@ void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double vel, dou
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) { if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION); motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
} }
motor->setQd(vel); motor->setTarget(j.rad,vel);
motor->setQ(j.rad); // motor->setQd(vel);
// motor->setQ(j.rad);
} }
} }
} }

3
cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
View File

@ -31,6 +31,9 @@ namespace cmvr {
grpc::Status getPose(grpc::ServerContext *context, const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) override; grpc::Status getPose(grpc::ServerContext *context, const cmvr::api::GetPose_Request *request, cmvr::api::GetPose_Response *response) override;
grpc::Status calibrateZeroQ(grpc::ServerContext* context, const cmvr::api::CalibrateZeroQ_Request* request, cmvr::api::CalibrateZeroQ_Response* response) override; grpc::Status calibrateZeroQ(grpc::ServerContext* context, const cmvr::api::CalibrateZeroQ_Request* request, cmvr::api::CalibrateZeroQ_Response* response) override;
grpc::Status servoJ(grpc::ServerContext* context, const cmvr::api::ServoJ_Request* request, cmvr::api::ServoJ_Response* response) override;
private: private:
device::DeviceManager& dmgr_; device::DeviceManager& dmgr_;
}; };

26
cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
View File

@ -244,3 +244,29 @@ grpc::Status gRPCHumanoidRobotServiceImpl::calibrateZeroQ(grpc::ServerContext* c
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret; return ret;
} }
grpc::Status gRPCHumanoidRobotServiceImpl::servoJ(grpc::ServerContext* context
, const cmvr::api::ServoJ_Request* request
, cmvr::api::ServoJ_Response* response)
{
grpc::Status ret = grpc::Status::OK;
try {
auto robot = dmgr_.getDevice<AbstractRobot>(request->header().device_id());
std::vector<JointPoint> cmds{};
for (const auto& jc : request->cmds()) {
cmds.emplace_back(jc.joint_name(),jc.rad(),jc.vel());
}
robot->servoJ(cmds,request->vel(),0);
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
}catch (const std::exception& e) {
response->mutable_header()->set_success(false);
response->mutable_header()->set_error_message(e.what());
ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what());
}
*response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
return ret;
}

14
protos/cmvr/api/humanoid_robot_command.proto
View File

@ -144,3 +144,17 @@ message CalibrateZeroQ {
CommandHeader.Feedback header= 1; CommandHeader.Feedback header= 1;
} }
} }
message ServoJ{
message Request{
CommandHeader.Request header = 1;
repeated JointCmd cmds = 2;
double vel = 3;
}
message Response{
CommandHeader.Feedback header= 1;
}
}

1
protos/cmvr/api/humanoid_robot_service.proto
View File

@ -15,4 +15,5 @@ service HumanoidRobotService{
rpc getJointState(JointRequest) returns (JointResponse); rpc getJointState(JointRequest) returns (JointResponse);
rpc getPose(GetPose.Request) returns (GetPose.Response); rpc getPose(GetPose.Request) returns (GetPose.Response);
rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response); rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response);
rpc servoJ(ServoJ.Request) returns (ServoJ.Response);
} }