From b1794421b7307e81bd582dfa93ea08d307c62c68 Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Fri, 18 Oct 2024 23:47:34 +0800 Subject: [PATCH 1/9] [BUG] update for 22.04 jazzy 1. add the MJResourceManager class. 2. the robot_description is now read from topic. 3. fix the controller configuration failure. 4. modify CMakeLists to support the source-build mujoco. --- .gitignore | 7 +- mujoco_ros2_control/CMakeLists.txt | 12 +- .../mujoco_ros2_control.hpp | 8 +- .../src/mujoco_ros2_control.cpp | 169 +++++++++--------- .../src/mujoco_ros2_control_node.cpp | 16 +- mujoco_ros2_control/src/mujoco_system.cpp | 5 +- .../config/cartpole_controller_effort.yaml | 2 +- .../launch/cart_example_effort.launch.py | 12 +- 8 files changed, 135 insertions(+), 96 deletions(-) diff --git a/.gitignore b/.gitignore index 94b7260..d582254 100644 --- a/.gitignore +++ b/.gitignore @@ -3,9 +3,14 @@ build/ install/ log/ +# clion +**/cmake-build-debug +**/.idea # vscode **/.vscode **/.devcontainer # mujoco log -**/MUJOCO_LOG.TXT \ No newline at end of file +**/MUJOCO_LOG.TXT + + diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index fc7a4f4..3a18f19 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -36,7 +36,17 @@ set(THIS_PACKAGE_DEPENDS ) set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE) -find_library(MUJOCO_LIB mujoco HINTS $ENV{MUJOCO_DIR}/lib) +find_package(mujoco QUIET) +if(mujoco_FOUND) + message(STATUS "Mujoco build from source") + set(MUJOCO_LIB mujoco::mujoco) + set(MUJOCO_INCLUDE_DIR ${MUJOCO_INCLUDE_DIR}) +else (mujoco_FOUND) + message(STATUS "Mujoco build from binary") + find_library(MUJOCO_LIB mujoco HINTS $ENV{MUJOCO_DIR}/lib) + set(MUJOCO_INCLUDE_DIR $ENV{MUJOCO_DIR}/include) +endif (mujoco_FOUND) + include_directories(include) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp index b7d0886..d921d15 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp @@ -12,17 +12,21 @@ namespace mujoco_ros2_control { +// declare in advance +class MJResourceManager; + class MujocoRos2Control { public: - MujocoRos2Control(rclcpp::Node::SharedPtr & node, mjModel* mujoco_model, mjData* mujoco_data); + MujocoRos2Control(rclcpp::Node::SharedPtr & node, rclcpp::NodeOptions cm_node_option, mjModel* mujoco_model, mjData* mujoco_data); ~MujocoRos2Control(); void init(); void update(); private: void publish_sim_time(rclcpp::Time sim_time); - rclcpp::Node::SharedPtr node_; // TODO: delete node + rclcpp::Node::SharedPtr node_; + rclcpp::NodeOptions cm_node_option_; mjModel* mj_model_; mjData* mj_data_; diff --git a/mujoco_ros2_control/src/mujoco_ros2_control.cpp b/mujoco_ros2_control/src/mujoco_ros2_control.cpp index 25ae3a1..0943f01 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control.cpp @@ -6,9 +6,75 @@ namespace mujoco_ros2_control { -MujocoRos2Control::MujocoRos2Control(rclcpp::Node::SharedPtr & node, mjModel* mujoco_model, mjData* mujoco_data) +class MJResourceManager : public hardware_interface::ResourceManager{ +public: + MJResourceManager(rclcpp::Node::SharedPtr& node, mjModel* mj_model, mjData* mj_data) + : hardware_interface::ResourceManager(node->get_node_clock_interface(), node->get_node_logging_interface()), + mj_system_loader_("mujoco_ros2_control", "mujoco_ros2_control::MujocoSystemInterface"), + logger_(node->get_logger().get_child("MJResourceManager")) + { + mj_model_ = mj_model; + mj_data_ = mj_data; + node_ = node; + } + MJResourceManager(const MJResourceManager&) = delete; + + /// Called from ControllerManager when {robot_description} is initialized from callback. + /** + * Override from hardware_interface::ResourceManager + * \param[in] urdf string containing the URDF. + * \param[in] update_rate Update rate of the controller manager to calculate calling frequency + * of async components. + */ + bool load_and_initialize_components(const std::string& urdf, unsigned int update_rate) override + { + components_are_loaded_and_initialized_ = true; + + const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + + for (const auto& individual_hardware_info : hardware_info) + { + std::string robot_hw_sim_type_str_ = individual_hardware_info.hardware_plugin_name; + RCLCPP_DEBUG(logger_, "Load hardware interface %s ...", robot_hw_sim_type_str_.c_str()); + + // Load hardware + std::unique_ptr mjSimSystem; + std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); + try + { + mjSimSystem = std::unique_ptr( + mj_system_loader_.createUnmanagedInstance(robot_hw_sim_type_str_)); + } catch (pluginlib::PluginlibException & ex) { + RCLCPP_ERROR_STREAM(logger_, "The plugin failed to load. Error: " << ex.what()); + continue; + } + + // initialize simulation required resource from the hardware info. + urdf::Model urdf_model; + urdf_model.initString(urdf); + if(!mjSimSystem->init_sim(node_, mj_model_, mj_data_, urdf_model, individual_hardware_info)) + { + RCLCPP_FATAL(logger_, "Could not initialize robot simulation interface"); + components_are_loaded_and_initialized_ = false; + break; + } + RCLCPP_DEBUG(logger_, "Initialized hardware interface %s !", robot_hw_sim_type_str_.c_str()); + import_component(std::move(mjSimSystem), individual_hardware_info); + } + return components_are_loaded_and_initialized_; + } + +private: + std::shared_ptr node_; + pluginlib::ClassLoader mj_system_loader_; + rclcpp::Logger logger_; + mjModel* mj_model_; + mjData* mj_data_; +}; + +MujocoRos2Control::MujocoRos2Control(rclcpp::Node::SharedPtr & node, rclcpp::NodeOptions cm_node_option, mjModel* mujoco_model, mjData* mujoco_data) : node_(node), mj_model_(mujoco_model), mj_data_(mujoco_data), logger_(rclcpp::get_logger(node_->get_name() + std::string(".mujoco_ros2_control"))), - control_period_(rclcpp::Duration(1, 0)), last_update_sim_time_ros_(0, 0, RCL_ROS_TIME) + control_period_(rclcpp::Duration(1, 0)), last_update_sim_time_ros_(0, 0, RCL_ROS_TIME), cm_node_option_(cm_node_option) { } @@ -23,83 +89,15 @@ MujocoRos2Control::~MujocoRos2Control() void MujocoRos2Control::init() { clock_publisher_ = node_->create_publisher("/clock", 10); - // Read urdf from ros parameter server then - // setup actuators and mechanism control node. - std::string urdf_string; - std::vector control_hardware_info; - try - { - urdf_string = node_->get_parameter("robot_description").as_string(); - control_hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf_string); - } - catch (const std::runtime_error & ex) - { - RCLCPP_ERROR_STREAM(logger_, "Error parsing URDF : " << ex.what()); - return; - } - - try - { - robot_hw_sim_loader_.reset( - new pluginlib::ClassLoader( - "mujoco_ros2_control", - "mujoco_ros2_control::MujocoSystemInterface")); - } - catch (pluginlib::LibraryLoadException & ex) - { - RCLCPP_ERROR_STREAM(logger_, "Failed to create hardware interface loader: " << ex.what()); - return; - } - std::unique_ptr resource_manager = - std::make_unique(); - - try - { - resource_manager->load_urdf(urdf_string, false, false); - } - catch (...) - { - RCLCPP_ERROR(logger_, "Error while initializing URDF!"); - } - - for (const auto& hardware : control_hardware_info) - { - std::string robot_hw_sim_type_str_ = hardware.hardware_class_type; - std::unique_ptr mujoco_system; - try - { - mujoco_system = std::unique_ptr( - robot_hw_sim_loader_->createUnmanagedInstance(robot_hw_sim_type_str_)); - } - catch (pluginlib::PluginlibException & ex) - { - RCLCPP_ERROR_STREAM(logger_, "The plugin failed to load. Error: " << ex.what()); - continue; - } - - urdf::Model urdf_model; - urdf_model.initString(urdf_string); - if (!mujoco_system->init_sim(node_, mj_model_, mj_data_, urdf_model, hardware)) - { - RCLCPP_FATAL(logger_, "Could not initialize robot simulation interface"); - return; - } - - resource_manager->import_component(std::move(mujoco_system), hardware); - - rclcpp_lifecycle::State state( - lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE, - hardware_interface::lifecycle_state_names::ACTIVE); - resource_manager->set_component_state(hardware.name, state); - } + std::make_unique(node_, mj_model_, mj_data_); // Create the controller manager RCLCPP_INFO(logger_, "Loading controller_manager"); cm_executor_ = std::make_shared(); controller_manager_.reset(new controller_manager::ControllerManager( std::move(resource_manager), cm_executor_, - "controller_manager", node_->get_namespace())); + "controller_manager", node_->get_namespace(), cm_node_option_)); cm_executor_->add_node(controller_manager_); @@ -113,27 +111,31 @@ void MujocoRos2Control::init() std::chrono::duration_cast( std::chrono::duration(1.0 / static_cast(update_rate)))); - // Force setting of use_sime_time parameter + // Force setting of use_sim_time parameter controller_manager_->set_parameter(rclcpp::Parameter("use_sim_time", rclcpp::ParameterValue(true))); - stop_cm_thread_ = false; auto spin = [this]() - { - while (rclcpp::ok() && !stop_cm_thread_) { - cm_executor_->spin_once(); - } - }; + { + while (rclcpp::ok() && !stop_cm_thread_) { + cm_executor_->spin_once(); + } + }; cm_thread_ = std::thread(spin); + + // Waiting RM to be initialized through topic robot_description + while(!controller_manager_->is_resource_manager_initialized()) + { + RCLCPP_WARN(logger_, "Waiting RM to load and initialize hardware..."); + std::this_thread::sleep_for(std::chrono::microseconds (2000000)); + } } void MujocoRos2Control::update() { // Get the simulation time and period - auto sim_time = mj_data_->time; - int sim_time_sec = static_cast(sim_time); - int sim_time_nanosec = static_cast((sim_time - sim_time_sec)*1000000000); + std::chrono::duration sim_time(static_cast(mj_data_->time)); - rclcpp::Time sim_time_ros(sim_time_sec, sim_time_nanosec, RCL_ROS_TIME); + rclcpp::Time sim_time_ros(std::chrono::duration_cast(sim_time).count(), RCL_ROS_TIME); rclcpp::Duration sim_period = sim_time_ros - last_update_sim_time_ros_; publish_sim_time(sim_time_ros); @@ -145,7 +147,6 @@ void MujocoRos2Control::update() controller_manager_->update(sim_time_ros, sim_period); last_update_sim_time_ros_ = sim_time_ros; } - // use same time as for read and update call - this is how it is done in ros2_control_node controller_manager_->write(sim_time_ros, sim_period); diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index 92c12de..da9b7d1 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -15,6 +15,19 @@ int main(int argc, const char** argv) { rclcpp::init(argc, argv); std::shared_ptr node = rclcpp::Node::make_shared("mujoco_ros2_control_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); + // get the ros arg + rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); + std::vector node_arguments = cm_node_options.arguments(); + for(int i = 1; i < argc; ++i) + { + if(node_arguments.empty() && std::string(argv[i]) != "--ros-args") continue; + node_arguments.emplace_back(argv[i]); + } + cm_node_options.arguments(node_arguments); + for(const auto & it:node_arguments) + std::cout<get_logger(), "Initializing mujoco_ros2_control node..."); auto model_path = node->get_parameter("mujoco_model_path").as_string(); @@ -34,7 +47,7 @@ int main(int argc, const char** argv) { mujoco_data = mj_makeData(mujoco_model); // initialize mujoco control - auto control = mujoco_ros2_control::MujocoRos2Control(node, mujoco_model, mujoco_data); + auto control = mujoco_ros2_control::MujocoRos2Control(node, cm_node_options, mujoco_model, mujoco_data); control.init(); RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco ros2 controller has been successfully initialized !"); @@ -61,6 +74,7 @@ int main(int argc, const char** argv) { // free MuJoCo model and data mj_deleteData(mujoco_data); mj_deleteModel(mujoco_model); + RCLCPP_INFO(node->get_logger(), "Mujoco Sim Stop ..."); return 1; } diff --git a/mujoco_ros2_control/src/mujoco_system.cpp b/mujoco_ros2_control/src/mujoco_system.cpp index 5fae580..7f50a01 100644 --- a/mujoco_ros2_control/src/mujoco_system.cpp +++ b/mujoco_ros2_control/src/mujoco_system.cpp @@ -2,7 +2,7 @@ namespace mujoco_ros2_control { -MujocoSystem::MujocoSystem() : logger_(rclcpp::get_logger("")) +MujocoSystem::MujocoSystem() : logger_(rclcpp::get_logger("MujocoSystem")) { } @@ -43,11 +43,13 @@ hardware_interface::return_type MujocoSystem::read(const rclcpp::Time & time, co data.torque.data.y() = -mj_data_->sensordata[data.torque.mj_sensor_index + 1]; data.torque.data.z() = -mj_data_->sensordata[data.torque.mj_sensor_index + 2]; } + return hardware_interface::return_type::OK; } hardware_interface::return_type MujocoSystem::write(const rclcpp::Time & time, const rclcpp::Duration & period) { // update mimic joint + for (auto& joint_state : joint_states_) { if (joint_state.is_mimic) @@ -98,6 +100,7 @@ hardware_interface::return_type MujocoSystem::write(const rclcpp::Time & time, c mj_data_->qfrc_applied[joint_state.mj_vel_adr] = clamp(joint_state.effort_command, min_eff, max_eff); } } + return hardware_interface::return_type::OK; } bool MujocoSystem::init_sim(rclcpp::Node::SharedPtr& node, mjModel* mujoco_model, mjData *mujoco_data, diff --git a/mujoco_ros2_control_demos/config/cartpole_controller_effort.yaml b/mujoco_ros2_control_demos/config/cartpole_controller_effort.yaml index 5f0046c..97d4368 100644 --- a/mujoco_ros2_control_demos/config/cartpole_controller_effort.yaml +++ b/mujoco_ros2_control_demos/config/cartpole_controller_effort.yaml @@ -4,11 +4,11 @@ controller_manager: effort_controller: type: effort_controllers/JointGroupEffortController - joint_state_broadcaster: type: joint_state_broadcaster/JointStateBroadcaster effort_controller: ros__parameters: + type: effort_controllers/JointGroupEffortController joints: - slider_to_cart \ No newline at end of file diff --git a/mujoco_ros2_control_demos/launch/cart_example_effort.launch.py b/mujoco_ros2_control_demos/launch/cart_example_effort.launch.py index af35262..08feac6 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_effort.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_effort.launch.py @@ -31,10 +31,10 @@ def generate_launch_description(): executable='mujoco_ros2_control', output='screen', parameters=[ - robot_description, controller_config_file, {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} - ] + ], + # arguments=["--ros-args", "--log-level", "debug"] ) node_robot_state_publisher = Node( @@ -50,11 +50,13 @@ def generate_launch_description(): output='screen' ) - load_joint_trajectory_controller = ExecuteProcess( - cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', 'effort_controller'], + load_joint_effort_controller = ExecuteProcess( + cmd=['ros2', 'control', 'load_controller', '--set-state', 'active', + 'effort_controller'], output='screen' ) + return LaunchDescription([ RegisterEventHandler( event_handler=OnProcessStart( @@ -65,7 +67,7 @@ def generate_launch_description(): RegisterEventHandler( event_handler=OnProcessExit( target_action=load_joint_state_controller, - on_exit=[load_joint_trajectory_controller], + on_exit=[load_joint_effort_controller], ) ), node_mujoco_ros2_control, From 39361782bfb38c6a07f018ce8c8b37a3a58abfc6 Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Fri, 18 Oct 2024 23:58:20 +0800 Subject: [PATCH 2/9] [BUG] update for 22.04 jazzy 1. modify the demo launch.py --- mujoco_ros2_control/CMakeLists.txt | 2 +- .../launch/cart_example_position.launch.py | 1 - .../launch/cart_example_velocity.launch.py | 1 - mujoco_ros2_control_demos/launch/diff_drive.launch.py | 1 - 4 files changed, 1 insertion(+), 4 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 3a18f19..81cd2de 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -53,7 +53,7 @@ include_directories(include) add_library(mujoco_system_plugins SHARED src/mujoco_system.cpp) ament_target_dependencies(mujoco_system_plugins ${THIS_PACKAGE_DEPENDS}) target_link_libraries(mujoco_system_plugins ${MUJOCO_LIB}) -target_include_directories(mujoco_system_plugins PUBLIC $ENV{MUJOCO_DIR}/include ${EIGEN3_INCLUDE_DIR}) +target_include_directories(mujoco_system_plugins PUBLIC ${MUJOCO_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR}) install(TARGETS mujoco_system_plugins diff --git a/mujoco_ros2_control_demos/launch/cart_example_position.launch.py b/mujoco_ros2_control_demos/launch/cart_example_position.launch.py index cee7ad8..55e6f80 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_position.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_position.launch.py @@ -31,7 +31,6 @@ def generate_launch_description(): executable='mujoco_ros2_control', output='screen', parameters=[ - robot_description, controller_config_file, {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} ] diff --git a/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py b/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py index 6d2cc4e..b853370 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -30,7 +30,6 @@ def generate_launch_description(): executable='mujoco_ros2_control', output='screen', parameters=[ - robot_description, controller_config_file, {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} ] diff --git a/mujoco_ros2_control_demos/launch/diff_drive.launch.py b/mujoco_ros2_control_demos/launch/diff_drive.launch.py index 0e91bb4..7928184 100644 --- a/mujoco_ros2_control_demos/launch/diff_drive.launch.py +++ b/mujoco_ros2_control_demos/launch/diff_drive.launch.py @@ -31,7 +31,6 @@ def generate_launch_description(): executable='mujoco_ros2_control', output='screen', parameters=[ - robot_description, controller_config_file, {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_diff_drive.xml')} ] From 786c8177cca9a7ac9a67a8c0a04874fb0a810e35 Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Fri, 18 Oct 2024 23:58:20 +0800 Subject: [PATCH 3/9] [BUG] modify the demo launch.py --- mujoco_ros2_control/CMakeLists.txt | 2 +- .../launch/cart_example_position.launch.py | 1 - .../launch/cart_example_velocity.launch.py | 1 - mujoco_ros2_control_demos/launch/diff_drive.launch.py | 1 - 4 files changed, 1 insertion(+), 4 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 3a18f19..81cd2de 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -53,7 +53,7 @@ include_directories(include) add_library(mujoco_system_plugins SHARED src/mujoco_system.cpp) ament_target_dependencies(mujoco_system_plugins ${THIS_PACKAGE_DEPENDS}) target_link_libraries(mujoco_system_plugins ${MUJOCO_LIB}) -target_include_directories(mujoco_system_plugins PUBLIC $ENV{MUJOCO_DIR}/include ${EIGEN3_INCLUDE_DIR}) +target_include_directories(mujoco_system_plugins PUBLIC ${MUJOCO_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR}) install(TARGETS mujoco_system_plugins diff --git a/mujoco_ros2_control_demos/launch/cart_example_position.launch.py b/mujoco_ros2_control_demos/launch/cart_example_position.launch.py index cee7ad8..55e6f80 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_position.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_position.launch.py @@ -31,7 +31,6 @@ def generate_launch_description(): executable='mujoco_ros2_control', output='screen', parameters=[ - robot_description, controller_config_file, {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} ] diff --git a/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py b/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py index 6d2cc4e..b853370 100644 --- a/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py +++ b/mujoco_ros2_control_demos/launch/cart_example_velocity.launch.py @@ -30,7 +30,6 @@ def generate_launch_description(): executable='mujoco_ros2_control', output='screen', parameters=[ - robot_description, controller_config_file, {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_cart.xml')} ] diff --git a/mujoco_ros2_control_demos/launch/diff_drive.launch.py b/mujoco_ros2_control_demos/launch/diff_drive.launch.py index 0e91bb4..7928184 100644 --- a/mujoco_ros2_control_demos/launch/diff_drive.launch.py +++ b/mujoco_ros2_control_demos/launch/diff_drive.launch.py @@ -31,7 +31,6 @@ def generate_launch_description(): executable='mujoco_ros2_control', output='screen', parameters=[ - robot_description, controller_config_file, {'mujoco_model_path':os.path.join(mujoco_ros2_control_demos_path, 'mujoco_models', 'test_diff_drive.xml')} ] From 0eb9b9e5889d806505bdab477beac437aa46ee43 Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Tue, 22 Oct 2024 10:09:54 +0800 Subject: [PATCH 4/9] [Feature] IMU support 1. IMU support 2. modify MJCF and URDF for demos 3. fix a typo in CMakeLists.txt --- mujoco_ros2_control/CMakeLists.txt | 3 +- .../mujoco_ros2_control/mujoco_system.hpp | 3 +- .../src/mujoco_ros2_control_node.cpp | 6 +- mujoco_ros2_control/src/mujoco_system.cpp | 81 +++++++++++++++++-- .../examples/example_diff_drive.cpp | 21 ++--- .../mujoco_models/test_diff_drive.xml | 7 ++ .../urdf/test_diff_drive.xacro.urdf | 13 +++ 7 files changed, 108 insertions(+), 26 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 81cd2de..51158d5 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -47,7 +47,6 @@ else (mujoco_FOUND) set(MUJOCO_INCLUDE_DIR $ENV{MUJOCO_DIR}/include) endif (mujoco_FOUND) - include_directories(include) add_library(mujoco_system_plugins SHARED src/mujoco_system.cpp) @@ -66,7 +65,7 @@ install(TARGETS add_executable(mujoco_ros2_control src/mujoco_ros2_control_node.cpp src/mujoco_rendering.cpp src/mujoco_ros2_control.cpp) ament_target_dependencies(mujoco_ros2_control ${THIS_PACKAGE_DEPENDS}) target_link_libraries(mujoco_ros2_control ${MUJOCO_LIB} glfw) -target_include_directories(mujoco_ros2_control PUBLIC $ENV{MUJOCO_DIR}/include ${EIGEN3_INCLUDE_DIR}) +target_include_directories(mujoco_ros2_control PUBLIC ${MUJOCO_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR}) install(TARGETS mujoco_ros2_control diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp index f4b3b11..6f21f11 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp @@ -79,6 +79,7 @@ class MujocoSystem : public MujocoSystemInterface SensorData> orientation; SensorData angular_velocity; SensorData linear_velocity; + SensorData linear_acceleration; }; private: @@ -102,7 +103,7 @@ class MujocoSystem : public MujocoSystemInterface mjModel* mj_model_; mjData* mj_data_; - rclcpp::Logger logger_; // TODO: delete? + rclcpp::Logger logger_; }; } // namespace mujoco_ros2_control diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index da9b7d1..4afd203 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -11,11 +11,10 @@ mjData* mujoco_data = nullptr; // main function int main(int argc, const char** argv) { - rclcpp::init(argc, argv); std::shared_ptr node = rclcpp::Node::make_shared("mujoco_ros2_control_node", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true)); - // get the ros arg + // get the ros arg, mainly for getting --param-file for cm rclcpp::NodeOptions cm_node_options = controller_manager::get_cm_node_options(); std::vector node_arguments = cm_node_options.arguments(); for(int i = 1; i < argc; ++i) @@ -24,9 +23,6 @@ int main(int argc, const char** argv) { node_arguments.emplace_back(argv[i]); } cm_node_options.arguments(node_arguments); - for(const auto & it:node_arguments) - std::cout<get_logger(), "Initializing mujoco_ros2_control node..."); auto model_path = node->get_parameter("mujoco_model_path").as_string(); diff --git a/mujoco_ros2_control/src/mujoco_system.cpp b/mujoco_ros2_control/src/mujoco_system.cpp index 7f50a01..668f87d 100644 --- a/mujoco_ros2_control/src/mujoco_system.cpp +++ b/mujoco_ros2_control/src/mujoco_system.cpp @@ -29,7 +29,19 @@ hardware_interface::return_type MujocoSystem::read(const rclcpp::Time & time, co // IMU Sensor data for (auto& data : imu_sensor_data_) { - // TODO + std::vector*> ptr_vec{&data.angular_velocity, + &data.linear_velocity, + &data.linear_acceleration}; + data.orientation.data.w() = mj_data_->sensordata[data.orientation.mj_sensor_index]; + data.orientation.data.x() = mj_data_->sensordata[data.orientation.mj_sensor_index + 1]; + data.orientation.data.y() = mj_data_->sensordata[data.orientation.mj_sensor_index + 2]; + data.orientation.data.z() = mj_data_->sensordata[data.orientation.mj_sensor_index + 3]; + for (int i = 0; i < 3; ++i) + { + ptr_vec[i]->data.x() = mj_data_->sensordata[ptr_vec[i]->mj_sensor_index]; + ptr_vec[i]->data.y() = mj_data_->sensordata[ptr_vec[i]->mj_sensor_index + 1]; + ptr_vec[i]->data.z() = mj_data_->sensordata[ptr_vec[i]->mj_sensor_index + 2]; + } } // FT Sensor data @@ -113,7 +125,7 @@ bool MujocoSystem::init_sim(rclcpp::Node::SharedPtr& node, mjModel* mujoco_model logger_ = rclcpp::get_logger(node_->get_name() + std::string("mujoco_system")); register_joints(urdf_model, hardware_info); - register_sensors(urdf_model,hardware_info); + register_sensors(urdf_model, hardware_info); set_initial_pose(); return true; @@ -279,12 +291,65 @@ void MujocoSystem::register_joints(const urdf::Model& urdf_model, const hardware void MujocoSystem::register_sensors(const urdf::Model& urdf_model, const hardware_interface::HardwareInfo & hardware_info) { - // TODO: for now, assuming all sensors are ft_sensor - ft_sensor_data_.resize(hardware_info.sensors.size()); + // count the number of different sensor + std::vector ft_idx{}, imu_idx{}; + for(size_t info_idx = 0; info_idx < hardware_info.sensors.size(); ++info_idx) + { + auto sensor_info = hardware_info.sensors[info_idx]; + if(sensor_info.parameters.find("type") == sensor_info.parameters.end()) + { + RCLCPP_WARN(logger_, "Sensor missing \"type\" tag in URDF, skipping ..."); + continue; + } + const std::string type = sensor_info.parameters["type"]; + if(type == "IMU") imu_idx.push_back(int(info_idx)); + else if(type == "FTSensor") ft_idx.push_back(int(info_idx)); + } + ft_sensor_data_.resize(ft_idx.size()); + imu_sensor_data_.resize(imu_idx.size()); + + for(size_t sensor_idx = 0; sensor_idx < imu_idx.size(); ++sensor_idx) + { + auto sensor_info = hardware_info.sensors[imu_idx[sensor_idx]]; + IMUSensorData& imu_data = imu_sensor_data_[sensor_idx]; + imu_data.name = sensor_info.name; + imu_data.angular_velocity.name = imu_data.name + "_Gyro"; + imu_data.orientation.name = imu_data.name + "_Framequat"; + imu_data.linear_velocity.name = imu_data.name + "_Velocimeter"; + imu_data.linear_acceleration.name = imu_data.name + "_Accelerometer"; + + int angular_vel_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, imu_data.angular_velocity.name.c_str()); + int orientation_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, imu_data.orientation.name.c_str()); + int linear_acc_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, imu_data.linear_acceleration.name.c_str()); + int linear_vel_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, imu_data.linear_velocity.name.c_str()); + if (angular_vel_id == -1 || orientation_id == -1 || linear_acc_id == -1 || linear_vel_id == -1) + { + RCLCPP_ERROR_STREAM(logger_, "IMU sensor lack of sub-sensor, check the MJCF for IMU sensor: " << imu_data.name); + continue; + } + imu_data.angular_velocity.mj_sensor_index = mj_model_->sensor_adr[angular_vel_id]; + imu_data.orientation.mj_sensor_index = mj_model_->sensor_adr[orientation_id]; + imu_data.linear_velocity.mj_sensor_index = mj_model_->sensor_adr[linear_vel_id]; + imu_data.linear_acceleration.mj_sensor_index = mj_model_->sensor_adr[linear_acc_id]; + + state_interfaces_.emplace_back(imu_data.name, "orientation.w", &imu_data.orientation.data.w()); + state_interfaces_.emplace_back(imu_data.name, "orientation.x", &imu_data.orientation.data.x()); + state_interfaces_.emplace_back(imu_data.name, "orientation.y", &imu_data.orientation.data.y()); + state_interfaces_.emplace_back(imu_data.name, "orientation.z", &imu_data.orientation.data.z()); + state_interfaces_.emplace_back(imu_data.name, "angular_velocity.x", &imu_data.angular_velocity.data.x()); + state_interfaces_.emplace_back(imu_data.name, "angular_velocity.y", &imu_data.angular_velocity.data.y()); + state_interfaces_.emplace_back(imu_data.name, "angular_velocity.z", &imu_data.angular_velocity.data.z()); + state_interfaces_.emplace_back(imu_data.name, "linear_velocity.x", &imu_data.linear_velocity.data.x()); + state_interfaces_.emplace_back(imu_data.name, "linear_velocity.y", &imu_data.linear_velocity.data.y()); + state_interfaces_.emplace_back(imu_data.name, "linear_velocity.z", &imu_data.linear_velocity.data.z()); + state_interfaces_.emplace_back(imu_data.name, "linear_acceleration.x", &imu_data.linear_acceleration.data.x()); + state_interfaces_.emplace_back(imu_data.name, "linear_acceleration.y", &imu_data.linear_acceleration.data.y()); + state_interfaces_.emplace_back(imu_data.name, "linear_acceleration.z", &imu_data.linear_acceleration.data.z()); + } - for (size_t sensor_index = 0; sensor_index < hardware_info.sensors.size(); sensor_index++) + for (size_t sensor_idx = 0; sensor_idx < ft_idx.size(); ++sensor_idx) { - auto sensor = hardware_info.sensors.at(sensor_index); + auto sensor = hardware_info.sensors.at(sensor_idx); FTSensorData sensor_data; sensor_data.name = sensor.name; @@ -303,8 +368,8 @@ void MujocoSystem::register_sensors(const urdf::Model& urdf_model, const hardwar sensor_data.force.mj_sensor_index = mj_model_->sensor_adr[force_sensor_id]; sensor_data.torque.mj_sensor_index = mj_model_->sensor_adr[torque_sensor_id]; - ft_sensor_data_.at(sensor_index) = sensor_data; - auto& last_sensor_data = ft_sensor_data_.at(sensor_index); + ft_sensor_data_.at(sensor_idx) = sensor_data; + auto& last_sensor_data = ft_sensor_data_.at(sensor_idx); for (const auto& state_if : sensor.state_interfaces) { diff --git a/mujoco_ros2_control_demos/examples/example_diff_drive.cpp b/mujoco_ros2_control_demos/examples/example_diff_drive.cpp index 14bdc3a..e79b476 100644 --- a/mujoco_ros2_control_demos/examples/example_diff_drive.cpp +++ b/mujoco_ros2_control_demos/examples/example_diff_drive.cpp @@ -16,7 +16,7 @@ #include -#include +#include using namespace std::chrono_literals; @@ -27,22 +27,23 @@ int main(int argc, char * argv[]) std::shared_ptr node = std::make_shared("diff_drive_test_node"); - auto publisher = node->create_publisher( - "/diff_drive_base_controller/cmd_vel_unstamped", 10); + auto publisher = node->create_publisher( + "/diff_drive_base_controller/cmd_vel", 10); RCLCPP_INFO(node->get_logger(), "node created"); - geometry_msgs::msg::Twist command; + geometry_msgs::msg::TwistStamped command; - command.linear.x = 0.2; - command.linear.y = 0.0; - command.linear.z = 0.0; + command.twist.linear.x = 0.2; + command.twist.linear.y = 0.0; + command.twist.linear.z = 0.0; - command.angular.x = 0.0; - command.angular.y = 0.0; - command.angular.z = 0.1; + command.twist.angular.x = 0.0; + command.twist.angular.y = 0.0; + command.twist.angular.z = 0.1; while (1) { + command.header.stamp = node->now(); publisher->publish(command); std::this_thread::sleep_for(50ms); rclcpp::spin_some(node); diff --git a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml index 0de25db..72851f6 100644 --- a/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml +++ b/mujoco_ros2_control_demos/mujoco_models/test_diff_drive.xml @@ -11,6 +11,7 @@ + @@ -31,4 +32,10 @@ + + + + + + \ No newline at end of file diff --git a/mujoco_ros2_control_demos/urdf/test_diff_drive.xacro.urdf b/mujoco_ros2_control_demos/urdf/test_diff_drive.xacro.urdf index 48baaff..fa43dbb 100644 --- a/mujoco_ros2_control_demos/urdf/test_diff_drive.xacro.urdf +++ b/mujoco_ros2_control_demos/urdf/test_diff_drive.xacro.urdf @@ -1,5 +1,15 @@ + + + + + + + + + + @@ -146,5 +156,8 @@ + + IMU + \ No newline at end of file From 3c6abbd7c25d2db524b2c77661342c7b3ee211c6 Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Mon, 4 Nov 2024 15:34:41 +0800 Subject: [PATCH 5/9] [Format]: suggestions 1. return value in main function; 2. format in .gitignore; 3. modify the construction function of `MujocoRos2Control`. --- .gitignore | 4 +--- .../include/mujoco_ros2_control/mujoco_ros2_control.hpp | 6 +++--- mujoco_ros2_control/src/mujoco_ros2_control.cpp | 7 ++++--- mujoco_ros2_control/src/mujoco_ros2_control_node.cpp | 2 +- 4 files changed, 9 insertions(+), 10 deletions(-) diff --git a/.gitignore b/.gitignore index d582254..0063fd2 100644 --- a/.gitignore +++ b/.gitignore @@ -11,6 +11,4 @@ log/ **/.devcontainer # mujoco log -**/MUJOCO_LOG.TXT - - +**/MUJOCO_LOG.TXT \ No newline at end of file diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp index d921d15..6252267 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp @@ -18,7 +18,7 @@ class MJResourceManager; class MujocoRos2Control { public: - MujocoRos2Control(rclcpp::Node::SharedPtr & node, rclcpp::NodeOptions cm_node_option, mjModel* mujoco_model, mjData* mujoco_data); + MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option, mjModel* mujoco_model, mjData* mujoco_data); ~MujocoRos2Control(); void init(); void update(); @@ -26,9 +26,9 @@ class MujocoRos2Control private: void publish_sim_time(rclcpp::Time sim_time); rclcpp::Node::SharedPtr node_; - rclcpp::NodeOptions cm_node_option_; mjModel* mj_model_; mjData* mj_data_; + rclcpp::NodeOptions cm_node_option_; rclcpp::Logger logger_; std::shared_ptr> robot_hw_sim_loader_; @@ -36,7 +36,7 @@ class MujocoRos2Control std::shared_ptr controller_manager_; rclcpp::executors::MultiThreadedExecutor::SharedPtr cm_executor_; std::thread cm_thread_; - bool stop_cm_thread_; + bool stop_cm_thread_ = false; rclcpp::Duration control_period_; rclcpp::Time last_update_sim_time_ros_; diff --git a/mujoco_ros2_control/src/mujoco_ros2_control.cpp b/mujoco_ros2_control/src/mujoco_ros2_control.cpp index 0943f01..a0de40d 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control.cpp @@ -72,9 +72,10 @@ class MJResourceManager : public hardware_interface::ResourceManager{ mjData* mj_data_; }; -MujocoRos2Control::MujocoRos2Control(rclcpp::Node::SharedPtr & node, rclcpp::NodeOptions cm_node_option, mjModel* mujoco_model, mjData* mujoco_data) - : node_(node), mj_model_(mujoco_model), mj_data_(mujoco_data), logger_(rclcpp::get_logger(node_->get_name() + std::string(".mujoco_ros2_control"))), - control_period_(rclcpp::Duration(1, 0)), last_update_sim_time_ros_(0, 0, RCL_ROS_TIME), cm_node_option_(cm_node_option) +MujocoRos2Control::MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option, mjModel* mujoco_model, mjData* mujoco_data) + : node_(node), mj_model_(mujoco_model), mj_data_(mujoco_data), cm_node_option_(cm_node_option), + logger_(rclcpp::get_logger(node_->get_name() + std::string(".mujoco_ros2_control"))), + control_period_(rclcpp::Duration(1, 0)), last_update_sim_time_ros_(0, 0, RCL_ROS_TIME) { } diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index 4afd203..d955ec1 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -72,5 +72,5 @@ int main(int argc, const char** argv) { mj_deleteModel(mujoco_model); RCLCPP_INFO(node->get_logger(), "Mujoco Sim Stop ..."); - return 1; + return 0; } From d6334d23b65c8561a126f32e0a3724fa04428a2a Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Tue, 5 Nov 2024 12:51:49 +0800 Subject: [PATCH 6/9] [Feature]: about scope_exit and RAII 1. use destructor to handle the release of the rendering resource; 2. allocate the mj_data and mj_model in RM member method(not in constructor) and deallocate them in destructor. --- .../mujoco_ros2_control/mujoco_rendering.hpp | 1 + .../mujoco_ros2_control.hpp | 8 ++-- mujoco_ros2_control/src/mujoco_rendering.cpp | 5 +++ .../src/mujoco_ros2_control.cpp | 43 +++++++++++++++---- .../src/mujoco_ros2_control_node.cpp | 35 +++------------ 5 files changed, 51 insertions(+), 41 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp index 3bbfd56..51abffa 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_rendering.hpp @@ -12,6 +12,7 @@ class MujocoRendering public: MujocoRendering(const MujocoRendering& obj) = delete; void operator=(const MujocoRendering &) = delete; + ~MujocoRendering(); static MujocoRendering* get_instance(); void init(rclcpp::Node::SharedPtr & node, mjModel* mujoco_model, mjData* mujoco_data); diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp index 6252267..b94fc8b 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp @@ -18,16 +18,18 @@ class MJResourceManager; class MujocoRos2Control { public: - MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option, mjModel* mujoco_model, mjData* mujoco_data); + MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option); ~MujocoRos2Control(); void init(); void update(); + mjData* getMjData(); + mjModel* getMjModel(); private: void publish_sim_time(rclcpp::Time sim_time); rclcpp::Node::SharedPtr node_; - mjModel* mj_model_; - mjData* mj_data_; + mjModel* mj_model_ = nullptr; + mjData* mj_data_ = nullptr; rclcpp::NodeOptions cm_node_option_; rclcpp::Logger logger_; diff --git a/mujoco_ros2_control/src/mujoco_rendering.cpp b/mujoco_ros2_control/src/mujoco_rendering.cpp index 00f9dc1..619ea14 100644 --- a/mujoco_ros2_control/src/mujoco_rendering.cpp +++ b/mujoco_ros2_control/src/mujoco_rendering.cpp @@ -2,6 +2,11 @@ namespace mujoco_ros2_control { +MujocoRendering::~MujocoRendering() +{ + close(); +} + MujocoRendering* MujocoRendering::instance_ = nullptr; MujocoRendering* MujocoRendering::get_instance() diff --git a/mujoco_ros2_control/src/mujoco_ros2_control.cpp b/mujoco_ros2_control/src/mujoco_ros2_control.cpp index a0de40d..bd44346 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control.cpp @@ -8,17 +8,23 @@ namespace mujoco_ros2_control { class MJResourceManager : public hardware_interface::ResourceManager{ public: - MJResourceManager(rclcpp::Node::SharedPtr& node, mjModel* mj_model, mjData* mj_data) + MJResourceManager(rclcpp::Node::SharedPtr& node, mjModel*& mj_model, mjData*& mj_data) : hardware_interface::ResourceManager(node->get_node_clock_interface(), node->get_node_logging_interface()), mj_system_loader_("mujoco_ros2_control", "mujoco_ros2_control::MujocoSystemInterface"), - logger_(node->get_logger().get_child("MJResourceManager")) + logger_(node->get_logger().get_child("MJResourceManager")), mj_model_(mj_model), mj_data_(mj_data) { - mj_model_ = mj_model; - mj_data_ = mj_data; node_ = node; } + MJResourceManager(const MJResourceManager&) = delete; + ~MJResourceManager() override + { + // release resources when exit or failure + mj_deleteModel(mj_model_); + mj_deleteData(mj_data_); + } + /// Called from ControllerManager when {robot_description} is initialized from callback. /** * Override from hardware_interface::ResourceManager @@ -32,6 +38,22 @@ class MJResourceManager : public hardware_interface::ResourceManager{ const auto hardware_info = hardware_interface::parse_control_resources_from_urdf(urdf); + // generate mj_data_ and mj_model_ for SystemInterface + auto model_path = node_->get_parameter("mujoco_model_path").as_string(); + // load and compile model + char error[1000] = "Could not load binary model"; + if (std::strlen(model_path.c_str())>4 && !std::strcmp(model_path.c_str()+std::strlen(model_path.c_str())-4, ".mjb")) { + mj_model_ = mj_loadModel(model_path.c_str(), 0); + } else { + mj_model_ = mj_loadXML(model_path.c_str(), 0, error, 1000); + } + if (!mj_model_) { + mju_error("Load model error: %s", error); + return !components_are_loaded_and_initialized_; + } + RCLCPP_INFO_STREAM(logger_, "Mujoco model has been successfully loaded !"); + mj_data_ = mj_makeData(mj_model_); + for (const auto& individual_hardware_info : hardware_info) { std::string robot_hw_sim_type_str_ = individual_hardware_info.hardware_plugin_name; @@ -68,12 +90,12 @@ class MJResourceManager : public hardware_interface::ResourceManager{ std::shared_ptr node_; pluginlib::ClassLoader mj_system_loader_; rclcpp::Logger logger_; - mjModel* mj_model_; - mjData* mj_data_; + mjModel*& mj_model_; + mjData*& mj_data_; }; -MujocoRos2Control::MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option, mjModel* mujoco_model, mjData* mujoco_data) - : node_(node), mj_model_(mujoco_model), mj_data_(mujoco_data), cm_node_option_(cm_node_option), +MujocoRos2Control::MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option) + : node_(node), cm_node_option_(cm_node_option), logger_(rclcpp::get_logger(node_->get_name() + std::string(".mujoco_ros2_control"))), control_period_(rclcpp::Duration(1, 0)), last_update_sim_time_ros_(0, 0, RCL_ROS_TIME) { @@ -124,6 +146,7 @@ void MujocoRos2Control::init() cm_thread_ = std::thread(spin); // Waiting RM to be initialized through topic robot_description + // mj_data_ and mj_model_ will be allocated in the RM simultaneously while(!controller_manager_->is_resource_manager_initialized()) { RCLCPP_WARN(logger_, "Waiting RM to load and initialize hardware..."); @@ -154,6 +177,10 @@ void MujocoRos2Control::update() mj_step2(mj_model_, mj_data_); } +mjData* MujocoRos2Control::getMjData() {return mj_data_;} + +mjModel* MujocoRos2Control::getMjModel() {return mj_model_;} + void MujocoRos2Control::publish_sim_time(rclcpp::Time sim_time) { // TODO diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index d955ec1..df90b4f 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -5,10 +5,6 @@ #include "mujoco_ros2_control/mujoco_ros2_control.hpp" #include "mujoco_ros2_control/mujoco_rendering.hpp" -// MuJoCo data structures -mjModel* mujoco_model = nullptr; -mjData* mujoco_data = nullptr; - // main function int main(int argc, const char** argv) { rclcpp::init(argc, argv); @@ -24,30 +20,15 @@ int main(int argc, const char** argv) { } cm_node_options.arguments(node_arguments); - RCLCPP_INFO_STREAM(node->get_logger(), "Initializing mujoco_ros2_control node..."); - auto model_path = node->get_parameter("mujoco_model_path").as_string(); - - // load and compile model - char error[1000] = "Could not load binary model"; - if (std::strlen(model_path.c_str())>4 && !std::strcmp(model_path.c_str()+std::strlen(model_path.c_str())-4, ".mjb")) { - mujoco_model = mj_loadModel(model_path.c_str(), 0); - } else { - mujoco_model = mj_loadXML(model_path.c_str(), 0, error, 1000); - } - if (!mujoco_model) { - mju_error("Load model error: %s", error); - } - - RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco model has been successfully loaded !"); - // make data - mujoco_data = mj_makeData(mujoco_model); - // initialize mujoco control - auto control = mujoco_ros2_control::MujocoRos2Control(node, cm_node_options, mujoco_model, mujoco_data); + RCLCPP_INFO_STREAM(node->get_logger(), "Initializing mujoco_ros2_control node..."); + auto control = mujoco_ros2_control::MujocoRos2Control(node, cm_node_options); control.init(); RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco ros2 controller has been successfully initialized !"); - // initialize mujoco redering + // initialize mujoco rendering + mjData* mujoco_data = control.getMjData(); + mjModel* mujoco_model = control.getMjModel(); auto rendering = mujoco_ros2_control::MujocoRendering::get_instance(); rendering->init(node, mujoco_model, mujoco_data); RCLCPP_INFO_STREAM(node->get_logger(), "Mujoco rendering has been successfully initialized !"); @@ -65,12 +46,6 @@ int main(int argc, const char** argv) { rendering->update(); } - rendering->close(); - - // free MuJoCo model and data - mj_deleteData(mujoco_data); - mj_deleteModel(mujoco_model); RCLCPP_INFO(node->get_logger(), "Mujoco Sim Stop ..."); - return 0; } From e03076098cacb7d4f33ae90403b0c0db26fbf909 Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Wed, 6 Nov 2024 23:32:17 +0800 Subject: [PATCH 7/9] [Feature]: rewrite sensors 1. rewrite the sensors, they are now inherited from SensorBase class; 2. fix the bugs of FTSensor example. --- mujoco_ros2_control/CMakeLists.txt | 2 +- .../mujoco_ros2_control/mujoco_sensor.hpp | 65 +++++++++ .../mujoco_ros2_control/mujoco_system.hpp | 29 +--- mujoco_ros2_control/src/mujoco_sensor.cpp | 126 ++++++++++++++++++ mujoco_ros2_control/src/mujoco_system.cpp | 123 +++-------------- .../urdf/test_ft_sensor.xacro.urdf | 1 + 6 files changed, 213 insertions(+), 133 deletions(-) create mode 100644 mujoco_ros2_control/include/mujoco_ros2_control/mujoco_sensor.hpp create mode 100644 mujoco_ros2_control/src/mujoco_sensor.cpp diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index 51158d5..cb1c670 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -49,7 +49,7 @@ endif (mujoco_FOUND) include_directories(include) -add_library(mujoco_system_plugins SHARED src/mujoco_system.cpp) +add_library(mujoco_system_plugins SHARED src/mujoco_system.cpp src/mujoco_sensor.cpp) ament_target_dependencies(mujoco_system_plugins ${THIS_PACKAGE_DEPENDS}) target_link_libraries(mujoco_system_plugins ${MUJOCO_LIB}) target_include_directories(mujoco_system_plugins PUBLIC ${MUJOCO_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR}) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_sensor.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_sensor.hpp new file mode 100644 index 0000000..32f0a69 --- /dev/null +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_sensor.hpp @@ -0,0 +1,65 @@ +#ifndef MUJOCO_ROS2_CONTROL_MUJOCO_SENSOR_HPP +#define MUJOCO_ROS2_CONTROL_MUJOCO_SENSOR_HPP + +#include +#include +#include +#include "hardware_interface/system_interface.hpp" +#include "mujoco/mujoco.h" + +namespace mujoco_ros2_control +{ +template +struct SensorData +{ + std::string name; + T data; + int mj_sensor_index; +}; + +struct FTSensorData +{ + std::string name; + SensorData force; + SensorData torque; +}; + +class SensorBase{ +public: + std::string name_; + + SensorBase() = default; + + virtual bool init(const hardware_interface::ComponentInfo& sensor_info, const mjModel* mj_model) = 0; + virtual void registerStateIface(std::vector& state_interface) = 0; + virtual void read(const mjData* mj_data) = 0; +}; + +class IMUSensor final : public SensorBase{ +public: + IMUSensor() : SensorBase(){}; + bool init(const hardware_interface::ComponentInfo& sensor_info, const mjModel* mj_model) override; + void registerStateIface(std::vector& state_interface) override; + void read(const mjData* mj_data) override; +private: + SensorData> orientation; + SensorData angular_velocity; + SensorData linear_velocity; + SensorData linear_acceleration; +}; + +class FTSensor final : public SensorBase{ +public: + FTSensor() : SensorBase(){}; + bool init(const hardware_interface::ComponentInfo& sensor_info, const mjModel* mj_model) override; + void registerStateIface(std::vector& state_interface) override; + void read(const mjData* mj_data) override; +private: + SensorData force; + SensorData torque; + std::vector iface_lists; +}; + +} // namespace mujoco_ros2_control + +#endif // MUJOCO_ROS2_CONTROL_MUJOCO_SENSOR_HPP diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp index 6f21f11..2b3839b 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system.hpp @@ -3,6 +3,7 @@ #include #include "mujoco_ros2_control/mujoco_system_interface.hpp" +#include "mujoco_ros2_control/mujoco_sensor.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_limits/joint_limits.hpp" #include "control_toolbox/pid.hpp" @@ -58,30 +59,6 @@ class MujocoSystem : public MujocoSystemInterface int mj_vel_adr; }; - template - struct SensorData - { - std::string name; - T data; - int mj_sensor_index; - }; - - struct FTSensorData - { - std::string name; - SensorData force; - SensorData torque; - }; - - struct IMUSensorData - { - std::string name; - SensorData> orientation; - SensorData angular_velocity; - SensorData linear_velocity; - SensorData linear_acceleration; - }; - private: void register_joints(const urdf::Model& urdf_model, const hardware_interface::HardwareInfo & hardware_info); void register_sensors(const urdf::Model& urdf_model, const hardware_interface::HardwareInfo & hardware_info); @@ -97,8 +74,8 @@ class MujocoSystem : public MujocoSystemInterface std::vector command_interfaces_; std::vector joint_states_; - std::vector ft_sensor_data_; - std::vector imu_sensor_data_; + std::vector imu_sensors_; + std::vector ft_sensors_; mjModel* mj_model_; mjData* mj_data_; diff --git a/mujoco_ros2_control/src/mujoco_sensor.cpp b/mujoco_ros2_control/src/mujoco_sensor.cpp new file mode 100644 index 0000000..f3beb21 --- /dev/null +++ b/mujoco_ros2_control/src/mujoco_sensor.cpp @@ -0,0 +1,126 @@ +#include "mujoco_ros2_control/mujoco_sensor.hpp" +#include + +namespace mujoco_ros2_control +{ +bool IMUSensor::init(const hardware_interface::ComponentInfo& sensor_info, const mjModel* mj_model) +{ + name_ = sensor_info.name; + angular_velocity.name = name_ + "_Gyro"; + orientation.name = name_ + "_Framequat"; + linear_velocity.name = name_ + "_Velocimeter"; + linear_acceleration.name = name_ + "_Accelerometer"; + + int angular_vel_id = mj_name2id(mj_model, mjtObj::mjOBJ_SENSOR, angular_velocity.name.c_str()); + int orientation_id = mj_name2id(mj_model, mjtObj::mjOBJ_SENSOR, orientation.name.c_str()); + int linear_acc_id = mj_name2id(mj_model, mjtObj::mjOBJ_SENSOR, linear_acceleration.name.c_str()); + int linear_vel_id = mj_name2id(mj_model, mjtObj::mjOBJ_SENSOR, linear_velocity.name.c_str()); + if (angular_vel_id == -1 || orientation_id == -1 || linear_acc_id == -1 || linear_vel_id == -1) + return false; + + angular_velocity.mj_sensor_index = mj_model->sensor_adr[angular_vel_id]; + orientation.mj_sensor_index = mj_model->sensor_adr[orientation_id]; + linear_velocity.mj_sensor_index = mj_model->sensor_adr[linear_vel_id]; + linear_acceleration.mj_sensor_index = mj_model->sensor_adr[linear_acc_id]; + return true; +} + +void IMUSensor::registerStateIface(std::vector& state_interfaces) +{ + state_interfaces.emplace_back(name_, "orientation.w", &orientation.data.w()); + state_interfaces.emplace_back(name_, "orientation.x", &orientation.data.x()); + state_interfaces.emplace_back(name_, "orientation.y", &orientation.data.y()); + state_interfaces.emplace_back(name_, "orientation.z", &orientation.data.z()); + state_interfaces.emplace_back(name_, "angular_velocity.x", &angular_velocity.data.x()); + state_interfaces.emplace_back(name_, "angular_velocity.y", &angular_velocity.data.y()); + state_interfaces.emplace_back(name_, "angular_velocity.z", &angular_velocity.data.z()); + state_interfaces.emplace_back(name_, "linear_velocity.x", &linear_velocity.data.x()); + state_interfaces.emplace_back(name_, "linear_velocity.y", &linear_velocity.data.y()); + state_interfaces.emplace_back(name_, "linear_velocity.z", &linear_velocity.data.z()); + state_interfaces.emplace_back(name_, "linear_acceleration.x", &linear_acceleration.data.x()); + state_interfaces.emplace_back(name_, "linear_acceleration.y", &linear_acceleration.data.y()); + state_interfaces.emplace_back(name_, "linear_acceleration.z", &linear_acceleration.data.z()); +} + +void IMUSensor::read(const mjData* mj_data) +{ + std::vector*> ptr_vec{&angular_velocity, + &linear_velocity, + &linear_acceleration}; + orientation.data.w() = mj_data->sensordata[orientation.mj_sensor_index]; + orientation.data.x() = mj_data->sensordata[orientation.mj_sensor_index + 1]; + orientation.data.y() = mj_data->sensordata[orientation.mj_sensor_index + 2]; + orientation.data.z() = mj_data->sensordata[orientation.mj_sensor_index + 3]; + for (int i = 0; i < 3; ++i) + { + ptr_vec[i]->data.x() = mj_data->sensordata[ptr_vec[i]->mj_sensor_index]; + ptr_vec[i]->data.y() = mj_data->sensordata[ptr_vec[i]->mj_sensor_index + 1]; + ptr_vec[i]->data.z() = mj_data->sensordata[ptr_vec[i]->mj_sensor_index + 2]; + } +} + +bool FTSensor::init(const hardware_interface::ComponentInfo &sensor_info, const mjModel *mj_model) +{ + name_ = sensor_info.name; + force.name = name_ + "_force"; + torque.name = name_ + "_torque"; + + int force_sensor_id = mj_name2id(mj_model, mjtObj::mjOBJ_SENSOR, force.name.c_str()); + int torque_sensor_id = mj_name2id(mj_model, mjtObj::mjOBJ_SENSOR, torque.name.c_str()); + if (force_sensor_id == -1 || torque_sensor_id == -1) + return false; + + force.mj_sensor_index = mj_model->sensor_adr[force_sensor_id]; + torque.mj_sensor_index = mj_model->sensor_adr[torque_sensor_id]; + + size_t iface_size = sensor_info.state_interfaces.size(); + iface_lists.resize(iface_size); + for (size_t idx = 0; idx < iface_size; ++idx) + iface_lists[idx] = sensor_info.state_interfaces[idx].name; + + return true; +} + +void FTSensor::registerStateIface(std::vector& state_interfaces) +{ + for (const auto& iface_name : iface_lists) + { + if (iface_name == "force.x") + { + state_interfaces.emplace_back(name_, iface_name, &force.data.x()); + } + else if (iface_name == "force.y") + { + state_interfaces.emplace_back(name_, iface_name, &force.data.y()); + } + else if (iface_name == "force.z") + { + state_interfaces.emplace_back(name_, iface_name, &force.data.z()); + } + else if (iface_name == "torque.x") + { + state_interfaces.emplace_back(name_, iface_name, &torque.data.x()); + } + else if (iface_name == "torque.y") + { + state_interfaces.emplace_back(name_, iface_name, &torque.data.y()); + } + else if (iface_name == "torque.z") + { + state_interfaces.emplace_back(name_, iface_name, &torque.data.z()); + } + } +} + +void FTSensor::read(const mjData* mj_data) +{ + force.data.x() = -mj_data->sensordata[force.mj_sensor_index]; + force.data.y() = -mj_data->sensordata[force.mj_sensor_index + 1]; + force.data.z() = -mj_data->sensordata[force.mj_sensor_index + 2]; + + torque.data.x() = -mj_data->sensordata[torque.mj_sensor_index]; + torque.data.y() = -mj_data->sensordata[torque.mj_sensor_index + 1]; + torque.data.z() = -mj_data->sensordata[torque.mj_sensor_index + 2]; +} + +} // namespace mujoco_ros2_control diff --git a/mujoco_ros2_control/src/mujoco_system.cpp b/mujoco_ros2_control/src/mujoco_system.cpp index 668f87d..7cf1722 100644 --- a/mujoco_ros2_control/src/mujoco_system.cpp +++ b/mujoco_ros2_control/src/mujoco_system.cpp @@ -27,34 +27,13 @@ hardware_interface::return_type MujocoSystem::read(const rclcpp::Time & time, co } // IMU Sensor data - for (auto& data : imu_sensor_data_) - { - std::vector*> ptr_vec{&data.angular_velocity, - &data.linear_velocity, - &data.linear_acceleration}; - data.orientation.data.w() = mj_data_->sensordata[data.orientation.mj_sensor_index]; - data.orientation.data.x() = mj_data_->sensordata[data.orientation.mj_sensor_index + 1]; - data.orientation.data.y() = mj_data_->sensordata[data.orientation.mj_sensor_index + 2]; - data.orientation.data.z() = mj_data_->sensordata[data.orientation.mj_sensor_index + 3]; - for (int i = 0; i < 3; ++i) - { - ptr_vec[i]->data.x() = mj_data_->sensordata[ptr_vec[i]->mj_sensor_index]; - ptr_vec[i]->data.y() = mj_data_->sensordata[ptr_vec[i]->mj_sensor_index + 1]; - ptr_vec[i]->data.z() = mj_data_->sensordata[ptr_vec[i]->mj_sensor_index + 2]; - } - } + for (auto& data : imu_sensors_) + data.read(mj_data_); // FT Sensor data - for (auto& data : ft_sensor_data_) - { - data.force.data.x() = -mj_data_->sensordata[data.force.mj_sensor_index]; - data.force.data.y() = -mj_data_->sensordata[data.force.mj_sensor_index + 1]; - data.force.data.z() = -mj_data_->sensordata[data.force.mj_sensor_index + 2]; + for (auto& data : ft_sensors_) + data.read(mj_data_); - data.torque.data.x() = -mj_data_->sensordata[data.torque.mj_sensor_index]; - data.torque.data.y() = -mj_data_->sensordata[data.torque.mj_sensor_index + 1]; - data.torque.data.z() = -mj_data_->sensordata[data.torque.mj_sensor_index + 2]; - } return hardware_interface::return_type::OK; } @@ -291,7 +270,7 @@ void MujocoSystem::register_joints(const urdf::Model& urdf_model, const hardware void MujocoSystem::register_sensors(const urdf::Model& urdf_model, const hardware_interface::HardwareInfo & hardware_info) { - // count the number of different sensor + // count the number of different sensors and store the hardware_info index std::vector ft_idx{}, imu_idx{}; for(size_t info_idx = 0; info_idx < hardware_info.sensors.size(); ++info_idx) { @@ -305,99 +284,31 @@ void MujocoSystem::register_sensors(const urdf::Model& urdf_model, const hardwar if(type == "IMU") imu_idx.push_back(int(info_idx)); else if(type == "FTSensor") ft_idx.push_back(int(info_idx)); } - ft_sensor_data_.resize(ft_idx.size()); - imu_sensor_data_.resize(imu_idx.size()); + ft_sensors_.resize(ft_idx.size()); + imu_sensors_.resize(imu_idx.size()); for(size_t sensor_idx = 0; sensor_idx < imu_idx.size(); ++sensor_idx) { - auto sensor_info = hardware_info.sensors[imu_idx[sensor_idx]]; - IMUSensorData& imu_data = imu_sensor_data_[sensor_idx]; - imu_data.name = sensor_info.name; - imu_data.angular_velocity.name = imu_data.name + "_Gyro"; - imu_data.orientation.name = imu_data.name + "_Framequat"; - imu_data.linear_velocity.name = imu_data.name + "_Velocimeter"; - imu_data.linear_acceleration.name = imu_data.name + "_Accelerometer"; - - int angular_vel_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, imu_data.angular_velocity.name.c_str()); - int orientation_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, imu_data.orientation.name.c_str()); - int linear_acc_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, imu_data.linear_acceleration.name.c_str()); - int linear_vel_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, imu_data.linear_velocity.name.c_str()); - if (angular_vel_id == -1 || orientation_id == -1 || linear_acc_id == -1 || linear_vel_id == -1) + const auto& sensor_info = hardware_info.sensors[imu_idx[sensor_idx]]; + IMUSensor& sensor = imu_sensors_[sensor_idx]; + if(!sensor.init(sensor_info, mj_model_)) { - RCLCPP_ERROR_STREAM(logger_, "IMU sensor lack of sub-sensor, check the MJCF for IMU sensor: " << imu_data.name); + RCLCPP_ERROR_STREAM(logger_, "IMU sensor lack of sub-sensor, check the MJCF for IMU sensor: " << sensor_info.name); continue; } - imu_data.angular_velocity.mj_sensor_index = mj_model_->sensor_adr[angular_vel_id]; - imu_data.orientation.mj_sensor_index = mj_model_->sensor_adr[orientation_id]; - imu_data.linear_velocity.mj_sensor_index = mj_model_->sensor_adr[linear_vel_id]; - imu_data.linear_acceleration.mj_sensor_index = mj_model_->sensor_adr[linear_acc_id]; - - state_interfaces_.emplace_back(imu_data.name, "orientation.w", &imu_data.orientation.data.w()); - state_interfaces_.emplace_back(imu_data.name, "orientation.x", &imu_data.orientation.data.x()); - state_interfaces_.emplace_back(imu_data.name, "orientation.y", &imu_data.orientation.data.y()); - state_interfaces_.emplace_back(imu_data.name, "orientation.z", &imu_data.orientation.data.z()); - state_interfaces_.emplace_back(imu_data.name, "angular_velocity.x", &imu_data.angular_velocity.data.x()); - state_interfaces_.emplace_back(imu_data.name, "angular_velocity.y", &imu_data.angular_velocity.data.y()); - state_interfaces_.emplace_back(imu_data.name, "angular_velocity.z", &imu_data.angular_velocity.data.z()); - state_interfaces_.emplace_back(imu_data.name, "linear_velocity.x", &imu_data.linear_velocity.data.x()); - state_interfaces_.emplace_back(imu_data.name, "linear_velocity.y", &imu_data.linear_velocity.data.y()); - state_interfaces_.emplace_back(imu_data.name, "linear_velocity.z", &imu_data.linear_velocity.data.z()); - state_interfaces_.emplace_back(imu_data.name, "linear_acceleration.x", &imu_data.linear_acceleration.data.x()); - state_interfaces_.emplace_back(imu_data.name, "linear_acceleration.y", &imu_data.linear_acceleration.data.y()); - state_interfaces_.emplace_back(imu_data.name, "linear_acceleration.z", &imu_data.linear_acceleration.data.z()); + sensor.registerStateIface(state_interfaces_); } for (size_t sensor_idx = 0; sensor_idx < ft_idx.size(); ++sensor_idx) { - auto sensor = hardware_info.sensors.at(sensor_idx); - - FTSensorData sensor_data; - sensor_data.name = sensor.name; - sensor_data.force.name = sensor.name + "_force"; - sensor_data.torque.name = sensor.name + "_torque"; - - int force_sensor_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, sensor_data.force.name.c_str()); - int torque_sensor_id = mj_name2id(mj_model_, mjtObj::mjOBJ_SENSOR, sensor_data.torque.name.c_str()); - - if (force_sensor_id == -1 || torque_sensor_id == -1) + const auto& sensor_info = hardware_info.sensors[ft_idx[sensor_idx]]; + FTSensor& sensor = ft_sensors_[sensor_idx]; + if(!sensor.init(sensor_info, mj_model_)) { - RCLCPP_ERROR_STREAM(logger_, "Failed to find sensor in mujoco model, sensor name: " << sensor.name); + RCLCPP_ERROR_STREAM(logger_, "Failed to find sensor in mujoco model, sensor name: " << sensor_info.name); continue; } - - sensor_data.force.mj_sensor_index = mj_model_->sensor_adr[force_sensor_id]; - sensor_data.torque.mj_sensor_index = mj_model_->sensor_adr[torque_sensor_id]; - - ft_sensor_data_.at(sensor_idx) = sensor_data; - auto& last_sensor_data = ft_sensor_data_.at(sensor_idx); - - for (const auto& state_if : sensor.state_interfaces) - { - if (state_if.name == "force.x") - { - state_interfaces_.emplace_back(sensor.name, state_if.name, &last_sensor_data.force.data.x()); - } - else if (state_if.name == "force.y") - { - state_interfaces_.emplace_back(sensor.name, state_if.name, &last_sensor_data.force.data.y()); - } - else if (state_if.name == "force.z") - { - state_interfaces_.emplace_back(sensor.name, state_if.name, &last_sensor_data.force.data.z()); - } - else if (state_if.name == "torque.x") - { - state_interfaces_.emplace_back(sensor.name, state_if.name, &last_sensor_data.torque.data.x()); - } - else if (state_if.name == "torque.y") - { - state_interfaces_.emplace_back(sensor.name, state_if.name, &last_sensor_data.torque.data.y()); - } - else if (state_if.name == "torque.z") - { - state_interfaces_.emplace_back(sensor.name, state_if.name, &last_sensor_data.torque.data.z()); - } - } + sensor.registerStateIface(state_interfaces_); } } diff --git a/mujoco_ros2_control_demos/urdf/test_ft_sensor.xacro.urdf b/mujoco_ros2_control_demos/urdf/test_ft_sensor.xacro.urdf index 40b4010..6c4c336 100644 --- a/mujoco_ros2_control_demos/urdf/test_ft_sensor.xacro.urdf +++ b/mujoco_ros2_control_demos/urdf/test_ft_sensor.xacro.urdf @@ -78,6 +78,7 @@ + FTSensor From 3b62b1b9d5b5ddb0bbdfe7daabef21e77851a3ab Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Wed, 22 Jan 2025 18:31:28 +0800 Subject: [PATCH 8/9] [Bugs]: fix the error fix the error after closing the mujoco window --- mujoco_ros2_control/CMakeLists.txt | 5 +++- .../src/mujoco_ros2_control.cpp | 23 ++++++++++--------- .../src/mujoco_ros2_control_node.cpp | 1 + 3 files changed, 17 insertions(+), 12 deletions(-) diff --git a/mujoco_ros2_control/CMakeLists.txt b/mujoco_ros2_control/CMakeLists.txt index cb1c670..7eaddbb 100644 --- a/mujoco_ros2_control/CMakeLists.txt +++ b/mujoco_ros2_control/CMakeLists.txt @@ -62,7 +62,10 @@ install(TARGETS ) # TODO: make it simple -add_executable(mujoco_ros2_control src/mujoco_ros2_control_node.cpp src/mujoco_rendering.cpp src/mujoco_ros2_control.cpp) +add_executable(mujoco_ros2_control + src/mujoco_ros2_control_node.cpp src/mujoco_rendering.cpp + src/mujoco_ros2_control.cpp src/mujoco_system.cpp + src/mujoco_sensor.cpp) ament_target_dependencies(mujoco_ros2_control ${THIS_PACKAGE_DEPENDS}) target_link_libraries(mujoco_ros2_control ${MUJOCO_LIB} glfw) target_include_directories(mujoco_ros2_control PUBLIC ${MUJOCO_INCLUDE_DIR} ${EIGEN3_INCLUDE_DIR}) diff --git a/mujoco_ros2_control/src/mujoco_ros2_control.cpp b/mujoco_ros2_control/src/mujoco_ros2_control.cpp index bd44346..dafd278 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control.cpp @@ -10,7 +10,7 @@ class MJResourceManager : public hardware_interface::ResourceManager{ public: MJResourceManager(rclcpp::Node::SharedPtr& node, mjModel*& mj_model, mjData*& mj_data) : hardware_interface::ResourceManager(node->get_node_clock_interface(), node->get_node_logging_interface()), - mj_system_loader_("mujoco_ros2_control", "mujoco_ros2_control::MujocoSystemInterface"), + //mj_system_loader_("mujoco_ros2_control", "mujoco_ros2_control::MujocoSystemInterface"), logger_(node->get_logger().get_child("MJResourceManager")), mj_model_(mj_model), mj_data_(mj_data) { node_ = node; @@ -62,15 +62,16 @@ class MJResourceManager : public hardware_interface::ResourceManager{ // Load hardware std::unique_ptr mjSimSystem; std::scoped_lock guard(resource_interfaces_lock_, claimed_command_interfaces_lock_); - try - { - mjSimSystem = std::unique_ptr( - mj_system_loader_.createUnmanagedInstance(robot_hw_sim_type_str_)); - } catch (pluginlib::PluginlibException & ex) { - RCLCPP_ERROR_STREAM(logger_, "The plugin failed to load. Error: " << ex.what()); - continue; - } - +// try +// { +// mjSimSystem = std::unique_ptr( +// mj_system_loader_.createUnmanagedInstance(robot_hw_sim_type_str_)); +// } catch (pluginlib::PluginlibException & ex) { +// RCLCPP_ERROR_STREAM(logger_, "The plugin failed to load. Error: " << ex.what()); +// continue; +// } + std::unique_ptr mjSystemIface = std::make_unique(); + mjSimSystem = std::move(mjSystemIface); // initialize simulation required resource from the hardware info. urdf::Model urdf_model; urdf_model.initString(urdf); @@ -88,7 +89,7 @@ class MJResourceManager : public hardware_interface::ResourceManager{ private: std::shared_ptr node_; - pluginlib::ClassLoader mj_system_loader_; + //pluginlib::ClassLoader mj_system_loader_; rclcpp::Logger logger_; mjModel*& mj_model_; mjData*& mj_data_; diff --git a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp index df90b4f..79a0aa7 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control_node.cpp @@ -47,5 +47,6 @@ int main(int argc, const char** argv) { } RCLCPP_INFO(node->get_logger(), "Mujoco Sim Stop ..."); + rclcpp::shutdown(); return 0; } From b7c05e1e406f02790e6f11f2a59ed74f82f4c860 Mon Sep 17 00:00:00 2001 From: LitheshSari <1513503320@qq.com> Date: Wed, 22 Jan 2025 20:46:17 +0800 Subject: [PATCH 9/9] [Bug]: modify the initializer of MujocoRos2Control --- .../mujoco_ros2_control/mujoco_ros2_control.hpp | 4 ++-- mujoco_ros2_control/src/mujoco_ros2_control.cpp | 13 +++++++++++-- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp index b94fc8b..b14e747 100644 --- a/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp +++ b/mujoco_ros2_control/include/mujoco_ros2_control/mujoco_ros2_control.hpp @@ -28,9 +28,9 @@ class MujocoRos2Control private: void publish_sim_time(rclcpp::Time sim_time); rclcpp::Node::SharedPtr node_; - mjModel* mj_model_ = nullptr; - mjData* mj_data_ = nullptr; rclcpp::NodeOptions cm_node_option_; + mjModel* mj_model_; + mjData* mj_data_; rclcpp::Logger logger_; std::shared_ptr> robot_hw_sim_loader_; diff --git a/mujoco_ros2_control/src/mujoco_ros2_control.cpp b/mujoco_ros2_control/src/mujoco_ros2_control.cpp index dafd278..7003774 100644 --- a/mujoco_ros2_control/src/mujoco_ros2_control.cpp +++ b/mujoco_ros2_control/src/mujoco_ros2_control.cpp @@ -21,8 +21,16 @@ class MJResourceManager : public hardware_interface::ResourceManager{ ~MJResourceManager() override { // release resources when exit or failure - mj_deleteModel(mj_model_); - mj_deleteData(mj_data_); + if (mj_data_) + { + mj_deleteData(mj_data_); + mj_data_ = nullptr; + } + if (mj_model_) + { + mj_deleteModel(mj_model_); + mj_model_ = nullptr; + } } /// Called from ControllerManager when {robot_description} is initialized from callback. @@ -97,6 +105,7 @@ class MJResourceManager : public hardware_interface::ResourceManager{ MujocoRos2Control::MujocoRos2Control(const rclcpp::Node::SharedPtr & node, const rclcpp::NodeOptions & cm_node_option) : node_(node), cm_node_option_(cm_node_option), + mj_model_(nullptr), mj_data_(nullptr), logger_(rclcpp::get_logger(node_->get_name() + std::string(".mujoco_ros2_control"))), control_period_(rclcpp::Duration(1, 0)), last_update_sim_time_ros_(0, 0, RCL_ROS_TIME) {