Unable to plan to pose using C++ MoveIt

edit

We are using the UR3 with the Universal_Robots_ROS_Driver (and the fmauch description, as described in the readme)

I can move the robot to a jointpose, using RVIZ and my C++ node. I can move the robot to a pose using RVIZ, I can't move the robot to a pose using my C++ node:

my C++ node; target_pose is a copy of currentPose, so it should be valid:

#include <ros/ros.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/DisplayTrajectory.h>
#include <moveit_msgs/DisplayRobotState.h>
#include <moveit_msgs/AttachedCollisionObject.h>
#include <moveit_msgs/CollisionObject.h>
#include <moveit/robot_model_loader/robot_model_loader.h>
#include <moveit/robot_model/robot_model.h>
#include <moveit/robot_state/robot_state.h>

ros::Publisher display_publisher;

int main(int argc, char **argv)
{

  ros::init(argc, argv, "custom_planning");
  ros::NodeHandle nh;
  ros::AsyncSpinner spinner(1);
  spinner.start();
  sleep(1.0);

  //planning group that we would like to control
  moveit::planning_interface::MoveGroupInterface group("manipulator");
  //we can add or remove collision objects in our virtual world scene
  moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  //raw pointers are used to refer to the planning group for improved performance
  const robot_state::JointModelGroup *joint_model_group = group.getCurrentState()->getJointModelGroup("manipulator");
  group.setEndEffectorLink("tcp");
  group.setPoseReferenceFrame("world");
  group.setPlannerId("RRTstar");
  group.setNumPlanningAttempts(10);
  group.setPlanningTime(50.0);
  group.allowReplanning(true);
  group.setGoalJointTolerance(0.0001);
  group.setGoalPositionTolerance(0.0001);
  group.setGoalOrientationTolerance(0.001);

  group.setNamedTarget("home");
  group.move();
  sleep(1.0);


  //save current pose ( == home)
  geometry_msgs::PoseStamped validPose = group.getCurrentPose();

  //move robot by joint values (only joint 1 will move)
  std::vector<double> jointValues = {-1, -1.57, 0, -1.57, 0, 0};
  group.setJointValueTarget(jointValues);
  if (!group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS)
  {
    std::cout << "planning unsuccesfull" << std::endl;
  }
  else
  {
    if (!group.execute(my_plan))
    {
      std::cout << "execute plan not finished" << std::endl;
    }
    else
    {
      std::cout << "moved to jointValues succesfully" << std::endl;
    }
  }

  //move robot to pose
  geometry_msgs::Pose target_pose = validPose.pose;

  group.setStartStateToCurrentState();
  group.setPoseTarget(target_pose);
  //group.setPoseTarget(target_pose);

  if (!group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS)
  {
    std::cout << "planning unsuccesfull" << std::endl;
  }
  else
  {
    if (!group.execute(my_plan))
    {
      std::cout << " execute plan not finished" << std::endl;
    }
    else
    {
      std::cout << "!!! moved to pose succesfully !!!" << std::endl;
    }
  }

  sleep(1.0);
  ros::shutdown();

  return 0;
}

The outputted error when using RTTstar as planner:

........
[ INFO] [1639051955.890086461]: manipulator[RRTstar]: Created 13704 new states. Checked 47098712 rewire options. 0 goal states in tree. Final solution cost inf
[ WARN] [1639051955.890182136]: ParallelPlan::solve(): Unable to find solution by any of the threads in 49.977496 seconds
[ INFO] [1639051955.890243532]: Unable to solve the planning problem

The outputted error when using RTTConnect as planner:

[ WARN] [1639053098.198024043]: Fail: ABORTED: No motion plan found. No execution attempted.


[ INFO] [1639053098.177055365]: manipulator[RRTConnect]: Starting planning with 1 states already in datastructure
[ERROR] [1639053098.196880491]: manipulator[RRTConnect]: Unable to sample any valid states for goal tree
[ INFO] [1639053098.196926731]: manipulator[RRTConnect]: Created 1 states (1 start + 0 goal)
[ERROR] [1639053098.196956924]: manipulator[RRTConnect]: Unable to sample any valid ...