Skip to content
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.

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

Python Bindings - moveit_py #1546

Merged
merged 2 commits into from
Feb 25, 2023
Merged

Conversation

peterdavidfagan
Copy link
Member

@peterdavidfagan peterdavidfagan commented Sep 1, 2022

Description

This pull request adds a new Python library moveit which can be used to interface with MoveIt 2. Below is a basic summary of the initial feature set of the library:

  • moveit:
    • moveit.core: a python module containing implementations of moveit_core objects.

      • moveit.core.collision_detection:
        • AllowedCollisionMatrix
        • CollisionRequest
        • CollisionResult
      • moveit.core.kinematic_constraints:
        • KinematicConstraints
      • moveit.core.planning_interface:
        • MotionPlanResponse
      • moveit.core.planning_scene:
        • PlanningScene
      • moveit.core.robot_model:
        • JointModelGroup
        • RobotModel
      • moveit.core.robot_state:
        • RobotState
      • moveit.core.robot_trajectory
        • RobotTrajectory
      • moveit.core.transforms
        • Transforms
    • moveit.planning: a python module that exposes the planning functionalities of moveit_cpp

      • MoveItCpp
      • MultiPipelinePlanRequestParameters
      • PlanningComponent
      • PlanningSceneMonitor
      • SinglePlanRequestParameters
    • moveit.servo_client: a python module that defines a client for interacting with moveit_servo.

      • TeleopDevice
        • PS4DualShock
        • OcculusQuest2 (to be added)

Note: this is an initial release, in future releases further functionalities will be added (e.g. moveit task constructor; task planning tools; interfaces for machine learning experiments).

Related Pull Requests

As this is a new library we also have associated API documentation and tutorials that should be merged around the same time as the initial library release. Below is a list of the currently open pull requests:

API Documentation: moveit/moveit2_tutorials#504
Robot State and Model Tutorial: TBC
Motion Planning Tutorial: moveit/moveit2_tutorials#565
Jupyter Notebook Tutorial: moveit/moveit2_tutorials#564
Robot Learning Tutorial (Simplified application of QT-OPT or similar ): TBC (likely to be moved until after the initial release)

@peterdavidfagan peterdavidfagan changed the title initialize moveit_py library moveit_py initial release (WIP) Sep 1, 2022
@peterdavidfagan peterdavidfagan changed the title moveit_py initial release (WIP) WIP: moveit_py initial release Sep 1, 2022
@mergify
Copy link

mergify bot commented Sep 6, 2022

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

@mergify
Copy link

mergify bot commented Sep 27, 2022

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

@tylerjw
Copy link
Member

tylerjw commented Oct 16, 2022

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

Do you mind rebasing it so we can run CI?

@peterdavidfagan
Copy link
Member Author

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
Copy link
Member Author

peterdavidfagan commented Oct 23, 2022

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
Copy link
Member Author

peterdavidfagan commented Oct 23, 2022

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
Copy link
Member Author

peterdavidfagan commented Oct 23, 2022

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
Copy link
Member

AndyZe commented Oct 23, 2022

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
Copy link
Member Author

peterdavidfagan commented Oct 23, 2022

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

