Skip to content

Parmeterize mujoco robot description topic #41

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Open
saikishor wants to merge 4 commits into
base: main
Choose a base branch
from
+54 −34
Open

Parmeterize mujoco robot description topic #41

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
fa6e1be
Add common methods to reuse within the system component
saikishor Dec 20, 2025
6bc947d
Parameterize mujoco_robot_description topic with parameter
saikishor Dec 20, 2025
42eb0f9
update README.md
saikishor Dec 20, 2025
ac3f5b2
Merge branch 'main' into parmeterize_robot_description_topic
saikishor Dec 21, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
7 changes: 7 additions & 0 deletions README.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -76,6 +76,13 @@ Just specify the plugin and point to a valid MJCF on launch:
-->
<param name="override_start_position_file">$(find my_description)/config/start_positions.xml</param>

<!--
Optional parameter to choose a topic name from which it can subscribe and get the MJCF contents
to load the model. This is only used, when the mujoco_model parameter is not set.
By default, this parameter is initialized to /mujoco_robot_description
-->
<param name="mujoco_model_topic">/mujoco_robot_description</param>

<!--
Optional parameter to update the simulated camera's color and depth image publish rates. If no
parameter is set then all cameras will publish at 5 hz. Note that all cameras in the sim currently
Expand Down
81 changes: 47 additions & 34 deletions src/mujoco_system_interface.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -63,6 +63,28 @@ using Seconds = std::chrono::duration<double>;
/// \brief vector with the current actuator for each joint
std::unordered_map<std::string, std::string> actuator_map;

namespace
{
std::optional<std::string> get_hardware_parameter(const hardware_interface::HardwareInfo& hardware_info,
const std::string& key)
{
if (auto it = hardware_info.hardware_parameters.find(key); it != hardware_info.hardware_parameters.end())
{
return it->second;
}
return std::nullopt;
}

std::string get_hardware_parameter_or(const hardware_interface::HardwareInfo& hardware_info, const std::string& key,
const std::string& default_value)
{
if (auto it = hardware_info.hardware_parameters.find(key); it != hardware_info.hardware_parameters.end())
{
return it->second;
}
return default_value;
}
} // namespace
namespace mujoco_ros2_control
{
namespace mj = ::mujoco;
Expand Down Expand Up @@ -355,7 +377,7 @@ mjModel* loadModelFromFile(const char* file, mj::Simulate& sim)
return mnew;
}

mjModel* loadModelFromTopic(rclcpp::Node::SharedPtr node)
mjModel* loadModelFromTopic(rclcpp::Node::SharedPtr node, const std::string& topic_name)
{
mjModel* mnew = 0;
std::string robot_description;
Expand All @@ -366,7 +388,7 @@ mjModel* loadModelFromTopic(rclcpp::Node::SharedPtr node)

// Try to get mujoco_model via topic
auto mujoco_model_sub = node->create_subscription<std_msgs::msg::String>(
"/mujoco_robot_description", qos_profile, [&](const std_msgs::msg::String::SharedPtr msg) {
topic_name, qos_profile, [&](const std_msgs::msg::String::SharedPtr msg) {
if (!msg->data.empty() && robot_description.empty())
robot_description = msg->data;
});
Expand Down Expand Up @@ -412,7 +434,7 @@ mjModel* loadModelFromTopic(rclcpp::Node::SharedPtr node)
return mnew;
}

mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr node)
mjModel* LoadModel(const char* file, const std::string& topic, mj::Simulate& sim, rclcpp::Node::SharedPtr node)
{
// Try to get the mujoco model from URDF.
// If it is not available, create a subscription and listen for the model on a topic.
Expand All @@ -427,7 +449,7 @@ mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr
return loadModelFromFile(file, sim);
}
// Try to get the mujoco model from topic
return loadModelFromTopic(node);
return loadModelFromTopic(node, topic);
}

ActuatorType getActuatorType(const mjModel* mj_model, int mujoco_actuator_id)
Expand Down Expand Up @@ -521,17 +543,8 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
return hardware_interface::CallbackReturn::ERROR;
}

// Helper function to get parameters in hardware info.
auto get_parameter = [&](const std::string& key) -> std::optional<std::string> {
if (auto it = get_hardware_info().hardware_parameters.find(key); it != get_hardware_info().hardware_parameters.end())
{
return it->second;
}
return std::nullopt;
};

// Load the model path from hardware parameters
const auto model_path_maybe = get_parameter("mujoco_model");
const auto model_path_maybe = get_hardware_parameter(get_hardware_info(), "mujoco_model");
if (!model_path_maybe.has_value())
{
RCLCPP_INFO(get_logger(), "Parameter 'mujoco_model' not found in URDF.");
Expand All @@ -544,7 +557,7 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
}

// Pull the initial speed factor from the hardware parameters, if present
sim_speed_factor_ = std::stod(get_parameter("sim_speed_factor").value_or("-1"));
sim_speed_factor_ = std::stod(get_hardware_parameter(get_hardware_info(), "sim_speed_factor").value_or("-1"));
if (sim_speed_factor_ > 0)
{
RCLCPP_INFO_STREAM(get_logger(), "Running the simulation at " << sim_speed_factor_ * 100.0 << " percent speed");
Expand All @@ -555,12 +568,15 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
}

