def drive_pose(self): Robotiq.close(self.gripper, block=True) self.ur5.go([0.0, -0.2618, -2.618, 0.0, -1.562, 2.356], wait=True) print('To Drive') return
def detection_pose(self): Robotiq.close(self.gripper, block=True) self.ur5.go([0.0, -0.2618, -2.618, -1.7596, -1.562, 1.570], wait=True) print('To Detection') return
def cartesian_planning(self, cmd): if (cmd[0] == 'PICK'): Robotiq.open(self.gripper, block=True) rospy.sleep(1) waypoint = [] ''' wpose is initialized twice to prevent inconsistent pose value which may happen when get_current_pose() is called first. ''' wpose = self.ur5.get_current_pose().pose wpose = self.ur5.get_current_pose().pose waypoint.append(copy.deepcopy(wpose)) # Move TCP to target position wpose.position.y += round(float(cmd[2]), 3) wpose.position.z += round(float(cmd[3]), 3) + 0.03 wpose.position.x += round(float(cmd[1]), 3) waypoint.append(copy.deepcopy(wpose)) wpose.position.z -= 0.03 waypoint.append(copy.deepcopy(wpose)) plan, fraction = self.ur5.compute_cartesian_path(waypoint, 0.01, 0.0) print('Fraction : {}'.format(fraction)) self.ur5.execute(plan, wait=True) rospy.sleep(1) # Pick or Place or TCP Move Only if (cmd[0] == 'PICK'): Robotiq.close(self.gripper, block=True) elif (cmd[0] == 'PLACE'): Robotiq.open(self.gripper, block=True) else: return rospy.sleep(1) # Planning backward to the base pose rev_plan = self.reverse_planning(plan) self.ur5.execute(rev_plan, wait=True) self.ur5.stop() self.ur5.clear_pose_targets() print('cartesian planning done') return
pose_goal.orientation.x = current_pose.orientation.x pose_goal.orientation.y = current_pose.orientation.y pose_goal.orientation.z = current_pose.orientation.z pose_goal.orientation.w = current_pose.orientation.w pose_goal.position.x = current_pose.position.x pose_goal.position.y = current_pose.position.y pose_goal.position.z = current_pose.position.z - 0.1 group.set_pose_target(pose_goal) plan = group.plan() group.go(wait=True) if key == 'c': print "========close gripper=======" Robotiq.close(robotiq_client, speed=0.1, force=0, block=True) if key == 'o': print "========close gripper=======" Robotiq.open(robotiq_client, block=True) '''current_pose=group.get_current_pose().pose pose_goal = geometry_msgs.msg.Pose() pose_goal.orientation.x = current_pose.orientation.x pose_goal.orientation.y = current_pose.orientation.y pose_goal.orientation.z = current_pose.orientation.z pose_goal.orientation.w = current_pose.orientation.w pose_goal.position.x = current_pose.position.x pose_goal.position.y = current_pose.position.y pose_goal.position.z = current_pose.position.z-0.1 group.set_pose_target(pose_goal)