Support for different control modes with mujoco by defining PIDs

[saikishor]

Hello!

We added a new feature here. If you define the PIDs via an argument, then you can utilize different control modes. For instance, with a standard MuJoCo actuator, after defining gains of position and velocity PIDs, you can control the robot in position, velocity or effort.

We also added an example test case, so you can try it out

Kudos to @Ortisa

Similar to NASA-JSC-Robotics/mujoco_ros2_simulation#38

[eholum-nasa]

Thanks for putting this together all! Will definitely open some doors so great to see this in play. Have mostly cleanup and clarity comments. I haven't had a chance to actually build and play with this yet but I'm very curious. Will get to that as soon as I can.

In the interim please let me know if any of my incoherent ramblings don't make sense to you.

[eholum-nasa]

Can you add a comment explaining how these correspond to the actuators in the model itself?

https://mujoco.readthedocs.io/en/stable/XMLreference.html#actuator-general

This is basically just motor, position, velocity, or other, correct?

[saikishor]

Updated. Please check it.

[eholum-nasa]

FWIW this was very nearly copy and pasted from the upstream: https://github.com/google-deepmind/mujoco/blob/main/simulate/main.cc#L216

I would recommend breaking this up into two helper functions, something like loadModelFromFile and loadModelFromTopic, and just have loadModel attempt one, and fallback to the other. Does that make sense? That might keep the logic a little cleaner. E.g.

mjModel* LoadModel(const char* file, mj::Simulate& sim, rclcpp::Node::SharedPtr node)
{
  if (file && file[0])
  {
    return loadModelFromFile(file, sim);
  }
  return loadModelFromTopic(node, sim);
}

[saikishor]

Updated. Please check it.

[eholum-nasa]

Is this actually necessary? Or does the PID toolbox do a reasonable job of parsing the yaml? If not I would prefer to remove it, if so what parameters are actually being declared?

Not sure how feasible it is but are these something that could use generate params and possibly be reloadable during operation? It would be a really nice feature if you could tune them on the fly. Just thinking out lout here...

[saikishor]

Removed. Please check it.

[eholum-nasa]

Could we kick declaring/getting parameters out to a helper function in the class?

[saikishor]

This is a parameter of the URDF not the ros parameter

Similar to these:

mujoco_ros2_simulation/src/mujoco_system_interface.cpp

Lines 322 to 333 in 2bfa22a

	const auto model_path_maybe = get_parameter("mujoco_model");
	if (!model_path_maybe.has_value())
	{
	RCLCPP_FATAL(rclcpp::get_logger("MujocoSystemInterface"), "Missing 'mujoco_model' in <hardware_parameters>.");
	return hardware_interface::CallbackReturn::ERROR;
	}
	model_path_ = model_path_maybe.value();
	RCLCPP_INFO_STREAM(rclcpp::get_logger("MujocoSystemInterface"), "Loading 'mujoco_model' from: " << model_path_);
	// Pull the initial speed factor from the hardware parameters, if present
	sim_speed_factor_ = std::stod(get_parameter("sim_speed_factor").value_or("-1"));

[eholum-nasa]

I'd have to think about whether or not this is completely reliable for determining motor type. This is basically evaluating the gains set based on the tables here, correct? https://mujoco.readthedocs.io/en/stable/XMLreference.html#actuator-position. I think what you have here makes sense but I feel like it could get confused for different motor types.

In any case, can you also kick this out to a dedicated helper function with a comment so future us can remember what the logic and motivation was?

Something like:

ActuatorType getActuatorType(const mjModel* model, int actuator_id)

[saikishor]

Updated. Please check :)

[eholum-nasa]

Can joint.is_position_pid_control_enabled be true if joint.is_position_control_enabled is not?

[saikishor]

Added comments

[eholum-nasa]

These should all probably be failure cases rather than ignored.

[saikishor]

Better to add some error, so that it is easy for the end user to understand the issue

[eholum-nasa]

This section is hard for me the parse. Hate to be that guy but can you add notes about the allowable control modes here? It will likely be worth adding something to the readme, just to understand the allowable configurations, and especially for making sure that this logic lines up with what is expected. I think that would be important for users to have.

Some higher level questions:

If no PID gains are provided, will control be delegated to the mujoco actuators (which are essentially PD controlled)?

Position modes are either regular old mujoco delegated position, or position PID around velocity/effort?

Velocity modes are either regular old mujoco delegated velocity, or velocity around effort?

Only effort mode is pure mujoco delegated effort. I don't recall if you could get PID like control out of their gain settings but iirc it was only PD. If you know differently please let me know!

[saikishor]

Position modes are either regular old mujoco delegated position, or position PID around velocity/effort?

• If is_position_control_enabled is true → standard MuJoCo position control is used, directly assigning position_command to mj_data_control_->ctrl.
• If is_position_pid_control_enabled is true → a position PID is applied, computed from the error between position_command and the current position, and the resulting command is applied to qfrc_applied

Velocity modes are either regular old mujoco delegated velocity, or velocity around effort?

• If is_velocity_control_enabled → standard MuJoCo velocity control is applied by directly assigning velocity_command to mj_data_control_->ctrl.
• If is_velocity_pid_control_enabled → a velocity PID is computed from the error between velocity_command and the current velocity, and the resulting command is applied to qfrc_applied

Only effort mode is pure mujoco delegated effort. I don't recall if you could get PID like control out of their gain settings but iirc it was only PD. If you know differently please let me know!

• Yes — for effort mode, it’s pure MuJoCo effort control. The command is applied directly to the MuJoCo actuator.

[saikishor]

@eholum-nasa thanks for the review. We will try to address them tomorrow or at the earliest.

Thank you.