[feature] xml reader

document/chapter1/chapter1_quickstart.md

@@ -8,7 +8,7 @@ Aimrt_Mujoco_Sim 旨在为机器人开发者提供一个快速上手的仿真平
 ## 1.2 安装说明
 
 ### 1.2.1 环境与依赖 (dependencies)
-- 环境（参考 [AimRT 安装与引用](https://docs.aimrt.org/tutorials/quick_start/installation_cpp.html)， 以选择合适的开发环境）
+- 环境（参考 [AimRT 安装与引用](https://docs.aimrt.org/v0.10.0-rc1/tutorials/quick_start/installation_cpp.html)， 以选择合适的开发环境）
 
     - cmake 3.24+
     - gcc 11.4+
@@ -17,10 +17,10 @@ Aimrt_Mujoco_Sim 旨在为机器人开发者提供一个快速上手的仿真平
 
 - 依赖 （在构建过程均通过 cmake 自动下载）
 
-    | 依赖名称 | 版本    | 说明     | git地址                                |
-    | -------- | ------- | -------- | -------------------------------------- |
-    | AimRT    | 0.10.0+ | 通信框架 | https://github.com/AimRT/AimRT.git     |
-    | mujoco   | 3.1.6   | 物理引擎 | https://github.com/deepmind/mujoco.git |
+    | 依赖名称 | 版本        | 说明     | git地址                                |
+    | -------- | ----------- | -------- | -------------------------------------- |
+    | AimRT    | 0.10.0-rc1+ | 通信框架 | https://github.com/AimRT/AimRT.git     |
+    | mujoco   | 3.1.6       | 物理引擎 | https://github.com/deepmind/mujoco.git |
 
 
 ### 1.2.2 构建并安装 (build and install)
@@ -42,6 +42,7 @@ Aimrt_Mujoco_Sim 旨在为机器人开发者提供一个快速上手的仿真平
 ```shell
 ./start_examples_inverted_pendulum_with_pid_control.sh
 ```
+当弹出 mujoco 可视化界面并看到倒立摆模型运动时，说明运行成功。
 
 ## 1.3 项目结构
 ``` shell
@@ -61,5 +62,5 @@ AIMRT_MUJOCO_SIM/
 └── CMakeLists.txt           # 项目根目录的 CMake 配置
 ```
 
-
+[返回目录](../index.md)
 

src/module/mujoco_sim_module/common/xmodel_reader.cc

@@ -0,0 +1,129 @@
+// Copyright (c) 2023, AgiBot Inc.
+// All rights reserved.
+
+#include "mujoco_sim_module/common/xmodel_reader.h"
+#include <cstddef>
+
+namespace aimrt_mujoco_sim::mujoco_sim_module::common {
+std::optional<int32_t> GetJointSensorIdByJointName(const mjModel* m, std::string_view joint_name, mjtSensor sensor_type) {
+  int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data());
+  if (jointId < 0) {
+    AIMRT_ERROR_THROW("Invalid joint name: {}, cannot find a joint sensor.", joint_name);
+    return std::nullopt;
+  }
+
+  for (int32_t i = 0; i < m->nsensor; i++) {
+    if (m->sensor_type[i] == sensor_type && m->sensor_objid[i] == jointId) {
+      return i;
+    }
+  }
+  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) {
+  return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTPOS);
+}
+
+std::optional<int32_t> GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name) {
+  return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTACTFRC);
+}
+
+std::optional<std::string> GetJointSensorNameByJointName(const mjModel* m, std::string_view joint_name, mjtSensor sensor_type) {
+  if (auto sensor_id = GetJointSensorIdByJointName(m, joint_name, sensor_type)) {
+    return mj_id2name(m, mjOBJ_SENSOR, sensor_id.value_or(-1));
+  }
+  return std::nullopt;
+}
+
+std::optional<std::string> GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name) {
+  return GetJointSensorNameByJointName(m, joint_name, mjSENS_JOINTVEL);
+}
+
+std::optional<std::string> GetJointposNameByJointName(const mjModel* m, std::string_view joint_name) {
+  return GetJointSensorNameByJointName(m, joint_name, mjSENS_JOINTPOS);
+}
+
+std::optional<std::string> GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name) {
+  return GetJointSensorNameByJointName(m, joint_name, mjSENS_JOINTACTFRC);
+}
+
+std::optional<int32_t> GetJointActIdByJointName(const mjModel* m, std::string_view joint_name) {
+  int32_t joint_id = mj_name2id(m, mjOBJ_JOINT, joint_name.data());
+  if (joint_id < 0) {
+    AIMRT_ERROR_THROW("Invalid joint name: {}, cannot find a joint actuator.", joint_name);
+    return std::nullopt;
+  }
+
+  for (int i = 0; i < m->nu; i++) {
+    if (m->actuator_trntype[i] == mjTRN_JOINT && m->actuator_trnid[static_cast<ptrdiff_t>(i * 2)] == joint_id) {
+      return i;
+    }
+  }
+  return std::nullopt;
+}
+
+std::optional<std::string> GetJointActNameByJointName(const mjModel* m, std::string_view joint_name) {
+  if (auto actuator_id = GetJointActIdByJointName(m, joint_name.data())) {
+    return mj_id2name(m, mjOBJ_ACTUATOR, actuator_id.value_or(-1));
+  }
+  return std::nullopt;
+}
+
+std::optional<std::string> GetJointActTypeByJointName(const mjModel* m, std::string_view joint_name) {
+  if (!m || !joint_name.data()) return std::nullopt;
+  for (int32_t i = 0; i < m->nu; i++) {
+    int32_t targetId = m->actuator_trnid[static_cast<ptrdiff_t>(i * 2)];
+    if (m->actuator_trntype[i] == mjTRN_JOINT) {
+      const char* targetName = mj_id2name(m, mjOBJ_JOINT, targetId);
+      if (targetName && strcmp(targetName, joint_name.data()) == 0) {
+        // position
+        if (m->actuator_gaintype[i] == mjGAIN_FIXED &&
+            m->actuator_biastype[i] == mjBIAS_AFFINE &&
+            (m->actuator_dyntype[i] == mjDYN_NONE ||
+             m->actuator_dyntype[i] == mjDYN_FILTEREXACT)) {
+          if (m->actuator_biasprm[i * mjNBIAS + 1] == -m->actuator_gainprm[static_cast<ptrdiff_t>(i * mjNGAIN)]) {
+            return "position";
+          }
+        }
+        // velocity
+        if (m->actuator_gaintype[i] == mjGAIN_FIXED &&
+            m->actuator_biastype[i] == mjBIAS_AFFINE &&
+            m->actuator_dyntype[i] == mjDYN_NONE) {
+          if (m->actuator_biasprm[i * mjNBIAS + 2] == -m->actuator_gainprm[static_cast<ptrdiff_t>(i * mjNGAIN)]) {
+            return "velocity";
+          }
+        }
+        // motor
+        if (m->actuator_dyntype[i] == mjDYN_NONE &&
+            m->actuator_gaintype[i] == mjGAIN_FIXED &&
+            m->actuator_biastype[i] == mjBIAS_NONE) {
+          if (m->actuator_gainprm[static_cast<ptrdiff_t>(i * mjNGAIN)] == 1) {
+            return "motor";
+          }
+        }
+        // other (todo:(hj) adapt to other types of actuators)
+      }
+    }
+  }
+  // cannot find the joint name in the actuators
+  AIMRT_ERROR_THROW("The actuator type bound to joint {} is unsupported, only support position/velocity/motor.", joint_name);
+  return std::nullopt;
+}
+
+std::optional<int32_t> GetSensorIdBySensorName(const mjModel* m, std::string_view sensor_name) {
+  if (sensor_name.empty()) return std::nullopt;
+
+  int32_t sensor_id = mj_name2id(m, mjOBJ_SENSOR, sensor_name.data());
+  if (sensor_id < 0) {
+    AIMRT_ERROR_THROW("Invalid sensor name: {}.", sensor_name);
+    return std::nullopt;
+  }
+
+  return m->sensor_adr[sensor_id];
+}
+
+}  // namespace aimrt_mujoco_sim::mujoco_sim_module::common

