Dear reader,

I'm attempting to implement the MoveIt: MoveGroup C++ interface tutorial(https://ros-planning.github.io/moveit_tutorials/doc/move_group_interface/move_group_interface_tutorial.html).

Current setup: Noetic, using MoveIt1 combined with Gazebo. Running on Ubuntu 20.04. I'm using the Bio_IK solver to find IK solutions for the 5 DOF arms.

I'm customizing the tutorial for two cooperative 5 DOF robotic arms. I am able to specify a pose goal for my initial Arm (it is also possible to actually move to said pose goal). Whenever I attempt to set Path constraints i keep encountering the same warning and issue being: Invalid start state warning.

This seems quite odd, since I am sure I can move to the specified pose goal (now used as start state) and I am pretty sure it is valid. The Code snippet at the bottom does NOT showcase the whole tutorial code, The rest of the code is identical to the tutorial.

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.00370718;
  target_pose1.position.y = 0.133665;
  target_pose1.position.z = 0.273979;
  move_group_interface.setPoseTarget(target_pose1);

  moveit::planning_interface::MoveGroupInterface::Plan my_plan;

  bool success = (move_group_interface.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);

  ROS_INFO_NAMED("tutorial", "Visualizing plan 1 (pose goal) %s", success ? "" : "FAILED");


  moveit::core::RobotStatePtr current_state = move_group_interface.getCurrentState();

  move_group_interface.setMaxVelocityScalingFactor(0.05);
  move_group_interface.setMaxAccelerationScalingFactor(0.05);

  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "link5";
  ocm.header.frame_id = "base_link";
  ocm.orientation.w = 1.0;
  ocm.absolute_x_axis_tolerance = 0.1;
  ocm.absolute_y_axis_tolerance = 0.1;
  ocm.absolute_z_axis_tolerance = 0.1;
  ocm.weight = 0.5;

  // Now, set it as the path constraint for the group.
  moveit_msgs::Constraints test_constraints;
  test_constraints.orientation_constraints.push_back(ocm);
  move_group_interface.setPathConstraints(test_constraints);

  moveit::core::RobotState start_state(*move_group_interface.getCurrentState());
  geometry_msgs::Pose start_pose2;
  start_pose2.orientation.w = 1.0;
  start_pose2.position.x = 0.00370718;
  start_pose2.position.y = 0.133665;
  start_pose2.position.z = 0.273979;
  start_state.setFromIK(joint_model_group, start_pose2);
  move_group_interface.setStartState(start_state);

  // Now we will plan to the earlier pose target from the new
  // start state that we have just created.
  move_group_interface.setPoseTarget(target_pose1);

  // Planning with constraints can be slow because every sample must call an inverse kinematics solver.
  // Lets increase the planning time from the default 5 seconds to be sure the planner has enough time to succeed.
  move_group_interface.setPlanningTime(10.0);

  success = (move_group_interface.plan(my_plan) == moveit::core::MoveItErrorCode::SUCCESS);
  ROS_INFO_NAMED("tutorial", "Visualizing plan 3 (constraints) %s", success ? "" : "FAILED");

Error log:

 [ INFO] [1682431160.942987275, 876.261000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1682431160.944909963, 876.262000000]: Planner configuration 'Arm1' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1682431160.945081430, 876.262000000]: Arm1/Arm1: Starting planning with 1 states already in datastructure
[ INFO] [1682431162.447994385, 877.738000000]: Arm1/Arm1: Created 4 states (2 start + 2 goal)
[ INFO] [1682431162.448044675, 877.738000000]: Solution found in 1.502999 seconds
[ INFO] [1682431162.464745724, 877.756000000]: SimpleSetup: Path simplification took 0.000483 seconds and changed from 3 to 2 states
[ INFO] [1682431164.229105654, 879.521000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1682431164.229915562, 879.521000000]: Planner configuration 'Arm1' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1682431164.230078463, 879.521000000]: Arm1/Arm1: Starting planning with 1 states already in datastructure
[ INFO] [1682431164.240447016, 879.531000000]: Arm1/Arm1: Created 5 states (2 start + 3 goal)
[ INFO] [1682431164.240749224, 879.531000000]: Solution found in 0.010742 seconds
[ INFO] [1682431164.241940561, 879.533000000]: SimpleSetup: Path simplification took 0.001162 seconds and changed from 4 to 2 states
[ INFO] [1682431168.545188942, 883.821000000]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[ INFO] [1682431168.765452052, 884.027000000]: Planner configuration 'Arm1' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ WARN] [1682431168.765792253, 884.027000000]: Arm1/Arm1: Skipping invalid start state (invalid state)
[ERROR] [1682431168.765869164, 884.028000000]: Arm1/Arm1: Motion planning start tree could not be initialized!
[ INFO] [1682431168.765913508, 884.028000000]: No solution found after 0.000338 seconds
[ WARN] [1682431168.765928980, 884.028000000]: Invalid start state
[ WARN] [1682431168.775218905, 884.040000000]: Goal sampling thread never did any work.
[ INFO] [1682431168.775420675, 884.040000000]: Unable to solve the planning problem

Attempted Solutions: - Completely specifying the orientational constraint to be the same as the start state.

• Finding another valid pose with the orientational constraint W = 1.0 setting this as target_pose1

• Changing the Absolutexaxistolerance, Absoluteyaxistolerance and Absolutezaxis_tolerance Values to: 3.14, 6.28 and 0.0 To see if it would accept the start state when the tolerances should accept any state.

• Picking a new and different start state (also tried the default x=0,y=0,z=0 pose).

• Moving the Robotic arm to the Start_pose then setting the start state and attempting to plan to the same pose.

• Running just the constraint parts in a different c++ project.

p.s. if any information is missing please let me know, this is my first post on the ROS forums.

Thank you in advance,

Bas