class PlanningSceneHelper: def __init__(self, package=None, mesh_folder_path=None): # interface to the planning and collision scenes self.psi = PlanningSceneInterface() # self.scene_pub = Publisher("/collision_object", CollisionObject, # queue_size=1, latch=True) self.vis_pub = Publisher("/collision_scene", MarkerArray, queue_size=10) self.marker_array = MarkerArray() sleep(1.0) if package is not None and mesh_folder_path is not None: # Build folder path for use in load rospack = RosPack() self.package = package self.mesh_folder_path = mesh_folder_path self.package_path = rospack.get_path(package) self.folder_path = os.path.join(self.package_path, mesh_folder_path)\ + os.sep else: loginfo( "Missing package or mesh folder path supplied to planning_scene_helper; no meshes can be added" ) # Create dict of objects, for removing markers later self.objects = {} self.next_id = 1 # Loads the selected mesh file into collision scene at the desired location. def add_mesh(self, object_id, frame, filename, visual=None, pose=None, position=None, orientation=None, rpy=None, color=None): if self.package is None: logwarn("No package was provided; no meshes can be loaded.") if visual is None: visual = filename filename = self.folder_path + filename stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) try: self.psi.add_mesh(object_id, stamped_pose, filename) loginfo("Loaded " + object_id + " from" + filename + " as collision object.") marker = self.make_marker_stub(stamped_pose, color=color) marker.type = Marker.MESH_RESOURCE # marker.mesh_resource = "file:/" + self.folder_path + visual marker.mesh_resource = "package://" + self.package + os.sep + self.mesh_folder_path + os.sep + visual self.marker_array = self.make_new_marker_array_msg() self.marker_array.markers.append(marker) self.vis_pub.publish(self.marker_array) self.objects[object_id] = marker loginfo("Loaded " + object_id + " from " + marker.mesh_resource + " as marker.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) except AssimpError as ae: loginfo("Problem loading " + object_id + " collision object: " + str(ae)) def add_cylinder(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) # Cylinders are special - they are not supported directly by # moveit_commander. It must be published manually to collision scene cyl = CollisionObject() cyl.operation = CollisionObject.ADD cyl.id = object_id cyl.header = stamped_pose.header prim = SolidPrimitive() prim.type = SolidPrimitive.CYLINDER prim.dimensions = [size[0], size[1]] cyl.primitives = [prim] cyl.primitive_poses = [stamped_pose.pose] # self.scene_pub.publish(cyl) marker = self.make_marker_stub(stamped_pose, [size[1] * 2, size[2] * 2, size[0]], color=color) marker.type = Marker.CYLINDER self.publish_marker(object_id, marker) sleep(0.5) logdebug("Loaded " + object_id + " as cylindrical collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def add_sphere(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) self.psi.add_sphere(object_id, stamped_pose, size[0]) loginfo("got past adding collision scene object") marker = self.make_marker_stub( stamped_pose, [size[0] * 2, size[1] * 2, size[2] * 2], color=color) marker.type = Marker.SPHERE self.publish_marker(object_id, marker) sleep(0.5) loginfo("Loaded " + object_id + " as collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def add_box(self, object_id, frame, size, pose=None, position=None, orientation=None, rpy=None, color=None): try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy) self.psi.add_box(object_id, stamped_pose, size) marker = self.make_marker_stub(stamped_pose, size, color=color) marker.type = Marker.CUBE self.publish_marker(object_id, marker) sleep(0.5) loginfo("Loaded " + object_id + " as collision object.") except ROSException as e: loginfo("Problem loading " + object_id + " collision object: " + str(e)) def attach_box(self, link, object_id, frame, size, attach_to_link, pose=None, position=None, orientation=None, rpy=None): """TODO: color is not yet supported, since it's not internally supported by psi.attach_box. Basically duplicate this method but with color support.""" try: stamped_pose = self.make_stamped_pose(frame=frame, pose=pose, position=position, orientation=orientation, rpy=rpy), self.psi.attach_box(link, object_id, stamped_pose, size, attach_to_link) sleep(0.5) loginfo("Attached " + object_id + " as collision object.") except ROSException as e: loginfo("Problem attaching " + object_id + " collision object: " + str(e)) @staticmethod def make_stamped_pose(frame, pose=None, position=None, orientation=None, rpy=None): if orientation is not None and rpy is not None: logwarn("Collision object has both orientation (quaternion) and " "Rotation (rpy) defined! Defaulting to quaternion " "representation") stamped_pose = PoseStamped() stamped_pose.header.frame_id = frame stamped_pose.header.stamp = Time.now() # use a pose if one is provided, otherwise, make your own from the # position and orientation. if pose is not None: stamped_pose.pose = pose else: stamped_pose.pose.position.x = position[0] stamped_pose.pose.position.y = position[1] stamped_pose.pose.position.z = position[2] # for orientation, allow either quaternion or RPY specification if orientation is not None: stamped_pose.pose.orientation.x = orientation[0] stamped_pose.pose.orientation.y = orientation[1] stamped_pose.pose.orientation.z = orientation[2] stamped_pose.pose.orientation.w = orientation[3] elif rpy is not None: quaternion = transformations.quaternion_from_euler( rpy[0], rpy[1], rpy[2]) stamped_pose.pose.orientation.x = quaternion[0] stamped_pose.pose.orientation.y = quaternion[1] stamped_pose.pose.orientation.z = quaternion[2] stamped_pose.pose.orientation.w = quaternion[3] else: stamped_pose.pose.orientation.w = 1 # make basic quaternion return stamped_pose # Fill a ROS Marker message with the provided data def make_marker_stub(self, stamped_pose, size=None, color=None): if color is None: color = (0.5, 0.5, 0.5, 1.0) if size is None: size = (1, 1, 1) marker = Marker() marker.header = stamped_pose.header # marker.ns = "collision_scene" marker.id = self.next_id marker.lifetime = Time(0) # forever marker.action = Marker.ADD marker.pose = stamped_pose.pose marker.color.r = color[0] marker.color.g = color[1] marker.color.b = color[2] if len(color) == 4: alpha = color[3] else: alpha = 1.0 marker.color.a = alpha marker.scale.x = size[0] marker.scale.y = size[1] marker.scale.z = size[2] self.next_id += 1 return marker def make_new_marker_array_msg(self): ma = MarkerArray() return ma # publish a single marker def publish_marker(self, object_id, marker): loginfo("Publishing marker for object {}".format(object_id)) self.marker_array = self.make_new_marker_array_msg() self.marker_array.markers.append(marker) self.vis_pub.publish(self.marker_array) self.objects[object_id] = marker # Remove the provided object from the world. def remove(self, object_id): # if object_id not in self.objects: # logwarn("PlanningSceneHelper was not used to create object with id " # + object_id + ". Attempting to remove it anyway.") try: # first remove the actual collision object self.psi.remove_world_object(object_id) if object_id in self.objects: # then remove the marker associated with it marker = self.objects[object_id] marker.action = Marker.DELETE self.publish_marker(object_id, marker) self.objects.pop(object_id) logdebug("Marker for collision object " + object_id + " removed.") except ROSException as e: loginfo("Problem removing " + object_id + " from collision scene:" + str(e)) return except KeyError as e: loginfo("Problem removing " + object_id + " from collision scene:" + str(e)) return loginfo("Model " + object_id + " removed from collision scene.") # Remove the provided attached collision object. def remove_attached(self, link, object_id): try: self.psi.remove_attached_object(link=link, name=object_id) loginfo("Attached object '" + object_id + "' removed from collision scene.") except: loginfo("Problem attached object '" + object_id + "' removing from collision scene.")
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()
def __init__(self): # 初始化move_group的API moveit_commander.roscpp_initialize(sys.argv) # 初始化ROS节点 rospy.init_node('moveit_obstacles_demo') # 初始化场景对象 scene = PlanningSceneInterface() # 创建一个发布场景变化信息的发布者 self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=5) rospy.Subscriber("chatter", Float64MultiArray, callback) # 创建一个存储物体颜色的字典对象 self.colors = dict() # 等待场景准备就绪 rospy.sleep(1) # 初始化需要使用move group控制的机械臂中的arm group arm = MoveGroupCommander('arm') gripper = MoveGroupCommander('gripper') # 获取终端link的名称 end_effector_link = arm.get_end_effector_link() # 设置位置(单位:米)和姿态(单位:弧度)的允许误差 arm.set_goal_position_tolerance(0.01) gripper.set_goal_position_tolerance(0.01) arm.set_goal_orientation_tolerance(0.2) # 当运动规划失败后,允许重新规划 arm.allow_replanning(True) # 设置目标位置所使用的参考坐标系 reference_frame = 'base_link' arm.set_pose_reference_frame(reference_frame) # 设置每次运动规划的时间限制:5s arm.set_planning_time(5) # 设置场景物体的名称 table_id = 'table' # cy_id = 'cy' box1_id = 'box1' box2_id = 'box2' box3_id = 'box3' sphere_id = 'sphere' # 移除场景中之前运行残留的物体 scene.remove_world_object(table_id) scene.remove_world_object(box1_id) scene.remove_world_object(box2_id) scene.remove_world_object(box3_id) scene.remove_world_object(sphere_id) rospy.sleep(1) #控制机械臂先回到初始化位置 arm.set_named_target('init') arm.go() rospy.sleep(1) # arm.set_named_target('start') # arm.go() # rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(0) # 设置桌面的高度 table_ground = 0.37 # 设置table、box1和box2的三维尺寸 table_size = [0.2, 0.3, 0.01] box1_size = [0.01, 0.01, 0.19] box2_size = [0.01, 0.01, 0.19] box3_size = [0.005, 0.01, 0.3] sphere_R = 0.01 error = 0.03 # 将三个物体加入场景当中 table_pose = PoseStamped() table_pose.header.frame_id = reference_frame table_pose.pose.position.x = -table_size[0] / 2.0 table_pose.pose.position.y = 0.0 table_pose.pose.position.z = table_ground + table_size[2] / 2.0 table_pose.pose.orientation.w = 1.0 scene.add_box(table_id, table_pose, table_size) #scene.add_cylinder box1_pose = PoseStamped() box1_pose.header.frame_id = reference_frame box1_pose.pose.position.x = -0.09 box1_pose.pose.position.y = table_size[0] / 2.0 box1_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box1_pose.pose.orientation.w = 1.0 scene.add_box(box1_id, box1_pose, box1_size) box2_pose = PoseStamped() box2_pose.header.frame_id = reference_frame box2_pose.pose.position.x = -0.09 box2_pose.pose.position.y = -table_size[0] / 2.0 box2_pose.pose.position.z = 0.18 + box1_size[2] / 2.0 box2_pose.pose.orientation.w = 1.0 scene.add_box(box2_id, box2_pose, box2_size) # box3_pose = PoseStamped() # box3_pose.header.frame_id = reference_frame # box3_pose.pose.position.x = pos_aim[0] # box3_pose.pose.position.y = pos_aim[1] # box3_pose.pose.position.z = box3_size[2]/2.0+0.1 # box3_pose.pose.orientation.w = 1.0 # scene.add_box(box3_id, box3_pose, box3_size) sphere_pose = PoseStamped() sphere_pose.header.frame_id = reference_frame sphere_pose.pose.position.x = pos_aim[0] + 0 sphere_pose.pose.position.y = pos_aim[1] sphere_pose.pose.position.z = pos_aim[2] sphere_pose.pose.orientation.w = 1.0 scene.add_sphere(sphere_id, sphere_pose, sphere_R) # 将桌子设置成红色,两个box设置成橙色 self.setColor(table_id, 0, 0, 0, 1) # self.setColor(table_id, 0.8, 0, 0, 1.0) # self.setColor(box1_id, 0.8, 0.4, 0, 1.0) self.setColor(box1_id, 1, 1, 1, 1.0) self.setColor(box2_id, 1, 1, 1, 1.0) self.setColor(box3_id, 1, 1, 1, 1.0) self.setColor(sphere_id, 0.8, 0, 0, 1.0) # 将场景中的颜色设置发布 self.sendColors() # rospy.INFO("waiting...") # if(Recive_FLAG==1): # rospy.INFO("OK!") # 设置机械臂的运动目标位置 target_pose = PoseStamped() target_pose.header.frame_id = reference_frame target_pose.pose.position.x = pos_aim[0] - error target_pose.pose.position.y = pos_aim[1] target_pose.pose.position.z = pos_aim[2] # target_pose.pose.orientation.w = 1.0 #####0.3 # 0.2 0.2 0.2 0.15 0.15 # 0.12 0.11 0.12 0.15 0.15 # 0.30 0.245 0.35 0.35 0.25 # 控制机械臂运动到目标位置 arm.set_pose_target(target_pose, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.03]) gripper.go() rospy.sleep(1) # # 控制机械臂终端向x移动5cm arm.shift_pose_target(0, 0.01, end_effector_link) arm.go() rospy.sleep(1) gripper.set_joint_value_target([0.05]) gripper.go() rospy.sleep(1) # # 设置机械臂的运动目标位置,进行避障规划 # target_pose2 = PoseStamped() # target_pose2.header.frame_id = reference_frame # target_pose2.pose.position.x = 0.15 # target_pose2.pose.position.y = 0 #-0.25 # target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05 # target_pose2.pose.orientation.w = 1.0 # # 控制机械臂运动到目标位置 # arm.set_pose_target(target_pose2, end_effector_link) # arm.go() # rospy.sleep(2) #控制机械臂回到初始化位置 arm.set_named_target('init') arm.go() # 关闭并退出moveit moveit_commander.roscpp_shutdown() moveit_commander.os._exit(0) rospy.spin()
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)
print ">>>>> add scenes" # publish a demo scene p = PoseStamped() p.header.frame_id = robot.get_planning_frame() p.pose.position.x = 0 p.pose.position.y = -1 p.pose.position.z = 0.2 p.pose.orientation.w = 1.0 scene.add_box("pole", p, (0.4, 0.1, 0.4)) p.pose.position.x = 0 p.pose.position.y = 0.8 p.pose.position.z = 0.1 p.pose.orientation.w = 1.0 scene.add_sphere("ball", p, 0.1) p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.1 scene.add_box("table", p, (1.0, 2.6, 0.2)) p.pose.position.x = 0 p.pose.position.y = 0 p.pose.position.z = -0.4 scene.add_plane("ground_plane", p) rospy.sleep(20) ## We can get the name of the reference frame for this robot print ">>>>> Reference frame: %s" % group.get_planning_frame()
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')