class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) #self.robot_arm.set_num_planning_attempts(5) self.robot_arm.remember_joint_values("zeros", None) rospy.sleep(2) # Allow replanning to increase the odds of a solution self.robot_arm.allow_replanning(True) #add Tabl p = PoseStamped() p.header.frame_id = FIXED_FRAME p.pose.position.x = 0.3 p.pose.position.y = 0.2 p.pose.position.z = 0.1 self.scene.add_box("table", p, (0.02, 0.02, 0.02)) pose_goal.position.x = 0.3 pose_goal.position.y = 0.2 pose_goal.position.z = 0.15 #Orientation = [1.57,0,0] Orientation[0] = 0 Orientation[1] = 0 Orientation[2] = 1.57 def WASD(self): quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) rospy.sleep(2) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] rospy.sleep(2) self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) print("trying done") print("\n") print("WASD keyboard control") while (True): print("Available commands : ") print(" W - move X forward") print(" S - move X backward") print(" A - move Y left") print(" D - move Y right") print(" R - move Z left") print(" F - move Z right") print("*****************") print(" X - pick object") print(" Y - pick object") print("current orient", Orientation) command = raw_input("Control : ") if command == 'w': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x + 0.05 pose_goal.position.y = pose_goal.position.y pose_goal.position.z = pose_goal.position.z pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 's': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x - 0.05 pose_goal.position.y = pose_goal.position.y pose_goal.position.z = pose_goal.position.z pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'a': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x pose_goal.position.y = pose_goal.position.y - 0.05 pose_goal.position.z = pose_goal.position.z pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'd': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x pose_goal.position.y = pose_goal.position.y + 0.05 pose_goal.position.z = pose_goal.position.z pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'r': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x pose_goal.position.y = pose_goal.position.y pose_goal.position.z = pose_goal.position.z + 0.05 pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'f': pose_goal.orientation.w = 0.0 pose_goal.position.x = pose_goal.position.x pose_goal.position.y = pose_goal.position.y pose_goal.position.z = pose_goal.position.z - 0.05 pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'x': self.robot_arm.attach_object("table") elif command == 'y': self.robot_arm.detach_object("table") elif command == 'v': self.set_pose_target(pose_goal) elif command == 'i': Orientation[0] = Orientation[0] + 0.2 Orientation[1] = Orientation[1] Orientation[2] = Orientation[2] quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'k': Orientation[0] = Orientation[0] - 0.2 Orientation[1] = Orientation[1] Orientation[2] = Orientation[2] quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) # yaw elif command == 'u': Orientation[0] = Orientation[0] Orientation[1] = Orientation[1] + 0.2 Orientation[2] = Orientation[2] quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'o': Orientation[0] = Orientation[0] Orientation[1] = Orientation[1] - 0.2 Orientation[2] = Orientation[2] quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) # roll elif command == 'j': Orientation[0] = Orientation[0] Orientation[1] = Orientation[1] Orientation[2] = Orientation[2] + 0.2 quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) elif command == 'l': Orientation[0] = Orientation[0] Orientation[1] = Orientation[1] Orientation[2] = Orientation[2] - 0.2 quater = quaternion_from_euler(Orientation[0], Orientation[1], Orientation[2]) pose_goal.orientation.x = quater[0] pose_goal.orientation.y = quater[1] pose_goal.orientation.z = quater[2] pose_goal.orientation.w = quater[3] self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True)
class TestMove(): def __init__(self): roscpp_initialize(sys.argv) rospy.init_node('ur3_move', anonymous=True) self.scene = PlanningSceneInterface() self.robot_cmd = RobotCommander() self.robot_arm = MoveGroupCommander(GROUP_NAME_ARM) #robot_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) self.robot_arm.set_goal_orientation_tolerance(0.005) self.robot_arm.set_planning_time(5) self.robot_arm.set_num_planning_attempts(5) self.robot_arm.remember_joint_values("zeros", None) rospy.sleep(2) # Allow replanning to increase the odds of a solution #self.robot_arm.allow_replanning(True) #scene = moveit_commander.PlanningSceneInterface() #robot = moveit_commander.RobotCommander() #rospy.sleep(2) #add Table p = PoseStamped() p.header.frame_id = FIXED_FRAME p.pose.position.x = 0.0 p.pose.position.y = 0.2 p.pose.position.z = 0.1 self.scene.add_box("table", p, (0.3, 0.3, 0.3)) def move_code(self): self.robot_arm.set_named_target("home") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to home 1 ======") rospy.sleep(1) self.robot_arm.attach_object("table") pose_goal.orientation.w = 0.0 pose_goal.position.x = 0.4 # red line 0.2 0.2 pose_goal.position.y = 0.15 # green line 0.15 0.15 pose_goal.position.z = 0.2 # blue line # 0.35 0.6 self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) #if (input == 'w') #pose_goal.position.x += 0.1 # red line 0.2 0.2 print("====== move plan go to up ======") self.robot_arm.set_named_target("zeros") #go to goal state. self.robot_arm.go(wait=True) print("====== move plan go to zeros ======") # rospy.sleep(1) #self.robot_arm.place("table", pose_goal) # robot_arm.set_named_target("up") # robot_arm.go(wait=True) robot_state = self.robot_arm.get_current_pose() robot_angle = self.robot_arm.get_current_joint_values() self.robot_arm.detach_object("table") pose_goal.orientation.w = 0.0 pose_goal.position.x = 0.2 # red line 0.2 0.2 pose_goal.position.y = 0.3 # green line 0.15 0.15 pose_goal.position.z = 0.1 # blue line # 0.35 0.6 self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) self.robot_arm.set_pose_target(pose_goal) self.robot_arm.go(True) print(robot_state)