moveit_py/README.md Outdated Show resolved Hide resolved
moveit_py/README.md Outdated Show resolved Hide resolved
moveit_py/docs/source/conf.py Outdated Show resolved Hide resolved
moveit_py/moveit_py/core.pyi Outdated Show resolved Hide resolved
moveit_py/moveit_py/servo_client/devices/ps4_dualshock.py Outdated Show resolved Hide resolved
moveit_py/test/unit/fixtures/panda.srdf Show resolved Hide resolved
void PlanningSceneMonitor::applyPlanningSceneServiceCallback(std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene::Request> req,
std::shared_ptr<moveit_msgs::srv::ApplyPlanningScene::Response> res)
{
updateFrameTransforms();
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

It shouldn't be necessary to update the transforms. Also, it's important to notice that newPlanningSceneMessage() only slots a new update event, the actual change will be applied after this service call has returned successfully (and might still fail). If you want to apply the planning scene immediately, you'd have to call PlanningScene::usePlanningSceneMsg() while locking the scene.

Copy link
Member Author

@peterdavidfagan peterdavidfagan Nov 18, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@henningkayser should I remove the apply planning scene service from the services provided by the planning scene monitor? I think this service was only leveraged by the planning_scene_interface package which we no longer include in the bindings.

Copy link
Member Author

@peterdavidfagan peterdavidfagan Nov 18, 2022

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I went ahead and removed the apply planning scene service functionality, please let me know if you agree with this 790223c.

moveit_py/moveit_py/planning_interface.pyi Outdated Show resolved Hide resolved
moveit_py/moveit_py/planning.pyi Outdated Show resolved Hide resolved
@AndyZe
Copy link
Member

AndyZe commented Nov 2, 2022

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
Copy link
Member Author

peterdavidfagan commented Nov 2, 2022

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
Copy link
Member

AndyZe commented Nov 2, 2022

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

@henningkayser henningkayser changed the title WIP: moveit_py initial release Python Bindings - moveit_py Nov 9, 2022
@mergify
Copy link

mergify bot commented Nov 10, 2022

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

1 similar comment
@mergify
Copy link

mergify bot commented Nov 22, 2022

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

@RemiFabre
Copy link

Hi,
I'm trying to use this PR but I have compilation errors, such as:
ws_moveit2/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:183:22: error: ‘struct planning_interface::MotionPlanResponse’ has no member named ‘trajectory_’; did you mean ‘trajectory’? 183 | result = res.trajectory_;

I have a separate workspace to build moveit2 from source, and I was able to build it on branch humble.

Is there another repository where I should change branches?

Thanks,

@JafarAbdi
Copy link
Contributor

Hi, I'm trying to use this PR but I have compilation errors, such as: ws_moveit2/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:183:22: error: ‘struct planning_interface::MotionPlanResponse’ has no member named ‘trajectory_’; did you mean ‘trajectory’? 183 | result = res.trajectory_;

I have a separate workspace to build moveit2 from source, and I was able to build it on branch humble.

Is there another repository where I should change branches?

Thanks,

You need to update your moveit_task_constructor's ros2 branch

@peterdavidfagan
Copy link
Member Author

peterdavidfagan commented Feb 20, 2023

I have a separate workspace to build moveit2 from source, and I was able to build it on branch humble.

Also be sure to uninstall preexisting debians as these may also cause this error (if you installed the debian for moveit_task_constructor by mistake):

sudo apt remove ros-rolling-moveit*

https://moveit.ros.org/install-moveit2/source/

@RemiFabre
Copy link

RemiFabre commented Feb 20, 2023

Hi, I'm trying to use this PR but I have compilation errors, such as: ws_moveit2/src/moveit_task_constructor/core/src/solvers/pipeline_planner.cpp:183:22: error: ‘struct planning_interface::MotionPlanResponse’ has no member named ‘trajectory_’; did you mean ‘trajectory’? 183 | result = res.trajectory_;
I have a separate workspace to build moveit2 from source, and I was able to build it on branch humble.
Is there another repository where I should change branches?
Thanks,

You need to update your moveit_task_constructor's ros2 branch

Thanks for the fast answer. Indeed I had 2 repositories that needed to be updated since last thursday. I deleted the build folders and made a full build, I have different yet similar errors:

ws_moveit2/src/moveit2_tutorials/doc/examples/move_group_interface/src/move_group_interface_tutorial.cpp:156:46: error: ‘struct moveit::planning_interface::MoveGroupInterface::Plan’ has no member named ‘trajectory_’; did you mean ‘trajectory’? 156 | visual_tools.publishTrajectoryLine(my_plan.trajectory_, joint_model_group);

Any idea? My workspace has:
launch_param_builder -> on main
moveit2 -> on this PR
moveit2_tutorials -> on humble
moveit_resources -> on ros2
moveit_task_constructor -> on ros2
moveit_visual_tools -> on ros2
rosparam_shortcuts -> on ros2
srdfdom -> on ros2

Best,

Edit: I changed by hand the few instances of this problem (trajectory_ and error_code_) and now it compiles.

@@ -308,6 +308,7 @@ bool PlanningComponent::setStartState(const std::string& start_state_name)
return false;
}
moveit::core::RobotState start_state(moveit_cpp_->getRobotModel());
start_state.setToDefaultValues(); // required to ensure all joints are initialized
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I don't think this's needed, what if the current state of the gripper is open, wouldn't this cause the planner to plan with that state rather than the actual one?

Copy link
Member Author

@peterdavidfagan peterdavidfagan Feb 22, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I was encountering a segfault without initializing all joints to default values. There's a related discussion on Discord for this here and a related issue here. Without this line of code some joints remain uninitialised if they are not part of the planning_group you set the start state for.

Yes this may be an issue, my understanding is that we assume that the user only cares about the given planning group? They can always define another planning group to set values for all joints they care about. Maybe this requires an update to the documentation to highlight this point? If you have another suggestion I would be happy to work on it.

For Reference:
Default value definition from API docs: "Set all joints to their default positions. The default position is 0, or if that is not within bounds then half way between min and max bound."

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

@JafarAbdi do you think it is reasonable to resolve this item with better documentation?

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Having to use this in multiple places makes me think that we have a deeper bug, I'm more than happy to debug it if you have a small reproducible example.

