From a789283b6627ebec4c91767ed0aff03734be632f Mon Sep 17 00:00:00 2001
From: Erik Andresen <erik@vontaene.de>
Date: Thu, 21 May 2020 11:03:38 +0200
Subject: [PATCH] Added circle.py - circle movement with cartesian path
---
scripts/circle.py | 65 +++++++++++++++++++++++++++++++++++++++++++++++
1 file changed, 65 insertions(+)
create mode 100755 scripts/circle.py
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()
--
2.39.5