// Pull the camera publish rate out of the info, if present, otherwise default to 5 hz.
const auto camera_publish_rate = std::stod(get_parameter("camera_publish_rate").value_or("5.0"));
const auto camera_publish_rate =
std::stod(get_hardware_parameter(get_hardware_info(), "camera_publish_rate").value_or("5.0"));
// Pull the lidar publish rate out of the info, if present, otherwise default to 5 hz.
const auto lidar_publish_rate = std::stod(get_parameter("lidar_publish_rate").value_or("5.0"));
const auto lidar_publish_rate =
std::stod(get_hardware_parameter(get_hardware_info(), "lidar_publish_rate").value_or("5.0"));

// Check for headless mode
bool headless = hardware_interface::parse_bool(get_parameter("headless").value_or("false"));
bool headless =
hardware_interface::parse_bool(get_hardware_parameter(get_hardware_info(), "headless").value_or("false"));
RCLCPP_INFO_EXPRESSION(get_logger(), headless, "Running in HEADLESS mode.");

// We essentially reconstruct the 'simulate.cc::main()' function here, and
Expand Down Expand Up @@ -650,7 +666,7 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf

// Construct and start the ROS node spinning
/// The PIDs config file
const auto pids_config_file = get_parameter("pids_config_file");
const auto pids_config_file = get_hardware_parameter(get_hardware_info(), "pids_config_file");
rclcpp::NodeOptions node_options;
node_options.append_parameter_override("use_sim_time", rclcpp::ParameterValue(true));
if (pids_config_file.has_value())
Expand All @@ -674,7 +690,9 @@ MujocoSystemInterface::on_init(const hardware_interface::HardwareComponentInterf
executor_->add_node(mujoco_node_);
executor_thread_ = std::thread([this]() { executor_->spin(); });

mj_model_ = LoadModel(model_path_.c_str(), *sim_, mujoco_node_);
const std::string mujoco_model_topic =
get_hardware_parameter_or(get_hardware_info(), "mujoco_model_topic", "/mujoco_robot_description");
mj_model_ = LoadModel(model_path_.c_str(), mujoco_model_topic, *sim_, mujoco_node_);
if (!mj_model_)
{
RCLCPP_FATAL(get_logger(), "Mujoco failed to load the model");
Expand Down Expand Up @@ -1446,15 +1464,6 @@ void MujocoSystemInterface::register_joints(const hardware_interface::HardwareIn

void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareInfo& hardware_info)
{
// Helper function to get parameters in hardware info.
auto get_parameter_or = [&](const std::string& key, const std::string& default_value) -> std::string {
if (auto it = get_hardware_info().hardware_parameters.find(key); it != get_hardware_info().hardware_parameters.end())
{
return it->second;
}
return default_value;
};

for (size_t sensor_index = 0; sensor_index < hardware_info.sensors.size(); sensor_index++)
{
auto sensor = hardware_info.sensors.at(sensor_index);
Expand Down Expand Up @@ -1490,8 +1499,10 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI
{
FTSensorData sensor_data;
sensor_data.name = sensor_name;
sensor_data.force.name = mujoco_sensor_name + get_parameter_or("force_mjcf_suffix", "_force");
sensor_data.torque.name = mujoco_sensor_name + get_parameter_or("torque_mjcf_suffix", "_torque");
sensor_data.force.name =
mujoco_sensor_name + get_hardware_parameter_or(get_hardware_info(), "force_mjcf_suffix", "_force");
sensor_data.torque.name =
mujoco_sensor_name + get_hardware_parameter_or(get_hardware_info(), "torque_mjcf_suffix", "_torque");

int force_sensor_id = mj_name2id(mj_model_, mjOBJ_SENSOR, sensor_data.force.name.c_str());
int torque_sensor_id = mj_name2id(mj_model_, mjOBJ_SENSOR, sensor_data.torque.name.c_str());
Expand All @@ -1513,11 +1524,13 @@ void MujocoSystemInterface::register_sensors(const hardware_interface::HardwareI
{
IMUSensorData sensor_data;
sensor_data.name = sensor_name;
sensor_data.orientation.name = mujoco_sensor_name + get_parameter_or("orientation_mjcf_suffix", "_quat");
sensor_data.orientation.name =
mujoco_sensor_name + get_hardware_parameter_or(get_hardware_info(), "orientation_mjcf_suffix", "_quat");
sensor_data.angular_velocity.name =
mujoco_sensor_name + get_parameter_or("angular_velocity_mjcf_suffix", "_gyro");
mujoco_sensor_name + get_hardware_parameter_or(get_hardware_info(), "angular_velocity_mjcf_suffix", "_gyro");
sensor_data.linear_acceleration.name =
mujoco_sensor_name + get_parameter_or("linear_acceleration_mjcf_suffix", "_accel");
mujoco_sensor_name +
get_hardware_parameter_or(get_hardware_info(), "linear_acceleration_mjcf_suffix", "_accel");

// Initialize to all zeros as we do not use these yet.
sensor_data.orientation_covariance.resize(9, 0.0);
Expand Down
Loading