From 1cc7a2b1473a8fcb7dbf3eea2e906c4c665c6382 Mon Sep 17 00:00:00 2001 From: hanjun Date: Fri, 14 Mar 2025 09:39:38 +0800 Subject: [PATCH 01/14] fix --- .../publisher/imu_sensor_publisher.cc | 7 ++++--- .../mujoco_sim_module/publisher/imu_sensor_publisher.h | 10 ++++------ .../publisher/joint_sensor_publisher.cc | 1 - .../publisher/joint_sensor_publisher.h | 7 ++++--- .../publisher/touch_sensor_publisher.h | 7 ++++--- .../subscriber/joint_actuator_subscriber.cc | 1 - .../subscriber/joint_actuator_subscriber.h | 2 -- 7 files changed, 16 insertions(+), 19 deletions(-) diff --git a/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc b/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc index e1a1906..f083114 100644 --- a/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc +++ b/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc @@ -7,7 +7,6 @@ namespace YAML { template <> struct convert { using Options = aimrt_mujoco_sim::mujoco_sim_module::publisher::ImuSensorPublisherBase::Options; - static Node encode(const Options& rhs) { Node node; @@ -67,7 +66,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(publisher_), "Register publish type failed."); + AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType(publisher_), + "Register publish type failed."); } void ImuSensorPublisher::PublishSensorData() { @@ -123,7 +123,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(publisher_), "Register publish type failed."); + AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType(publisher_), + "Register publish type failed."); } void ImuSensorRos2Publisher::PublishSensorData() { diff --git a/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.h b/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.h index 87cf604..18401e3 100644 --- a/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.h +++ b/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 @@ -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: diff --git a/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.cc b/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.cc index 9e8460d..7c94a15 100644 --- a/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.cc +++ b/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.cc @@ -179,5 +179,4 @@ void JointSensorRos2Publisher::PublishSensorData() { } } #endif - } // namespace aimrt_mujoco_sim::mujoco_sim_module::publisher \ No newline at end of file diff --git a/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.h b/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.h index f8aaf2c..8b9e308 100644 --- a/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.h +++ b/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" diff --git a/src/module/mujoco_sim_module/publisher/touch_sensor_publisher.h b/src/module/mujoco_sim_module/publisher/touch_sensor_publisher.h index 557da7e..0cd11cb 100644 --- a/src/module/mujoco_sim_module/publisher/touch_sensor_publisher.h +++ b/src/module/mujoco_sim_module/publisher/touch_sensor_publisher.h @@ -2,13 +2,14 @@ // All rights reserved. #pragma once -#include "aimrt_module_protobuf_interface/channel/protobuf_channel.h" -#include "aimrt_module_protobuf_interface/util/protobuf_tools.h" -#include "mujoco_sim_module/global.h" #include "mujoco_sim_module/publisher/publisher_base.h" #include "mujoco_sim_module/publisher/utils.h" #include "touch_sensor_state.pb.h" +#include "aimrt_module_protobuf_interface/channel/protobuf_channel.h" +#include "aimrt_module_protobuf_interface/util/protobuf_tools.h" +#include "mujoco_sim_module/global.h" + #ifdef AIMRT_MUJOCO_SIM_BUILD_WITH_ROS2 #include "aimrt_module_ros2_interface/channel/ros2_channel.h" #include "sensor_ros2/msg/touch_sensor_state.hpp" 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 aabeb39..0b48022 100644 --- a/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc +++ b/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc @@ -140,7 +140,6 @@ void JointActuatorSubscriber::EventHandle(const std::shared_ptr - #include "mujoco_sim_module/global.h" #include "mujoco_sim_module/subscriber/subscriber_base.h" From ca5e4053b459ecdad2a97dacebc6dc49b6f0ce07 Mon Sep 17 00:00:00 2001 From: hanjun Date: Fri, 14 Mar 2025 09:49:04 +0800 Subject: [PATCH 02/14] test --- document/chapter1/chapter1_quickstart.md | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/document/chapter1/chapter1_quickstart.md b/document/chapter1/chapter1_quickstart.md index b0574b8..18f21f6 100644 --- a/document/chapter1/chapter1_quickstart.md +++ b/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 From 4b8fa1b95415b68a44a999a61643a25385134a3e Mon Sep 17 00:00:00 2001 From: hanjun Date: Fri, 14 Mar 2025 09:52:37 +0800 Subject: [PATCH 03/14] test --- document/chapter1/chapter1_quickstart.md | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/document/chapter1/chapter1_quickstart.md b/document/chapter1/chapter1_quickstart.md index 18f21f6..a65f83c 100644 --- a/document/chapter1/chapter1_quickstart.md +++ b/document/chapter1/chapter1_quickstart.md @@ -62,5 +62,14 @@ AIMRT_MUJOCO_SIM/ └── CMakeLists.txt # 项目根目录的 CMake 配置 ``` - +🚀 返回主目录 From cf83a0c01d650a61363c039bb88036246bf2610a Mon Sep 17 00:00:00 2001 From: hanjun Date: Fri, 14 Mar 2025 09:56:35 +0800 Subject: [PATCH 04/14] test --- document/chapter1/chapter1_quickstart.md | 11 +---------- 1 file changed, 1 insertion(+), 10 deletions(-) diff --git a/document/chapter1/chapter1_quickstart.md b/document/chapter1/chapter1_quickstart.md index a65f83c..d6d9038 100644 --- a/document/chapter1/chapter1_quickstart.md +++ b/document/chapter1/chapter1_quickstart.md @@ -62,14 +62,5 @@ AIMRT_MUJOCO_SIM/ └── CMakeLists.txt # 项目根目录的 CMake 配置 ``` -🚀 返回主目录 +[返回目录](../index.md) From 107110a80d529c8f0dc507f53d832227da9a0170 Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 10:43:53 +0800 Subject: [PATCH 05/14] add xmodel_reader --- .../mujoco_sim_module/common/xmodel_reader.cc | 140 ++++++++++++++++++ .../mujoco_sim_module/common/xmodel_reader.h | 27 ++++ 2 files changed, 167 insertions(+) create mode 100644 src/module/mujoco_sim_module/common/xmodel_reader.cc create mode 100644 src/module/mujoco_sim_module/common/xmodel_reader.h diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.cc b/src/module/mujoco_sim_module/common/xmodel_reader.cc new file mode 100644 index 0000000..fdecb6c --- /dev/null +++ b/src/module/mujoco_sim_module/common/xmodel_reader.cc @@ -0,0 +1,140 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#include "mujoco_sim_module/common/xmodel_reader.h" + +namespace aimrt_mujoco_sim::mujoco_sim_module::common { + +std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name) { + int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + if (jointId < 0) return std::nullopt; + + for (int32_t i = 0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_JOINTVEL && m->sensor_objid[i] == jointId) { + return mj_id2name(m, mjOBJ_SENSOR, i); + } + } + return std::nullopt; +} + +std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name) { + int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + if (jointId < 0) return std::nullopt; + + for (int32_t i = 0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_JOINTPOS && m->sensor_objid[i] == jointId) { + return mj_id2name(m, mjOBJ_SENSOR, i); + } + } + return std::nullopt; +} + +std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name) { + int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + if (jointId < 0) return std::nullopt; + + for (int32_t i = 0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_JOINTACTFRC && m->sensor_objid[i] == jointId) { + return mj_id2name(m, mjOBJ_SENSOR, i); + } + } + return std::nullopt; +} + +std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name) { + int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + if (jointId < 0) return std::nullopt; + + for (int32_t i = 0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_JOINTVEL && m->sensor_objid[i] == jointId) { + return i; + } + } + return std::nullopt; +} + +std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name) { + int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + if (jointId < 0) return std::nullopt; + + for (int32_t i = 0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_JOINTPOS && m->sensor_objid[i] == jointId) { + return i; + } + } + return std::nullopt; +} + +std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name) { + int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + if (jointId < 0) return std::nullopt; + + for (int32_t i = 0; i < m->nsensor; i++) { + if (m->sensor_type[i] == mjSENS_JOINTACTFRC && m->sensor_objid[i] == jointId) { + return i; + } + } + return std::nullopt; +} + +std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name) { + int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + if (jointId < 0) return std::nullopt; + + for (int i = 0; i < m->nu; i++) { + if (m->actuator_trntype[i] == mjTRN_JOINT && m->actuator_trnid[static_cast(i * 2)] == jointId) { + return i; + } + } + return std::nullopt; +} + +std::optional GetJointActNameByJointName(const mjModel* m, std::string_view joint_name) { + int32_t actuatorId = GetJointActIdByJointName(m, joint_name.data()).value_or(-1); + if (actuatorId < 0) return std::nullopt; + + return mj_id2name(m, mjOBJ_ACTUATOR, actuatorId); +} + +std::optional 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(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(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(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[i * mjNGAIN] == 1) { + return "motor"; + } + } + // other (todo:(hj) adapt to other types of actuators) + return std::nullopt; + } + } + } + // cannot find the joint name in the actuators + return std::nullopt; +} + +} // namespace aimrt_mujoco_sim::mujoco_sim_module::common \ No newline at end of file diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.h b/src/module/mujoco_sim_module/common/xmodel_reader.h new file mode 100644 index 0000000..5723dbf --- /dev/null +++ b/src/module/mujoco_sim_module/common/xmodel_reader.h @@ -0,0 +1,27 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#pragma once + +#include +#include "mujoco/mjmodel.h" +#include "simulate.h" + +namespace aimrt_mujoco_sim::mujoco_sim_module::common { +// joint sensor +std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name); + +std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name); + +// joint actuator +std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointActNameByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointActTypeByJointName(const mjModel* m, std::string_view joint_name); + +// normal sensor + +} // namespace aimrt_mujoco_sim::mujoco_sim_module::common From f6a894302d5cf2397c4ada3aa2b452f9f3237cde Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 11:02:54 +0800 Subject: [PATCH 06/14] joint actuator --- .../subscriber/joint_actuator_subscriber.cc | 30 ++++++++----------- .../subscriber/joint_actuator_subscriber.h | 3 +- 2 files changed, 13 insertions(+), 20 deletions(-) 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 0b48022..d3a7180 100644 --- a/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc +++ b/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc @@ -3,6 +3,9 @@ #include "mujoco_sim_module/subscriber/joint_actuator_subscriber.h" +#include +#include "mujoco_sim_module/common/xmodel_reader.h" + namespace YAML { template <> struct convert { @@ -15,8 +18,6 @@ struct convert(), - .bind_joint = joint_node["bind_joint"].as(), - .bind_actuator_type = joint_node["bind_actuator_type"].as(), - .bind_actuator_name = joint_node["bind_actuator_name"].as()}; - - AIMRT_ASSERT(joint_node_options.bind_actuator_type == "position" || - joint_node_options.bind_actuator_type == "velocity" || - joint_node_options.bind_actuator_type == "motor", - "Invalid bind_actuator_type '{}'.", joint_node_options.bind_actuator_type); + .bind_joint = joint_node["bind_joint"].as()}; rhs.joints.emplace_back(std::move(joint_node_options)); } @@ -64,15 +58,15 @@ void JointActuatorSubscriberBase::ApplyCtrlData() { } void JointActuatorSubscriberBase::RegisterActuatorAddr() { - uint32_t idx = 0; for (auto const& joint : options_.joints) { - uint32_t actuator_id = mj_name2id(m_, mjOBJ_ACTUATOR, joint.bind_actuator_name.c_str()); - AIMRT_CHECK_ERROR_THROW(actuator_id >= 0, "Invalid bind_actuator_name '{}'.", joint.bind_actuator_name); + int32_t actuator_id = common::GetJointActIdByJointName(m_, joint.bind_joint).value_or(-1); + AIMRT_CHECK_ERROR_THROW(actuator_id >= 0, "Invalid bind_actuator_name '{}'.", common::GetJointActNameByJointName(m_, joint.bind_joint).value_or("")); actuator_addr_vec_.emplace_back(actuator_id); joint_names_vec_.emplace_back(joint.name); + joint_actuator_type_vec_.emplace_back(common::GetJointActTypeByJointName(m_, joint.bind_joint).value_or("")); - auto joint_id = m_->actuator_trnid[actuator_id * 2]; + auto joint_id = m_->actuator_trnid[static_cast(actuator_id * 2)]; actuator_bind_joint_sensor_addr_vec_.emplace_back(ActuatorBindJointSensorAddr{ .pos_addr = m_->jnt_qposadr[joint_id], .vel_addr = m_->jnt_dofadr[joint_id], @@ -120,9 +114,9 @@ void JointActuatorSubscriber::EventHandle(const std::shared_ptr joints; }; @@ -64,6 +62,7 @@ class JointActuatorSubscriberBase : public SubscriberBase { std::vector actuator_addr_vec_; std::atomic command_array_{nullptr}; std::vector joint_names_vec_; + std::vector joint_actuator_type_vec_; std::vector actuator_bind_joint_sensor_addr_vec_; }; From 57aaa29734356e46d486fb9d0324820bdb182ec8 Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 11:17:47 +0800 Subject: [PATCH 07/14] joint sensor --- .../publisher/joint_sensor_publisher.cc | 20 ++++--------------- .../publisher/joint_sensor_publisher.h | 3 --- 2 files changed, 4 insertions(+), 19 deletions(-) diff --git a/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.cc b/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.cc index 7c94a15..ebb66a2 100644 --- a/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.cc +++ b/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(); joint_node_options.bind_joint = joint_node["bind_joint"].as(); - if (joint_node["bind_jointpos_sensor"]) { - joint_node_options.bind_jointpos_sensor = joint_node["bind_jointpos_sensor"].as(); - } - if (joint_node["bind_jointvel_sensor"]) { - joint_node_options.bind_jointvel_sensor = joint_node["bind_jointvel_sensor"].as(); - } - if (joint_node["bind_jointactuatorfrc_sensor"]) { - joint_node_options.bind_jointactuatorfrc_sensor = joint_node["bind_jointactuatorfrc_sensor"].as(); - } - 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); } diff --git a/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.h b/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.h index 8b9e308..ee324c8 100644 --- a/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.h +++ b/src/module/mujoco_sim_module/publisher/joint_sensor_publisher.h @@ -23,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 joints; }; From 65bc0f440ec325d1a3b0363783dddeaa597ef578 Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 11:17:59 +0800 Subject: [PATCH 08/14] change type --- src/module/mujoco_sim_module/common/xmodel_reader.cc | 8 ++++---- src/module/mujoco_sim_module/common/xmodel_reader.h | 8 ++++---- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.cc b/src/module/mujoco_sim_module/common/xmodel_reader.cc index fdecb6c..8cb88a6 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.cc +++ b/src/module/mujoco_sim_module/common/xmodel_reader.cc @@ -41,7 +41,7 @@ std::optional GetJointactfrcNameByJointName(const mjModel* m, std:: return std::nullopt; } -std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name) { +std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name) { int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); if (jointId < 0) return std::nullopt; @@ -53,7 +53,7 @@ std::optional GetJointvelIdByJointName(const mjModel* m, std::string_v return std::nullopt; } -std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name) { +std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name) { int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); if (jointId < 0) return std::nullopt; @@ -65,7 +65,7 @@ std::optional GetJointposIdByJointName(const mjModel* m, std::string_v return std::nullopt; } -std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name) { +std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name) { int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); if (jointId < 0) return std::nullopt; @@ -77,7 +77,7 @@ std::optional GetJointactfrcIdByJointName(const mjModel* m, std::strin return std::nullopt; } -std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name) { +std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name) { int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); if (jointId < 0) return std::nullopt; diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.h b/src/module/mujoco_sim_module/common/xmodel_reader.h index 5723dbf..9fe590a 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.h +++ b/src/module/mujoco_sim_module/common/xmodel_reader.h @@ -13,12 +13,12 @@ std::optional GetJointvelNameByJointName(const mjModel* m, std::str std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name); -std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name); -std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name); -std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name); // joint actuator -std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointActNameByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointActTypeByJointName(const mjModel* m, std::string_view joint_name); From f158c7789da0793fe928b5334db31df36e19cfdc Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 11:27:21 +0800 Subject: [PATCH 09/14] checkout joint actuator type --- .../subscriber/joint_actuator_subscriber.cc | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) 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 d3a7180..e0f3fe9 100644 --- a/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc +++ b/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc @@ -118,7 +118,7 @@ void JointActuatorSubscriber::EventHandle(const std::shared_ptrqpos[actuator_bind_joint_sensor_addr_vec_[joint_idx].pos_addr]; double state_velocity = d_->qvel[actuator_bind_joint_sensor_addr_vec_[joint_idx].vel_addr]; @@ -126,6 +126,8 @@ void JointActuatorSubscriber::EventHandle(const std::shared_ptrqpos[actuator_bind_joint_sensor_addr_vec_[joint_idx].pos_addr]; double state_velocity = d_->qvel[actuator_bind_joint_sensor_addr_vec_[joint_idx].vel_addr]; @@ -174,6 +176,8 @@ void JointActuatorRos2Subscriber::EventHandle(const std::shared_ptr Date: Mon, 17 Mar 2025 11:27:39 +0800 Subject: [PATCH 10/14] check joint exist --- src/module/mujoco_sim_module/common/xmodel_reader.cc | 7 +++++++ src/module/mujoco_sim_module/common/xmodel_reader.h | 2 ++ 2 files changed, 9 insertions(+) diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.cc b/src/module/mujoco_sim_module/common/xmodel_reader.cc index 8cb88a6..086f810 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.cc +++ b/src/module/mujoco_sim_module/common/xmodel_reader.cc @@ -5,6 +5,13 @@ namespace aimrt_mujoco_sim::mujoco_sim_module::common { +bool IsJointExist(const mjModel* m, std::string_view joint_name) { + if (!m || !joint_name.data()) return false; + int jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + + return (jointId >= 0); +} + std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name) { int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); if (jointId < 0) return std::nullopt; diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.h b/src/module/mujoco_sim_module/common/xmodel_reader.h index 9fe590a..00897c9 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.h +++ b/src/module/mujoco_sim_module/common/xmodel_reader.h @@ -9,6 +9,8 @@ namespace aimrt_mujoco_sim::mujoco_sim_module::common { // joint sensor +bool IsJointExist(const mjModel* m, std::string_view joint_name); + std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name); From 6a9828729d79099a69c7daa827451082ace7eaa1 Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 11:30:32 +0800 Subject: [PATCH 11/14] check joint exist --- src/module/mujoco_sim_module/common/xmodel_reader.cc | 8 -------- src/module/mujoco_sim_module/common/xmodel_reader.h | 2 -- .../subscriber/joint_actuator_subscriber.cc | 3 ++- 3 files changed, 2 insertions(+), 11 deletions(-) diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.cc b/src/module/mujoco_sim_module/common/xmodel_reader.cc index 086f810..f2ae72b 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.cc +++ b/src/module/mujoco_sim_module/common/xmodel_reader.cc @@ -4,14 +4,6 @@ #include "mujoco_sim_module/common/xmodel_reader.h" namespace aimrt_mujoco_sim::mujoco_sim_module::common { - -bool IsJointExist(const mjModel* m, std::string_view joint_name) { - if (!m || !joint_name.data()) return false; - int jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); - - return (jointId >= 0); -} - std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name) { int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); if (jointId < 0) return std::nullopt; diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.h b/src/module/mujoco_sim_module/common/xmodel_reader.h index 00897c9..9fe590a 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.h +++ b/src/module/mujoco_sim_module/common/xmodel_reader.h @@ -9,8 +9,6 @@ namespace aimrt_mujoco_sim::mujoco_sim_module::common { // joint sensor -bool IsJointExist(const mjModel* m, std::string_view joint_name); - std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name); 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 e0f3fe9..0598171 100644 --- a/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc +++ b/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc @@ -60,7 +60,8 @@ void JointActuatorSubscriberBase::ApplyCtrlData() { void JointActuatorSubscriberBase::RegisterActuatorAddr() { for (auto const& joint : options_.joints) { int32_t actuator_id = common::GetJointActIdByJointName(m_, joint.bind_joint).value_or(-1); - AIMRT_CHECK_ERROR_THROW(actuator_id >= 0, "Invalid bind_actuator_name '{}'.", common::GetJointActNameByJointName(m_, joint.bind_joint).value_or("")); + AIMRT_CHECK_ERROR_THROW(actuator_id >= 0, "Joint actuator id for joint '{}' is not found.", + joint.bind_joint); actuator_addr_vec_.emplace_back(actuator_id); joint_names_vec_.emplace_back(joint.name); From c7bbf0d420340ace9599cdfef6f5f1a9c0eaea1f Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 11:46:14 +0800 Subject: [PATCH 12/14] tiny optim --- .../mujoco_sim_module/common/xmodel_reader.cc | 69 ++++++------------- .../mujoco_sim_module/common/xmodel_reader.h | 10 +-- 2 files changed, 26 insertions(+), 53 deletions(-) diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.cc b/src/module/mujoco_sim_module/common/xmodel_reader.cc index f2ae72b..10190d9 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.cc +++ b/src/module/mujoco_sim_module/common/xmodel_reader.cc @@ -4,76 +4,47 @@ #include "mujoco_sim_module/common/xmodel_reader.h" namespace aimrt_mujoco_sim::mujoco_sim_module::common { -std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name) { +std::optional 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) return std::nullopt; for (int32_t i = 0; i < m->nsensor; i++) { - if (m->sensor_type[i] == mjSENS_JOINTVEL && m->sensor_objid[i] == jointId) { - return mj_id2name(m, mjOBJ_SENSOR, i); + if (m->sensor_type[i] == sensor_type && m->sensor_objid[i] == jointId) { + return i; } } return std::nullopt; } -std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name) { - int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); - if (jointId < 0) return std::nullopt; - - for (int32_t i = 0; i < m->nsensor; i++) { - if (m->sensor_type[i] == mjSENS_JOINTPOS && m->sensor_objid[i] == jointId) { - return mj_id2name(m, mjOBJ_SENSOR, i); - } - } - return std::nullopt; +std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTVEL); } -std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name) { - int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); - if (jointId < 0) return std::nullopt; - - for (int32_t i = 0; i < m->nsensor; i++) { - if (m->sensor_type[i] == mjSENS_JOINTACTFRC && m->sensor_objid[i] == jointId) { - return mj_id2name(m, mjOBJ_SENSOR, i); - } - } - return std::nullopt; +std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTPOS); } -std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name) { - int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); - if (jointId < 0) return std::nullopt; +std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTACTFRC); +} - for (int32_t i = 0; i < m->nsensor; i++) { - if (m->sensor_type[i] == mjSENS_JOINTVEL && m->sensor_objid[i] == jointId) { - return i; - } +std::optional 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); } return std::nullopt; } -std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name) { - int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); - if (jointId < 0) return std::nullopt; - - for (int32_t i = 0; i < m->nsensor; i++) { - if (m->sensor_type[i] == mjSENS_JOINTPOS && m->sensor_objid[i] == jointId) { - return i; - } - } - return std::nullopt; +std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorNameByJointName(m, joint_name, mjSENS_JOINTVEL); } -std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name) { - int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); - if (jointId < 0) return std::nullopt; +std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorNameByJointName(m, joint_name, mjSENS_JOINTPOS); +} - for (int32_t i = 0; i < m->nsensor; i++) { - if (m->sensor_type[i] == mjSENS_JOINTACTFRC && m->sensor_objid[i] == jointId) { - return i; - } - } - return std::nullopt; +std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorNameByJointName(m, joint_name, mjSENS_JOINTACTFRC); } std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name) { diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.h b/src/module/mujoco_sim_module/common/xmodel_reader.h index 9fe590a..03fb7a6 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.h +++ b/src/module/mujoco_sim_module/common/xmodel_reader.h @@ -9,14 +9,16 @@ namespace aimrt_mujoco_sim::mujoco_sim_module::common { // joint sensor -std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name); -std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name); -std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name); - +std::optional GetJointSensorIdByJointName(const mjModel* m, std::string_view joint_name, mjtSensor sensor_type); std::optional GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointSensorNameByJointName(const mjModel* m, std::string_view joint_name, mjtSensor sensor_type); +std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name); +std::optional GetJointactfrcNameByJointName(const mjModel* m, std::string_view joint_name); + // joint actuator std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name); std::optional GetJointActNameByJointName(const mjModel* m, std::string_view joint_name); From 1af26331ddf36f2829243d93991063c9a490c7c2 Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 13:59:59 +0800 Subject: [PATCH 13/14] fix --- .../mujoco_sim_module/common/xmodel_reader.cc | 29 +++++++++++++++---- .../mujoco_sim_module/common/xmodel_reader.h | 3 +- .../publisher/imu_sensor_publisher.cc | 7 +++-- .../publisher/touch_sensor_publisher.cc | 4 ++- .../mujoco_sim_module/publisher/utils.h | 11 ------- 5 files changed, 32 insertions(+), 22 deletions(-) diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.cc b/src/module/mujoco_sim_module/common/xmodel_reader.cc index 10190d9..b0d227a 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.cc +++ b/src/module/mujoco_sim_module/common/xmodel_reader.cc @@ -2,11 +2,15 @@ // All rights reserved. #include "mujoco_sim_module/common/xmodel_reader.h" +#include namespace aimrt_mujoco_sim::mujoco_sim_module::common { std::optional 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) return std::nullopt; + if (jointId < 0) { + AIMRT_ERROR_THROW("Invalid joint name: {}.", 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) { @@ -48,11 +52,14 @@ std::optional GetJointactfrcNameByJointName(const mjModel* m, std:: } std::optional GetJointActIdByJointName(const mjModel* m, std::string_view joint_name) { - int32_t jointId = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); - if (jointId < 0) return std::nullopt; + int32_t joint_id = mj_name2id(m, mjOBJ_JOINT, joint_name.data()); + if (joint_id < 0) { + AIMRT_ERROR_THROW("Invalid joint name: {}.", 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(i * 2)] == jointId) { + if (m->actuator_trntype[i] == mjTRN_JOINT && m->actuator_trnid[static_cast(i * 2)] == joint_id) { return i; } } @@ -94,17 +101,27 @@ std::optional GetJointActTypeByJointName(const mjModel* m, std::str if (m->actuator_dyntype[i] == mjDYN_NONE && m->actuator_gaintype[i] == mjGAIN_FIXED && m->actuator_biastype[i] == mjBIAS_NONE) { - if (m->actuator_gainprm[i * mjNGAIN] == 1) { + if (m->actuator_gainprm[static_cast(i * mjNGAIN)] == 1) { return "motor"; } } // other (todo:(hj) adapt to other types of actuators) - return std::nullopt; } } } // 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 GetSensorIdBySensorName(const mjModel* m, std::string_view sensor_name) { + 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 \ No newline at end of file diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.h b/src/module/mujoco_sim_module/common/xmodel_reader.h index 03fb7a6..38dc765 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.h +++ b/src/module/mujoco_sim_module/common/xmodel_reader.h @@ -5,6 +5,7 @@ #include #include "mujoco/mjmodel.h" +#include "mujoco_sim_module/global.h" #include "simulate.h" namespace aimrt_mujoco_sim::mujoco_sim_module::common { @@ -25,5 +26,5 @@ std::optional GetJointActNameByJointName(const mjModel* m, std::str std::optional GetJointActTypeByJointName(const mjModel* m, std::string_view joint_name); // normal sensor - +std::optional GetSensorIdBySensorName(const mjModel* m, std::string_view sensor_name); } // namespace aimrt_mujoco_sim::mujoco_sim_module::common diff --git a/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc b/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc index f083114..5f6e176 100644 --- a/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc +++ b/src/module/mujoco_sim_module/publisher/imu_sensor_publisher.cc @@ -2,6 +2,7 @@ // All rights reserved. #include "mujoco_sim_module/publisher/imu_sensor_publisher.h" +#include "mujoco_sim_module/common/xmodel_reader.h" namespace YAML { template <> @@ -45,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) { diff --git a/src/module/mujoco_sim_module/publisher/touch_sensor_publisher.cc b/src/module/mujoco_sim_module/publisher/touch_sensor_publisher.cc index ee77de6..371a6f0 100644 --- a/src/module/mujoco_sim_module/publisher/touch_sensor_publisher.cc +++ b/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 { @@ -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()); diff --git a/src/module/mujoco_sim_module/publisher/utils.h b/src/module/mujoco_sim_module/publisher/utils.h index 4506659..adea5d7 100644 --- a/src/module/mujoco_sim_module/publisher/utils.h +++ b/src/module/mujoco_sim_module/publisher/utils.h @@ -36,15 +36,4 @@ inline double GetAvgIntervalBase(const uint32_t channel_frq) { return avg_interval_base; } -[[nodiscard]] inline int32_t GetSensorAddr(const mjModel* m, std::string_view sensor_name) { - if (sensor_name.empty()) { - return -1; - } - - int32_t sensor_id = mj_name2id(m, mjOBJ_SENSOR, sensor_name.data()); - AIMRT_CHECK_ERROR_THROW(sensor_id >= 0, "Invalid sensor name: {}.", sensor_name); - - return m->sensor_adr[sensor_id]; -} - } // namespace aimrt_mujoco_sim::mujoco_sim_module::publisher \ No newline at end of file From 40544aeb37d4453659f87ca75a38ec09fcd38ce8 Mon Sep 17 00:00:00 2001 From: hanjun Date: Mon, 17 Mar 2025 14:24:01 +0800 Subject: [PATCH 14/14] add check --- .../mujoco_sim_module/common/xmodel_reader.cc | 16 +++++++++------- .../subscriber/joint_actuator_subscriber.cc | 2 -- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/module/mujoco_sim_module/common/xmodel_reader.cc b/src/module/mujoco_sim_module/common/xmodel_reader.cc index b0d227a..5dab297 100644 --- a/src/module/mujoco_sim_module/common/xmodel_reader.cc +++ b/src/module/mujoco_sim_module/common/xmodel_reader.cc @@ -8,7 +8,7 @@ namespace aimrt_mujoco_sim::mujoco_sim_module::common { std::optional 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: {}.", joint_name); + AIMRT_ERROR_THROW("Invalid joint name: {}, cannot find a joint sensor.", joint_name); return std::nullopt; } @@ -34,7 +34,7 @@ std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string std::optional 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); + return mj_id2name(m, mjOBJ_SENSOR, sensor_id.value_or(-1)); } return std::nullopt; } @@ -54,7 +54,7 @@ std::optional GetJointactfrcNameByJointName(const mjModel* m, std:: std::optional 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: {}.", joint_name); + AIMRT_ERROR_THROW("Invalid joint name: {}, cannot find a joint actuator.", joint_name); return std::nullopt; } @@ -67,10 +67,10 @@ std::optional GetJointActIdByJointName(const mjModel* m, std::string_vi } std::optional GetJointActNameByJointName(const mjModel* m, std::string_view joint_name) { - int32_t actuatorId = GetJointActIdByJointName(m, joint_name.data()).value_or(-1); - if (actuatorId < 0) return std::nullopt; - - return mj_id2name(m, mjOBJ_ACTUATOR, actuatorId); + 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 GetJointActTypeByJointName(const mjModel* m, std::string_view joint_name) { @@ -115,6 +115,8 @@ std::optional GetJointActTypeByJointName(const mjModel* m, std::str } std::optional 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); 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 0598171..e26f497 100644 --- a/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc +++ b/src/module/mujoco_sim_module/subscriber/joint_actuator_subscriber.cc @@ -60,8 +60,6 @@ void JointActuatorSubscriberBase::ApplyCtrlData() { void JointActuatorSubscriberBase::RegisterActuatorAddr() { for (auto const& joint : options_.joints) { int32_t actuator_id = common::GetJointActIdByJointName(m_, joint.bind_joint).value_or(-1); - AIMRT_CHECK_ERROR_THROW(actuator_id >= 0, "Joint actuator id for joint '{}' is not found.", - joint.bind_joint); actuator_addr_vec_.emplace_back(actuator_id); joint_names_vec_.emplace_back(joint.name);