scripts/circle.py

   1 #!/usr/bin/env python
   2 # -*- coding: iso-8859-15 -*-
   3
   4 import sys
   5 import copy
   6 import rospy
   7 import moveit_commander
   8 from time import sleep
   9 from math import *
  10
  11 class OpenManipCircle:
  12         def __init__(self):
  13                 # Initialize the move_group API
  14                 moveit_commander.roscpp_initialize(sys.argv)
  15
  16                 rospy.init_node('open_manip_circle')
  17                 self.arm        = moveit_commander.MoveGroupCommander("arm")
  18
  19                 start_pose = self.arm.get_current_pose().pose
  20                 while start_pose.position.x == 0.211 and start_pose.position.z == 0.3755: # bug sometimes zero position is used as start position
  21                         start_pose = self.arm.get_current_pose().pose
  22
  23                 # circle
  24                 cartesian_plan, fraction = self.plan_cartesian_path(start_pose, num=100, radius=0.07)
  25                 self.arm.execute(cartesian_plan, wait=True)
  26                 self.arm.clear_pose_targets()
  27                 self.arm.stop()
  28
  29                 # back to origin
  30                 self.arm.set_pose_target(start_pose)
  31                 plan = self.arm.go(wait=True)
  32                 self.arm.clear_pose_targets()
  33                 self.arm.stop()
  34
  35                 # Shut down MoveIt cleanly
  36                 moveit_commander.roscpp_shutdown()
  37
  38                 # Exit the script
  39                 moveit_commander.os._exit(0)
  40
  41         def plan_cartesian_path(self, start_pose, num=1, radius=0.05):
  42                 waypoints = []
  43                 for i in range(num):
  44                         for angle in range(0,360,10):
  45                                 wpose = copy.deepcopy(start_pose)
  46                                 wpose.position.x += radius*cos(angle*pi/180)
  47                                 wpose.position.y += radius*sin(angle*pi/180)
  48
  49                                 waypoints.append(copy.deepcopy(wpose))
  50
  51                 # We want the Cartesian path to be interpolated at a resolution of 1 cm
  52                 # which is why we will specify 0.01 as the eef_step in Cartesian
  53                 # translation.  We will disable the jump threshold by setting it to 0.0,
  54                 # ignoring the check for infeasible jumps in joint space, which is sufficient
  55                 # for this tutorial.
  56                 (plan, fraction) = self.arm.compute_cartesian_path(
  57                                                waypoints,   # waypoints to follow
  58                                                0.01,        # eef_step
  59                                                0.0)         # jump_threshold
  60
  61                 # Note: We are just planning, not asking move_group to actually move the robot yet:
  62                 return plan, fraction
  63
  64 if __name__ == "__main__":
  65         OpenManipCircle()