diff --git a/document/chapter1/chapter1_quickstart.md b/document/chapter1/chapter1_quickstart.md index b0574b8..d6d9038 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 @@ -61,5 +62,5 @@ AIMRT_MUJOCO_SIM/ └── CMakeLists.txt # 项目根目录的 CMake 配置 ``` - +[返回目录](../index.md) 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..5dab297 --- /dev/null +++ b/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 + +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: {}, 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 GetJointvelIdByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTVEL); +} + +std::optional GetJointposIdByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTPOS); +} + +std::optional GetJointactfrcIdByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorIdByJointName(m, joint_name, mjSENS_JOINTACTFRC); +} + +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.value_or(-1)); + } + return std::nullopt; +} + +std::optional GetJointvelNameByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorNameByJointName(m, joint_name, mjSENS_JOINTVEL); +} + +std::optional GetJointposNameByJointName(const mjModel* m, std::string_view joint_name) { + return GetJointSensorNameByJointName(m, joint_name, mjSENS_JOINTPOS); +} + +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) { + 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(i * 2)] == joint_id) { + return i; + } + } + return std::nullopt; +} + +std::optional 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 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[static_cast(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 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 \ 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..38dc765 --- /dev/null +++ b/src/module/mujoco_sim_module/common/xmodel_reader.h @@ -0,0 +1,30 @@ +// Copyright (c) 2023, AgiBot Inc. +// All rights reserved. + +#pragma once + +#include +#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 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); +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 e1a1906..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,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 { 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(publisher_), "Register publish type failed."); + AIMRT_CHECK_ERROR_THROW(aimrt::channel::RegisterPublishType(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(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..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); } @@ -179,5 +167,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..ee324c8 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" @@ -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 joints; }; 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/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/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 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..e26f497 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,14 @@ 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); 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,11 +113,11 @@ 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]; @@ -132,6 +125,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]; @@ -181,6 +175,8 @@ void JointActuatorRos2Subscriber::EventHandle(const std::shared_ptr - #include "mujoco_sim_module/global.h" #include "mujoco_sim_module/subscriber/subscriber_base.h" @@ -24,8 +22,6 @@ class JointActuatorSubscriberBase : public SubscriberBase { struct Joint { std::string name; std::string bind_joint; - std::string bind_actuator_type; - std::string bind_actuator_name; }; std::vector joints; }; @@ -66,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_; };