class Pick_Place: def __init__(self): self.object_list = {} self.arm = moveit_commander.MoveGroupCommander("irb_120") self.gripper = moveit_commander.MoveGroupCommander("robotiq_85") self.arm.set_goal_tolerance(0.05) self.gripper.set_goal_tolerance(0.02) self.scene = PlanningSceneInterface() self.robot = RobotCommander() rospy.sleep(1) self.add_objects() self.add_table() #self.add_ground() self.approach_retreat_desired_dist = 0.2 self.approach_retreat_min_dist = 0.1 rospy.sleep(1.0) # pick up object in pose def pickup(self, object_name, pose): grasps = self.generate_grasps(object_name, pose) self.arm.pick(object_name, grasps) #self.gripper.stop() rospy.loginfo('Pick up successfully') self.arm.detach_object(object_name) self.clean_scene(object_name) #rospy.sleep(1) # place object to goal pose def place(self, pose): self.move_pose_arm(pose) rospy.sleep(1) # pose.position.z -= 0.1 # self.move_pose_arm(pose) waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z -= 0.15 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.move_joint_hand(0) rospy.sleep(1) # pose.position.z += 0.1 # self.move_pose_arm(pose) waypoints = [] wpose = self.arm.get_current_pose().pose wpose.position.z += 0.15 waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) rospy.loginfo('Place successfully') def get_object_pose(self, object_name): pose = self.object_list[object_name].pose return pose def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def add_ground(self): self.clean_scene("ground") p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.01 size = (5, 5, 0.02) #self.scene.add_box("ground",p, size) rospy.sleep(2) def add_table(self): self.clean_scene("table") p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.8 p.pose.position.y = 0.0 p.pose.position.z = 0.1 size = (0.8, 1.5, 0.028) self.scene.add_box("table", p, size) rospy.sleep(2) def add_objects(self): p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() # add box name = "box" self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.025 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) size = (0.05, 0.05, 0.05) self.scene.add_box(name, p, size) height = 0.05 width = 0.05 shape = "cube" color = "red" self.object_list[name] = Object(p.pose, height, width, shape, color) print("add box") rospy.sleep(1) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() # add cylinder name = "cylinder" self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = 0.2 p.pose.position.z = 0.05 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) height = 0.1 radius = 0.03 self.scene.add_cylinder(name, p, height, radius) height = 0.1 width = 0.03 shape = "cylinder" color = "green" self.object_list[name] = Object(p.pose, height, width, shape, color) print("add cylinder") rospy.sleep(1) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() # add sphere name = "ball" self.clean_scene(name) p.pose.position.x = 0.5 p.pose.position.y = -0.2 p.pose.position.z = 0.03 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) radius = 0.03 self.scene.add_sphere(name, p, radius) height = 0.03 width = 0.03 shape = "sphere" color = "red" self.object_list[name] = Object(p.pose, height, width, shape, color) print("add ball") rospy.sleep(1) #print(self.object_list) def pose2msg(self, roll, pitch, yaw, x, y, z): pose = geometry_msgs.msg.Pose() quat = quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] pose.position.x = x pose.position.y = y pose.position.z = z return pose def msg2pose(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] return roll, pitch, yaw, x, y, z def back_to_home(self): self.move_joint_arm(0, 0, 0, 0, 0, 0) self.move_joint_hand(0) rospy.sleep(1) # Forward Kinematics (FK): move the arm by axis values def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4, joint_5): joint_goal = self.arm.get_current_joint_values() joint_goal[0] = joint_0 joint_goal[1] = joint_1 joint_goal[2] = joint_2 joint_goal[3] = joint_3 joint_goal[4] = joint_4 joint_goal[5] = joint_5 self.arm.go(joint_goal, wait=True) self.arm.stop() # To guarantee no residual movement # Inverse Kinematics: Move the robot arm to desired pose def move_pose_arm(self, pose_goal): self.arm.set_pose_target(pose_goal) self.arm.go(wait=True) self.arm.stop() # To guarantee no residual movement self.arm.clear_pose_targets() # Move the Robotiq gripper by master axis def move_joint_hand(self, gripper_finger1_joint): joint_goal = self.gripper.get_current_joint_values() joint_goal[2] = gripper_finger1_joint # Gripper master axis self.gripper.go(joint_goal, wait=True) self.gripper.stop() # To guarantee no residual movement def generate_grasps(self, name, pose): grasps = [] now = rospy.Time.now() angle = 0 grasp = Grasp() grasp.grasp_pose.header.stamp = now grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame() grasp.grasp_pose.pose = copy.deepcopy(pose) # Setting pre-grasp approach grasp.pre_grasp_approach.direction.header.stamp = now grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.pre_grasp_approach.direction.vector.z = -0.5 grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist # Setting post-grasp retreat grasp.post_grasp_retreat.direction.header.stamp = now grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.post_grasp_retreat.direction.vector.z = 0.5 grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle) grasp.grasp_pose.pose.orientation = Quaternion(*q) grasp.max_contact_force = 1000 grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0.0) traj.time_from_start = rospy.Duration.from_sec(0.5) grasp.pre_grasp_posture.points.append(traj) grasp.grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() if name == "box": traj.positions.append(0.4) elif name == "ball": traj.positions.append(0.3) elif name == "cylinder": traj.positions.append(0.3) #traj.velocities.append(0.2) #traj.effort.append(100) traj.time_from_start = rospy.Duration.from_sec(5.0) grasp.grasp_posture.points.append(traj) grasps.append(grasp) return grasps # Implement the main algorithm here def MyAlgorithm(self): self.back_to_home() # pick cylinder object_name = "cylinder" pose = self.get_object_pose(object_name) print(pose.position.y) pose.position.z += 0.16 self.pickup(object_name, pose) roll = 0.0 pitch = numpy.deg2rad(90.0) yaw = 0.0 x = 0 y = 0.6 z = pose.position.z + 0.01 place_pose = self.pose2msg(roll, pitch, yaw, x, y, z) self.place(place_pose) self.back_to_home() # pick box object_name = "box" pose = self.get_object_pose(object_name) print(pose.position.y) pose.position.z += 0.15 self.pickup(object_name, pose) roll = 0.0 pitch = numpy.deg2rad(90.0) yaw = 0.0 x = 0.1 y = 0.6 z = pose.position.z + 0.01 place_pose = self.pose2msg(roll, pitch, yaw, x, y, z) self.place(place_pose) self.back_to_home() # pick ball object_name = "ball" pose = self.get_object_pose(object_name) print(pose.position.y) pose.position.z += 0.14 self.pickup(object_name, pose) roll = 0.0 pitch = numpy.deg2rad(90.0) yaw = 0.0 x = -0.1 y = 0.6 z = pose.position.z + 0.01 place_pose = self.pose2msg(roll, pitch, yaw, x, y, z) self.place(place_pose) self.back_to_home()
class Pick_Place: def __init__(self): self.objects_name = ["cylinder", "box", "sphere"] self.object_width = 0.03 self.arm = moveit_commander.MoveGroupCommander("irb_120") self.gripper = moveit_commander.MoveGroupCommander("robotiq_85") self.arm.set_goal_tolerance(0.2) self.gripper.set_goal_tolerance(0.05) self.scene = PlanningSceneInterface() self.robot = RobotCommander() self.add_ground() rospy.sleep(1.0) for i in range(3): rospy.loginfo("Moving arm to HOME point") self.move_pose_arm(0, 1.57, 0, 0.4, 0, 0.6) rospy.sleep(1) self.object_name = self.objects_name[i] self.scene.remove_world_object(self.object_name) self.pose_object, dx, dy, dz = self.add_object(self.object_name) rospy.sleep(1.0) self.pose_object.position.x += dx self.pose_object.position.y += dy self.pose_object.position.z += dz print(self.objects_name[i], self.pose_object.position) self.approach_retreat_desired_dist = 0.2 self.approach_retreat_min_dist = 0.1 print("start pick and place") # Pick grasps = self.generate_grasps(self.object_name, self.pose_object) self.arm.pick(self.object_name, grasps) self.gripper.stop() rospy.loginfo('Pick up successfully') self.arm.detach_object(self.object_name) self.clean_scene(self.object_name) rospy.sleep(1) curr_pose = self.arm.get_current_pose().pose x = curr_pose.position.x y = curr_pose.position.y z = curr_pose.position.z # quaternion = (curr_pose.orientation.x, # curr_pose.orientation.y, # curr_pose.orientation.z, # curr_pose.orientation.w) # euler = euler_from_quaternion(quaternion) # roll = euler[0] # pitch = euler[1] # yaw = euler[2] roll = 0.0 pitch = numpy.deg2rad(90.0) yaw = 0.0 z += 0.1 self.move_pose_arm(roll, pitch, yaw, x, y, z) x = -curr_pose.position.y y = 0.6 z -= 0.1 #z = self.pose_object.position.z+0.05 # if self.object_name == "cylinder": # yaw = numpy.deg2rad(90.0) #z+= 0.001 self.move_pose_arm(roll, pitch, yaw, x, y, z) rospy.sleep(0.5) z -= 0.1 self.move_pose_arm(roll, pitch, yaw, x, y, z) self.move_joint_hand(0) #if self.object_name == "cylinder": z += 0.1 self.move_pose_arm(roll, pitch, yaw, x, y, z) # target_pose = curr_pose # target_pose.position.x = x # target_pose.position.y = y # target_pose.position.z = z # q = quaternion_from_euler(roll, pitch, yaw) # target_pose.orientation = Quaternion(*q) # place = self.generate_places(target_pose) # self.arm.place(self.object_name, place) # self.arm.detach_object(self.object_name) # self.clean_scene(self.object_name) rospy.loginfo('Place successfully') rospy.loginfo("Moving arm to HOME point") self.move_pose_arm(0, 0.8, 0, 0.4, 0, 0.6) rospy.sleep(1) def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def add_ground(self): p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = 0.0 p.pose.position.y = 0.0 p.pose.position.z = -0.01 size = (5, 5, 0.02) self.scene.add_box("ground", p, size) def add_object(self, name): p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() dx = 0 dy = 0 dz = 0 if name == "box": p.pose.position.x = 0.5 p.pose.position.y = 0.0 p.pose.position.z = 0.015 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) size = (0.03, 0.03, 0.03) self.scene.add_box(name, p, size) dz = 0.16 elif name == "cylinder": p.pose.position.x = 0.5 p.pose.position.y = 0.2 p.pose.position.z = 0.05 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) height = 0.1 radius = 0.03 self.scene.add_cylinder(name, p, height, radius) dz = 0.16 #dx = -0.135 elif name == "sphere": p.pose.position.x = 0.5 p.pose.position.y = -0.2 p.pose.position.z = 0.03 + 0.115 q = quaternion_from_euler(0.0, 0.0, 0.0) p.pose.orientation = Quaternion(*q) radius = 0.03 self.scene.add_sphere(name, p, radius) dz = 0.15 return p.pose, dx, dy, dz def move_pose_arm(self, roll, pitch, yaw, x, y, z): pose_goal = geometry_msgs.msg.Pose() quat = quaternion_from_euler(roll, pitch, yaw) pose_goal.orientation.x = quat[0] pose_goal.orientation.y = quat[1] pose_goal.orientation.z = quat[2] pose_goal.orientation.w = quat[3] pose_goal.position.x = x pose_goal.position.y = y pose_goal.position.z = z self.arm.set_pose_target(pose_goal) self.arm.go(wait=True) self.arm.stop() # To guarantee no residual movement self.arm.clear_pose_targets() # Move the Robotiq gripper by master axis def move_joint_hand(self, gripper_finger1_joint): joint_goal = self.gripper.get_current_joint_values() joint_goal[2] = gripper_finger1_joint # Gripper master axis self.gripper.go(joint_goal, wait=True) self.gripper.stop() # To guarantee no residual movement def generate_grasps(self, name, pose): grasps = [] now = rospy.Time.now() #for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(10.0)): # Create place location: angle = 0 grasp = Grasp() grasp.grasp_pose.header.stamp = now grasp.grasp_pose.header.frame_id = self.robot.get_planning_frame() grasp.grasp_pose.pose = copy.deepcopy(pose) # Setting pre-grasp approach grasp.pre_grasp_approach.direction.header.stamp = now grasp.pre_grasp_approach.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.pre_grasp_approach.direction.vector.z = -0.2 grasp.pre_grasp_approach.min_distance = self.approach_retreat_min_dist grasp.pre_grasp_approach.desired_distance = self.approach_retreat_desired_dist # Setting post-grasp retreat grasp.post_grasp_retreat.direction.header.stamp = now grasp.post_grasp_retreat.direction.header.frame_id = self.robot.get_planning_frame( ) grasp.post_grasp_retreat.direction.vector.z = 0.2 grasp.post_grasp_retreat.min_distance = self.approach_retreat_min_dist grasp.post_grasp_retreat.desired_distance = self.approach_retreat_desired_dist # if name != "cylinder": q = quaternion_from_euler(0.0, numpy.deg2rad(90.0), angle) grasp.grasp_pose.pose.orientation = Quaternion(*q) # else: # #grasp.pre_grasp_approach.direction.vector.z = -0.05 # grasp.pre_grasp_approach.direction.vector.x = 0.1 # #grasp.post_grasp_retreat.direction.vector.z = 0.05 # grasp.post_grasp_retreat.direction.vector.x = -0.1 grasp.max_contact_force = 100 grasp.pre_grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0.03) traj.time_from_start = rospy.Duration.from_sec(0.5) grasp.pre_grasp_posture.points.append(traj) grasp.grasp_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() if name == "box": traj.positions.append(0.57) elif name == "sphere": traj.positions.append(0.3) else: traj.positions.append(0.3) #traj.velocities.append(0.2) traj.effort.append(800) traj.time_from_start = rospy.Duration.from_sec(5.0) grasp.grasp_posture.points.append(traj) grasps.append(grasp) return grasps def generate_places(self, target): #places = [] now = rospy.Time.now() # for angle in numpy.arange(0.0, numpy.deg2rad(360.0), numpy.deg2rad(30.0)): # Create place location: place = PlaceLocation() place.place_pose.header.stamp = now place.place_pose.header.frame_id = self.robot.get_planning_frame() # Set target position: place.place_pose.pose = copy.deepcopy(target) # Generate orientation (wrt Z axis): # q = quaternion_from_euler(0.0, 0, 0.0) # place.place_pose.pose.orientation = Quaternion(*q) # Generate pre place approach: place.pre_place_approach.desired_distance = self.approach_retreat_desired_dist place.pre_place_approach.min_distance = self.approach_retreat_min_dist place.pre_place_approach.direction.header.stamp = now place.pre_place_approach.direction.header.frame_id = self.robot.get_planning_frame( ) place.pre_place_approach.direction.vector.x = 0 place.pre_place_approach.direction.vector.y = 0 place.pre_place_approach.direction.vector.z = -0.2 # Generate post place approach: place.post_place_retreat.direction.header.stamp = now place.post_place_retreat.direction.header.frame_id = self.robot.get_planning_frame( ) place.post_place_retreat.desired_distance = self.approach_retreat_desired_dist place.post_place_retreat.min_distance = self.approach_retreat_min_dist place.post_place_retreat.direction.vector.x = 0 place.post_place_retreat.direction.vector.y = 0 place.post_place_retreat.direction.vector.z = 0.2 place.allowed_touch_objects.append(self.object_name) place.post_place_posture.joint_names.append("gripper_finger1_joint") traj = JointTrajectoryPoint() traj.positions.append(0) #traj.effort.append(0) traj.time_from_start = rospy.Duration.from_sec(1.0) place.post_place_posture.points.append(traj) # Add place: #places.append(place) return place
class ModelManager: def __init__(self): rospy.wait_for_service("gazebo/spawn_sdf_model") self.spawn_model_srv = rospy.ServiceProxy("gazebo/spawn_sdf_model", SpawnModel) rospy.wait_for_service("gazebo/spawn_sdf_model") self.delete_model_srv = rospy.ServiceProxy("gazebo/delete_model", DeleteModel) self.scene = PlanningSceneInterface() self.robot = RobotCommander() rospy.sleep(1) self.object_list = {} def pose2msg(self, x, y, z, roll, pitch, yaw): pose = Pose() pose.position.x = x pose.position.y = y pose.position.z = z quat = quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] return pose def msg2pose(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] return roll, pitch, yaw, x, y, z def spawn_model(self, model_name, model_pose): with open( os.path.join( rospkg.RosPack().get_path('irb120_robotiq85_gazebo'), 'models', model_name, 'model.sdf'), "r") as f: model_xml = f.read() self.spawn_model_srv(model_name, model_xml, "", model_pose, "world") def delete_model(self, model_name): self.delete_model_srv(model_name) def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def respawn_all_objects(self): objects_name = self.object_list.keys() for object_name in objects_name: this_object = self.object_list[object_name] print("Respawning {}".format(object_name)) # remove old objects in Gazebo self.delete_model(object_name) # respawn new objects in Gazebo roll, pitch, yaw, x, y, z = self.msg2pose(this_object.abs_pose) object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) self.spawn_model(object_name, object_pose) # respawn objects in Rviz p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() self.clean_scene(object_name) p.pose = copy.copy(this_object.relative_pose) shape = this_object.shape if shape == "box": x = this_object.length y = this_object.width z = this_object.height size = (x, y, z) self.scene.add_box(object_name, p, size) elif shape == "cylinder": height = this_object.height radius = this_object.width / 2 self.scene.add_cylinder(object_name, p, height, radius) elif shape == "sphere": radius = this_object.width / 2 self.scene.add_sphere(object_name, p, radius) rospy.sleep(0.5) rospy.loginfo("All objects are respawned") def spawn_all_model(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_kinematics', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) robot_x = objects_info["robot"]["pose"]["x"] robot_y = objects_info["robot"]["pose"]["y"] robot_z = objects_info["robot"]["pose"]["z"] robot_roll = objects_info["robot"]["pose"]["roll"] robot_pitch = objects_info["robot"]["pose"]["pitch"] robot_yaw = objects_info["robot"]["pose"]["yaw"] rospy.loginfo("Spawning Objects in Gazebo and planning scene") objects = objects_info["objects"] objects_name = objects.keys() for object_name in objects_name: name = object_name shape = objects[name]["shape"] color = objects[name]["color"] # add object in Gazebo # self.delete_model(object_name) x = objects[name]["pose"]["x"] y = objects[name]["pose"]["y"] z = objects[name]["pose"]["z"] roll = objects[name]["pose"]["roll"] pitch = objects[name]["pose"]["pitch"] yaw = objects[name]["pose"]["yaw"] object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) self.spawn_model(object_name, object_pose) ## add object in planning scene(Rviz) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() self.clean_scene(name) p.pose.position.x = x - robot_x p.pose.position.y = y - robot_y p.pose.position.z = z - robot_z q = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation = Quaternion(*q) if shape == "box": x = objects[name]["size"]["x"] y = objects[name]["size"]["y"] z = objects[name]["size"]["z"] p.pose.position.z += z / 2 size = (x, y, z) self.scene.add_box(name, p, size) height = z width = y length = x self.object_list[name] = Object(p.pose, object_pose, height, width, length, shape, color) elif shape == "cylinder": height = objects[name]["size"]["height"] radius = objects[name]["size"]["radius"] p.pose.position.z += height / 2 self.scene.add_cylinder(name, p, height, radius) self.object_list[name] = Object(p.pose, object_pose, height, radius * 2, radius * 2, shape, color) elif shape == "sphere": radius = objects[name]["size"] p.pose.position.z += radius self.scene.add_sphere(name, p, radius) self.object_list[name] = Object(p.pose, object_pose, radius * 2, radius * 2, radius * 2, shape, color) rospy.sleep(0.5) rospy.loginfo("Spawning Obstacles in planning scene") obstacles = objects_info["obstacles"] obstacles_name = obstacles.keys() for obstacle_name in obstacles_name: name = obstacle_name p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() self.clean_scene(name) p.pose.position.x = obstacles[name]["pose"]["x"] - robot_x p.pose.position.y = obstacles[name]["pose"]["y"] - robot_y p.pose.position.z = obstacles[name]["pose"]["z"] - robot_z q = quaternion_from_euler(robot_roll, robot_pitch, robot_yaw) p.pose.orientation = Quaternion(*q) x = obstacles[name]["size"]["x"] y = obstacles[name]["size"]["y"] z = obstacles[name]["size"]["z"] size = (x, y, z) self.scene.add_box(name, p, size) rospy.sleep(0.5)
def init(): global marker_array_pub, marker_pub, tf_broadcaster, tf_listener global move_group, turntable global camera_mesh, camera_pos, camera_size, min_distance, max_distance, num_positions, num_spins, object_size, photobox_pos, photobox_size, reach, simulation, test, turntable_pos, working_dir rospy.init_node('acquisition') camera_mesh = rospy.get_param('~camera_mesh', None) camera_orientation = rospy.get_param('~camera_orientation', None) camera_pos = rospy.get_param('~camera_pos', [0.0, 0.0, 0.0]) camera_size = rospy.get_param('~camera_size', [0.1, 0.1, 0.1]) min_distance = rospy.get_param('~min_distance', 0.2) max_distance = rospy.get_param('~max_distance', min_distance) max_velocity = rospy.get_param('~max_velocity', 0.1) num_positions = rospy.get_param('~num_positions', 15) num_spins = rospy.get_param('~num_spins', 8) object_size = numpy.array(rospy.get_param('~object_size', [0.2, 0.2, 0.2])) photobox_pos = rospy.get_param('~photobox_pos', [0.0, -0.6, 0.0]) photobox_size = rospy.get_param('~photobox_size', [0.7, 0.7, 1.0]) ptpip = rospy.get_param('~ptpip', None) reach = rospy.get_param('~reach', 0.85) simulation = '/gazebo' in get_node_names() test = rospy.get_param('~test', True) turntable_pos = rospy.get_param( '~turntable_pos', photobox_pos[:2] + [photobox_pos[2] + 0.05]) turntable_radius = rospy.get_param('~turntable_radius', 0.2) wall_thickness = rospy.get_param('~wall_thickness', 0.04) marker_array_pub = rospy.Publisher('visualization_marker_array', MarkerArray, queue_size=1, latch=True) marker_pub = rospy.Publisher('visualization_marker', Marker, queue_size=1, latch=True) tf_broadcaster = tf.TransformBroadcaster() tf_listener = tf.TransformListener() move_group = MoveGroupCommander('manipulator') move_group.set_max_acceleration_scaling_factor( 1.0 if simulation else max_velocity) move_group.set_max_velocity_scaling_factor( 1.0 if simulation else max_velocity) robot = RobotCommander() scene = PlanningSceneInterface(synchronous=True) try: st_control = load_source( 'st_control', os.path.join(RosPack().get_path('iai_scanning_table'), 'scripts', 'iai_scanning_table', 'st_control.py')) turntable = st_control.ElmoUdp() if turntable.check_device(): turntable.configure() turntable.reset_encoder() turntable.start_controller() turntable.set_speed_deg(30) except Exception as e: print(e) sys.exit('Could not connect to turntable.') if simulation: move_home() elif not test: working_dir = rospy.get_param('~working_dir', None) if working_dir is None: sys.exit('Working directory not specified.') elif not camera.init( os.path.join(os.path.dirname(os.path.abspath(__file__)), '..', 'out', working_dir, 'images'), ptpip): sys.exit('Could not initialize camera.') # add ground plane ps = PoseStamped() ps.header.frame_id = robot.get_planning_frame() scene.add_plane('ground', ps) # add photobox ps.pose.position.x = photobox_pos[ 0] + photobox_size[0] / 2 + wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_left', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[ 0] - photobox_size[0] / 2 - wall_thickness / 2 ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_right', ps, (wall_thickness, photobox_size[1], photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[ 1] - photobox_size[1] / 2 - wall_thickness / 2 ps.pose.position.z = photobox_pos[2] + photobox_size[2] / 2 scene.add_box('box_wall_back', ps, (photobox_size[0], wall_thickness, photobox_size[2])) ps.pose.position.x = photobox_pos[0] ps.pose.position.y = photobox_pos[1] ps.pose.position.z = photobox_pos[2] - wall_thickness / 2 scene.add_box('box_ground', ps, (photobox_size[0], photobox_size[1], wall_thickness)) # add turntable turntable_height = turntable_pos[2] - photobox_pos[2] ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = photobox_pos[2] + turntable_height / 2 scene.add_cylinder('turntable', ps, turntable_height, turntable_radius) # add object on turntable ps.pose.position.x = turntable_pos[0] ps.pose.position.y = turntable_pos[1] ps.pose.position.z = turntable_pos[2] + object_size[2] / 2 scene.add_cylinder('object', ps, object_size[2], max(object_size[:2]) / 2) # add cable mounts scene.remove_attached_object('upper_arm_link', 'upper_arm_cable_mount') scene.remove_attached_object('forearm_link', 'forearm_cable_mount') scene.remove_world_object('upper_arm_cable_mount') scene.remove_world_object('forearm_cable_mount') if ptpip is None and not simulation: size = [0.08, 0.08, 0.08] ps.header.frame_id = 'upper_arm_link' ps.pose.position.x = -0.13 ps.pose.position.y = -0.095 ps.pose.position.z = 0.135 scene.attach_box(ps.header.frame_id, 'upper_arm_cable_mount', ps, size) ps.header.frame_id = 'forearm_link' ps.pose.position.x = -0.275 ps.pose.position.y = -0.08 ps.pose.position.z = 0.02 scene.attach_box(ps.header.frame_id, 'forearm_cable_mount', ps, size) # add camera eef_link = move_group.get_end_effector_link() ps = PoseStamped() ps.header.frame_id = eef_link scene.remove_attached_object(eef_link, 'camera_0') scene.remove_attached_object(eef_link, 'camera_1') scene.remove_world_object('camera_0') scene.remove_world_object('camera_1') ps.pose.position.y = camera_pos[1] ps.pose.position.z = camera_pos[2] if camera_mesh: ps.pose.position.x = camera_pos[0] quaternion = tf.transformations.quaternion_from_euler( math.radians(camera_orientation[0]), math.radians(camera_orientation[1]), math.radians(camera_orientation[2])) ps.pose.orientation.x = quaternion[0] ps.pose.orientation.y = quaternion[1] ps.pose.orientation.z = quaternion[2] ps.pose.orientation.w = quaternion[3] scene.attach_mesh(eef_link, 'camera_0', ps, os.path.expanduser(camera_mesh), camera_size) if not simulation: scene.attach_mesh(eef_link, 'camera_1', ps, os.path.expanduser(camera_mesh), numpy.array(camera_size) * 1.5) vertices = scene.get_attached_objects( ['camera_0'])['camera_0'].object.meshes[0].vertices camera_size[0] = max(vertice.x for vertice in vertices) - min( vertice.x for vertice in vertices) camera_size[1] = max(vertice.y for vertice in vertices) - min( vertice.y for vertice in vertices) camera_size[2] = max(vertice.z for vertice in vertices) - min( vertice.z for vertice in vertices) else: ps.pose.position.x = camera_pos[0] + camera_size[0] / 2 scene.attach_box(eef_link, 'camera_0', ps, camera_size)
class Pick_Place: def __init__(self): # rospy.init_node('pick_and_place') self.scene = PlanningSceneInterface() self.robot = RobotCommander() filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'joints_setup.yaml') with open(filename) as file: joints_setup = yaml.load(file) jointslimit = joints_setup["joints_limit"] home_value = joints_setup["home_value"] j1 = home_value["joint_1"] j2 = home_value["joint_2"] j3 = home_value["joint_3"] j4 = home_value["joint_4"] j5 = home_value["joint_5"] j6 = home_value["joint_6"] g = home_value["gripper"] self.set_home_value([j1, j2, j3, j4, j5, j6, g]) home_value = joints_setup["pick_place_home_value"] j1 = home_value["joint_1"] j2 = home_value["joint_2"] j3 = home_value["joint_3"] j4 = home_value["joint_4"] j5 = home_value["joint_5"] j6 = home_value["joint_6"] g = home_value["gripper"] self.set_pick_place_home_value([j1, j2, j3, j4, j5, j6, g]) self.object_list = {} self.obstacle_list = {} filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) rospy.loginfo("Spawning Objects in Gazebo and planning scene") objects = objects_info["objects"] objects_name = objects.keys() for object_name in objects_name: name = object_name shape = objects[name]["shape"] color = objects[name]["color"] x = objects[name]["pose"]["x"] y = objects[name]["pose"]["y"] z = objects[name]["pose"]["z"] roll = objects[name]["pose"]["roll"] pitch = objects[name]["pose"]["pitch"] yaw = objects[name]["pose"]["yaw"] object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = x p.pose.position.y = y p.pose.position.z = z q = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation = Quaternion(*q) if shape == "box": x = objects[name]["size"]["x"] y = objects[name]["size"]["y"] z = objects[name]["size"]["z"] p.pose.position.z += z / 2 height = z width = y length = x self.object_list[name] = Object(p.pose, p.pose, height, width, length, shape, color) elif shape == "cylinder": height = objects[name]["size"]["height"] radius = objects[name]["size"]["radius"] p.pose.position.z += height / 2 self.object_list[name] = Object(p.pose, p.pose, height, radius * 2, radius * 2, shape, color) elif shape == "sphere": radius = objects[name]["size"] p.pose.position.z += radius self.object_list[name] = Object(p.pose, p.pose, radius * 2, radius * 2, radius * 2, shape, color) objects = objects_info["objects"] obstacles = objects_info["obstacles"] obstacles_name = obstacles.keys() for object_name in obstacles_name: name = object_name x = obstacles[name]["pose"]["x"] y = obstacles[name]["pose"]["y"] z = obstacles[name]["pose"]["z"] roll = obstacles[name]["pose"]["roll"] pitch = obstacles[name]["pose"]["pitch"] yaw = obstacles[name]["pose"]["yaw"] object_pose = self.pose2msg(x, y, z, roll, pitch, yaw) p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose.position.x = x p.pose.position.y = y p.pose.position.z = z x = obstacles[name]["size"]["x"] y = obstacles[name]["size"]["y"] z = obstacles[name]["size"]["z"] p.pose.position.z += z / 2 height = z width = y length = x self.obstacle_list[name] = Obstacle(p.pose, p.pose, height, width, length) # self.object_list = object_list self.goal_list = {} self.set_target_info() self.gripper_width = {} self.set_gripper_width_relationship() self.arm = moveit_commander.MoveGroupCommander("ur10_manipulator") self.arm.set_goal_tolerance(0.01) self.arm.set_pose_reference_frame("ur10_base_link") self.gripperpub = rospy.Publisher("gripper_controller/command", JointTrajectory, queue_size=0) self.transform_arm_to_baselink = Point() self.get_arm_to_baselink() self.gripper_length = 0.33 self.get_workspace() self.message_pub = rospy.Publisher("/gui_message", String, queue_size=0) self.updatepose_pub = rospy.Publisher("/updatepose", Bool, queue_size=0) self.robot_pose = Pose() self.odom_sub = rospy.Subscriber("/odom", Odometry, self.robot_pose_callback) def send_message(self, message): msg = String() msg.data = message self.message_pub.publish(msg) def updatepose_trigger(self, value): msg = Bool() msg.data = value self.updatepose_pub.publish(msg) def clean_scene(self, object_name): self.scene.remove_world_object(object_name) def clean_all_objects_in_scene(self): objects_name = self.object_list.keys() for object_name in objects_name: self.clean_scene(object_name) def spawn_all_objects(self): objects_name = self.object_list.keys() for object_name in objects_name: self.spawn_object_rviz(object_name) def spawn_object_rviz(self, object_name): self.clean_scene(object_name) this_object = self.object_list[object_name] p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose = copy.deepcopy(this_object.abs_pose) robot_pose = self.get_robot_pose() p.pose.position.x -= robot_pose.position.x p.pose.position.y -= robot_pose.position.y quaternion = (robot_pose.orientation.x, robot_pose.orientation.y, robot_pose.orientation.z, robot_pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = -euler[2] quat = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation.x = quat[0] p.pose.orientation.y = quat[1] p.pose.orientation.z = quat[2] p.pose.orientation.w = quat[3] self.object_list[object_name].relative_pose = self.baselink2arm(p.pose) shape = this_object.shape if shape == "box": x = this_object.length y = this_object.width z = this_object.height size = (x, y, z) self.scene.add_box(object_name, p, size) elif shape == "cylinder": height = this_object.height radius = this_object.width / 2 self.scene.add_cylinder(object_name, p, height, radius) elif shape == "sphere": radius = this_object.width / 2 self.scene.add_sphere(object_name, p, radius) rospy.sleep(0.5) def spawn_obstacle_rviz(self, obstacle_name): self.clean_scene(obstacle_name) this_object = self.obstacle_list[obstacle_name] p = PoseStamped() p.header.frame_id = self.robot.get_planning_frame() p.header.stamp = rospy.Time.now() p.pose = copy.deepcopy(this_object.abs_pose) robot_pose = self.get_robot_pose() # p.pose.position.x -= robot_pose.position.x # p.pose.position.y -= robot_pose.position.y quaternion = (robot_pose.orientation.x, robot_pose.orientation.y, robot_pose.orientation.z, robot_pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = -euler[2] quat = quaternion_from_euler(roll, pitch, yaw) p.pose.orientation.x = quat[0] p.pose.orientation.y = quat[1] p.pose.orientation.z = quat[2] p.pose.orientation.w = quat[3] x = p.pose.position.x - robot_pose.position.x y = p.pose.position.y - robot_pose.position.y p.pose.position.x = math.cos(yaw) * x - math.sin(yaw) * y p.pose.position.y = math.sin(yaw) * x + math.cos(yaw) * y x = this_object.length y = this_object.width z = this_object.height size = (x, y, z) self.scene.add_box(obstacle_name, p, size) rospy.sleep(0.5) def set_target_info(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) robot_x = objects_info["robot"]["pose"]["x"] robot_y = objects_info["robot"]["pose"]["y"] robot_z = objects_info["robot"]["pose"]["z"] targets = objects_info["targets"] target_name = targets.keys() for name in target_name: position = Point() position.x = targets[name]["x"] - robot_x position.y = targets[name]["y"] - robot_y position.z = targets[name]["z"] - robot_z self.goal_list[name] = position def set_gripper_width_relationship(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'interfaces', 'models_info.yaml') with open(filename) as file: objects_info = yaml.load(file) gripper_joint_value = objects_info["gripper_joint_value"] objects_width = gripper_joint_value.keys() for object_width in objects_width: self.gripper_width[object_width] = gripper_joint_value[ object_width] def get_object_list(self): return self.object_list.keys() def get_target_list(self): return self.goal_list.keys() def get_object_pose(self, object_name): return copy.deepcopy(self.object_list[object_name].relative_pose) def get_object_info(self, object_name): this_object = copy.deepcopy(self.object_list[object_name]) pose = this_object.relative_pose height = this_object.height width = this_object.width length = this_object.length shape = this_object.shape color = this_object.color return pose, height, width, length, shape, color def get_target_position(self, target_name): position = copy.deepcopy(self.goal_list[target_name]) # print("before transformation",position) p = Pose() p.position = position robot_pose = self.get_robot_pose() quaternion = (robot_pose.orientation.x, robot_pose.orientation.y, robot_pose.orientation.z, robot_pose.orientation.w) euler = euler_from_quaternion(quaternion) yaw = -euler[2] x = position.x - robot_pose.position.x y = position.y - robot_pose.position.y position.x = math.cos(yaw) * x - math.sin(yaw) * y position.y = math.sin(yaw) * x + math.cos(yaw) * y # print("after transformation",position, yaw) position = self.baselink2arm(p).position return position def robot_pose_callback(self, msg): self.robot_pose.position = msg.pose.pose.position self.robot_pose.orientation = msg.pose.pose.orientation def get_robot_pose(self): # try: # listener = tf.TransformListener() # (trans,rot) = listener.lookupTransform('/map', "/base_link", rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # rospy.loginfo("no transformation") # return # pose = Pose() # pose.position.x = trans[0] # pose.position.y = trans[1] # pose.position.z = trans[2] # pose.orientation.x = rot[0] # pose.orientation.y = rot[1] # pose.orientation.z = rot[2] # pose.orientation.w = rot[3] return self.robot_pose def get_arm_to_baselink(self): # try: # listener = tf.TransformListener() # (trans,rot) = listener.lookupTransform('/base_link', "/ur10_base_link", rospy.Time(0)) # except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): # rospy.loginfo("no transformation") # return # self.transform_arm_to_baselink.x = trans[0] # self.transform_arm_to_baselink.y = trans[1] # self.transform_arm_to_baselink.z = trans[2] # print(self.transform_arm_to_baselink) self.transform_arm_to_baselink.x = 0.205 self.transform_arm_to_baselink.y = 0 self.transform_arm_to_baselink.z = 0.802 def get_workspace(self): filename = os.path.join( rospkg.RosPack().get_path('rqt_industrial_robot'), 'src', 'rqt_mobile_manipulator', 'joints_setup.yaml') with open(filename) as file: joints_setup = yaml.load(file) workspace = joints_setup["workspace"] x = workspace["center"]["x"] y = workspace["center"]["y"] z = workspace["center"]["z"] min_r = workspace["r"]["min"] max_r = workspace["r"]["max"] min_z = workspace["min_z"] self.workspace = WorkSpace(x, y, z, min_r, max_r, min_z) # check if the position is inside workspace def is_inside_workspace(self, x, y, z): if z > self.workspace.min_z: dx = x - self.workspace.x dy = y - self.workspace.y dz = z - self.workspace.z r = math.sqrt(dx**2 + dy**2 + dz**2) if self.workspace.min_r < r < self.workspace.max_r: return True return False # move one joint of the arm to value def set_arm_joint(self, joint_id, value): joint_goal = self.arm.get_current_joint_values() joint_goal[joint_id - 1] = value self.arm.go(joint_goal, wait=True) self.arm.stop() # Forward Kinematics (FK): move the arm by axis values def move_joint_arm(self, joint_0, joint_1, joint_2, joint_3, joint_4, joint_5): joint_goal = self.arm.get_current_joint_values() joint_goal[0] = joint_0 joint_goal[1] = joint_1 joint_goal[2] = joint_2 joint_goal[3] = joint_3 joint_goal[4] = joint_4 joint_goal[5] = joint_5 self.arm.go(joint_goal, wait=True) self.arm.stop() # To guarantee no residual movement def get_joint_value(self, joint_id): joints = self.arm.get_current_joint_values() return joints[joint_id - 1] def get_joints_value(self): joints = self.arm.get_current_joint_values() return joints def get_arm_pose(self): pose = self.arm.get_current_pose().pose pose = self.baselink2arm(pose) pose = self.TCP2gripper(pose, self.gripper_length) # print(pose) return self.msg2pose(pose) def set_random_pose(self): self.arm.set_random_target() def set_gripper_length(self, length): self.gripper_length = length def set_home_value(self, home_value): self.home_value = home_value def back_to_home(self, move_gripper=True): j1, j2, j3, j4, j5, j6, g = self.home_value self.move_joint_arm(j1, j2, j3, j4, j5, j6) if move_gripper: self.move_joint_hand(g) def set_pick_place_home_value(self, home_value): self.pick_place_home_value = home_value def move_to_pick_place_home(self, move_gripper=True): j1, j2, j3, j4, j5, j6, g = self.pick_place_home_value self.move_joint_arm(j1, j2, j3, j4, j5, j6) if move_gripper: self.move_joint_hand(g) def fold_robot_arm(self, ): self.move_joint_arm(0, 0, 0, 0, 0, 0) self.move_joint_hand(0) def gripper2TCP(self, pose, length=0): roll, pitch, yaw, x, y, z = self.msg2pose(pose) T = euler_matrix(roll, pitch, yaw) T[0:3, 3] = numpy.array([x, y, z]) pos_gripper_tcp = numpy.array([0, -length, 0, 1]) pos_tcp = T.dot(pos_gripper_tcp) pose.position.x = pos_tcp[0] pose.position.y = pos_tcp[1] pose.position.z = pos_tcp[2] return pose def TCP2gripper(self, pose, length=0): roll, pitch, yaw, x, y, z = self.msg2pose(pose) T = euler_matrix(roll, pitch, yaw) T[0:3, 3] = numpy.array([x, y, z]) pos_gripper_tcp = numpy.array([0, length, 0, 1]) pos_tcp = T.dot(pos_gripper_tcp) pose.position.x = pos_tcp[0] pose.position.y = pos_tcp[1] pose.position.z = pos_tcp[2] return pose def arm2baselink(self, in_pose): pose = copy.deepcopy(in_pose) pose.position.x += self.transform_arm_to_baselink.x pose.position.y += self.transform_arm_to_baselink.y pose.position.z += self.transform_arm_to_baselink.z return pose def baselink2arm(self, in_pose): pose = copy.deepcopy(in_pose) pose.position.x -= self.transform_arm_to_baselink.x pose.position.y -= self.transform_arm_to_baselink.y pose.position.z -= self.transform_arm_to_baselink.z return pose # Inverse Kinematics (IK): move TCP to given position and orientation def move_pose_arm(self, pose_goal): # pose_goal = self.pose2msg(roll, pitch, yaw, x, y, z) # pose_goal = self.gripper2TCP(pose_goal, self.gripper_length) x = pose_goal.position.x y = pose_goal.position.y z = pose_goal.position.z if not self.is_inside_workspace(x, y, z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') return False self.arm.set_pose_target(pose_goal) self.arm.go(wait=True) self.arm.stop() # To guarantee no residual movement self.arm.clear_pose_targets() return True # Move gripper def move_joint_hand(self, finger1_goal, finger2_goal=10, finger3_goal=10): if finger2_goal == 10 and finger3_goal == 10: finger2_goal, finger3_goal = finger1_goal, finger1_goal jointtrajectory = JointTrajectory() jointtrajectory.header.stamp = rospy.Time.now() jointtrajectory.joint_names.extend(["H1_F1J3", "H1_F2J3", "H1_F3J3"]) joint = JointTrajectoryPoint() joint.positions.extend([finger1_goal, finger2_goal, finger3_goal]) joint.time_from_start = rospy.Duration(1) jointtrajectory.points.append(joint) self.gripperpub.publish(jointtrajectory) def pose2msg(self, roll, pitch, yaw, x, y, z): pose = geometry_msgs.msg.Pose() quat = quaternion_from_euler(roll, pitch, yaw) pose.orientation.x = quat[0] pose.orientation.y = quat[1] pose.orientation.z = quat[2] pose.orientation.w = quat[3] pose.position.x = x pose.position.y = y pose.position.z = z return pose def msg2pose(self, pose): x = pose.position.x y = pose.position.y z = pose.position.z quaternion = (pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w) euler = euler_from_quaternion(quaternion) roll = euler[0] pitch = euler[1] yaw = euler[2] return roll, pitch, yaw, x, y, z def set_grasp_distance(self, min_distance, desired_distance): self.approach_retreat_min_dist = min_distance self.approach_retreat_desired_dist = desired_distance def count_gripper_width(self, object_name): object_width = self.object_list[object_name].width if object_width in self.gripper_width: return self.gripper_width[object_width] else: rospy.loginfo( "Cannot find suitable gripper joint value for this object width" ) return 0 # pick object to goal position def pickup(self, object_name, position, width, distance=0.12): if not self.is_inside_workspace(position.x, position.y, position.z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') rospy.loginfo('Stop placing') return pose = Pose() pose.position = position q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180)) pose.orientation = Quaternion(*q) # transform from gripper to TCP pose = self.gripper2TCP(pose, self.gripper_length) pose.position.z += distance rospy.loginfo('Start picking') self.move_pose_arm(pose) rospy.sleep(1) # move down waypoints = [] wpose = self.arm.get_current_pose().pose wpose = self.baselink2arm(wpose) print(wpose) wpose.position.z -= distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) # pick rospy.sleep(0.5) self.move_joint_hand(width) rospy.sleep(3) # if object_name != "yellow_ball": # self.arm.attach_object(object_name) # move up waypoints = [] wpose = self.arm.get_current_pose().pose wpose = self.baselink2arm(wpose) wpose.position.z += distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) rospy.loginfo('Pick finished') # place object to goal position def place(self, object_name, position, width=-0.2, distance=0.12): if not self.is_inside_workspace(position.x, position.y, position.z): rospy.loginfo('***** GOAL POSE IS OUT OF ROBOT WORKSPACE *****') rospy.loginfo('Stop placing') return pose = Pose() pose.position = position q = quaternion_from_euler(numpy.deg2rad(-90), 0, numpy.deg2rad(180)) pose.orientation = Quaternion(*q) # transform from gripper to TCP pose = self.gripper2TCP(pose, self.gripper_length) pose.position.z += distance rospy.loginfo('Start placing') self.move_pose_arm(pose) rospy.sleep(1) # move down waypoints = [] wpose = self.arm.get_current_pose().pose wpose = self.baselink2arm(wpose) print(wpose) wpose.position.z -= distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) # place self.move_joint_hand(width) rospy.sleep(3) # if object_name != "yellow_ball": # self.arm.detach_object(object_name) # self.clean_scene(object_name) self.object_list.pop(object_name) # move up waypoints = [] wpose = self.arm.get_current_pose().pose wpose = self.baselink2arm(wpose) wpose.position.z += distance waypoints.append(copy.deepcopy(wpose)) (plan, fraction) = self.arm.compute_cartesian_path( waypoints, # waypoints to follow 0.01, # eef_step 0.0) # jump_threshold self.arm.execute(plan, wait=True) self.updatepose_trigger(True) rospy.loginfo('Place finished')
def __init__(self): # Initialize move_group API moveit_commander.roscpp_initialize(sys.argv) # Initialize ROS node rospy.init_node('moveit_obstacles_demo') # Initialize scene scene = PlanningSceneInterface() # Create a publisher for scene change information self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) # Create a dictionary to store object color self.colors = dict() # Wait for scene to be ready rospy.sleep(1) # Initialize arm_group which needs to be controlled by move_group arm = MoveGroupCommander('arm') # Get end link name end_effector_link = arm.get_end_effector_link() # Set tolerance for position (m) and orientation (rad) arm.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.05) # Allow replanning if failed arm.allow_replanning(True) # Set reference frame for target position reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # Set planning time limitation arm.set_planning_time(5) # Set scene object name front_board_id = 'front_board' back_board_id = 'back_board' left_board_id = 'left_board' right_board_id = "right_board" bottle_id = 'bottle' # Remove previous object scene.remove_world_object(front_board_id) scene.remove_world_object(back_board_id) scene.remove_world_object(left_board_id) scene.remove_world_object(right_board_id) scene.remove_world_object(bottle_id) rospy.sleep(1) # Move arm to initial position arm.set_named_target('home') arm.go() rospy.sleep(2) # Initialize gripper_group which needs to be controlled by move_group gripper = moveit_commander.MoveGroupCommander('gripper') # Set object dimension front_board_size = [0.2, 0.25, 0.01] back_board_size = [0.2, 0.25, 0.01] left_board_size = [0.25, 0.14, 0.01] right_board_size = [0.25, 0.14, 0.01] bottle_height, bottle_radius = 0.06, 0.02 # Add object to scene front_board_pose = PoseStamped() front_board_pose.header.frame_id = reference_frame front_board_pose.pose.position.x = 0 front_board_pose.pose.position.y = 0.1 front_board_pose.pose.position.z = 0.125 front_board_pose.pose.orientation.w = 0.707 front_board_pose.pose.orientation.x = 0.707 scene.add_box(front_board_id, front_board_pose, front_board_size) back_board_pose = PoseStamped() back_board_pose.header.frame_id = reference_frame back_board_pose.pose.position.x = 0 back_board_pose.pose.position.y = 0.24 back_board_pose.pose.position.z = 0.125 back_board_pose.pose.orientation.w = 0.707 back_board_pose.pose.orientation.x = 0.707 scene.add_box(back_board_id, back_board_pose, back_board_size) left_board_pose = PoseStamped() left_board_pose.header.frame_id = reference_frame left_board_pose.pose.position.x = 0.1 left_board_pose.pose.position.y = 0.17 left_board_pose.pose.position.z = 0.125 left_board_pose.pose.orientation.w = 0.707 left_board_pose.pose.orientation.y = 0.707 scene.add_box(left_board_id, left_board_pose, left_board_size) right_board_pose = PoseStamped() right_board_pose.header.frame_id = reference_frame right_board_pose.pose.position.x = -0.1 right_board_pose.pose.position.y = 0.17 right_board_pose.pose.position.z = 0.125 right_board_pose.pose.orientation.w = 0.707 right_board_pose.pose.orientation.y = 0.707 scene.add_box(right_board_id, right_board_pose, right_board_size) bottle_pose = PoseStamped() bottle_pose.header.frame_id = reference_frame bottle_pose.pose.position.x = 0.1 bottle_pose.pose.position.y = -0.2 bottle_pose.pose.position.z = 0.02 bottle_pose.pose.orientation.w = 0.707 bottle_pose.pose.orientation.y = 0.707 scene.add_cylinder(bottle_id, bottle_pose, bottle_height, bottle_radius) # Set object color self.setColor(front_board_id, 1.0, 1.0, 1.0, 0.6) self.setColor(back_board_id, 1.0, 1.0, 1.0, 0.6) self.setColor(left_board_id, 1.0, 1.0, 1.0, 0.6) self.setColor(right_board_id, 1.0, 1.0, 1.0, 0.6) self.setColor(bottle_id, 0.6, 0.1, 0, 1.0) # Publish color self.sendColors() rospy.sleep(3) # Set target position for gripper and move gripper gripper_position = (np.array([36, -36, 36, 36]) * np.pi / 180.0).tolist() gripper.set_joint_value_target(gripper_position) gripper.go() rospy.sleep(5) # Set target position for arm joint_position = (np.array([28, -10, 50, -88, 58]) * np.pi / 180.0).tolist() arm.set_joint_value_target(joint_position) arm.go() # Set target position for arm joint_position = (np.array([28, -44, 30, -91, 58]) * np.pi / 180.0).tolist() arm.set_joint_value_target(joint_position) arm.go() # Set target position for gripper and move gripper gripper_position = (np.array([20, -20, 20, 20]) * np.pi / 180.0).tolist() gripper.set_joint_value_target(gripper_position) gripper.go() rospy.sleep(10) # Move arm to initial position arm.set_named_target('home') arm.go() # Set target position for gripper and move gripper gripper_position = (np.array([-33, 33, -33, -33]) * np.pi / 180.0).tolist() gripper.set_joint_value_target(gripper_position) gripper.go() # Close and exit MoveIt moveit_commander.roscpp_shutdown()