Improve Low-Level Controllers Topic

[01binary]

Description

There is a lot of confusion around different controller managers supported by MoveIt and their configuration settings, integrating controllers with MoveIt by linking them with existing controller handle allocator plugins, and writing controller handles and allocators. Issues reported on GitHub, posts on Robotics Stack Exchange, ROS Answers, Google Groups attest that this part of documentation is both critical to integrating new robots with MoveIt and lacks sufficient depth and clarity.

This pull request addresses issues created in MoveIt 1 repository:

• Improve tutorial on writing custom controller handles moveit#3555
• Expand the controller tutorial to include ros_control and fake_controllers moveit#112

Questions for reviewers:

• Should we try to setup Panda with ROS Controllers? I've been having to use basic_velocity_moveit_config for all of the YAML configuration examples because Panda's ros_controllers.yaml is blank for unknown reason, and there is no gazebo_demo.launch file in its moveit config package. Both features are needed for examples in this article.
• Where should links to ROS docs go?
  • Currently going to:
    https://docs.ros.org/en/noetic/*
  • This is already used in planning_with_approximated_constraint_manifolds_tutorial.rst so I assume it's fine.
• Where should links to ROS Controls GitHub go?
  • Currently going to:
    https://github.com/ros-controls/ros_controllers/blob/noetic-devel/*
  • This is set as their default branch, and there won't be another ROS 1 version coming out after Noetic, so this should be fine?
• MoveIt source links currently going to:
  https://github.com/ros-planning/moveit/blob/master/*
  • This seems fine because it's both the master branch and the default.

Checklist

• Format code using clang-format
• Review by @rhaschke
• Review by @bmagyar or @davetcoleman?

[welcome]

Thanks for helping in improving MoveIt and open source robotics!

[rhaschke]

Thanks for this thorough rework of the controller tutorial. There are many nice contributions. However, for some parts I liked the previous text more. Thus in my additional commit, I tried to merge the old and new stuff getting the best from both worlds.
I didn't yet worked through the new code sections. (Sorry, time is limited).

[01binary]

I don't know whether the ros_control tutorials explain that idea. Probably they do.

I checked ROS Control 1 documentation - it's just one page and links to presentation & paper. The presentation mentions "resources" but does not say that resources are joints. No specific mention of controllers claiming joints. Also checked ROS Control 2 documentation - it's smoother but still has large gaps.

I recommend leaving this in, otherwise I will have to copy-and-paste it into 100 different answers on Robotics Stack Exchange - it's a lot easier to link to relevant documentation.

Other than that, things looking good. Last remaining issue is we need to use an example project that has ROS Control configuration, and Panda does not. I see you replaced all the examples with Panda config.

The MSA-generated project must have the following to be a valid example here:

• Have ROS Controllers configuration
• Have Gazebo launch configuration
• Have RViz & Gazebo launch configuration

Panda only has RViz configuration to launch with fake controllers.

Would you like me to setup panda_moveit_config with ROS controls in a separate PR, get that merged, and then come back here?

[rhaschke]

Would you like me to setup panda_moveit_config with ROS controls in a separate PR, get that merged, and then come back here?

No, please don't do that. If you want to augment a robot config with such a config, consider using the Panda robot or Fanuc robot in moveit_resources.

Before merging this, I need to work through the code example sections. I didn't find time for this yet.

[01binary]

Sorry, I didn't realize I was not being clear. By "setup panda with moveit config" I meant augmenting panda_moveit_config by re-running the wizard to make sure all the files normally generated by MSA exist, are filled out, and work.

[rhaschke]

I think I understood you correctly. And I suggest not to update the standard panda_moveit_config which is very special and deviates in many regards from the MSA-generated files. However, the panda_moveit_config in moveit_resources is much closer to the MSA defaults. Hence, you should use that version.

[01binary]

My first few attempts to re-gen panda_moveit_config in moveit_resources (using panda_description) failed.

EDIT 1

Apparently MSA does not add filename attribute to <plugin name="gazebo_ros_control> element in URDF generated for simulation. I haven't seen this error before because my robot URDF already had the <gazebo><plugin> section in it, so MSA didn't have to add one. After I added the attribute Gazebo finally spawned the model, but then crashed after unpausing physics.

Continuing to troubleshoot. If this takes too long I'm fine with giving up on this aspect, and getting it patched up later in another PR. No reason to over-extend and keep PRs open for a long time when there's always another PR.

EDIT 2

If you are looking to test the code, see third_draft branch in trajectory_controller_example repo. If you clone that into a ROS1 workspace and install packages with rosdep I feel like it should build on the first attempt with no issues.

[rhaschke]

Regarding the issues you observed with Gazebo:

1. I added the missing filename attribute in MSA via MSA: Add missing filename attribute to gazebo plugin tag moveit#3572
2. The crash of Gazebo when unpausing physics is caused by a bug in joint_state_controller, fixed via Fix uncaught exception in joint_state_controller ros-controls/ros_controllers#630

[rhaschke]

I don't see so much value in the two code sections. The mainly explain pluginlib usage, which is documented elsewhere.

[01binary]

