Sync feature/isaac-deploy from internal fork

mujoco_ros2_control/CMakeLists.txt

@@ -16,7 +16,9 @@ find_package(pluginlib REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(rclcpp_lifecycle REQUIRED)
 find_package(Threads REQUIRED)
+find_package(tf2_ros REQUIRED)
 find_package(transmission_interface REQUIRED)
+find_package(std_srvs REQUIRED)
 find_package(backward_ros REQUIRED)
 
 set(CMAKE_INSTALL_RPATH_USE_LINK_PATH TRUE)
@@ -166,6 +168,7 @@ set_target_properties(mujoco_ros2_control PROPERTIES
 target_link_libraries(mujoco_ros2_control
   ${MUJOCO_LIB}
   ${sensor_msgs_TARGETS}
+  ${std_srvs_TARGETS}
   mujoco::libsimulate
   controller_manager::controller_manager
   hardware_interface::hardware_interface
@@ -176,6 +179,7 @@ target_link_libraries(mujoco_ros2_control
   control_toolbox::control_toolbox
   ${nav_msgs_TARGETS}
   ${sensor_msgs_TARGETS}
+  tf2_ros::tf2_ros
   transmission_interface::transmission_interface
   Eigen3::Eigen
   Threads::Threads

mujoco_ros2_control/include/mujoco_ros2_control/data.hpp

@@ -89,12 +89,18 @@ struct InterfaceData
  * @param has_pos_pid Boolean flag indicating if a position PID controller is configured.
  * @param has_vel_pid Boolean flag indicating if a velocity PID controller is configured.
  */
+// Custom interface names for kp/kd gains (matching ros2_control pattern)
+constexpr char HW_IF_KP[] = "kp";
+constexpr char HW_IF_KD[] = "kd";
+
 struct MuJoCoActuatorData
 {
   std::string joint_name = "";
   InterfaceData position_interface{ hardware_interface::HW_IF_POSITION };
   InterfaceData velocity_interface{ hardware_interface::HW_IF_VELOCITY };
   InterfaceData effort_interface{ hardware_interface::HW_IF_EFFORT };
+  InterfaceData kp_interface{ HW_IF_KP };
+  InterfaceData kd_interface{ HW_IF_KD };
   std::shared_ptr<control_toolbox::PidROS> pos_pid{ nullptr };
   std::shared_ptr<control_toolbox::PidROS> vel_pid{ nullptr };
   ActuatorType actuator_type{ ActuatorType::UNKNOWN };
@@ -110,6 +116,7 @@ struct MuJoCoActuatorData
   bool is_velocity_pid_control_enabled{ false };
   bool is_velocity_control_enabled{ false };
   bool is_effort_control_enabled{ false };
+  bool is_impedance_control_enabled{ false };  // Uses commanded kp/kd for PD control
   bool has_pos_pid{ false };
   bool has_vel_pid{ false };
 
@@ -118,13 +125,15 @@ struct MuJoCoActuatorData
     position_interface.transmission_passthrough_ = position_interface.state_;
     velocity_interface.transmission_passthrough_ = velocity_interface.state_;
     effort_interface.transmission_passthrough_ = effort_interface.state_;
+    // kp/kd don't have state, only command
   }
 
   void copy_command_from_transmission()
   {
     position_interface.command_ = position_interface.transmission_passthrough_;
     velocity_interface.command_ = velocity_interface.transmission_passthrough_;
     effort_interface.command_ = effort_interface.transmission_passthrough_;
+    // kp/kd commands are set directly, not through transmissions
   }
 
   void copy_command_to_state()
@@ -155,6 +164,8 @@ struct URDFJointData
   InterfaceData position_interface{ hardware_interface::HW_IF_POSITION };
   InterfaceData velocity_interface{ hardware_interface::HW_IF_VELOCITY };
   InterfaceData effort_interface{ hardware_interface::HW_IF_EFFORT };
+  InterfaceData kp_interface{ HW_IF_KP };
+  InterfaceData kd_interface{ HW_IF_KD };
 
   std::vector<std::string> command_interfaces = {};
 
@@ -165,6 +176,7 @@ struct URDFJointData
   bool is_position_control_enabled{ false };
   bool is_velocity_control_enabled{ false };
   bool is_effort_control_enabled{ false };
+  bool is_impedance_control_enabled{ false };  // Uses commanded kp/kd for PD control
 
   void copy_state_from_transmission()
   {

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_lidar.hpp

@@ -116,12 +116,7 @@ class MujocoLidar
   // LaserScan publishing rate in Hz
   double lidar_publish_rate_;
 
-  // Rendering options for the cameras, currently hard coded to defaults
-  mjvOption mjv_opt_;
-  mjvScene mjv_scn_;
-  mjrContext mjr_con_;
-
-  // Containers for ladar data and ROS constructs
+  // Containers for lidar data and ROS constructs
   std::vector<LidarData> lidar_sensors_;
 
   // Lidar processing thread

mujoco_ros2_control/include/mujoco_ros2_control/mujoco_system_interface.hpp

@@ -30,14 +30,17 @@
 #include <hardware_interface/hardware_info.hpp>
 #include <hardware_interface/system_interface.hpp>
 #include <hardware_interface/types/hardware_interface_return_values.hpp>
+#include <geometry_msgs/msg/transform_stamped.hpp>
 #include <nav_msgs/msg/odometry.hpp>
 #include <rclcpp/macros.hpp>
+#include <tf2_ros/transform_broadcaster.hpp>
 #include <rclcpp/rclcpp.hpp>
 #include <rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp>
 #include <rclcpp_lifecycle/state.hpp>
 #include <realtime_tools/realtime_publisher.hpp>
 #include <rosgraph_msgs/msg/clock.hpp>
 #include <sensor_msgs/msg/joint_state.hpp>
+#include <std_srvs/srv/trigger.hpp>
 
 #include <mujoco/mujoco.h>
 
@@ -243,6 +246,13 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
    */
   void set_initial_pose();
 
+  /**
+   * @brief Reset the simulation to initial state.
+   *
+   * Resets velocities, accelerations, and time to zero, then restores initial positions.
+   */
+  void reset_simulation();
+
   /**
    * @brief Spins the physics simulation for the Simulate Application
    */
@@ -295,6 +305,14 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
   std::shared_ptr<rclcpp::Publisher<rosgraph_msgs::msg::Clock>> clock_publisher_;
   realtime_tools::RealtimePublisher<rosgraph_msgs::msg::Clock>::SharedPtr clock_realtime_publisher_;
 
+  // Accumulated time offset to keep published clock monotonic across sim resets.
+  // mj_resetData sets mj_data_->time to 0; we add this offset so /clock never
+  // goes backward.
+  double clock_offset_{0.0};
+
+  // Reset service to reset simulation to initial state
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr reset_service_;
+
   // Actuators state publisher
   std::shared_ptr<rclcpp::Publisher<sensor_msgs::msg::JointState>> actuator_state_publisher_ = nullptr;
   realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>::SharedPtr actuator_state_realtime_publisher_ =
@@ -306,6 +324,11 @@ class MujocoSystemInterface : public hardware_interface::SystemInterface
   realtime_tools::RealtimePublisher<nav_msgs::msg::Odometry>::SharedPtr floating_base_realtime_publisher_ = nullptr;
   nav_msgs::msg::Odometry floating_base_msg_;
 
+  // Floating base TF broadcaster
+  std::unique_ptr<tf2_ros::TransformBroadcaster> floating_base_tf_broadcaster_ = nullptr;
+  geometry_msgs::msg::TransformStamped floating_base_tf_msg_;
+  bool publish_floating_base_tf_ = true;
+
   // Free joint data
   int free_joint_id_ = -1;
   int free_joint_qpos_adr_ = -1;

mujoco_ros2_control/include/mujoco_ros2_control/utils.hpp

@@ -30,10 +30,9 @@ namespace mujoco_ros2_control
 inline std::optional<hardware_interface::ComponentInfo>
 get_sensor_from_info(const hardware_interface::HardwareInfo& hardware_info, const std::string& name)
 {
-  for (size_t sensor_index = 0; sensor_index < hardware_info.sensors.size(); sensor_index++)
+  for (const auto& sensor : hardware_info.sensors)
   {
-    const auto& sensor = hardware_info.sensors.at(sensor_index);
-    if (hardware_info.sensors.at(sensor_index).name == name)
+    if (sensor.name == name)
     {
       return sensor;
     }

mujoco_ros2_control/package.xml

@@ -16,12 +16,14 @@
   <depend>controller_manager</depend>
   <depend>hardware_interface</depend>
   <depend>nav_msgs</depend>
+  <depend>tf2_ros</depend>
   <depend>libglfw3-dev</depend>
   <depend>pluginlib</depend>
   <depend>python3-pykdl</depend>
   <depend>rclcpp_lifecycle</depend>
   <depend>rclcpp</depend>
   <depend>sensor_msgs</depend>
+  <depend>std_srvs</depend>
   <depend>transmission_interface</depend>
 
   <test_depend>ament_cmake_gtest</test_depend>