From 1f9fdc40594d6bf73b30f393cbd19f2773db1434 Mon Sep 17 00:00:00 2001
From: hanjun <hanjun@agibot.com>
Date: Mon, 13 Oct 2025 16:00:54 +0800
Subject: [PATCH] update aimrt and fix joint sensor reader

---
 cmake/GetAimRT.cmake                          |  2 +-
 .../mujoco_sim_module/common/xmodel_reader.cc | 22 +++++++--
 .../subscriber/joint_actuator_subscriber.cc   |  4 +-
 src/protocols/CMakeLists.txt                  |  1 -
 src/protocols/actuator/CMakeLists.txt         | 40 ----------------
 src/protocols/actuator/joint_command.proto    | 47 -------------------
 6 files changed, 20 insertions(+), 96 deletions(-)
 delete mode 100644 src/protocols/actuator/CMakeLists.txt
 delete mode 100644 src/protocols/actuator/joint_command.proto

diff --git a/cmake/GetAimRT.cmake b/cmake/GetAimRT.cmake
index e0070bd..28adaee 100644
--- a/cmake/GetAimRT.cmake
+++ b/cmake/GetAimRT.cmake
@@ -6,7 +6,7 @@ include(FetchContent)
 message(STATUS "get aimrt ...")
 
 set(aimrt_DOWNLOAD_URL
-    "https://github.com/AimRT/AimRT/archive/refs/tags/v0.10.0.tar.gz"
+    "https://github.com/AimRT/AimRT/archive/refs/tags/v1.2.0.tar.gz"
     CACHE STRING "")
 
 if(aimrt_LOCAL_SOURCE)
diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.cc b/src/module/mujoco_sim_module/common/xmodel_reader.cc
index 5dab297..fc81d3a 100644
--- a/src/module/mujoco_sim_module/common/xmodel_reader.cc
+++ b/src/module/mujoco_sim_module/common/xmodel_reader.cc
@@ -20,16 +20,28 @@ std::optional<int32_t> GetJointSensorIdByJointName(const mjModel* m, std::string
   return std::nullopt;
 }
 
-std::optional<int32_t> GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name) {
-  return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTVEL);
+std::optional<int32_t> GetJointposIdByJointName(const mjModel* m, std::string_view joint_name) {
+  auto sensor_id = GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTPOS);
+  if (!sensor_id.has_value()) {
+    return std::nullopt;
+  }
+  return m->sensor_adr[sensor_id.value()];
 }
 
