WIP: moveit_py initial release

[peterdavidfagan]

Important:this pull request is not yet ready for review, I am opening it for the purposes of providing visibility on the current status of the moveit_py library. The library documentation, unittests and additional bindings still need to be completed.

Note: the commit history will be reconciled into multiple commits to make these changes easier to review and further information will be provided in the description section of this pull request.

Description

1. Removes moveit_core legacy bindings.
2. Creates Python bindings for functionalities in moveit_core (further description to be added).
3. Creates Python bindings for functionalities in moveit_cpp (further description to be added).
4. Creates Python module that provides a service client for interacting with the Servo node.

[mergify[bot]]

This pull request is in conflict. Could you fix it @peterdavidfagan?

[mergify[bot]]

This pull request is in conflict. Could you fix it @peterdavidfagan?

[tylerjw]

You marked this as WIP. Is it ready to review/merge?

Do you mind rebasing it so we can run CI?

[peterdavidfagan]

Hi @tylerjw

You marked this as WIP. Is it ready to review/merge?

It is not ready in its current form, I spoke with @henningkayser in our last check in and the goal is to have it prepared for review by this Sunday.

Do you mind rebasing it so we can run CI?

I will perform a rebase as requested when working on this later today.

[peterdavidfagan]

Hi @tylerjw I wish to push some changes today to the overall structure of the project. I will ping you here once it is ready for review.

[peterdavidfagan]

I am starting to revise testing of the current functionalities of the library (there is still a bit to cover).

For integration test cases, I am now encountering an issue with the simple_controller_manager timing out.

[ros2_control_node-5] [INFO] [1666531177.095537121] [panda_arm_controller]: Goal reached, success!
[motion_planning-4] [WARN] [1666531177.966506222] [moveit.simple_controller_manager.follow_joint_trajectory_controller_handle]: waitForExecution timed out
[motion_planning-4] [ERROR] [1666531177.966569705] [moveit_ros.trajectory_execution_manager]: Controller is taking too long to execute trajectory (the expected upper bound for the trajectory execution was 2.878089 seconds). Stopping trajectory.

I have seen that there were some recent changes to the moveit_plugins to include moveit_ros_control_interface. Should I be updating my launch files and controller configs to leverage the new controller manager? Tagging @AndyZe as it relates to part of the codebase he has worked on. I am happy to spend more time on this myself but in case there is a quick response I thought to post here.

I am also seeing some messages relating to kinematics solvers being initialized:

[motion_planning-4] [ERROR] [1666530989.283799301] [moveit_kinematics_base.kinematics_base]: Group 'hand' is not a chain
[motion_planning-4] [ERROR] [1666530989.283825586] [kinematics_plugin_loader]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'hand'
[motion_planning-4] [ERROR] [1666530989.283971858] [moveit_kinematics_base.kinematics_base]: Group 'hand' is not a chain
[motion_planning-4] [ERROR] [1666530989.283983814] [kinematics_plugin_loader]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'hand'
[motion_planning-4] [ERROR] [1666530989.284040866] [moveit_kinematics_base.kinematics_base]: Group 'hand' is not a chain
[motion_planning-4] [ERROR] [1666530989.284049333] [kinematics_plugin_loader]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'hand'
[motion_planning-4] [ERROR] [1666530989.284066834] [moveit_ros.robot_model_loader]: Kinematics solver could not be instantiated for joint group hand.
[motion_planning-4] [ERROR] [1666530989.284129962] [moveit_kinematics_base.kinematics_base]: Group 'panda_arm_hand' is not a chain
[motion_planning-4] [ERROR] [1666530989.284138340] [kinematics_plugin_loader]: Kinematics solver of type 'kdl_kinematics_plugin/KDLKinematicsPlugin' could not be initialized for group 'panda_arm_hand'

The above appears to be tracked in the following issue. I need to work on a presentation this evening so it will be tomorrow before I can return to this but I plan to dedicate ~2 hours tomorrow to continue adding tests and cleaning up this pr.

[peterdavidfagan]

Hi @tylerjw,

