Skipping invalid start state (invalid state)

edit

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 ...