Moveit pick and place using moveit_simple_grasps generator

[Mostafa Said]

Moveit! Users

I am working on a pick and place trial (excavator robot) using moveit pick and place & moveit_simple_grasps to generate grasps for the pick and place pipeline. I have the grasp generator working fine but no matter what parameters i change in the config .yaml file required,  I always get an error with no motion plan generated. could someone help with the problem ? attached is the c++ code and the grasp_data.yaml file required in moveit_simple_grasps generator .

Error

[INFO] [1521633757.233173297]: Added plan for pipeline 'pick'. Queue is now of size 130 
[ INFO] [1521633757.233387878]: Added plan for pipeline 'pick'. Queue is now of size 131
[ INFO] [1521633757.233565758]: Added plan for pipeline 'pick'. Queue is now of size 132 
[ INFO] [1521633757.233661266]: Added plan for pipeline 'pick'. Queue is now of size 133 
[ INFO] [1521633757.259952855]: Manipulation plan 17 failed at stage 'reachable & valid pose filter' on thread 2 
[ INFO] [1521633757.270448057]: Manipulation plan 119 failed at stage 'reachable & valid pose filter' on thread 3 
[ INFO] [1521633757.270590459]: Manipulation plan 16 failed at stage 'reachable & valid pose filter' on thread 1
[ INFO] [1521633757.270922774]: Manipulation plan 118 failed at stage 'reachable & valid pose filter' on thread 0 
[ INFO] [1521633757.294972059]: Manipulation plan 51 failed at stage 'reachable & valid pose filter' on thread 2 
[ INFO] [1521633757.306820635]: Manipulation plan 84 failed at stage 'reachable & valid pose filter' on thread 1 
[ INFO] [1521633757.309723185]: Manipulation plan 85 failed at stage 'reachable & valid pose filter' on thread 0 
[ INFO] [1521633758.492223810]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1 
[ INFO] [1521633758.500294621]: IK failed 
[ INFO] [1521633758.507225297]: IK failed 
[ INFO] [1521633758.515416594]: IK failed 
[ INFO] [1521633758.515482202]: Sampler failed to produce a state 
[ INFO] [1521633758.515531577]: Manipulation plan 1 failed at stage 'reachable & valid pose filter' on thread 0
[ INFO] [1521633758.515687029]: Pickup planning completed after 1.270707 seconds 
[rviz_mostafabakr_Inspiron_3537_29798_1993362218023763011-5] 
killing on exit [move_group-4] killing on exit 
[robot_state_publisher-3] killing on exit 
[joint_state_publisher-2] killing on exit 
[virtual_joint_broadcaster_0-1] killing on exit shutting down processing monitor...





C++ code

#include <ros/ros.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <geometric_shapes/solid_primitive_dims.h>
#include <moveit_simple_grasps/simple_grasps.h>
#include <moveit_simple_grasps/grasp_data.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

void pick(moveit::planning_interface::MoveGroupInterface &group){
  
  ros::NodeHandle nh;
  moveit_simple_grasps::SimpleGraspsPtr simpleGrasps;
  moveit_visual_tools::MoveItVisualToolsPtr visualTools;
  moveit_simple_grasps::GraspData graspData;
  //graspData.base_link_ = "chassis";

  if (!graspData.loadRobotGraspData(nh, "gripper"))
    ros::shutdown();

  visualTools.reset(new moveit_visual_tools::MoveItVisualTools("chassis"));
  simpleGrasps.reset(new moveit_simple_grasps::SimpleGrasps(visualTools));

  geometry_msgs::Pose objectPose;
  objectPose.position.x = 3.0;
  objectPose.position.y = 0.0;
  objectPose.position.z = 0.25;
  objectPose.orientation.x = 0.0;
  objectPose.orientation.y = 0.0;
  objectPose.orientation.z = 0.0;
  objectPose.orientation.w = 1.0;

  std::vector<moveit_msgs::Grasp> possibleGrasps;
  simpleGrasps->generateBlockGrasps(objectPose, graspData, possibleGrasps);
  ROS_INFO_STREAM(possibleGrasps[0].grasp_pose.header.frame_id);

  group.setPlanningTime(50.0);
  group.pick("barrel", possibleGrasps);
}


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

  ros::init(argc, argv, "grippertrial");
  //ros::NodeHandle nh2("~");
  ros::AsyncSpinner spinner(1);
  spinner.start();

  moveit::planning_interface::MoveGroupInterface moveGroup("e1_complete");
  moveit::planning_interface::PlanningSceneInterface planningScene;
  const robot_state::JointModelGroup* jointModelGroup = moveGroup.getCurrentState()->getJointModelGroup("gripper");

  moveGroup.setPlanningTime(45.0);

  moveit_msgs::CollisionObject barrel;
  barrel.id = "barrel";

  barrel.operation = moveit_msgs::CollisionObject::ADD;
  shape_msgs::SolidPrimitive primitive;
  primitive.type = primitive.CYLINDER;
  primitive.dimensions.resize(2);
  primitive.dimensions[0] = 0.5;   //length
  primitive.dimensions[1] = 0.2;   //radius

  /*primitive.type = primitive.BOX;
  primitive.dimensions.resize(3);
  primitive.dimensions[0] = 0.5;
  primitive.dimensions[1] = 0.2;
  primitive.dimensions[2] = 0.2;*/


  //barrel upright
  geometry_msgs::Pose barrelPose;
  barrelPose.position.x = 3.0;
  barrelPose.position.y = 0.0;
  barrelPose.position.z = 0.25;
  barrelPose.orientation.x = 0.0;
  barrelPose.orientation.y = 0.0;
  barrelPose.orientation.z = 0.0;
  barrelPose.orientation.w = 1.0;

  barrel.primitives.push_back(primitive);
  barrel.primitive_poses.push_back(barrelPose);

  std::vector<moveit_msgs::CollisionObject> collisionObjectsVector;
  collisionObjectsVector.push_back(barrel);
  std::vector<std::string> objectIds;
  objectIds.push_back(barrel.id);
  planningScene.removeCollisionObjects(objectIds);
  planningScene.addCollisionObjects(collisionObjectsVector);
  //sleep(5.0);

  ros::WallDuration(5.0).sleep();

  ros::WallDuration(1.0).sleep();
  pick(moveGroup);

  ros::WallDuration(1.0).sleep();
  return 0;
}


.yaml file


base_link: 'chassis'

gripper:
  end_effector_name: 'gripper' #eef group name 

  # Default grasp params
  joints: ['gripper_rotator_joint' , 'gripper_left_joint' , 'gripper_right_joint']

  #open Gripper
  pregrasp_posture: [0.0, 1.0, 1.0]
  pregrasp_time_from_start: 4.0

  #closed Gripper
  grasp_posture: [0.0, 0.8, 0.8]
  grasp_time_from_start: 4.0

  postplace_time_from_start: 4.0

  #max object width that can fit between fingers
  max_grasp_width : 1.0

  # Desired pose from end effector to grasp [x, y, z] + [R, P, Y]
  grasp_pose_to_eef: [3.07846,0,0.234393]
  grasp_pose_to_eef_rotation: [0.0, 0.1963494, 0.0]

  end_effector_parent_link: 'quickcoupler'


####################################################################


I tried to play around with the bold part in the .yaml file and also changing the pose and dimensions of the cylinder but nothing seems to work!
could the problem be in the parameters of the .yaml or is there something wrong with the code usage?? 
Thanks in advance