I made some updates to move some of the binding code out of the files core.cpp and planning.cpp and into the files in the subfolder as @henningkayser suggested we do so. I used the methodology outlined here.

I don't think the library is ready (lots of TODO comments and improvements that need to be made) but if people wish to start reviewing the code I will work on any suggestions that are made during the hours I have dedicated in my current schedule to MoveIt.

The strict deadline from GSoC for my code to be finalized is November 26th.

[AndyZe]

I have seen that there were some recent changes to the moveit_plugins to include moveit_ros_control_interface. Should I be updating my launch files and controller configs to leverage the new controller manager?

The only change I'm aware of recently is a renaming. This is actually an optional renaming with a proper deprecation warning, so it shouldn't fail if you don't make the change. But you should go ahead and make the text replacement anyway:

MoveItControllerManager->Ros2ControlManager and MoveItMultiControllerManager->Ros2ControlMultiManager

I don't know think that would have anything to do with "waitForExecution timed out". The timeout sounds like an action response is getting lost in the middleware somehow. Are you using CycloneDDS?

[peterdavidfagan]

Thanks for the quick response on this @AndyZe, I haven't altered the RMW_IMPLEMENTATION environment variable so I assume it is running the default DDS software for Rolling. This appears to be rmw_fastrtps_cpp but do correct me if I am wrong.

The only change I'm aware of recently is a renaming. This is actually an optional renaming with a proper deprecation warning, so it shouldn't fail if you don't make the change.

I did test changing the moveit_controller_manager in the gripper_moveit_controllers.yaml file but encountered the same issue as listed above when doing so. Does the structure of the yaml file not also change?

The README instructions for the ROS2ControlManager contain a controller list, I did try providing this.

ros_control_namespace: /ROS_CONTROL_NODE
controller_list:
  - name: /ROS_CONTROL_NODE/position_trajectory_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true
    joints:
      - joint_a1
      - joint_a2
      - joint_a3
      - joint_a4
      - joint_a5
      - joint_a6
      - joint_a7

[AndyZe]

Is there any good way to test this, or should I just looks at the tests for usage?

