2 # -*- coding: iso-8859-15 -*-
7 import moveit_commander
11 class OpenManipCircle:
13 # Initialize the move_group API
14 moveit_commander.roscpp_initialize(sys.argv)
16 rospy.init_node('open_manip_circle')
17 self.arm = moveit_commander.MoveGroupCommander("arm")
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
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()
30 self.arm.set_pose_target(start_pose)
31 plan = self.arm.go(wait=True)
32 self.arm.clear_pose_targets()
35 # Shut down MoveIt cleanly
36 moveit_commander.roscpp_shutdown()
39 moveit_commander.os._exit(0)
41 def plan_cartesian_path(self, start_pose, num=1, radius=0.05):
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)
49 waypoints.append(copy.deepcopy(wpose))
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
56 (plan, fraction) = self.arm.compute_cartesian_path(
57 waypoints, # waypoints to follow
61 # Note: We are just planning, not asking move_group to actually move the robot yet:
64 if __name__ == "__main__":