From: Erik Andresen <erik@vontaene.de>
Date: Thu, 21 May 2020 09:03:38 +0000 (+0200)
Subject: Added circle.py - circle movement with cartesian path
X-Git-Url: https://defiant.homedns.org/gitweb/?a=commitdiff_plain;h=a789283b6627ebec4c91767ed0aff03734be632f;p=wt_open_manipulator.git

Added circle.py - circle movement with cartesian path
---

diff --git a/scripts/circle.py b/scripts/circle.py
new file mode 100755
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()