(Edit: I found the moveit2_tutorials PR. I'll add a link to the PR description)

[peterdavidfagan]

Is there any good way to test this, or should I just looks at the tests for usage?

(Edit: I found the moveit2_tutorials PR. I'll add a link to the PR description)

Do you mean the entire set of functionalities of the library? I am looking to increase test coverage by adding unit tests here.

The following has examples of usage of the library: https://github.com/peterdavidfagan/moveit_py_example.

I will open up a pr for moveit2_tutorials with similar content to the above linked repository.

[AndyZe]

Planning to a pose with moveit_cpp is what I'm selfishly concerned with right now ;)

[mergify[bot]]

This pull request is in conflict. Could you fix it @peterdavidfagan?

[mergify[bot]]

This pull request is in conflict. Could you fix it @peterdavidfagan?

[rhaschke]

Some general remarks to this PR:

• Unfortunately, you started from the old ROS1 master branch, while we already put some significant effort into improving python integration there:

  • https://github.com/ros-planning/moveit/pull/2910
  • https://github.com/ros-planning/moveit/pull/2613
  • https://github.com/ros-planning/moveit/issues/3301

  These changes were not yet merged due to a fundamental catkin issue (https://github.com/ros/catkin/pull/1153). Luckily this doesn't apply to ROS2. Hence, it would make sense to reuse this code, which leads to great simplifications in the wrapper code. Most importantly, we can drop manual type conversions of ROS msgs, but use pybind11's type_caster mechanism instead.

• In those ROS1 PRs we migrated to namespaced python packages, which is more modular:

  import moveit.core
  import moveit.planning_interface 
  import moveit.task_constructor
  

  I suggest doing this here as well and also using the simpler module name moveit instead of moveit_py.

[peterdavidfagan]

Thanks so much @rhaschke for taking the time to start reviewing my pull request.

Hence, it would make sense to reuse this code, which leads to great simplifications in the wrapper code.

I did take a look at this previous work and I agree it is valuable and should be leveraged where possible. I also agree that these bindings have greater simplifications but I think some of these come at a cost to the Python API. For instance rather than binding multiple get methods, I leverage properties which is arguably more Pythonic. I'd be interested in getting your opinion on where one set of bindings may be better than the other.

Most importantly, we can drop manual type conversions of ROS msgs, but use pybind11's type_caster mechanism instead.

Agreed on this point. I found in testing that manually specifying the copy of data was more efficient at least when compared to serializing and deserialzing messages. I'll review this point and tag you once I have a follow up commit.

In those ROS1 PRs we migrated to namespaced python packages, which is more modular:

Ah I see, so rather than grouping moveit_core bindings under the core module you are suggesting that I inherit the namespacing employed in moveit_core (e.g. moveit.core.kinematic_constraints for kinematic constraints bindings). This makes a lot of sense I will look to implement this change.

I suggest doing this here as well and also using the simpler module name moveit instead of moveit_py.

Agreed, I will update the library name.

[rhaschke]

Hence, it would make sense to reuse this code, which leads to great simplifications in the wrapper code.

I did take a look at this previous work and I agree it is valuable and should be leveraged where possible. I also agree that these bindings have greater simplifications but I think some of these come at a cost to the Python API. For instance rather than binding multiple get methods, I leverage properties which is arguably more Pythonic. I'd be interested in getting your opinion on where one set of bindings may be better than the other.

Using properties instead of getters is absolutely fine.

Most importantly, we can drop manual type conversions of ROS msgs, but use pybind11's type_caster mechanism instead.

Agreed on this point. I found in testing that manually specifying the copy of data was more efficient at least when compared to serializing and deserialzing messages. I'll review this point and tag you once I have a follow up commit.

Sure, manually converting data between C++ and python is faster than serialization/deserialization. However, as far as I see, you make frequent use of serializeMsg/deserializeMsg and don't yet use type_caster. Did I miss something?

[peterdavidfagan]

Sure, manually converting data between C++ and python is faster than serialization/deserialization. However, as far as I see, you make frequent use of serializeMsg/deserializeMsg and don't yet use type_caster. Did I miss something?

This is correct, you didn't miss anything, I need to revise this. I'll be working on MoveIt shortly so I'll look to push commits to address the points you raised.

[peterdavidfagan]

Hey @rhaschke👋, I just got the custom typecaster using the ROS2 equivalents of the libraries you used to work 🎉. I will simplify all methods that explicitly serialize/deserialize messages tomorrow morning and push a commit for this.

[rhaschke]

That's great news. I'm curious to see the implementation. I pushed two cleanup commits to make CI happy. I also found your copy_ros_msg.* files doing the C++ <-> python conversion manually.

[codecov[bot]]

Codecov Report

Base: 50.29% // Head: 50.29% // No change to project coverage :thumbsup:

Coverage data is based on head (3c31c8b) compared to base (cc63547). Patch has no changes to coverable lines.

Additional details and impacted files

@@           Coverage Diff           @@
##             main    #1546   +/-   ##
=======================================
  Coverage   50.29%   50.29%           
=======================================
  Files         374      374           
  Lines       31358    31358           
=======================================
  Hits        15769    15769           
  Misses      15589    15589           

Help us with your feedback. Take ten seconds to tell us how you rate us. Have a feature suggestion? Share it here.

:umbrella: View full report at Codecov.
:loudspeaker: Do you have feedback about the report comment? Let us know in this issue.

[rhaschke]

Sorry for interfering with your commits. Unfortunately, by force-pushing you destroyed my commits, which fixed CI for a short time. To apply pre-commit fixes, just download the pre-commit artifact (here) and run git apply pre-commit.patch on the unzipped content as suggested on the page.

Even better, install pre-commit locally: sudo pip install pre-commit

and install the pre-commit hook in your moveit git repository: pre-commit install

Then those checks will run automatically before each commit. To manually run them on the full repo: pre-commit run -a

[peterdavidfagan]

Apologies about this it was getting messy having to perform a rebase due to the arising conflicts with changes I had made locally. I was going to follow up with a message about correcting this, I have assigned you as author for the recreated commits. I have recreated the pre-commit changes and will look to commit changes from clang-tidy next. I have copies of your commits but I am also looking to go through the process myself as well.

To apply pre-commit fixes, just download the pre-commit artifact (here) and run git apply pre-commit.patch on the unzipped content as suggested on the page.

This is really useful to know thanks for this.

Even better, install pre-commit locally:

I will do this, I have followed the workflow you outlined for past Python projects and found it very useful.

[peterdavidfagan]

Thanks for this valuable feedback, I will address each of these improvement points.

[peterdavidfagan]

• Avoid defining (long) doc strings in definitions. This makes the wrapper code pretty much unreadable. (I followed the same approach in MTC, but don't like it.) Rather, use .pyi files and provide the documentation there. There are also tools available to generate raw .pyi files automatically via API inspection. So you can generate the skeleton and add doc strings in those native python files.

I agree with this observation, the only issue is that at the moment the sphinx API documentation is generated from the pybind11 code. It might take a lot of rework to address this issue. Would it be feasible to implement the other changes and add an issue for this as something to be revisited?

[rhaschke]

At the moment the sphinx API documentation is generated from the pybind11 code. It might take a lot of rework to address this issue. Would it be feasible to implement the other changes and add an issue for this as something to be revisited?

I guess the API documentation is auto-generated from source. For this, it doesn't matter whether the source comes from .pyi files or pybind11 modules. However, I agree that moving the docs from .cpp to .pyi files is a lot of work. I don't know your time frame. But you should try to address that within your GoC project anyway. But I also agree that the functionality should have highest priority.

[rhaschke]

To fix the remaining CI failures, you should have a look at their logs. Currently, all of them can be (kind of) auto-fixed by applying the suggested clang-tidy modifications. As above for clang-format, download the provided artifact, unzip and git-apply it. I just pushed a corresponding commit, which also includes some minor cleanup.

[peterdavidfagan]

Thanks for applying the clang-tidy fixes. I will be working on this again in the afternoon so I will post updates as I resume working on this.

[peterdavidfagan]

You should try to minimize the wrapper code as much as possible. Usually, there is no need to write extra wrapper functions. You can directly def the existing class methods. The reasoning behind this is that all this extra code means extra maintenance in the future as soon as something changes in the original functions...

I agree, the places where I have added wrapper code were for the purpose of making the API more Pythonic (with exception of cases where I was explicitly serializing/deserializing this was simply suboptimal when compared with using a custom typecaster). I am happy to work on maintaining the Python API going forward and keeping it updated based on changes to the C++ codebase. This being said I would be more than happy to discuss both APIs and make changes where necessary.

I guess the API documentation is auto-generated from source. For this, it doesn't matter whether the source comes from .pyi files or pybind11 modules. However, I agree that moving the docs from .cpp to .pyi files is a lot of work. I don't know your time frame. But you should try to address that within your GoC project anyway. But I also agree that the functionality should have highest priority.

I briefly started to look into this, I came across the following open issue: https://github.com/sphinx-doc/sphinx/issues/7630

I suggest doing this here as well and also using the simpler module name moveit instead of moveit_py.

Is this still a requirement before merging? It is non-trivial as the name of the package for the entire repository is also moveit so colcon complains. I could move things around to account for this.

I will focus on completing tutorials, API documentation and writing tests unless there are more major changes to make.

Update: introducing the type checking has caused errors in Python -> C++ conversion so I am revising this. I have also bserved one or two inconsistencies in the API that need to be addressed.

[peterdavidfagan]

Thanks for the extended feedback, I will work to address these points. For the following point I raised this in the MoveIt standup recently.

PlanningComponent::set_goal() just dispatches a long list of optional args to handle overloads. However, pybind11 supports overloading! Just provide set_goal definitions for each C++ overload.

Python doesn't support overloading as in C++. I know with pybind11 it is possible to make overloaded functions available in Python but is this not confusing for Python users? Is it better to instead provide an API that deals with multiple input datatypes via arguments or in some cases different function names as a Python user might expect?