I think we found a happy middle here: the topic starts with a survey of controller managers and their settings (split by type), then goes into integrating controllers (again split by type)

[rhaschke]

I explicitly moved that section to the end as it is not strongly related to the current sections.

[welcome]

Congrats on getting your first MoveIt pull request merged and improving open source robotics!

[01binary]

Thanks for the merge! Sorry the PR had to bounce as both of us were busy at different times.

[simonschmeisser]

Sorry for being late to the party but I still find this very confusing and would love to see more clarity

[simonschmeisser]

I'm currently trying to use ros_control for the first time and it is not clear to me when I would use which controller manager. It says "Alternatively" here but not why I would choose this alternative.

Also the actual name of the plugin is not spelled out.

[simonschmeisser]

It was not clear to me where to put this parameter and it belongs to the previous section?

[01binary]

I agree that this part assumes you are very comfortable with ROS configuration.

The "~" means root or top level (you could say "global") ROS configuration namespace. There is a number of ways to set global parameters, such as specifying them as command line arguments, putting them in launch files, loading them from YAML, or setting them via command line CLI commands.

This parameter is being read by controller_manager_plugin.cpp:

/**
 * \brief The default constructor. Reads the namespace from ~ros_control_namespace param and defaults to /
 */
MoveItControllerManager()
  : ns_(ros::NodeHandle("~").param("ros_control_namespace", std::string("/")))
  , loader_("moveit_ros_control_interface", "moveit_ros_control_interface::ControllerHandleAllocator")
{
  ROS_INFO_STREAM("Started moveit_ros_control_interface::MoveItControllerManager for namespace " << ns_);
}

The easiest way to set it would probably be in ros_controllers.launch launch file by using param tag, but you could also pick any other launch file where you think it makes the most sense.

<param name="ros_control_namespace" value="/myrobotcontrol" />

[simonschmeisser]

It seems the "debugging info" is now above?

Also it would be super helpful to add more debugging tips. Currently my robot is not moving and I have no idea why. Is there a way to make ros_control acknowledge that it received a trajectory? Print the content? This kind of thing

[01binary]

Yes, this was moved too many times. We can get this fixed.

[01binary]

@simonschmeisser we can setup Zoom if you like, to make it a bit more interactive.

First question I have, is the current documentation after this pull request better than the previous version?

it is not clear to me when I would use which controller manager

The "Low-Level Controllers" topic follows the following macro-structure:

1. Enumerate controller manager types
2. Explain the use case for each controller manager and its configuration
3. Explain how controllers interact with controller managers

Here's a high-level summary, let me know if this makes sense.

• There are several controller manager types: Fake, Simple, ROS Control, Multi, Custom (roll your own)
• Some controllers are used only in special scenarios:
  • Fake controller manager is used for basic visualization of inverse kinematics, so that you can test basic configuration without loading all of the complex machinery needed to actually move the robot arm. You might want this for testing inverse kinematics and basic robot description for correctness.
  • Multi controller manager is used if you have multiple ROS Controller Manager nodes. This can happen in advanced scenarios if you have a robot with many controllers, or mixing and matching controllers that were designed to work on another robot.
• Remaining controllers are arranged from simple to complex by usage scenario:
  • Simple: minimum configuration needed to load and move a robot. Supports two most common controller types people might want. You might do this if you are starting out and want to test basic functionality with less moving parts in play. When you have a stack with lots of moving parts so to speak, it's harder to troubleshoot.
  • ROS Control: the most common configuration for robots using controllers from ROS Controller package. Intermediate level of complexity for someone very familiar with ROS Control and ROS Controllers. This also lets you load your own custom controller, or use custom controllers written by someone else (Intermediate to Advanced level of difficulty).
  • Custom: you can write your own controller manager to have complete control over what's happening. This degree of control may be useful for someone who has an old robot with a custom stack that they decide to integrate with MoveIt, or an enterprise/large company scenario where you have many specific requirements for your business, and you need something that checks all the boxes, and have the time to do all the customization.

Currently my robot is not moving and I have no idea why. Is there a way to make ros_control acknowledge that it received a trajectory? Print the content? This kind of thing

• You can echo the topic on which the ROS controller would receive its trajectory command to verify MoveIt is telling the controller to move joints
  • If you list topics with rostopic list you should get a topic called /your_controller_name/follow_joint_trajectory/goal, this receives goals from MoveIt
  • You can then use rostopic echo <topicname> to watch it
• Console errors
  • You could look for Completed trajectory execution with status (followed by status) or Controller 'name' successfully finished
• Debug messages
  • Console messages logged with "debug" severity are not normally displayed, you could enable those. See any logging statements that start with ROS_ in joint_trajectory_controller_impl.h (ROS Controllers GitHub repo) to get an idea of what messages to look for
• Debug the ROS controller
  • You can attach to Gazebo Server using ROS Attach command in Visual Studio Code. It's the executable called "gzserver". Set a breakpoint in joint_trajectory_controller_impl.h, the goalCB method. This is the method that gets called when the /follow_joint_trajectory/goal topic I mentioned earlier gets a goal from MoveIt.