Moveit execution trajectory fails on O3DE Manipulation Template

[utk-hua]

Bug Description In a project created with the O3DE Manipulation Template , when planning a trajectory on the Moveit Rviz interface, the planning step succeeds but the execution step results in a fail status with no movement of the robot on o3de.

Assets required O3DE Manipulation Template

Steps to reproduce Steps to reproduce the behavior:

1. Build the O3DE Engine with PhysX 5 enabled. (point-release/23102)
2. Download o3de-extras ROS Gem (point-release/23102 ) and register with the engine.
3. Create a new project from the O3DE Manipulation Template and build the project.
4. Start an existing level (RoboticManipulation) and go into Game Mode.
5. Run the ros launch file ros2 launch Examples/panda_moveit_config_demo.launch.py
6. Rviz opens, you can see the o3de camera image in the image panel of Rviz and the robot model.
7. Now plan a trajectory by moving any link of the robot around.
8. Press plan on the Moveit planner. You should see a plan being generated on the robot visualizer.
9. Press Plan&Execute on the Moveit planner, you would see no movement on the O3DE side and the Moveit panel in Rviz reports failed.

Expected behavior After pressing plan&execute on the moveit panel of Rviz, the robot should move in o3de.

Actual behavior After pressing Plan&Execute on the Moveit planner, no movement of the robot on the O3DE side and the Moveit panel in Rviz reports failed.

Screenshots/Video

Found in Branch point-release/23102 for both o3de and o3de-extras

Commit ID from o3de/o3de and o3de/o3de-extras repositories o3de : e22213ea946932dc2f94d9f1392e98b4bf2b2c3f o3de-extras : e2426a4242e9fd11c7b902f6aff3379af06d3556

Desktop/Device (please complete the following information):

• Device: PC
• OS: Linux Ubuntu 22.04
• Kernel: 6.5.0-15-generic
• CPU : Intel i7-13700KF
• GPU : Nvidia RTX 4090
• Memory : 128GB

[macmacal]

A couple of days ago I have very similar problem during my first O3DE + ROS 2 Gem setup, but I was able to overcome them. I used the most recent version from the development branches, both for the o3de and o3de-extras ( I can't check the commit hashes in this particular moment).

For your step 3, I was launching Project Manger from ${ENGINE_PATH}/build/linux/bin/profile/o3de which I used to create & build new project based on the Manipulation Example. Launching editor and running the game gave me a frozen vertical standing Panda robot - the initial positions from the JointsManipulationEditorComponent weren't set at all.

Double check if you are sticking to the project setup tutorial, especially with all CLI commands. Creating a fresh project and building it with proper flags resolved my case:

${O3DE_HOME}/scripts/o3de.sh create-project --project-path $PROJECT_PATH --template-path ${O3DE_EXTRAS_HOME}/Templates/Ros2RoboticManipulationTemplate
cd $PROJECT_PATH
cmake -B build/linux -G "Ninja Multi-Config" -DLY_DISABLE_TEST_MODULES=ON -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -DLY_STRIP_DEBUG_SYMBOLS=ON -DAZ_USE_PHYSX5:BOOL=ON 
cmake --build build/linux --config profile --target ${PROJECT_NAME} Editor ${PROJECT_NAME}.Assets
${PROJECT_PATH}/build/linux/bin/profile/Editor

[jhanca-robotecai]

I used the most recent version from the development branches, both for the o3de and o3de-extras ( I can't check the commit hashes in this particular moment).

Unless developing O3DE itself, I highly discourage using development branches at this moment. A few breaking changes are happening right now or happened very recently and, most likely, many problems will be detected.

---

@utk-hua I did a fresh build using o3de/o3de-extras 2310.2 (same commits as yours) and the template worked correctly for me.

It is hard to say what could go wrong in your setup. I do not know if you have any experience running O3DE demos, ROS2 and MoveIt, so please excuse me if some of my ideas are trivial.

1. Please check carefully which locale is set on your machine. O3DE has some problems while parsing numbers if separators for thousands and decimals are any different than US English. I faced this problem (I'm Polish) when trying to move a robot around, but all friction parameters were defaulted to 0.0 by O3DE, so there was no grip. I use LC_NUMERIC=en_US.UTF-8 on my machines.
2. Please check if you have all the required packages to run moveit. E.g., while developing ROSCon2023Demo we noticed it was enough to install ros-${ROS_DISTRO}-nav2-msgs package to get all navigation packages, but due to changes in package dependencies now we recommend to install ros-${ROS_DISTRO}-navigation2 instead. Something similar might have happened to this template and I cannot reproduce it in my setup.
3. Please double-check if you used PhysX5 variable when building

   jhanca@robotec-o3de:~/devroot/projects/Ros2RoboticManipulation/build/linux$ grep -nr --include '*.txt' 'PHYSX5'
   CMakeCache.txt:53:AZ_USE_PHYSX5:BOOL=ON

4. Please ensure ROS 2 is correctly sourced in your terminal, please ensure you use Cyclone DDS as your ROS 2 middleware

   jhanca@robotec-o3de:~$ echo $ROS_DISTRO $RMW_IMPLEMENTATION
   humble rmw_cyclonedds_cpp

5. Please check if you see the required topics, my topic list just in case:

   
   jhanca@robotec-o3de:~$ ros2 topic list
   /attached_collision_object
   /camera_image_color
   /camera_image_depth
   /clicked_point
   /clock
   /collision_object
   /color_camera_info
   /depth_camera_info
   /display_contacts
   /display_planned_path
   /goal_pose
   /initialpose
   /joint_states
   /monitored_planning_scene
   /motion_plan_request
   /parameter_events
   /planning_scene
   /planning_scene_world
   /pointcloud
   /recognized_object_array
   /robot_description
   /robot_description_semantic
   /rosout
   /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback
   /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/update
   /tf
   /tf_static
   /trajectory_execution_event

One more question about your setup: do you see any movement of the robot when you start the game? By default, some joints should move
![image](https://github.com/o3de/o3de-extras/assets/134940295/5a87a660-1f2f-4f60-ba63-82bb0370bbbb)

[michalpelka]

Hello @utk-hua did you managed to run articulated robot with guidance from Jan? Please give us feedback to have this issue concluded.

[jhanca-robotecai]

I will close the issue due to no response. Please reopen if you still have any problems.