I think we should merge this PR and handle that issue in a separate one!

Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Usually you would want to copy and modify some sort of 'current' RobotState instead of creating a new one.
(I haven't looked at this code, just read the discussion)

Copy link
Member Author

@peterdavidfagan peterdavidfagan Feb 24, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think we should merge this PR and handle that issue in a separate one!

Happy to open up a separate issue for this and address it there.

Usually you would want to copy and modify some sort of 'current' RobotState instead of creating a new one.
(I haven't looked at this code, just read the discussion)

This makes sense, currently we call the getRobotState on the moveit_cpp instance to retrieve the robot state before setting the state of joints in a given planning_group. Joints not within the planning_group we are setting sometimes have uninitialised values which results in the errors we saw in the past :(.

@v4hn suggested the following in the past:

but it's really bad that we do not have proper error messages for these cases :/
So if these initializations/updates fix your problem consider writing a small patch that yields a proper error message (or just works) when someone tries to do it again

I think having a proper error messages is important, it is likely bad to assume that the user only cares about the state of the planning_group when setting the start state. We should probably check if joints outside the planning_group are uninitialised and print some sort of message to notify the user that we set them to default values as they were uninitialised.

Copy link
Member Author

@peterdavidfagan peterdavidfagan Feb 24, 2023

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I opened the following issue.

It might also be worth making @henningkayser aware of this as it relates to moveit_cpp.

I don't have capacity to work on this today as I have to focus on writing today, I would be happy to pick this up next week though.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I think this solution is perfectly fine. The values are not initialized to default values because of performance reasons. In Python this doesn't exist, afaik, so always using default values is the way to go.

Copy link
Contributor

@sea-bass sea-bass left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I'll approve this as things work for me, but I have a few minor comments that would be nice to get in. Hopefully they don't take long.

There are also still some lingering comments in moveit_py/src/moveit/moveit_core/planning_scene/planning_scene.cpp, moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp, and `moveit_py/src/moveit/moveit_ros/moveit_cpp/planning_component.cpp, from my last reviews. They are all related to docstrings, but still would be good to get in.

moveit_py/moveit/servo_client/__init__.py Outdated Show resolved Hide resolved
moveit_py/moveit/utils.py Outdated Show resolved Hide resolved
Check if a transform to the frame id is known.
This will be known if id is a link name, an attached body id or a collision object.
Args:
robot_state (:py:class:`moveit_py.core.RobotState`): The robot state to check.
Copy link
Contributor

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Bump. This docstring should still be fixed, right? Looking again is it just that the robot_state arg should be removed in this one?

@peterdavidfagan
Copy link
Member Author

peterdavidfagan commented Feb 23, 2023

By not providing "Findservice_msgs.cmake" in CMAKE_MODULE_PATH this project
has asked CMake to find a package configuration file provided by
"service_msgs", but CMake did not find one.

Could not find a package configuration file provided by "service_msgs" with
any of the following names:

  service_msgsConfig.cmake
  service_msgs-config.cmake

Add the installation prefix of "service_msgs" to CMAKE_PREFIX_PATH or set
"service_msgs_DIR" to a directory containing one of the above files.  If
"service_msgs" provides a separate development package or SDK, be sure it
has been installed. 
Call Stack (most recent call first):
/opt/ros/rolling/share/sensor_msgs/cmake/sensor_msgsConfig.cmake:41 (include)
CMakeLists.txt:22 (find_package)

CI for rolling is currently failing during the build stage for the moveit_msgs package. It seems related to the service_msgs dependency not being found. Should I look to fix this through opening a pr on ROS 2 repos or should I ignore this CI error for now?

@peterdavidfagan
Copy link
Member Author

@sea-bass @JafarAbdi do we want to keep the commit history as is in its current form. I don't want to squash anything that has a co-contributor as I believe they should get credit for these commits. Happy to squash and add multiple co-contributors to the one add_new_python_bindings commit.

@sea-bass
Copy link
Contributor

I would say definitely squash. If you can give others credit in that squash commit, that's good!

Co-authored-by: Henning Kayser <henningkayser@picknik.ai>
Co-authored-by: Michael Gorner <me@v4hn.de>
Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
Co-authored-by: AndyZe <zelenak@picknik.ai>
Co-authored-by: Peter Mitrano <mitranopeter@gmail.com>
Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com>
Co-authored-by: Jafar <jafar.uruc@gmail.com>
Co-authored-by: Shahwas Khan <shahwazk@usc.edu>
@peterdavidfagan
Copy link
Member Author

peterdavidfagan commented Feb 23, 2023

@sea-bass I have squashed the commits as requested.

CI seems to be failing due to a dependency issue but this issue is unrelated to the changes in this pr.

@JafarAbdi
Copy link
Contributor

Ok, seems like no one wants to click the merge button. I'm doing it :D

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

10 participants