src/module/mujoco_sim_module/common/xmodel_reader.h

@@ -0,0 +1,30 @@
+// Copyright (c) 2023, AgiBot Inc.
+// All rights reserved.
+
+#pragma once
+
+#include <cstring>
+#include "mujoco/mjmodel.h"
+#include "mujoco_sim_module/global.h"
+#include "simulate.h"
+
+namespace aimrt_mujoco_sim::mujoco_sim_module::common {
+// joint sensor
+std::optional<int32_t> GetJointSensorIdByJointName(const mjModel* m, std::string_view joint_name, mjtSensor sensor_type);
+std::optional<int32_t> GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name);
+std::optional<int32_t> GetJointposIdByJointName(const mjModel* m, std::string_view joint_name);
+std::optional<int32_t> GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name);
+
+std::optional<std::string> GetJointSensorNameByJointName(const mjModel* m, std::string_view joint_name, mjtSensor sensor_type);
+std::optional<std::string> GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name);
+std::optional<std::string> GetJointposNameByJointName(const mjModel* m, std::string_view joint_name);
+std::optional<std::string> GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name);
+
+// joint actuator
+std::optional<int32_t> GetJointActIdByJointName(const mjModel* m, std::string_view joint_name);
+std::optional<std::string> GetJointActNameByJointName(const mjModel* m, std::string_view joint_name);
+std::optional<std::string> GetJointActTypeByJointName(const mjModel* m, std::string_view joint_name);
+
+// normal sensor
+std::optional<int32_t> GetSensorIdBySensorName(const mjModel* m, std::string_view sensor_name);
+}  // namespace aimrt_mujoco_sim::mujoco_sim_module::common

