Added circle.py - circle movement with cartesian path

diff --git a/scripts/circle.py b/scripts/circle.py
new file mode 100755 (executable)
index 0000000..f5ca621
--- /dev/null
+++ b/scripts/circle.py
@@ -0,0 +1,65 @@
+#!/usr/bin/env python
+# -*- coding: iso-8859-15 -*-
+
+import sys
+import copy
+import rospy
+import moveit_commander
+from time import sleep
+from math import *
+
+class OpenManipCircle:
+       def __init__(self):
+               # Initialize the move_group API
+               moveit_commander.roscpp_initialize(sys.argv)
+
+               rospy.init_node('open_manip_circle')
+               self.arm        = moveit_commander.MoveGroupCommander("arm")
+
+               start_pose = self.arm.get_current_pose().pose
+               while start_pose.position.x == 0.211 and start_pose.position.z == 0.3755: # bug sometimes zero position is used as start position
+                       start_pose = self.arm.get_current_pose().pose
+
+               # circle
+               cartesian_plan, fraction = self.plan_cartesian_path(start_pose, num=100, radius=0.07)
+               self.arm.execute(cartesian_plan, wait=True)
+               self.arm.clear_pose_targets()
+               self.arm.stop()
+
+               # back to origin
+               self.arm.set_pose_target(start_pose)
+               plan = self.arm.go(wait=True)
+               self.arm.clear_pose_targets()
+               self.arm.stop()
+
+               # Shut down MoveIt cleanly
+               moveit_commander.roscpp_shutdown()
+
+               # Exit the script
+               moveit_commander.os._exit(0)
+
+       def plan_cartesian_path(self, start_pose, num=1, radius=0.05):
+               waypoints = []
+               for i in range(num):
+                       for angle in range(0,360,10):
+                               wpose = copy.deepcopy(start_pose)
+                               wpose.position.x += radius*cos(angle*pi/180)
+                               wpose.position.y += radius*sin(angle*pi/180)
+
+                               waypoints.append(copy.deepcopy(wpose))
+
+               # We want the Cartesian path to be interpolated at a resolution of 1 cm
+               # which is why we will specify 0.01 as the eef_step in Cartesian
+               # translation.  We will disable the jump threshold by setting it to 0.0,
+               # ignoring the check for infeasible jumps in joint space, which is sufficient
+               # for this tutorial.
+               (plan, fraction) = self.arm.compute_cartesian_path(
+                                              waypoints,   # waypoints to follow
+                                              0.01,        # eef_step
+                                              0.0)         # jump_threshold
+
+               # Note: We are just planning, not asking move_group to actually move the robot yet:
+               return plan, fraction
+
+if __name__ == "__main__":
+       OpenManipCircle()