setPoseTarget & plan fails for a custom robot using the move_group_interface in c++

Description

Hello all,

we are currently working on a custom robot and i now want to get moveit working for it. Everything seems to be set up correctly so far, but i now run into an issue when I try to use the move_group_interface and try to plan a trajectory for a setPoseTarget. Find below some more information regarding my issue.

I have now spend too many hours trying to fix the issue but i can't get it to work. I am really looking forward to your support. It's probably a very small issue but I don't know where to look as the error messages don't help me.

If you have any further questions regarding my problem, don't hesitate to ask!

Kind regards, Niklas

Situation overview:

When launching our robot, gazebo, the robot_state_publisher, controller_manager and rviz2 are launched successfully.
We are able to control all axis using rqt and the controllers plugins.
Using Rviz2 we can drag on the endeffector or set joint values and plan&execute successfully. The robot is then moving in gazebo and in rviz2.
When using the move_group_interface in c++ I am able to retrieve correct information from the selected move_group regarding pose, position, etc. I am also able to setJointValueTarget, plan and execute successfully via the node.
What I want to achieve is to set a random pose for the move_group and plan&execute it. As far as I understand the received pose from the move_group function is a valid pose.
When setting a Pose (setPoseTarget) and planning, rviz/moveit receives a planning request. It also seems to try to calculate a solution as the computers fan speeds up. The planning fails with:

`[move_group-7] [INFO] [1686905421.771838032] [moveit_move_group_default_capabilities.move_action_capability]: Received request

[move_group-7] [INFO] [1686905421.772043455] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline. [move_group-7] [INFO] [1686905421.772920375] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'PM_Robot_Cam1_TCP' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed. [move_group-7] [ERROR] [1686905431.775821250] [ompl]: ./src/ompl/geometric/planners/rrt/src/RRTConnect.cpp:252 - PM_Robot_Cam1_TCP/PM_Robot_Cam1_TCP: Unable to sample any valid states for goal tree [move_group-7] [WARN] [1686905431.775864690] [moveit.ompl_planning.model_based_planning_context]: Timed out [move_group-7] [INFO] [1686905431.834494840] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem [move_group-7] [INFO] [1686905431.834547527] [moveit_move_group_default_capabilities.move_action_capability]: Timeout reached`

As a test I changed the move_group in my c++ node to "panda_arm" and launched the panda demo.launch.py. There I am able to setPoseTaget and also to plan&execute it via my node! Everything works as expected. So there is probably nothing wrong with my node.

I have added a file to our moveit-troubleshoot branch (see link for github below; there is a folder called "troubleshoot") with the terminal output for:

launching our robot
output when running the move_group c++ node
output in moveit when receiving a request

Our environment

Humble
Ubuntu 22.04
Binary build (Followed installation guide on moveit "Getting Started")
Our repo is installed in the same workspace as the moveit tutorials

Steps to reproduce

If you want to reproduce my issue you can run the launchfile for our robot and the node executing the planing request.

You can try our robot by cloning our repository and building it: https://github.com/match-PM/match_pm_robot.git

I have created a new branch for this issue (moveit-troubleshoot), as work is ongoing on the main branch. Our github also gives some instructions for installation.

To launch the robot execute: ros2 launch pm_robot_bringup pm_robot_sim_HW.launch.py

For the node there is another repo: https://github.com/match-PM/moveit_tests.git

Just build it and execute: ros2 run moveit_tests pm_moveit_tests

moveit / moveit2_tutorials