From 17043684fe2f5e3e228cfa00cc938af37cc360f6 Mon Sep 17 00:00:00 2001
From: linbo <1034003879@qq.com>
Date: Wed, 4 Mar 2026 17:07:05 +0800
Subject: [PATCH] add grpc servoJ function

---
 .../humanoid_robot/src/humanoid_robot.cpp     | 10 ++++---
 .../include/grpc_humanoid_robot_service.h     |  3 +++
 .../grpc/src/grpc_humanoid_robot_service.cpp  | 26 +++++++++++++++++++
 protos/cmvr/api/humanoid_robot_command.proto  | 14 ++++++++++
 protos/cmvr/api/humanoid_robot_service.proto  |  1 +
 5 files changed, 50 insertions(+), 4 deletions(-)

diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp
index e745d671..081f4702 100644
--- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp
+++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp
@@ -1379,8 +1379,9 @@ void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
             if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
                 motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
             }
-            motor->setQd(j.vel);
-            motor->setQ(j.rad);
+            motor->setTarget(j.rad,j.vel);
+            // motor->setQd(j.vel);
+            // motor->setQ(j.rad);
         }
     }
     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) {
                 motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
             }
-            motor->setQd(vel);
-            motor->setQ(j.rad);
+            motor->setTarget(j.rad,vel);
+            // motor->setQd(vel);
+            // motor->setQ(j.rad);
         }
     }
 }
diff --git a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
index a8a404ea..2b631e57 100644
--- a/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
+++ b/cmvr-es/service/grpc/include/grpc_humanoid_robot_service.h
@@ -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 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:
             device::DeviceManager& dmgr_;
         };
diff --git a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
index a87180b5..4be0d3b9 100644
--- a/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
+++ b/cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
@@ -244,3 +244,29 @@ grpc::Status gRPCHumanoidRobotServiceImpl::calibrateZeroQ(grpc::ServerContext* c
     *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime();
     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;
+
+}
diff --git a/protos/cmvr/api/humanoid_robot_command.proto b/protos/cmvr/api/humanoid_robot_command.proto
index f39ad778..13273a10 100644
--- a/protos/cmvr/api/humanoid_robot_command.proto
+++ b/protos/cmvr/api/humanoid_robot_command.proto
@@ -144,3 +144,17 @@ message CalibrateZeroQ {
         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;
+  }
+
+}
\ No newline at end of file
diff --git a/protos/cmvr/api/humanoid_robot_service.proto b/protos/cmvr/api/humanoid_robot_service.proto
index 33502e50..a4d52f50 100644
--- a/protos/cmvr/api/humanoid_robot_service.proto
+++ b/protos/cmvr/api/humanoid_robot_service.proto
@@ -15,4 +15,5 @@ service HumanoidRobotService{
   rpc getJointState(JointRequest) returns (JointResponse);
   rpc getPose(GetPose.Request) returns (GetPose.Response);
   rpc calibrateZeroQ(CalibrateZeroQ.Request) returns (CalibrateZeroQ.Response);
+  rpc servoJ(ServoJ.Request) returns (ServoJ.Response);
 }