src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc

@@ -2,12 +2,12 @@
 // All rights reserved.
 
 #include "mujoco_sim_module/publisher/imu_sensor_publisher.h"
+#include "mujoco_sim_module/common/xmodel_reader.h"
 
 namespace YAML {
 template <>
 struct convert<aimrt_mujoco_sim::mujoco_sim_module::publisher::ImuSensorPublisherBase::Options> {
   using Options = aimrt_mujoco_sim::mujoco_sim_module::publisher::ImuSensorPublisherBase::Options;
-
   static Node encode(const Options& rhs) {
     Node node;
 
@@ -46,9 +46,9 @@ void ImuSensorPublisherBase::SetMj(mjModel* m, mjData* d) {
 }
 
 void ImuSensorPublisherBase::RegisterSensorAddr() {
-  sensor_addr_group_.framequat_addr = GetSensorAddr(m_, options_.bind_framequat),
-  sensor_addr_group_.gyro_addr = GetSensorAddr(m_, options_.bind_gyro),
-  sensor_addr_group_.accelerometer_addr = GetSensorAddr(m_, options_.bind_accelerometer);
+  sensor_addr_group_.framequat_addr = common::GetSensorIdBySensorName(m_, options_.bind_framequat).value_or(-1),
+  sensor_addr_group_.gyro_addr = common::GetSensorIdBySensorName(m_, options_.bind_gyro).value_or(-1),
+  sensor_addr_group_.accelerometer_addr = common::GetSensorIdBySensorName(m_, options_.bind_accelerometer).value_or(-1);
 }
 
 void ImuSensorPublisherBase::CopySensorData(int addr, auto& dest, size_t n) {
@@ -67,7 +67,8 @@ void ImuSensorPublisherBase::InitializeBase(YAML::Node options_node) {
 
 void ImuSensorPublisher::Initialize(YAML::Node options_node) {
   InitializeBase(options_node);
-  AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType<aimrt::protocols::sensor::ImuState>(publisher_), "Register publish type failed.");
+  AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType<aimrt::protocols::sensor::ImuState>(publisher_),
+                          "Register publish type failed.");
 }
 
 void ImuSensorPublisher::PublishSensorData() {
@@ -123,7 +124,8 @@ void ImuSensorPublisher::PublishSensorData() {
 #ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2
 void ImuSensorRos2Publisher::Initialize(YAML::Node options_node) {
   InitializeBase(options_node);
-  AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType<sensor_msgs::msg::Imu>(publisher_), "Register publish type failed.");
+  AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType<sensor_msgs::msg::Imu>(publisher_),
+                          "Register publish type failed.");
 }
 
 void ImuSensorRos2Publisher::PublishSensorData() {

src/module/mujoco_sim_module/publisher/imu_sensor_publisher.h

@@ -2,13 +2,13 @@
 // All rights reserved.
 
 #pragma once
+#include "mujoco_sim_module/global.h"
+#include "mujoco_sim_module/publisher/publisher_base.h"
+#include "mujoco_sim_module/publisher/utils.h"
 
 #include "aimrt_module_protobuf_interface/channel/protobuf_channel.h"
 #include "aimrt_module_protobuf_interface/util/protobuf_tools.h"
 #include "imu.pb.h"
-#include "mujoco_sim_module/global.h"
-#include "mujoco_sim_module/publisher/publisher_base.h"
-#include "mujoco_sim_module/publisher/utils.h"
 
 #ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2
   #include <sensor_msgs/msg/imu.hpp>
@@ -38,11 +38,8 @@ class ImuSensorPublisherBase : public PublisherBase {
   void Shutdown() override {}
 
   void SetPublisherHandle(aimrt::channel::PublisherRef publisher_handle) override { publisher_ = publisher_handle; }
-
   void SetExecutor(aimrt::executor::ExecutorRef executor) override { executor_ = executor; };
-
   void SetFreq(uint32_t freq) override { channel_frq_ = freq; };
-
   void SetMj(mjModel* m, mjData* d) override;
 
  protected:
@@ -95,6 +92,7 @@ class ImuSensorPublisher : public ImuSensorPublisherBase {
   std::string_view Type() const noexcept override { return "imu_sensor"; }
   void PublishSensorData() override;
 };
+
 #ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2
 class ImuSensorRos2Publisher : public ImuSensorPublisherBase {
  public:

src/module/mujoco_sim_module/publisher/joint_sensor_publisher.cc

@@ -2,6 +2,7 @@
 // All rights reserved.
 
 #include "mujoco_sim_module/publisher/joint_sensor_publisher.h"
+#include "mujoco_sim_module/common/xmodel_reader.h"
 
 namespace YAML {
 template <>
@@ -16,9 +17,6 @@ struct convert<aimrt_mujoco_sim::mujoco_sim_module::publisher ::JointSensorPubli
       Node joint_node;
       joint_node["name"] = joint.name;
       joint_node["bind_joint"] = joint.bind_joint;
-      joint_node["bind_jointpos_sensor"] = joint.bind_jointpos_sensor;
-      joint_node["bind_jointvel_sensor"] = joint.bind_jointvel_sensor;
-      joint_node["bind_jointactuatorfrc_sensor"] = joint.bind_jointactuatorfrc_sensor;
       node["joints"].push_back(joint_node);
     }
 
@@ -32,16 +30,6 @@ struct convert<aimrt_mujoco_sim::mujoco_sim_module::publisher ::JointSensorPubli
         joint_node_options.name = joint_node["name"].as<std::string>();
         joint_node_options.bind_joint = joint_node["bind_joint"].as<std::string>();
 
-        if (joint_node["bind_jointpos_sensor"]) {
-          joint_node_options.bind_jointpos_sensor = joint_node["bind_jointpos_sensor"].as<std::string>();
-        }
-        if (joint_node["bind_jointvel_sensor"]) {
-          joint_node_options.bind_jointvel_sensor = joint_node["bind_jointvel_sensor"].as<std::string>();
-        }
-        if (joint_node["bind_jointactuatorfrc_sensor"]) {
-          joint_node_options.bind_jointactuatorfrc_sensor = joint_node["bind_jointactuatorfrc_sensor"].as<std::string>();
-        }
-
         rhs.joints.emplace_back(std::move(joint_node_options));
       }
     }
@@ -60,9 +48,9 @@ void JointSensorPublisherBase ::SetMj(mjModel* m, mjData* d) {
 void JointSensorPublisherBase::RegisterSensorAddr() {
   for (const auto& joint : options_.joints) {
     sensor_addr_vec_.emplace_back(SensorAddrGroup{
-        .jointpos_addr = GetSensorAddr(m_, joint.bind_jointpos_sensor),
-        .jointvel_addr = GetSensorAddr(m_, joint.bind_jointvel_sensor),
-        .jointactuatorfrc_addr = GetSensorAddr(m_, joint.bind_jointactuatorfrc_sensor)});
+        .jointpos_addr = common::GetJointposIdByJointName(m_, joint.bind_joint).value_or(-1),
+        .jointvel_addr = common::GetJointvelIdByJointName(m_, joint.bind_joint).value_or(-1),
+        .jointactuatorfrc_addr = common::GetJointactfrcIdByJointName(m_, joint.bind_joint).value_or(-1)});
 
     name_vec_.emplace_back(joint.name);
   }
@@ -179,5 +167,4 @@ void JointSensorRos2Publisher::PublishSensorData() {
   }
 }
 #endif
-
 }  // namespace aimrt_mujoco_sim::mujoco_sim_module::publisher

src/module/mujoco_sim_module/publisher/joint_sensor_publisher.h

@@ -3,13 +3,14 @@
 
 #pragma once
 
-#include "aimrt_module_protobuf_interface/channel/protobuf_channel.h"
-#include "aimrt_module_protobuf_interface/util/protobuf_tools.h"
-#include "joint_state.pb.h"
 #include "mujoco_sim_module/global.h"
 #include "mujoco_sim_module/publisher/publisher_base.h"
 #include "mujoco_sim_module/publisher/utils.h"
 
+#include "aimrt_module_protobuf_interface/channel/protobuf_channel.h"
+#include "aimrt_module_protobuf_interface/util/protobuf_tools.h"
+#include "joint_state.pb.h"
+
 #ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2
   #include "aimrt_module_ros2_interface/channel/ros2_channel.h"
   #include "sensor_ros2/msg/joint_state.hpp"
@@ -22,9 +23,6 @@ class JointSensorPublisherBase : public PublisherBase {
     struct Joint {
       std::string name;
       std::string bind_joint;
-      std::string bind_jointpos_sensor;
-      std::string bind_jointvel_sensor;
-      std::string bind_jointactuatorfrc_sensor;
     };
     std::vector<Joint> joints;
   };

src/module/mujoco_sim_module/publisher/touch_sensor_publisher.cc

@@ -2,6 +2,8 @@
 // All rights reserved.
 
 #include "mujoco_sim_module/publisher/touch_sensor_publisher.h"
+#include "mujoco_sim_module/common/xmodel_reader.h"
+
 namespace YAML {
 template <>
 struct convert<aimrt_mujoco_sim::mujoco_sim_module::publisher ::TouchSensorPublisherBase::Options> {
@@ -81,7 +83,7 @@ void TouchSensorPublisherBase::RegisterSensorAddr() {
                    options_.states[index].end(),
                    std::back_inserter(addr_vec),
                    [this](const Options::State& state) {
-                     return GetSensorAddr(m_, state.bind_touch_sensor);
+                     return common::GetSensorIdBySensorName(m_, state.bind_touch_sensor).value_or(-1);
                    });
 
     touch_sensor_num_vec_.emplace_back(addr_vec.size());