-std::optional<int32_t> GetJointposIdByJointName(const mjModel* m, std::string_view joint_name) {
-  return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTPOS);
+std::optional<int32_t> GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name) {
+  auto sensor_id = GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTVEL);
+  if (!sensor_id.has_value()) {
+    return std::nullopt;
+  }
+  return m->sensor_adr[sensor_id.value()];
 }
 
 std::optional<int32_t> GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name) {
-  return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTACTFRC);
+  auto sensor_id = GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTACTFRC);
+  if (!sensor_id.has_value()) {
+    return std::nullopt;
+  }
+  return m->sensor_adr[sensor_id.value()];
 }
 
 std::optional<std::string> GetJointSensorNameByJointName(const mjModel* m, std::string_view joint_name, mjtSensor sensor_type) {
diff --git a/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc b/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc
index 1bb96f3..a3f8ca7 100644
--- a/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc
+++ b/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc
@@ -113,8 +113,8 @@ void JointActuatorSubscriber::EventHandle(const std::shared_ptr<const aimrt::pro
 
     auto itr = std::ranges::find(joint_names_vec_, command.name());
     if (itr == joint_names_vec_.end()) [[unlikely]] {
-      AIMRT_WARN("Invalid msg for topic '{}', msg: {}",
-                 subscriber_.GetTopic(), aimrt::Pb2CompactJson(*commands));
+      AIMRT_WARN("Invalid msg for topic '{}', msg: {}, maybe the joint name : {} is not matched.",
+                 subscriber_.GetTopic(), aimrt::Pb2CompactJson(*commands), command.name());
 
       delete[] new_command_array;
       return;
diff --git a/src/protocols/CMakeLists.txt b/src/protocols/CMakeLists.txt
index 3367ff4..cfac049 100644
--- a/src/protocols/CMakeLists.txt
+++ b/src/protocols/CMakeLists.txt
@@ -3,7 +3,6 @@
 
 set_namespace()
 
-add_subdirectory(actuator)
 
 if(AIMRT_MUJOCO_SIM_BUILD_EXAMPLES)
   add_subdirectory(examples/inverted_pendulum)
diff --git a/src/protocols/actuator/CMakeLists.txt b/src/protocols/actuator/CMakeLists.txt
deleted file mode 100644
index 55db621..0000000
--- a/src/protocols/actuator/CMakeLists.txt
+++ /dev/null
@@ -1,40 +0,0 @@
-# Copyright (c) 2023, AgiBot Inc.
-# All rights reserved.
-
-# Get the current folder name
-string(REGEX REPLACE ".*/\(.*\)" "\\1" CUR_DIR ${CMAKE_CURRENT_SOURCE_DIR})
-
-# Get namespace
-get_namespace(CUR_SUPERIOR_NAMESPACE)
-
-set(CUR_SUPERIOR_NAMESPACE "aimrt::protocols")
-string(REPLACE "::" "_" CUR_SUPERIOR_NAMESPACE_UNDERLINE ${CUR_SUPERIOR_NAMESPACE})
-
-# Set target name
-set(CUR_TARGET_NAME ${CUR_SUPERIOR_NAMESPACE_UNDERLINE}_${CUR_DIR})
-set(CUR_TARGET_ALIAS_NAME ${CUR_SUPERIOR_NAMESPACE}::${CUR_DIR})
-
-# Add target
-add_protobuf_gencode_target_for_proto_path(
-  TARGET_NAME ${CUR_TARGET_NAME}_pb_gencode
-  PROTO_PATH ${CMAKE_CURRENT_SOURCE_DIR}
-  GENCODE_PATH ${CMAKE_CURRENT_BINARY_DIR}
-  DEP_PROTO_TARGETS aimrt::protocols::common_pb_gencode)
-add_library(${CUR_TARGET_ALIAS_NAME}_pb_gencode ALIAS ${CUR_TARGET_NAME}_pb_gencode)
-
-# Set installation
-if(AIMRT_INSTALL)
-  install(
-    DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}
-    DESTINATION "share"
-    FILES_MATCHING
-    PATTERN "*.proto")
-
-  set_property(TARGET ${CUR_TARGET_NAME}_pb_gencode PROPERTY EXPORT_NAME ${CUR_TARGET_ALIAS_NAME}_pb_gencode)
-  install(
-    TARGETS ${CUR_TARGET_NAME}_pb_gencode
-    EXPORT ${INSTALL_CONFIG_NAME}
-    ARCHIVE DESTINATION lib
-            FILE_SET HEADERS
-            DESTINATION include/${CUR_TARGET_NAME}_pb_gencode)
-endif()
diff --git a/src/protocols/actuator/joint_command.proto b/src/protocols/actuator/joint_command.proto
deleted file mode 100644
index b03735e..0000000
--- a/src/protocols/actuator/joint_command.proto
+++ /dev/null
@@ -1,47 +0,0 @@
-// Copyright (c) 2023, AgiBot Inc.
-// All rights reserved.
-
-syntax = "proto3";
-
-import "header.proto";
-
-package aimrt.protocols.sensor;
-
-/**
- * @brief Represents the command to control of a single joint with stiffness and damping values..
- */
-message JointCommand {
-  // The name of the joint.
-  string name = 1;
-
-  // The desired position of the joint
-  // (e.g., angle in radians or distance in meters).
-  double position = 2;
-
-  // The desired velocity of the joint
-  // (e.g., angular velocity in rad/s or linear velocity in m/s).
-  double velocity = 3;
-
-  // The desired effort applied to the joint
-  // (e.g., torque in Nm or force in N).
-  double effort = 4;
-
-  // The stiffness (or spring constant) associated with the joint's controller.
-  // Also known as the proportional gain (Kp) in a PD controller.
-  double stiffness = 5;
-
-  // The damping coefficient associated with the joint's controller.
-  // Also known as the derivative gain (Kd) in a PD controller.
-  double damping = 6;
-}
-
-/**
- * @brief Represents the command of multiple joints in a robotic system at a given time.
- */
-message JointCommandArray {
-  // Common header containing timestamp and other metadata.
-  aimrt.protocols.common.Header header = 1;
-
-  // A list of JointCommand messages, each representing the desired command of a specific joint.
-  repeated JointCommand joints = 2;
-}
\ No newline at end of file