I'm running on Ubuntu 16.0.4 with ROS kinetic installed.

Project background

I have an engineering background with not that much programming experience so I'm sorry if I assume some things that are wrong.

My goal is to make a robot scan a box and cut through the middle of it. For my project I don't have to use an actual scanner to get a point cloud of the box, we implement a virtual point cloud in it instead. To make a "minimal viable product", I want to insert the box at a known location, still make it move around above the box to scan it, and then make it cut through it. Weĺl use a standard size box for now with a known location so I only need it to move from known coordinate A to known coordinate B.

Problem description

I am currently using Godel (https://github.com/ros-industrial-consortium/godel) for the scan part so I'm using the following code in the terminal to launch a irb2400 robot with the standard Godel package:

roslaunch godel_irb2400_support irb2400_blending.launch

What I mean by using the scan part is that I use the first step of Godel to make the robot move around above the box where it would usually make the point cloud, but I think this is irrelevant for the problem.

To make the robot move from location A to location B I want to use this part of the Moveit! tutorial page (http://docs.ros.org/kinetic/api/moveit_tutorials/html/doc/move_group_python_interface/move_group_python_interface_tutorial.html). Specifically, I'm using the movegrouppythoninterfacetutorial.py file and I've adjusted it to fit my own purposes.

So far most parts of the python script work with both the Panda robot (the one that is in the Moveit! tutorial) and with the irb2400 (the arm from the Godel package) with some minor adjustments. However, the part where it needs to go to a certain "pose" doesn't work for me.

I get the following lines in the terminal I used to launch the Godel package

[ INFO] [1573566726.873179897]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline.
[ INFO] [1573566726.874144183]: Planning attempt 1 of at most 1
[ INFO] [1573566726.878820828]: Planner configuration 'manipulator' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1573566726.880605002]: RRTConnect: Starting planning with 1 states already in datastructure
[ERROR] [1573566731.884029276]: RRTConnect: Unable to sample any valid states for goal tree
[ INFO] [1573566731.884102184]: RRTConnect: Created 1 states (1 start + 0 goal)
[ INFO] [1573566731.884158985]: No solution found after 5.004028 seconds
[ INFO] [1573566731.907918805]: Unable to solve the planning problem
[ INFO] [1573566731.908854657]: Received event 'stop'

and in the Moveit! tutorial script terminal I get the following before it shuts down:

[ INFO] [1573566731.908515315]: ABORTED: No motion plan found. No execution attempted.

I've tried to isolate the problem in my script by cutting away some parts that are irrelevant without causing other problems/errors and this is what I have left of my code. The script is still pretty long so I apologize for that.

Does anyone know why I am getting this error and how to solve it? I've had similar errors with setting certain joint angles and that was caused by some angles being out of bounds for that joint. Now I've been playing around with other values for x,y,z and quaternion orientation w and I am pretty sure I'm not out of bounds there so I don't think the problem lies there.

Regards, Jesse

The python script

#!/usr/bin/env python

import sys
import copy
import rospy
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
from math import pi
from std_msgs.msg import String
from moveit_commander.conversions import pose_to_list

#############################################################################    
#############################################################################  
def all_close(goal, actual, tolerance):
  all_equal = True
  if type(goal) is list:
    for index in range(len(goal)):
      if abs(actual[index] - goal[index]) > tolerance:
        return False

  elif type(goal) is geometry_msgs.msg.PoseStamped:
    return all_close(goal.pose, actual.pose, tolerance)

  elif type(goal) is geometry_msgs.msg.Pose:
    return all_close(pose_to_list(goal), pose_to_list(actual), tolerance)

  return True

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

class MoveGroupPythonIntefaceTutorial(object):
  """MoveGroupPythonIntefaceTutorial"""
  def __init__(self):
    super(MoveGroupPythonIntefaceTutorial, self).__init__()

    moveit_commander.roscpp_initialize(sys.argv)
    rospy.init_node('move_group_python_interface_tutorial', anonymous=True)

    robot = moveit_commander.RobotCommander()

    scene = moveit_commander.PlanningSceneInterface()

    group_name = "manipulator"
    move_group = moveit_commander.MoveGroupCommander(group_name)

    display_trajectory_publisher = rospy.Publisher('/move_group/display_planned_path',
                                                   moveit_msgs.msg.DisplayTrajectory,
                                                   queue_size=20)

    planning_frame = move_group.get_planning_frame()
    print "============ Planning frame: %s" % planning_frame

    eef_link = move_group.get_end_effector_link()
    print "============ End effector link: %s" % eef_link
    group_names = robot.get_group_names()
    print "============ Available Planning Groups:", robot.get_group_names()
    print "============ Printing robot state"
    print robot.get_current_state()
    print ""


    ### Misc variables
    self.box_name = ''
    self.robot = robot
    self.scene = scene
    self.move_group = move_group
    self.display_trajectory_publisher = display_trajectory_publisher
    self.planning_frame = planning_frame
    self.eef_link = eef_link
    self.group_names = group_names

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

  def go_to_pose_goal(self):

    move_group = self.move_group
    pose_goal = geometry_msgs.msg.Pose()

    pose_goal.orientation.w = 1
    pose_goal.position.x = 0.5
    pose_goal.position.y = 0
    pose_goal.position.z = 0

    move_group.set_pose_target(pose_goal)
    plan = move_group.go(wait=True)
    move_group.stop()
    move_group.clear_pose_targets()

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

def main():
  try:
    print "============ Press `Enter` to begin "
    raw_input()
    tutorial = MoveGroupPythonIntefaceTutorial()

    print "============ Press `Enter` to execute a movement using a pose goal ..."
    raw_input()
    tutorial.go_to_pose_goal()

  except rospy.ROSInterruptException:
    return
  except KeyboardInterrupt:
    return

if __name__ == '__main__':
  main()