[BUG] Potential self-collision with fetch_moveit_config

[mptangtang]

Describe the bug
When I tried to move the fetch arm to some pose in cartesian space with torso fixed to some designated height, I found it is difficult to realize that and the method I figured out is easily causing self-collision or no solution.

To Reproduce
I attached my code to here.

The test program tries to raise fetch's torso to the height of 0.3 and then fixed the torsor and only move the fetch's 7DOF arm to two poses defined by user in cartesian space. The following code failed in gazebo simulation, with fetch arm hit the fetch platform.

#include <ros/ros.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include <vector>
#include <string>
using std::vector;
using std::string;

void print_joint_value(unsigned int joint_index, double joint_value){
  ROS_INFO("Joint %d's value is %f. ", joint_index, joint_value);
}
int main(int argc, char **argv)
{
  ros::init(argc, argv, "fetch_moveit_switch_move_group");
  ros::NodeHandle nh;

  /////////////////////////////////////////////////////
  //  Start ROS spinner
  ros::AsyncSpinner spinner(1);
  spinner.start();
  /////////////////////////////////////////////////////

  // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
  // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
  // are used interchangably.
  static const std::string ARM_TORSO_PLANNING_GROUP = "arm_with_torso";       //  arm torso
  static const std::string ARM_PLANNING_GROUP = "arm";                        //  arm

  // The :move_group_interface:`MoveGroupInterface` class can be easily
  // setup using just the name of the planning group you would like to control and plan for.
  moveit::planning_interface::MoveGroupInterface arm_torso_group(ARM_TORSO_PLANNING_GROUP);
  moveit::planning_interface::MoveGroupInterface arm_group(ARM_PLANNING_GROUP);

  // Raw pointers are frequently used to refer to the planning group for improved performance.
  const robot_state::JointModelGroup* arm_torso_joint_model_group = arm_torso_group.getCurrentState()->getJointModelGroup(ARM_TORSO_PLANNING_GROUP);
  const robot_state::JointModelGroup* arm_joint_model_group = arm_group.getCurrentState()->getJointModelGroup(ARM_PLANNING_GROUP);

  // We can get a list of all the groups in the robot:
  ROS_INFO("-----------------------------------------------");
  ROS_INFO_NAMED("tutorial_1", "Available Planning Groups1:");
  std::copy(arm_torso_group.getJointModelGroupNames().begin(), arm_torso_group.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));
  ROS_INFO("\n-----------------------------------------------");

  ROS_INFO_NAMED("tutorial_2", "Available Planning Groups2:");
  std::copy(arm_group.getJointModelGroupNames().begin(), arm_group.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));
  ROS_INFO("\n-----------------------------------------------");

  //  Show fix joints
  vector<string> joints = arm_group.getJoints();
  for (unsigned int i = 0; i<joints.size(); ++i) {
    ROS_INFO("The %dth arm_group joints are %s", i, joints[i].c_str());
  }
  // The package MoveItVisualTools provides many capabilties for visualizing objects, robots,
  // and trajectories in RViz as well as debugging tools such as step-by-step introspection of a script
  namespace rvt = rviz_visual_tools;
  moveit_visual_tools::MoveItVisualTools visual_tools("base_link");
  visual_tools.deleteAllMarkers();
  visual_tools.trigger();

  ///////////////////////////////////////////////////////////
  //  Raise the torso for initialization of fetch
  std::vector<double> fetch_default_positions;
  fetch_default_positions.resize(8);
  fetch_default_positions[0] = 0.3;
  fetch_default_positions[1] = 1.319999;
  fetch_default_positions[2] = 1.399986;
  fetch_default_positions[3] = -0.199989;
  fetch_default_positions[4] = 1.719962;
  fetch_default_positions[5] = 0.000001;
  fetch_default_positions[6] = 1.660008;
  fetch_default_positions[7] = -0.000001;

  moveit::core::RobotStatePtr current_state = arm_torso_group.getCurrentState();
  std::vector<double> current_group_positions;
  current_state->copyJointGroupPositions(arm_torso_joint_model_group, current_group_positions);
  ROS_INFO("----------------  Current Fetch Joint Values  ----------------");
  for(unsigned int i = 0; i<current_group_positions.size(); ++i){
    print_joint_value(i, current_group_positions[i]);
  }

  arm_torso_group.setJointValueTarget(fetch_default_positions);
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  bool success = (arm_torso_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  if(success){
    arm_torso_group.execute(my_plan);
    ROS_INFO("Fetch move to initial position in joint space.");
    sleep(2.0);
  }else{
    ROS_INFO("Planning fetch to initial joint space failed!");
    return 0;
  }
  visual_tools.prompt("Initialize fetch to the default position. \n"
                      "Press 'next' in the RvizVisualToolsGui window to continue the next movement.");

  //////////////////////////////////////////////////////////////////////////////
  /// \brief target_pose1

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.28;
  target_pose1.position.y = -0.3;
  target_pose1.position.z = 1.3;

  current_state = arm_torso_group.getCurrentState();
  current_state->copyJointGroupPositions(arm_torso_joint_model_group, current_group_positions);
  ROS_INFO("----------------  Current Fetch Joint Values  ----------------");
  for(unsigned int i = 0; i<current_group_positions.size(); ++i){
    print_joint_value(i, current_group_positions[i]);
  }

  robot_state::RobotState start_state(*arm_group.getCurrentState());

  start_state.setVariablePosition("torso_lift_joint", 0.3);
  start_state.update(true);

  ROS_INFO("The torso's position is %f", start_state.getVariablePosition("torso_lift_joint"));

  arm_group.setStartState(start_state);
  ROS_INFO("After setting start state, the torso's position is %f", arm_group.getCurrentState()->getVariablePosition("torso_lift_joint"));
  arm_group.rememberJointValues("torso_lift_joint");
  arm_group.setJointValueTarget("torso_lift_joint", 0.3);

  arm_group.setPoseTarget(target_pose1);
  ROS_INFO("After setting start state, the torso's position is %f", arm_group.getCurrentState()->getVariablePosition("torso_lift_joint"));

  moveit::planning_interface::MoveGroupInterface::Plan my_plan2;
  success = (arm_group.plan(my_plan2) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  ROS_INFO("After setting start state, the torso's position is %f", arm_group.getCurrentState()->getVariablePosition("torso_lift_joint"));

  if(success){
//    arm_group.move();
    arm_group.execute(my_plan2);
    sleep(3.0);
  }

  ROS_INFO("After movement, the torso's position is %f", arm_group.getCurrentState()->getVariablePosition("torso_lift_joint"));

  return 0;
}

After running this code in gazebo simulation. The fetch arm hit the fetch platform. After this I tried to use arm_with_torso move_group only. To realize the function that move the fetch arm with torso fixed. I add constraint to fetch arms. The testing code could be seen as follows. Although this solution realize the arm movement with fixed torso, however, there are some implicit problems. One problem is self-collision. When I test the code on physical fetch robot. The arm might easily hit the fetch platform. Althought it didn't happen all the time, it still have a very high potential with more than 50%. Another problem is that, since constraint is add to move_group, there are a very high probability that no solution is found. So I don't think this method should be the correct one. Could anybody help me with this ?

#include <ros/ros.h>

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

void print_joint_value(unsigned int joint_index, double joint_value){
  ROS_INFO("Joint %d's value is %f. ", joint_index, joint_value);
}


int main(int argc, char **argv)
{
  ros::init(argc, argv, "fetch_moveit_with_joint_constraint");
  ros::NodeHandle nh;

  /////////////////////////////////////////////////////
  //  Start ROS spinner
  ros::AsyncSpinner spinner(1);
  spinner.start();
  /////////////////////////////////////////////////////

  // MoveIt operates on sets of joints called "planning groups" and stores them in an object called
  // the `JointModelGroup`. Throughout MoveIt the terms "planning group" and "joint model group"
  // are used interchangably.
  static const std::string PLANNING_GROUP = "arm_with_torso";   //  arm

  // The :move_group_interface:`MoveGroupInterface` class can be easily
  // setup using just the name of the planning group you would like to control and plan for.
  moveit::planning_interface::MoveGroupInterface move_group(PLANNING_GROUP);
  move_group.setPlanningTime(10);
  move_group.setPlannerId("RRTConnectkConfigDefault");
  //move_group.setPlannerId("RRTstarkConfigDefault");

  // Raw pointers are frequently used to refer to the planning group for improved performance.
  const robot_state::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(PLANNING_GROUP);

  // We can get a list of all the groups in the robot:
  ROS_INFO_NAMED("tutorial", "Available Planning Groups:");
  std::copy(move_group.getJointModelGroupNames().begin(), move_group.getJointModelGroupNames().end(), std::ostream_iterator<std::string>(std::cout, ", "));

  ///////////////////////////////////////////////////////////
  //  Raise the torso for initialization of fetch
  std::vector<double> fetch_default_positions;
  fetch_default_positions.resize(8);
  fetch_default_positions[0] = 0.3;
  fetch_default_positions[1] = 1.319999;
  fetch_default_positions[2] = 1.399986;
  fetch_default_positions[3] = -0.199989;
  fetch_default_positions[4] = 1.719962;
  fetch_default_positions[5] = 0.000001;
  fetch_default_positions[6] = 1.660008;
  fetch_default_positions[7] = -0.000001;

  move_group.setStartState(*move_group.getCurrentState());
  move_group.setJointValueTarget(fetch_default_positions);
  moveit::planning_interface::MoveGroupInterface::Plan my_plan;
  bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  if(success){
    move_group.execute(my_plan);
    ROS_INFO("Fetch move to initial position in joint space.");
    sleep(2.0);
  }else{
    ROS_INFO("Planning fetch to initial joint space failed!");
    return 0;
  }

  ////////////////////////////////////////////////
  //  Move with torso joint fixed
  ////////////////////////////////////////////////

  moveit_msgs::JointConstraint jcm;
  jcm.joint_name = "torso_lift_joint";
  jcm.position = fetch_default_positions[0];
  jcm.tolerance_below = 0.01;
  jcm.tolerance_above = 0.01;
  jcm.weight = 1.0;

  moveit_msgs::Constraints fix_torso_constraint;
  fix_torso_constraint.joint_constraints.push_back(jcm);
  move_group.setPathConstraints(fix_torso_constraint);

  geometry_msgs::Pose target_pose1;
  target_pose1.orientation.w = 1.0;
  target_pose1.position.x = 0.38;
  target_pose1.position.y = -0.3;
  target_pose1.position.z = 0.8;

  geometry_msgs::Pose target_pose2;
  target_pose2.orientation.w = 1.0;
  target_pose2.position.x = 0.38;
  target_pose2.position.y = -0.4;
  target_pose2.position.z = 0.9;

  move_group.setStartState(*move_group.getCurrentState());
  move_group.setPoseTarget(target_pose1);
  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  if(success){
    move_group.execute(my_plan);
    ROS_INFO("Fetch move to pose 1!");
    sleep(3.0);
  }

  move_group.clearPathConstraints();
  moveit_msgs::OrientationConstraint ocm;
  ocm.link_name = "wrist_roll_link";
  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 = 1.0;
  fix_torso_constraint.orientation_constraints.push_back(ocm);
  move_group.setPathConstraints(fix_torso_constraint);

  move_group.setStartState(*move_group.getCurrentState());
  move_group.setPoseTarget(target_pose2);
  success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
  if(success){
    move_group.execute(my_plan);
    ROS_INFO("Fetch move to pose 2!");
    sleep(3.0);
  }

  ros::shutdown();
  return 0;
}

Expected behavior
The fetch robot firstly move the torso up to 0.3 and then the torso is fixed. Then the fetch robot move its arm to pose1 and then to pose2 with fixed torso height.

Screenshots
If applicable, add screenshots or links to YouTube.

catkin workspace (please complete the following information):

• fetch_ros version: melodic devel
• Ubuntu version: 18.04
• ROS version: melodic
• current catkin workspace information wstool info catkin build

Additional context
Add any other context about the problem here.

[mamoll]

@mptangtang can you check if PR #159 fixes this?