gistfile1.txt

#!/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):
  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 = "panda_arm"
    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, x, y, z, orientation):
    pose_goal = geometry_msgs.msg.Pose()
    pose_goal.orientation.z = orientation
    pose_goal.position.x = x
    pose_goal.position.y = y
    pose_goal.position.z = z

    self.move_group.set_pose_target(pose_goal)

    plan = self.move_group.go(wait=True)
    self.move_group.stop()
    self.move_group.clear_pose_targets()

    current_pose = self.move_group.get_current_pose().pose

    return all_close(pose_goal, current_pose, 0.01)


  def add_box(self, timeout=4):
      box_pose = geometry_msgs.msg.PoseStamped()
      box_pose.pose.position.x = 0.4
      box_pose.pose.position.y = 0.0
      box_pose.pose.position.z = 0.2
      box_pose.header.frame_id = self.robot.get_planning_frame()
      
      self.box_name = "box"
      self.scene.add_box(self.box_name, box_pose, size=(0.1,0.1,0.1))
      
      return self.wait_for_state_update(box_is_known=True, timeout=timeout)


  def remove_box(self, timeout=4):
      self.scene.remove_world_object(self.box_name)
      return self.wait_for_state_update(timeout=timeout)


  def wait_for_state_update(self, box_is_known=False, timeout=4):
    start = rospy.get_time()
    while (rospy.get_time() - start < timeout) and not rospy.is_shutdown():

      is_known = self.box_name in self.scene.get_known_object_names()
      if (box_is_known == is_known):
        return True

      rospy.sleep(0.1)

    return False

  def init_upright_path_constraints(self):

      pose = self.move_group.get_current_pose()

      orientation_constraint = moveit_msgs.msg.OrientationConstraint()
      orientation_constraint.header = pose.header
      orientation_constraint.link_name = self.eef_link
      orientation_constraint.orientation = pose.pose.orientation
      orientation_constraint.absolute_x_axis_tolerance = 0.1
      orientation_constraint.absolute_y_axis_tolerance = 0.1
      orientation_constraint.absolute_z_axis_tolerance = 0.1
      orientation_constraint.weight = 1

      self.upright_constraints = moveit_msgs.msg.Constraints()
      self.upright_constraints.name = "upright"
      self.upright_constraints.orientation_constraints.append(orientation_constraint)

  def enable_upright_path_constraints(self):
      self.move_group.set_path_constraints(self.upright_constraints)

  def disable_upright_path_constraints(self):
      self.move_group.set_path_constraints(None)


def main():
  try:
    print ""
    print "----------------------------------------------------------"
    print "Welcome to the MoveIt MoveGroup Python Interface Tutorial"
    print "----------------------------------------------------------"
    print "Press Ctrl-D to exit at any time"
    print ""
   
    tutorial = MoveGroupPythonIntefaceTutorial()

    tutorial.go_to_pose_goal(0.4, -0.5, 0.3, -1)
    tutorial.go_to_pose_goal(0.4, 0.5, 0.8, -1)
    
    #tutorial.add_box()
    tutorial.init_upright_path_constraints()
    tutorial.enable_upright_path_constraints()
  
    tutorial.go_to_pose_goal(0.4, -0.5, 0.3, -1)
    tutorial.go_to_pose_goal(0.4, 0.5, 0.8, -1)
    
    tutorial.disable_upright_path_constraints()
    #tutorial.remove_box()

    print "============ Python tutorial demo complete!"
  except rospy.ROSInterruptException:
    return
  except KeyboardInterrupt:
    return

if __name__ == '__main__':
  main()