def main(): rospy.init_node('part1_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height / 2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37 / 2) microwave_height = 0.28 microwave_width = 0.48 # microwave_depth = 0.33 microwave_depth = 0.27 microwave_x = 0.97 microwave_z = 0.06 microwave_y = 0.18 planning_scene.addBox('microwave', microwave_depth, microwave_width, microwave_height, microwave_x, microwave_y, table_height + microwave_z + microwave_height / 2) rospy.sleep(2) rospy.spin()
def tuck(): if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") move_thread.stop() return
def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return
class PickPlaceDemo(): def __init__(self): self._scene = PlanningSceneInterface('base_link') self._pickplace = PickPlaceInterface('xarm5', 'gripper', verbose=True) self._move_group = MoveGroupInterface('xarm5', 'base_link') def setup_scene(self): # remove previous objects for name in self._scene.getKnownCollisionObjects(): self._scene.removeCollisionObject(name, False) for name in self._scene.getKnownAttachedObjects(): self._scene.removeAttachedObject(name, False) self._scene.waitForSync() self.__addBoxWithOrientation('box', 0.25, 0.25, 0.25, -0.45, 0.1, 0.125, 0, 0, np.radians(45), wait=False) self._scene.waitForSync() def __addBoxWithOrientation(self, name, size_x, size_y, size_z, x, y, z, roll, pitch, yaw, wait=True): s = SolidPrimitive() s.dimensions = [size_x, size_y, size_z] s.type = s.BOX ps = PoseStamped() ps.header.frame_id = self._scene._fixed_frame ps.pose.position.x = x ps.pose.position.y = y ps.pose.position.z = z q = tf.transformations.quaternion_from_euler(roll, pitch, yaw) ps.pose.orientation.x = q[0] ps.pose.orientation.y = q[1] ps.pose.orientation.z = q[2] ps.pose.orientation.w = q[3] self._scene.addSolidPrimitive(name, s, ps.pose, wait) def pickup(self): g = Grasp() self._pickplace.pickup("box", [g], support_name="box")
def main(): rospy.init_node('a5_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01 / 2) planning_scene.addBox('table', 0.5, 1, 0.72, 1, 0, 0.72 / 2) rospy.sleep(2)
def main(): rospy.init_node('a5_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('top_shelf') planning_scene.removeCollisionObject('bottom_shelf') planning_scene.addBox('top_shelf', 0.32, 1, 0.4, 0.99, 0, 1.64) planning_scene.addBox('bottom_shelf', 0.5, 1, 1.09, 0.9, 0, 0.545) rospy.sleep(2)
class Arm: def __init__(self): self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link") self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.addCube("my_front_ground", 2, 1.4, 0.0, -0.48) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -0.6) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -0.6) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -0.6) def MoveToPose(self, X, Y, Z, Roll, Pitch, Yaw): orientation = tf.transformations.quaternion_from_euler( Roll, Pitch, Yaw) self.poseStamped = PoseStamped() self.poseStamped.header.frame_id = 'base_link' self.poseStamped.header.stamp = rospy.Time.now() self.poseStamped.pose.position.x = X self.poseStamped.pose.position.y = Y self.poseStamped.pose.position.z = Z self.poseStamped.pose.orientation.x = orientation[0] self.poseStamped.pose.orientation.y = orientation[1] self.poseStamped.pose.orientation.z = orientation[2] self.poseStamped.pose.orientation.w = orientation[3] self.moveGroup.moveToPose(self.poseStamped, 'gripper_link', 0.005, max_velocity_scaling_factor=0.2) self.result = self.moveGroup.get_move_action().get_result() def Tuck(self): joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] self.moveGroup.moveToJointPosition(joints, pose, 0.01, max_velocity_scaling_factor=0.2) while not rospy.is_shutdown(): self.result = self.moveGroup.get_move_action().get_result() if self.result.error_code.val == MoveItErrorCodes.SUCCESS: break def AddDice(self, name, X, Y, Z): self.planning_scene.addCube(name, 0.1, X, Y, Z) def RemoveDice(self, name): self.planning_scene.removeCollisionObject(name)
class GripperTeleopDown(GripperTeleop): def __init__(self, arm, gripper, im_server): super(GripperTeleopDown, self).__init__(arm, gripper, im_server) # subscribes to the visualization marker for the table coming from perception self._planning_scene = PlanningSceneInterface('base_link') self._table_sub = rospy.Subscriber('/visualization_marker', Marker, self.table_callback) def table_callback(self, msg): if msg.ns == 'table': self._planning_scene.removeCollisionObject('table') # TODO: use the msg.pose.orientation to conver the scale directly # instead of manually swapping x and y x = min(msg.scale.y, msg.scale.x) y = max(msg.scale.y, msg.scale.x) #self._planning_scene.addBox('table', x, y, msg.scale.z, msg.pose.position.x, msg.pose.position.y, msg.pose.position.z) if msg.ns == 'tray handle': #handle_im = create_handle_interactive_marker(create_pose_stamped(msg.pose).pose, msg.scale) pose = create_pose_stamped(msg.pose).pose pose.orientation = self._constraint_pose.orientation pose.position.z += 0.3 handle_im = create_gripper_interactive_marker( pose, 'handle', False, False) self._im_server.insert(handle_im, feedback_cb=self.handle_feedback) self._im_server.applyChanges() def start(self): mat = tft.identity_matrix() mat[:, 0] = np.array([0, 0, -1, 0]) mat[:, 2] = np.array([1, 0, 0, 0]) o = tft.quaternion_from_matrix(mat) self._constraint_pose = Pose(orientation=Quaternion(*o)) gripper_im = create_gripper_interactive_marker(self._constraint_pose, pregrasp=False, rotation_enabled=False) self._im_server.insert(gripper_im, feedback_cb=self.handle_feedback) self._im_server.applyChanges() oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'gripper_link' oc.orientation = self._constraint_pose.orientation oc.weight = 1.0 oc.absolute_z_axis_tolerance = 0.1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 self.contraint = oc
def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() else: rospy.loginfo("moveit already started") rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") keepout_pose = Pose() keepout_pose.position.z = 0.375 keepout_pose.orientation.w = 1.0 ground_pose = Pose() ground_pose.position.z = -0.03 ground_pose.orientation.w = 1.0 rospack = rospkg.RosPack() mesh_dir = os.path.join(rospack.get_path('fetch_teleop'), 'mesh') scene.addMesh('keepout', keepout_pose, os.path.join(mesh_dir, 'keepout.stl')) scene.addMesh('ground', ground_pose, os.path.join(mesh_dir, 'ground.stl')) joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.client.moveToJointPosition( joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") scene.removeCollisionObject("ground") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return
def main(): rospy.init_node('part2_obstacles') wait_for_time() planning_scene = PlanningSceneInterface('base_link') planning_scene.clear() planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('floor') planning_scene.addBox('floor', 2, 2, 0.01, 0, 0, 0.01/2) table_height = 0.767 table_width = 0.7 table_x = 0.95 planning_scene.addBox('table', table_width, 2, table_height, table_x, 0, table_height/2) planning_scene.addBox('robot_base', 0.54, 0.52, 0.37, 0, 0, 0.37/2) rospy.sleep(2) rospy.spin()
def setup_scene(): scene = PlanningSceneInterface('base_link') scene.removeCollisionObject('box') scene.addCube('box', 0.25, -0.45, 0.1, 0.125) pick_place = PickPlaceInterface('xarm5', 'gripper', verbose=True) grasp = Grasp() # fill in g # setup object named object_name using PlanningSceneInterface pick_place.pickup('box', [grasp], support_name='supporting_surface') place_loc = PlaceLocation() # fill in l pick_place.place('box'[place_loc], goal_is_eef=True, support_name='supporting_surface')
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects rospy.loginfo("Add collision objects") self.scene.removeCollisionObject("my_front_ground") self.scene.removeCollisionObject("my_back_ground") self.scene.removeCollisionObject("my_right_ground") self.scene.removeCollisionObject("my_left_ground") self.scene.removeCollisionObject("box_1") self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.scene.addBox("box_1", 0.44, 0.43, 0.68, 0.7, -0.4, .34) self.scene.waitForSync() def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def zero(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
def arm_move(self): move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) #planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_left_ground", 1, 1.5, self.translation[2][0], self.translation[2][1], self.translation[2][2]) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TF joint names joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # Lists of joint angles in the same order as in joint_names disco_poses = [[0, 2.5, -0.1, 3.0, 1.5, 3.0, 1.0, 3.0]] for pose in disco_poses: if rospy.is_shutdown(): break # Plans the joints in joint_names to angles in pose move_group.moveToJointPosition(joint_names, pose, wait=False) # Since we passed in wait=False above we need to wait here move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("Disco!") else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops move_group.get_move_action().cancel_all_goals()
class ArmController(object): def __init__(self): # Create move group interface for a fetch robot self._move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground self._planning_scene = PlanningSceneInterface("base_link") self._planning_scene.removeCollisionObject("my_front_ground") self._planning_scene.removeCollisionObject("my_back_ground") self._planning_scene.removeCollisionObject("my_right_ground") self._planning_scene.removeCollisionObject("my_left_ground") self._planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self._planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self._planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self._planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) def SetPose(self, target_pose): # Plans the joints in joint_names to angles in pose self._move_group.moveToJointPosition(target_pose.keys(), target_pose.values(), wait=False) # Since we passed in wait=False above we need to wait here self._move_group.get_move_action().wait_for_result() result = self._move_group.get_move_action().get_result() return_result = False if result: # Checking the MoveItErrorCode if result.error_code.val == MoveItErrorCodes.SUCCESS: rospy.loginfo("MoveIt pose reached.") return_result = True else: # If you get to this point please search for: # moveit_msgs/MoveItErrorCodes.msg rospy.logerr("Arm goal in state: %s", self._move_group.get_move_action().get_state()) else: rospy.logerr("MoveIt! failure no result returned.") # This stops all arm movement goals # It should be called when a program is exiting so movement stops self._move_group.get_move_action().cancel_all_goals() return return_result def SetPoseWithRetry(self, target_pose, max_time=3): for _ in range(max_time): if self.SetPose(target_pose): return True return False
class RobotArm: def __init__(self): self.moveGroup = MoveGroupInterface("arm_with_torso", "base_link") self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.removeCollisionObject("table1") def MoveToPose(self,X,Y,Z,x,y,z,w): self.poseStamped = PoseStamped() self.poseStamped.header.frame_id = 'base_link' self.poseStamped.header.stamp = rospy.Time.now() self.poseStamped.pose.position.x = X self.poseStamped.pose.position.y = Y self.poseStamped.pose.position.z = Z self.poseStamped.pose.orientation.x = x self.poseStamped.pose.orientation.y = y self.poseStamped.pose.orientation.z = z self.poseStamped.pose.orientation.w = w self.moveGroup.moveToPose(self.poseStamped, 'gripper_link') self.result = self.moveGroup.get_move_action().get_result() def OriginReturn(self): #Move to the origin joints = ["torso_lift_joint","shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] self.moveGroup.moveToJointPosition(joints, pose, 0.02) while 1: self.result = self.moveGroup.get_move_action().get_result() if self.result.error_code.val == MoveItErrorCodes.SUCCESS: break
def __init__(self): self.moving_mode = False self.plan_only = False self.prev_joy_buttons = [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0] self.joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] self.joy_subscriber = rospy.Subscriber("/fetch_joy", Joy, self.joy_callback) self.joints_subscriber = rospy.Subscriber("/joint_states", JointState, self.joints_states_callback) self.rgbimage_subscriber = rospy.Subscriber("/head_camera/depth/image", Image, self.depthimage_callback) self.depthimage_subscriber = rospy.Subscriber( "/head_camera/depth/image", Image, self.rgbimage_callback) self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_reset_pos", Empty, self.arm_reset_callback) self.reset_subscriber = rospy.Subscriber("/fetch_ai_arm_start_pos", Empty, self.arm_start_callback) self.arm_effort_pub = rospy.Publisher( "/arm_controller/weightless_torque/command", JointTrajectory, queue_size=2) self.gripper_client = actionlib.SimpleActionClient( "gripper_controller/gripper_action", GripperCommandAction) self.arm_cartesian_twist_pub = rospy.Publisher( "/arm_controller/cartesian_twist/command", Twist, queue_size=2) self.head_point_client = actionlib.SimpleActionClient( "head_controller/point_head", PointHeadAction) self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only=self.plan_only) planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def run(self): move_thread = None if not is_moveit_running(): rospy.loginfo("starting moveit") move_thread = MoveItThread() rospy.loginfo("Waiting for MoveIt...") self.client = MoveGroupInterface("arm_with_torso", "base_link") rospy.loginfo("...connected") # Padding does not work (especially for self collisions) # So we are adding a box above the base of the robot scene = PlanningSceneInterface("base_link") scene.addBox("keepout", 0.2, 0.5, 0.05, 0.15, 0.0, 0.375) joints = ["torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] if not self.untuck: pose = [0.05, 1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] else: pose = [0.05, 0.29, -0.60, -0.51, 1.45, 0.52, 0.88, -1.97] while not rospy.is_shutdown(): result = self.client.moveToJointPosition(joints, pose, 0.0, max_velocity_scaling_factor=0.5) if result and result.error_code.val == MoveItErrorCodes.SUCCESS: scene.removeCollisionObject("keepout") if move_thread: move_thread.stop() # On success quit # Stopping the MoveIt thread works, however, the action client # does not get shut down, and future tucks will not work. # As a work-around, we die and roslaunch will respawn us. rospy.signal_shutdown("done") sys.exit(0) return
def setBoundaries(): ''' This is a fix for the FETCH colliding with itself Define ground plane This creates objects in the planning scene that mimic the ground If these were not in place gripper could hit the ground ''' planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0)
def move(): rospy.init_node("move") move_group = MoveGroupInterface("arm_with_torso", "base_link") #planning scene setup planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) #move head camera client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction) rospy.loginfo("Waiting for head_controller...") client.wait_for_server() goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = "base_link" goal.target.point.x = 1.2 goal.target.point.y = 0.0 goal.target.point.z = 0.0 goal.min_duration = rospy.Duration(1.0) client.send_goal(goal) client.wait_for_result() #arm setup move_group = MoveGroupInterface("arm", "base_link") #set arm initial position joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class FetchRobotApi: def __init__(self): self.moving_mode = False self.waldo_mode = False self.prev_joy_buttons = 0 # See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions self.joint_names = [ #'torso_lift_joint', #'bellows_joint', #'head_pan_joint', #'head_tilt_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint', 'l_gripper_finger_joint', #'r_gripper_finger_joint', ] self.joint_name_map = dict([ (name, index) for index, name in enumerate(self.joint_names) ]) self.cur_joint_pos = np.zeros([len(self.joint_names)]) self.cur_joint_vel = np.zeros([len(self.joint_names)]) self.cur_joint_effort = np.zeros([len(self.joint_names)]) self.cur_base_scan = np.zeros([662], dtype=np.float32) self.cur_head_camera_depth_image = np.zeros([480, 640], dtype=np.float32) self.cur_head_camera_rgb_image = np.zeros([480, 640, 3], dtype=np.uint8) self.timeseq = None self.timeseq_mutex = threading.Lock() self.image_queue = Queue.Queue() self.image_compress_thread = threading.Thread( target=self.image_compress_main) self.image_compress_thread.daemon = True self.image_compress_thread.start() logger.info('Subscribing...') self.subs = [] if 1: self.subs.append( rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)) if 0: self.subs.append( rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb)) if 1: self.subs.append( rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb)) if 1: self.subs.append( rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb)) if 1: self.subs.append( rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb)) logger.info('Subscribed') self.arm_effort_pub = rospy.Publisher( '/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2) #self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2) self.gripper_client = actionlib.SimpleActionClient( 'gripper_controller/gripper_action', GripperCommandAction) self.arm_cartesian_twist_pub = rospy.Publisher( '/arm_controller/cartesian_twist/command', Twist, queue_size=2) self.head_point_client = actionlib.SimpleActionClient( 'head_controller/point_head', PointHeadAction) self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only=True) self.arm_trajectory_client = actionlib.SimpleActionClient( "arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) self.arm_trajectory_client.wait_for_server() if 0: logger.info('Creating MoveGroupInterface...') self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only=True) logger.info('Created MoveGroupInterface') if 0: logger.info('Creating PlanningSceneInterface...') self.planning_scene = PlanningSceneInterface('base_link') self.planning_scene.removeCollisionObject('my_front_ground') self.planning_scene.removeCollisionObject('my_back_ground') self.planning_scene.removeCollisionObject('my_right_ground') self.planning_scene.removeCollisionObject('my_left_ground') self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0) self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0) self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0) self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0) logger.warn('FetchRobotGymEnv ROS node running') self.head_point_client.wait_for_server() logger.warn('FetchRobotGymEnv ROS node connected') def start_timeseq(self, script): timeseq = TimeseqWriter.create(script) timeseq.add_channel('robot_joints', 'FetchRobotJoints') timeseq.add_channel('gripper_joints', 'FetchGripperJoints') timeseq.add_schema( 'FetchRobotJoints', ('torso_lift_joint', 'JointState'), ('head_pan_joint', 'JointState'), ('head_tilt_joint', 'JointState'), ('shoulder_pan_joint', 'JointState'), ('shoulder_lift_joint', 'JointState'), ('upperarm_roll_joint', 'JointState'), ('elbow_flex_joint', 'JointState'), ('forearm_roll_joint', 'JointState'), ('wrist_flex_joint', 'JointState'), ('wrist_roll_joint', 'JointState'), ) timeseq.add_schema( 'FetchGripperJoints', ('l_gripper_finger_joint', 'JointState'), ) timeseq.add_schema( 'JointState', ('pos', 'double'), ('vel', 'double'), ('effort', 'double'), ) timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction') timeseq.add_schema( 'FetchArmWeightlessTorqueAction', ('shoulder_pan_joint', 'WeightlessTorqueAction'), ('shoulder_lift_joint', 'WeightlessTorqueAction'), ('upperarm_roll_joint', 'WeightlessTorqueAction'), ('elbow_flex_joint', 'WeightlessTorqueAction'), ('forearm_roll_joint', 'WeightlessTorqueAction'), ('wrist_flex_joint', 'WeightlessTorqueAction'), ('wrist_roll_joint', 'WeightlessTorqueAction'), ) timeseq.add_schema( 'WeightlessTorqueAction', ('action', 'double'), ('effort', 'double'), ) timeseq.add_channel('gripper_action', 'FetchGripperAction') timeseq.add_schema( 'FetchGripperAction', ('action', 'double'), ('effort', 'double'), ('pos', 'double'), ) timeseq.add_channel('head_camera_rgb', 'VideoFrame') timeseq.add_schema( 'VideoFrame', ('url', 'string'), ) with self.timeseq_mutex: if self.timeseq: self.timeseq.close() self.timeseq = timeseq def close_timeseq(self): with self.timeseq_mutex: if self.timeseq is not None: self.timeseq.close() threading.Thread(target=self.timeseq.upload_s3).start() self.timeseq = None def start_axis_video(self): cameras = rospy.get_param('/axis_cameras') if cameras and len(cameras): with self.timeseq_mutex: if self.timeseq: for name, info in cameras.items(): logging.info('Camera %s: %s', name, repr(info)) self.timeseq.start_axis_video( timeseq_name=name, auth_header=info.get('auth_header'), daemon_endpoint=info.get('daemon_endpoint'), ipaddr=info.get('ipaddr'), local_link_prefix=info.get('local_link_prefix'), remote_traces_dir=info.get('remote_traces_dir'), resolution=info.get('resolution')) def stop_axis_video(self): with self.timeseq_mutex: if self.timeseq: self.timeseq.stop_axis_video() def base_scan_cb(self, data): # fmin replaces nans with 15 self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0) def head_camera_depth_image_cb(self, data): shape = [data.height, data.width] dtype = np.dtype(np.float32) npdata = np.fromstring(data.data, dtype=dtype).reshape(shape) npdata.strides = (data.step, dtype.itemsize) self.cur_head_camera_depth_image = np.fmin(npdata, 5.0) def head_camera_rgb_image_raw_cb(self, data): shape = [data.height, data.width, 3] dtype = np.dtype(np.uint8) npdata = np.fromstring(data.data, dtype=dtype).reshape(shape) npdata.strides = (data.step, dtype.itemsize * 3, 1) self.cur_head_camera_rgb_image = npdata if self.timeseq: self.image_queue.put(('head_camera_rgb', data)) def joint_states_cb(self, data): """ Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping. """ t0 = time.time() for i in range(len(data.name)): name = data.name[i] jni = self.joint_name_map.get(name, -1) if jni >= 0: self.cur_joint_pos[jni] = data.position[i] self.cur_joint_vel[jni] = data.velocity[i] self.cur_joint_effort[jni] = data.effort[i] with self.timeseq_mutex: if self.timeseq: channel, channel_type = ( (data.name[0] == 'l_wheel_joint' and ('robot_joints', 'FetchRobotJoints')) or (data.name[0] == 'l_gripper_finger_joint' and ('gripper_joints', 'FetchGripperJoints')) or (None, None)) if channel: state = dict([(jn, { '__type': 'JointState', 'pos': data.position[i], 'vel': data.velocity[i], 'effort': data.effort[i], }) for i, jn in enumerate(data.name)]) state['__type'] = channel_type state['rxtime'] = t0 self.timeseq.add(data.header.stamp.to_sec(), channel, state) if 0: t1 = time.time() print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % ( t0, t1 - t0, data.header.stamp.to_sec(), data.header.stamp.to_sec() - t0, channel) def image_compress_main(self): while True: channel, data = self.image_queue.get() if 0: print 'len(data.data)=%d data.width=%d data.height=%d' % (len( data.data), data.width, data.height) im = PIL.Image.frombytes('RGB', (data.width, data.height), data.data, 'raw') jpeg_writer = StringIO() im.save(jpeg_writer, 'jpeg') im_url = 'data:image/jpeg;base64,' + base64.b64encode( jpeg_writer.getvalue()) with self.timeseq_mutex: if self.timeseq: self.timeseq.add(data.header.stamp.to_sec(), channel, { '__type': 'VideoFrame', 'url': im_url, }) def move_to_start(self): self.moving_mode = True # Look down goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = '/base_link' goal.target.point.x = 1.5 goal.target.point.y = 0.0 goal.target.point.z = -0.2 goal.min_duration = rospy.Duration(0.5) if 0: logger.info('Point head to %s...', goal) self.head_point_client.send_goal(goal) if 0: logger.info('Point head sent') goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.1 self.gripper_client.send_goal(goal) joints = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint', ] pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0] result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True) if result.error_code.val == MoveItErrorCodes.SUCCESS: if 0: logger.info('Got trajectory %s', result.planned_trajectory) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = result.planned_trajectory.joint_trajectory logger.info('sending trajectory to arm...') self.arm_trajectory_client.send_goal(follow_goal) result = self.arm_trajectory_client.wait_for_result() logger.info('arm followed trajectory %s', result) else: logger.warn('moveToJointPosition returned %s', result) return result = self.head_point_client.wait_for_result() logger.info('head followed trajectory %s', result) logger.info('sending empty arm goal') empty_goal = FollowJointTrajectoryGoal() self.arm_trajectory_client.send_goal(empty_goal) logger.info('Point head done') self.moving_mode = False def set_arm_action(self, action): arm_joints = [ ('shoulder_pan_joint', 1.57, 33.82), ('shoulder_lift_joint', 1.57, 131.76), ('upperarm_roll_joint', 1.57, 76.94), ('elbow_flex_joint', 1.57, 66.18), ('forearm_roll_joint', 1.57, 29.35), ('wrist_flex_joint', 2.26, 25.70), ('wrist_roll_joint', 2.26, 7.36), ] arm_efforts = [ min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) * torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints ] arm_joint_names = [ name for name, vel_scale, torque_scale in arm_joints ] if 1: arm_joint_names.append('gravity_compensation') arm_efforts.append(1.0) arm_msg = JointTrajectory( joint_names=arm_joint_names, points=[JointTrajectoryPoint(effort=arm_efforts)]) self.arm_effort_pub.publish(arm_msg) with self.timeseq_mutex: if self.timeseq: state = dict([(jn, { '__type': 'WeightlessTorqueAction', 'action': action[self.joint_name_map.get(jn)], 'effort': arm_efforts[i], }) for i, jn in enumerate(arm_joint_names)]) state['__type'] = 'FetchArmWeightlessTorqueAction' self.timeseq.add(time.time(), 'arm_action', state) def set_gripper_action(self, action): grip = action[self.joint_name_map.get('l_gripper_finger_joint')] goal = GripperCommandGoal() if grip > 0: goal.command.max_effort = 60 * min(1.0, grip) goal.command.position = 0.0 else: goal.command.max_effort = 60 goal.command.position = 0.1 self.gripper_client.send_goal(goal) with self.timeseq_mutex: if self.timeseq: state = { '__type': 'FetchGripperAction', 'action': grip, 'effort': goal.command.max_effort, 'pos': goal.command.position, } self.timeseq.add(time.time(), 'gripper_action', state) def set_waldo_action(self, joy): twist = Twist() twist.linear.x = joy.axes[0] twist.linear.y = joy.axes[1] twist.linear.z = joy.axes[2] twist.angular.x = +3.0 * joy.axes[3] twist.angular.y = +3.0 * joy.axes[4] twist.angular.z = +2.0 * joy.axes[5] self.arm_cartesian_twist_pub.publish(twist) if joy.buttons[1] and not self.prev_joy_buttons[1]: goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.0 self.gripper_client.send_goal(goal) elif not joy.buttons[1] and self.prev_joy_buttons[1]: goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.1 self.gripper_client.send_goal(goal) def spacenav_joy_cb(self, joy): if 0: logger.info('joy %s', str(joy)) if joy.buttons[0] and not self.prev_joy_buttons[0]: if self.waldo_mode: self.stop_waldo_mode() elif not self.moving_mode: self.start_waldo_mode() if self.waldo_mode and not self.moving_mode: self.set_waldo_action(joy) self.prev_joy_buttons = joy.buttons def start_waldo_mode(self): logger.info('Start waldo mode') self.waldo_mode = True self.start_timeseq('fetchwaldo') self.start_axis_video() def stop_waldo_mode(self): logger.info('Stop waldo mode') self.waldo_mode = False timeseq_url = None if self.timeseq: timeseq_url = self.timeseq.get_url() logger.info('save_logs url=%s', timeseq_url) self.stop_axis_video() self.close_timeseq() mts_thread = threading.Thread(target=self.move_to_start) mts_thread.daemon = True mts_thread.start()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("left_arm", "left_hand", verbose = True) self.move_group = MoveGroupInterface("left_arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server(rospy.Duration(15.0)) self.cubes = [] self.tf_listener = TransformListener() # self.gripper_client = actionlib.SimpleActionClient("/robot/end_effector/left_gripper/gripper_action", GripperCommandAction) # self.gripper_client.wait_for_server() # rospy.loginfo("...connected.") rospy.sleep(2.0) def updateScene(self, remove_collision = False): if remove_collision: for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() return # find objects self.cubes = [] goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(15.0)) find_result = self.find_client.get_result() # rospy.loginfo(find_result) # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 print(find_result.objects) # print(find_result.support_surfaces) # for obj in find_result.objects: # idx += 1 # print(idx) # obj.object.name = "object%d" % idx # self.scene.addSolidPrimitive(obj.object.name, # obj.object.primitives[0], # obj.object.primitive_poses[0], # wait = False) # self.cubes.append(obj.object.primitive_poses[0]) # # for obj in find_result.support_surfaces: # # extend surface to floor, and make wider since we have narrow field of view # height = obj.primitive_poses[0].position.z # obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], # 1.5, # wider # obj.primitives[0].dimensions[2] + height] # obj.primitive_poses[0].position.z += -height / 2.0 # # # add to scene # self.scene.addSolidPrimitive(obj.name, # obj.primitives[0], # obj.primitive_poses[0], # wait = False) # # self.scene.waitForSync() # # # store for grasping # self.objects = find_result.objects # self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue return obj.object, obj.grasps # nothing detected return None, None def get_sequence(self, seq_list): start = [self.objects[seq_list[0]].object, self.objects[seq_list[0]].grasps] or [None, None] target = [self.objects[seq_list[1]].object, self.objects[seq_list[1]].grasps] or [None, None] res = {'start': start, 'target': target} return res def get_transform_pose(self, pose): point = PoseStamped() point.header.frame_id = 'base_link' point.header.stamp = rospy.Time() point.pose = pose return self.tf_listener.transformPose('/map', point).pose.position def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, retries = 5, support_name = block.support_surface, scene = self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene = self.scene, retries = 5) # print(success) return success def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] # pose =[ 1.58, -0.056, -0.01, -1.31, -0.178, -0.13, 0.16] gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 0.0 gripper_goal.command.position = 0.09 self.gripper_client.send_goal(gripper_goal) self.gripper_client.wait_for_result(rospy.Duration(5)) start = rospy.Time.now() while rospy.Time.now() - start <= rospy.Duration(10.0): # rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return return def gripper_opening(self, opening = 0.09): gripper_goal = GripperCommandGoal() gripper_goal.command.max_effort = 0.0 gripper_goal.command.position = opening self.gripper_client.send_goal(gripper_goal) self.gripper_client.wait_for_result(rospy.Duration(5))
def main(): rospy.init_node("arm_obstacle_demo") wait_for_time() argv = rospy.myargv() planning_scene = PlanningSceneInterface("base_link") # Create table obstacle planning_scene.removeCollisionObject('table') table_size_x = 0.5 table_size_y = 1 table_size_z = 0.03 table_x = 0.8 table_y = 0 table_z = 0.6 planning_scene.addBox('table', table_size_x, table_size_y, table_size_z, table_x, table_y, table_z) # Create divider obstacle planning_scene.removeCollisionObject('divider') size_x = 0.3 size_y = 0.01 size_z = 0.2 x = table_x - (table_size_x / 2) + (size_x / 2) y = 0 z = table_z + (table_size_z / 2) + (size_z / 2) planning_scene.addBox('divider', size_x, size_y, size_z, x, y, z) pose1 = PoseStamped() pose1.header.frame_id = 'base_link' pose1.pose.position.x = 0.5 pose1.pose.position.y = -0.3 pose1.pose.position.z = 0.75 pose1.pose.orientation.w = 1 pose2 = PoseStamped() pose2.header.frame_id = 'base_link' pose2.pose.position.x = 0.5 pose2.pose.position.y = 0.3 pose2.pose.position.z = 0.75 pose2.pose.orientation.w = 1 oc = OrientationConstraint() oc.header.frame_id = 'base_link' oc.link_name = 'wrist_roll_link' oc.orientation.w = 1 oc.absolute_x_axis_tolerance = 0.1 oc.absolute_y_axis_tolerance = 0.1 oc.absolute_z_axis_tolerance = 3.14 oc.weight = 1.0 gripper = robot_api.Gripper() arm = robot_api.Arm() def shutdown(): arm.cancel_all_goals() rospy.on_shutdown(shutdown) kwargs = { 'allowed_planning_time': 15, 'execution_timeout': 10, 'num_planning_attempts': 5, 'replan': True, 'orientation_constraint': oc } planning_scene.removeAttachedObject('tray') gripper.open() error = arm.move_to_pose(pose1, **kwargs) if error is not None: rospy.logerr('Pose 1 failed: {}'.format(error)) else: rospy.loginfo('Pose 1 succeeded') rospy.sleep(2) frame_attached_to = 'gripper_link' frames_okay_to_collide_with = [ 'gripper_link', 'l_gripper_finger_link', 'r_gripper_finger_link' ] planning_scene.attachBox('tray', 0.3, 0.07, 0.01, 0.05, 0, 0, frame_attached_to, frames_okay_to_collide_with) planning_scene.setColor('tray', 1, 0, 1) planning_scene.sendColors() gripper.close() rospy.sleep(2) error = arm.move_to_pose(pose2, **kwargs) if error is not None: rospy.logerr('Pose 2 failed: {}'.format(error)) else: rospy.loginfo('Pose 2 succeeded') gripper.open() planning_scene.removeAttachedObject('tray') planning_scene.removeCollisionObject('table') planning_scene.removeCollisionObject('divider') return
class GripperInterfaceClient: def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() rospy.loginfo("...connected.") self.scene = PlanningSceneInterface("base_link") self.attached_object_publisher = rospy.Publisher( "attached_collision_object", AttachedCollisionObject, queue_size=10 ) self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() self.debug_index = -1 self.graspables = [] def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print "==========", goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0] + goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, wait=False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, wait=False) rospy.sleep(0.5) # Gets rid of annoying error messages stemming from race condition. self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive( obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False ) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0] + 0.1, 1.5, # wider obj.primitives[0].dimensions[2] + height, ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size # if obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07 or \ # obj.object.primitives[0].dimensions[0] < 0.05 or \ # obj.object.primitives[0].dimensions[0] > 0.07: # continue # has to be on table # if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def set_target_object(self, primitive_type, target_pose, dimensions): primitive_types = {"BOX": 1, "SPHERE": 2, "CYLINDER": 3, "CONE": 4} self.sought_object = Object() primitive = SolidPrimitive() primitive.type = primitive_types[primitive_type.upper()] primitive.dimensions = dimensions self.sought_object.primitives.append(primitive) p = Pose() p.position.x = target_pose[0] p.position.y = target_pose[1] p.position.z = target_pose[2] quaternion = quaternion_from_euler(target_pose[3], target_pose[4], target_pose[5]) p.orientation.x = quaternion[0] p.orientation.y = quaternion[1] p.orientation.z = quaternion[2] p.orientation.w = quaternion[3] self.sought_object.primitive_poses.append(p) def find_graspable_object(self, tolerance=0.01): self.updateScene() self.current_grasping_target = None for obj in self.objects: # Must have grasps if len(obj.grasps) < 1: continue # Compare to sought object detected_object_dimensions = np.array(obj.object.primitives[0].dimensions) sought_object_dimensions = np.array(self.sought_object.primitives[0].dimensions) try: difference = detected_object_dimensions - sought_object_dimensions print "===DEBUG: Difference: ", difference mag_diff = np.linalg.norm(difference) print "===DEBUG: mag_diff: ", mag_diff if mag_diff <= tolerance: self.current_grasping_target = obj tolerance = mag_diff print "===DEBUG: Tolerance: ", tolerance else: rospy.loginfo("Object dimensions do not match. Trying another object...") except: rospy.loginfo("Object geometries do not match. Trying another object...") if self.current_grasping_target is None: tolerance += 0.01 self.find_graspable_object(tolerance=tolerance) # Nothing detected def computeGraspToPickMatrix(self): pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation pick_translation_distance = self.pick_result.grasp.pre_grasp_approach.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) rotation_matrix = quaternion_matrix(pick_to_grasp_quaternion) translation = [pick_translation_distance, 0, 0] rotation_matrix[[0, 1, 2], 3] = translation pick_to_grasp_matrix = np.mat(rotation_matrix) grasp_to_pick_matrix = pick_to_grasp_matrix.getI() return grasp_to_pick_matrix def computePlaceToBaseMatrix(self, place): place_quaternion = place[3:] rotation_matrix = quaternion_matrix(place_quaternion) translation = place[0:3] rotation_matrix[[0, 1, 2], 3] = translation place_to_base_matrix = rotation_matrix return place_to_base_matrix def broadcastCurrentGraspingTargetTransform(self): pose = self.current_grasping_target.object.primitive_poses[0] br = tf2_ros.TransformBroadcaster() t = geometry_msgs.msg.TransformStamped() t.header.stamp = rospy.Time.now() t.header.frame_id = "base_link" t.child_frame_id = "current_grasping_target" t.transform.translation.x = pose.position.x t.transform.translation.y = pose.position.y t.transform.translation.z = pose.position.z t.transform.rotation.x = pose.orientation.x t.transform.rotation.y = pose.orientation.y t.transform.rotation.z = pose.orientation.z t.transform.rotation.w = pose.orientation.w br.sendTransform(t) def plan_pick(self): success, pick_result = self.pickplace.pick_with_retry( self.current_grasping_target.object.name, self.current_grasping_target.grasps, support_name=self.current_grasping_target.object.support_surface, scene=self.scene, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", ) self.pick_result = pick_result print "DEBUG: plan_pick(): ", self.scene.getKnownAttachedObjects() if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return pick_result.trajectory_stages else: rospy.loginfo("Plan rejected. Getting new grasps for replan...") else: rospy.logwarn("Planning failed. Getting new grasps for replan...") self.find_graspable_object() return self.plan_pick() def plan_place(self, desired_pose): places = list() ps = PoseStamped() # ps.pose = self.current_grasping_target.object.primitive_poses[0] ps.header.frame_id = self.current_grasping_target.object.header.frame_id grasp_to_pick_matrix = self.computeGraspToPickMatrix() place_to_base_matrix = self.computePlaceToBaseMatrix(desired_pose) grasp2_to_place_matrix = place_to_base_matrix * grasp_to_pick_matrix position_vector = grasp2_to_place_matrix[0:-1, 3] quaternion = quaternion_from_matrix(grasp2_to_place_matrix) position_array = position_vector.getA1() l = PlaceLocation() direction_components = self.pick_result.grasp.pre_grasp_approach.direction.vector l.place_pose.header.frame_id = ps.header.frame_id l.place_pose.pose.position.x = position_array[0] l.place_pose.pose.position.y = position_array[1] l.place_pose.pose.position.z = position_array[2] l.place_pose.pose.orientation.x = quaternion[0] l.place_pose.pose.orientation.y = quaternion[1] l.place_pose.pose.orientation.z = quaternion[2] l.place_pose.pose.orientation.w = quaternion[3] # copy the posture, approach and retreat from the grasp used approach = copy.deepcopy(self.pick_result.grasp.pre_grasp_approach) approach.desired_distance /= 2.0 l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 90 degrees in roll direction l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 1.57, 0, 0) places.append(copy.deepcopy(l)) print "DEBUG: Places: ", places[0] success, place_result = self.pickplace.place_with_retry( self.current_grasping_target.object.name, places, scene=self.scene, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", ) if success: rospy.loginfo("Planning succeeded.") trajectory_approved = get_user_approval() if trajectory_approved: return place_result.trajectory_stages else: rospy.loginfo("Plan rejected. Replanning...") else: rospy.logwarn("Planning failed. Replanning...") desired_pose[2] += 0.05 return self.plan_place(desired_pose) def recognizeAttachedObject(self): print "========= Recognizing attached object" name = self.current_grasping_target.object.name size_x = self.current_grasping_target.object.primitives[0].dimensions[0] size_y = self.current_grasping_target.object.primitives[0].dimensions[1] size_z = self.current_grasping_target.object.primitives[0].dimensions[2] (x, y, z) = (0.03, 0.0, 0.0) link_name = "gripper_link" touch_links = ["l_gripper_finger_link", "r_gripper_finger_link", "gripper_link"] detach_posture = None weight = 0.0 wait = True """ object_x = self.current_grasping_target.object.primitive_poses[0].position.x object_y = self.current_grasping_target.object.primitive_poses[0].position.y object_z = self.current_grasping_target.object.primitive_poses[0].position.z pick_rot = self.current_grasping_target.object.primitive_poses[0].orientation grasp_rot = self.pick_result.grasp.grasp_pose.pose.orientation grasp_position = self.pick_result.grasp.grasp_pose.pose.position pick_translation_distance = self.pick_result.grasp.post_grasp_retreat.desired_distance pick_quaternion = [pick_rot.x, pick_rot.y, pick_rot.z, pick_rot.w] grasp_quaternion = [grasp_rot.x, grasp_rot.y, grasp_rot.z, grasp_rot.w] pick_to_grasp_quaternion = quaternion_multiply(quaternion_inverse(grasp_quaternion), pick_quaternion) grasp_to_base_matrix_array = quaternion_matrix(grasp_quaternion) grasp_to_base_matrix = np.matrix(grasp_to_base_matrix_array) displacement = grasp_to_base_matrix.dot(np.matrix([[pick_translation_distance-.03], [0], [0], [0]])) print displacement attached_location = list() attached_location.append(object_x - displacement[0,0]) attached_location.append(object_y - displacement[1,0]) attached_location.append(object_z - displacement[2,0]) print attached_location quaternion = Quaternion() quaternion.x = pick_to_grasp_quaternion[0] quaternion.y = pick_to_grasp_quaternion[1] quaternion.z = pick_to_grasp_quaternion[2] quaternion.w = pick_to_grasp_quaternion[3] pose = Pose() pose.position.x = attached_location[0] pose.position.y = attached_location[1] pose.position.z = attached_location[2] pose.orientation = self.current_grasping_target.object.primitive_poses[0].orientation collision_object = self.scene.makeSolidPrimitive(self.current_grasping_target.object.name, self.current_grasping_target.object.primitives[0], pose) attached_collision_object = self.scene.makeAttached("gripper_link", collision_object, touch_links, detach_posture, 0.0) self.attached_object_publisher.publish(attached_collision_object) """ self.scene.attachBox( name, size_x, size_y, size_z, x, y, z, link_name, touch_links=touch_links, detach_posture=detach_posture, weight=weight, wait=wait, ) def removeCollisionObjects(self): collision_object_names = self.scene.getKnownCollisionObjects() print self.current_grasping_target.object.name print collision_object_names raw_input("press ENTER") x = self.scene.getKnownAttachedObjects() print x raw_input("press Enter") for name in collision_object_names: if name != self.current_grasping_target.object.name: self.scene.removeCollisionObject(name, wait=False)
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.robot = moveit_commander.RobotCommander() self.model_pose = [0.0,0.0,0.0] self.set_forbidden() self.set_init_pose() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator","base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = ["shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint"] pose = [0.0, -1.98, 1.0, -0.64, -1.57, 0.0] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals() def callback(self,data): #print data.markers[0].pose.position x = data.markers[0].pose.position.x y = data.markers[0].pose.position.y z = data.markers[0].pose.position.z self.model_pose = [x,y,z] def start_subscriber(self): rospy.Subscriber("/visualization_marker_array", MarkerArray, self.callback) def publish_tf(self): br = tf.TransformBroadcaster() br.sendTransform((self.model_pose[0], self.model_pose[1], self.model_pose[2]), (0.0, 0.0, 0.0, 1.0), rospy.Time.now(), "object", "depth_camera_frame") def target(self): rate = rospy.Rate(1) rospy.sleep(1) ls = tf.TransformListener() while not rospy.is_shutdown(): self.publish_tf() try: (trans,rot) = ls.lookupTransform('/base_link', '/object', rospy.Time(0)) print "tf base_link to test",trans group = moveit_commander.MoveGroupCommander("manipulator") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = pose.orientation.x pose_target.orientation.y = pose.orientation.y pose_target.orientation.z = pose.orientation.z pose_target.orientation.w = pose.orientation.w pose_target.position.x = trans[0] pose_target.position.y = trans[1] pose_target.position.z = trans[2]+0.2 group.set_pose_target(pose_target) group.go() except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "e" rospy.sleep(1) self.set_init_pose()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects rospy.loginfo("get new grasps") goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.removeCollisionObject("my_front_ground") self.scene.removeCollisionObject("my_back_ground") self.scene.removeCollisionObject("my_right_ground") self.scene.removeCollisionObject("my_left_ground") self.scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TODO: ADD BOX self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z # added + .1 obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def zero(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
# Note: fetch_moveit_config move_group.launch must be running # Safety!: Do NOT run this script near people or objects. # Safety!: There is NO perception. # The ONLY objects the collision detection software is aware # of are itself & the floor. if __name__ == '__main__': rospy.init_node("simple_disco") # Create move group interface for a fetch robot move_group = MoveGroupInterface("arm_with_torso", "base_link") # Define ground plane # This creates objects in the planning scene that mimic the ground # If these were not in place gripper could hit the ground planning_scene = PlanningSceneInterface("base_link") planning_scene.removeCollisionObject("my_front_ground") planning_scene.removeCollisionObject("my_back_ground") planning_scene.removeCollisionObject("my_right_ground") planning_scene.removeCollisionObject("my_left_ground") planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) # TF joint names joint_names = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] # Lists of joint angles in the same order as in joint_names
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() obj_cts = 0 def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True print " find goal" self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) print " find client" find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): print "the object %s should be removed" %(name) self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): print "the object %s should be removed" %(name) self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene objects = list() idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx print "-----------------------" print obj.object.primitive_poses[0] print " x: %d" %(obj.object.primitive_poses[0].position.x) # if obj.object.primitive_poses[0].position.y > 0.0 self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service = False) # def addSolidPrimitive (self, name, solid, pose, use_service=True) print "1, %d" %(idx) if obj.object.primitive_poses[0].position.x < 1.25: objects.append([obj, obj.object.primitive_poses[0].position.z]) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view print "2," height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], use_service = True ) self.scene.waitForSync() # store for grasping #self.objects = find_result.objects self.surfaces = find_result.support_surfaces # store graspable objects by Z objects.sort(key=lambda object: object[1]) objects.reverse() self.objects = [object[0] for object in objects] rospy.loginfo("!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!") rospy.loginfo("number of objects...:::::::" + str(len(objects))) #for object in objects: # print(object[0].object.name, object[1]) #exit(-1) def getGraspableObject(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: rospy.loginfo("must more than one object") continue # check size if obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25 or \ obj.object.primitives[0].dimensions[0] < 0.03 or \ obj.object.primitives[0].dimensions[0] > 0.25: continue # has to located in +y if obj.object.primitive_poses[0].position.y < 0.0: print "has to located in +y" continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: print "z has to larger than 0.5 " continue rospy.loginfo("object pose:") print obj.object.primitive_poses[0], obj.object.primitives[0] return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m-1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def stow(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return def intermediate_stow(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [0.7, -0.3, 0.0, -0.3, 0.0, -0.57, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
def __init__(self): # Initialize the move_group API moveit_commander.roscpp_initialize(sys.argv) rospy.init_node('moveit_demo') # Use the planning scene object to add or remove objects scene = PlanningSceneInterface("base_link") # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene) # Create a publisher for displaying gripper poses self.gripper_pose_pub = rospy.Publisher('gripper_pose', PoseStamped) # Create a dictionary to hold object colors self.colors = dict() # Initialize the move group for the right arm right_arm = MoveGroupCommander(GROUP_NAME_ARM) # Initialize the move group for the right gripper right_gripper = MoveGroupCommander(GROUP_NAME_GRIPPER) # Get the name of the end-effector link end_effector_link = right_arm.get_end_effector_link() # Allow some leeway in position (meters) and orientation (radians) right_arm.set_goal_position_tolerance(0.05) right_arm.set_goal_orientation_tolerance(0.1) # Allow replanning to increase the odds of a solution right_arm.allow_replanning(True) # Set the right arm reference frame right_arm.set_pose_reference_frame(REFERENCE_FRAME) # Allow 5 seconds per planning attempt right_arm.set_planning_time(15) # Set a limit on the number of pick attempts before bailing max_pick_attempts = 5 # Set a limit on the number of place attempts max_place_attempts = 3 # Give the scene a chance to catch up rospy.sleep(2) # Connect to the UBR-1 find_objects action server rospy.loginfo("Connecting to basic_grasping_perception/find_objects...") find_objects = actionlib.SimpleActionClient("basic_grasping_perception/find_objects", FindGraspableObjectsAction) find_objects.wait_for_server() rospy.loginfo("...connected") # Give the scene a chance to catch up rospy.sleep(1) # Start the arm in the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(1) # Begin the main perception and pick-and-place loop while not rospy.is_shutdown(): # Initialize the grasping goal goal = FindGraspableObjectsGoal() # We don't use the UBR-1 grasp planner as it does not work with our gripper goal.plan_grasps = False # Send the goal request to the find_objects action server which will trigger # the perception pipeline find_objects.send_goal(goal) # Wait for a result find_objects.wait_for_result(rospy.Duration(5.0)) # The result will contain support surface(s) and objects(s) if any are detected find_result = find_objects.get_result() # Display the number of objects found rospy.loginfo("Found %d objects" % len(find_result.objects)) # Remove all previous objects from the planning scene for name in scene.getKnownCollisionObjects(): scene.removeCollisionObject(name, False) for name in scene.getKnownAttachedObjects(): scene.removeAttachedObject(name, False) scene.waitForSync() # Clear the virtual object colors scene._colors = dict() # Use the nearest object on the table as the target target_pose = PoseStamped() target_pose.header.frame_id = REFERENCE_FRAME target_size = None the_object = None the_object_dist = 1.0 count = -1 # Cycle through all detected objects and keep the nearest one for obj in find_result.objects: count += 1 scene.addSolidPrimitive("object%d"%count, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Choose the object nearest to the robot dx = obj.object.primitive_poses[0].position.x - args.x dy = obj.object.primitive_poses[0].position.y d = math.sqrt((dx * dx) + (dy * dy)) if d < the_object_dist: the_object_dist = d the_object = count # Get the size of the target target_size = obj.object.primitives[0].dimensions # Set the target pose target_pose.pose = obj.object.primitive_poses[0] # We want the gripper to be horizontal target_pose.pose.orientation.x = 0.0 target_pose.pose.orientation.y = 0.0 target_pose.pose.orientation.z = 0.0 target_pose.pose.orientation.w = 1.0 # Make sure we found at least one object before setting the target ID if the_object != None: target_id = "object%d"%the_object # Insert the support surface into the planning scene for obj in find_result.support_surfaces: # Extend surface to floor height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 2.0, # make table wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # Add to scene scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) # Get the table dimensions table_size = obj.primitives[0].dimensions # If no objects detected, try again if the_object == None or target_size is None: rospy.logerr("Nothing to grasp! try again...") continue # Wait for the scene to sync scene.waitForSync() # Set colors of the table and the object we are grabbing scene.setColor(target_id, 223.0/256.0, 90.0/256.0, 12.0/256.0) # orange scene.setColor(find_result.objects[the_object].object.support_surface, 0.3, 0.3, 0.3, 0.7) # grey scene.sendColors() # Skip pick-and-place if we are just detecting objects if args.objects: if args.once: exit(0) else: continue # Get the support surface ID support_surface = find_result.objects[the_object].object.support_surface # Set the support surface name to the table object right_arm.set_support_surface_name(support_surface) # Specify a pose to place the target after being picked up place_pose = PoseStamped() place_pose.header.frame_id = REFERENCE_FRAME place_pose.pose.position.x = target_pose.pose.position.x place_pose.pose.position.y = 0.03 place_pose.pose.position.z = table_size[2] + target_size[2] / 2.0 + 0.015 place_pose.pose.orientation.w = 1.0 # Initialize the grasp pose to the target pose grasp_pose = target_pose # Shift the grasp pose half the size of the target to center it in the gripper try: grasp_pose.pose.position.x += target_size[0] / 2.0 grasp_pose.pose.position.y -= 0.01 grasp_pose.pose.position.z += target_size[2] / 2.0 except: rospy.loginfo("Invalid object size so skipping") continue # Generate a list of grasps grasps = self.make_grasps(grasp_pose, [target_id]) # Publish the grasp poses so they can be viewed in RViz for grasp in grasps: self.gripper_pose_pub.publish(grasp.grasp_pose) rospy.sleep(0.2) # Track success/failure and number of attempts for pick operation result = None n_attempts = 0 # Set the start state to the current state right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_pick_attempts: result = right_arm.pick(target_id, grasps) n_attempts += 1 rospy.loginfo("Pick attempt: " + str(n_attempts)) rospy.sleep(1.0) # If the pick was successful, attempt the place operation if result == MoveItErrorCodes.SUCCESS: result = None n_attempts = 0 # Generate valid place poses places = self.make_places(place_pose) # Set the start state to the current state #right_arm.set_start_state_to_current_state() # Repeat until we succeed or run out of attempts while result != MoveItErrorCodes.SUCCESS and n_attempts < max_place_attempts: for place in places: result = right_arm.place(target_id, place) if result == MoveItErrorCodes.SUCCESS: break n_attempts += 1 rospy.loginfo("Place attempt: " + str(n_attempts)) rospy.sleep(0.2) if result != MoveItErrorCodes.SUCCESS: rospy.loginfo("Place operation failed after " + str(n_attempts) + " attempts.") else: rospy.loginfo("Pick operation failed after " + str(n_attempts) + " attempts.") rospy.sleep(2) # Open the gripper to the neutral position right_gripper.set_joint_value_target(GRIPPER_NEUTRAL) right_gripper.go() rospy.sleep(2) # Return the arm to the "resting" pose stored in the SRDF file right_arm.set_named_target('resting') right_arm.go() rospy.sleep(2) # Give the servos a rest arbotix_relax_all_servos() rospy.sleep(2) if args.once: # Shut down MoveIt cleanly moveit_commander.roscpp_shutdown() # Exit the script moveit_commander.os._exit(0)
description="Remove objects from the MoveIt planning scene.") parser.add_argument("name", nargs="?", help="Name of object to remove") parser.add_argument("--all", help="Remove all objects.", action="store_true") parser.add_argument("--attached", help="Remove an attached object.", action="store_true") args = parser.parse_args() if args.all: rospy.init_node("remove_objects") scene = PlanningSceneInterface("base_link") for name in scene.getKnownCollisionObjects(): print("Removing %s" % name) scene.removeCollisionObject(name, wait=False) scene.waitForSync() elif args.name: rospy.init_node("remove_objects") scene = PlanningSceneInterface("base_link") print("Removing %s" % args.name) if args.attached: scene.removeAttachedObject(args.name) else: scene.removeCollisionObject(args.name) else: parser.print_help()
class World: def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) """Calls a method to display the collision objects. """ def create_planning_scene(self): print "[INFO] Create_planning_scene" self.display_towers_and_disks() """This method collects all needed information and publish them to other methods. """ def display_towers_and_disks(self): print "[INFO] Display towers and disks" for tower in self.tower_array: # call method to publish new tower self.publish_scene(tower.get_position(), tower) # set color by name self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0) # publish color self.planning_scene_interface.sendColors() for disk in self.disk_array: self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors() # wait for sync after publishing collision objects self.planning_scene_interface.waitForSync(5.0) rospy.sleep(5) """This method creates a box or a cylinder with methods of the planning scene interface. :param position: the position of the new collision object :param tower: the tower object :param disk: the disk object """ def publish_scene(self, position, tower=None, disk=None): if tower is not None: self.planning_scene_interface.addBox(tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0, (self.tower_positions[tower.get_id() - 1][2] * 2), position[0], position[1], position[2]) else: self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2, position[0], position[1], position[2]) """This method cleans the planning scene. :param close: with this flag a new planning scene objects will be removed in sync otherwise the object will be deleted without syncing the scene """ def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass """This method corrects the position of the moved disk. :param tower: parent tower of the disk """ def refresh_disk_pose(self, tower): print "[INFO] Refresh disk pose" disk = tower.get_last() if self.DEBUG is 1: print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\ "tower pose", tower.get_position() # remove the disk from planning scene self.planning_scene_interface.removeCollisionObject(disk.get_id(), False) # publish collision object and set old color self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects # here i am declaring goal as an object. https://github.com/mikeferguson/grasping_msgs/blob/master/action/FindGraspableObjects.action goal = FindGraspableObjectsGoal() goal.plan_grasps = True # passing the object to find_client # now find_client is wer magic happens # on demo.launch file i am runnning basic_grasping_perception. (below <!-- Start Perception -->) # this keeps running on background and i use actionlib (initalize on init) to get a hook to it. # find_client is connected to basic_grasping_perception self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) # here we get all the objects find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() rospy.loginfo("updating scene") idx = 0 # insert objects to the planning scene #TODO so these two for loops yo can hardcode the values. try printing all the params and u will understand for obj in find_result.objects: rospy.loginfo("object number -> %d" %idx) obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) idx += 1 # for grp in obj.grasps: # grp.grasp_pose.pose.position.z = 0.37 # this is just minor adjustments i did mess up with this code. just follwed simple gasp thingy for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z rospy.loginfo("height before => %f" % height) obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 rospy.loginfo("height after => %f" % obj.primitive_poses[0].position.z) # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps rospy.loginfo("no. of grasps detected %d dim => %f" % (len(obj.grasps), obj.object.primitives[0].dimensions[0])) if len(obj.grasps) < 1: continue # check size our object is a 0.05^3 cube if obj.object.primitives[0].dimensions[0] < 0.04 or \ obj.object.primitives[0].dimensions[0] > 0.06 or \ obj.object.primitives[0].dimensions[1] < 0.04 or \ obj.object.primitives[0].dimensions[1] > 0.06 or \ obj.object.primitives[0].dimensions[2] < 0.04 or \ obj.object.primitives[0].dimensions[2] > 0.06: continue rospy.loginfo("###### size: x => %f, y => %f, z => %f" % (obj.object.primitive_poses[0].position.x,obj.object.primitive_poses[0].position.y,obj.object.primitive_poses[0].position.z)) # has to be on table #if obj.object.primitive_poses[0].position.z < 0.5:grasping_client # continue return obj.object, obj.grasps # nothing detected rospy.loginfo("nothing detected") return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m-1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success def tuck(self): # so for each joint i will pass a specific value. first arms are move to pose1 then pose2 then pose3 sequentially joints = ["shoulder_roll_joint", "shoulder_pitch_joint", "shoulder_yaw_joint", "elbow_pitch_joint", "elbow_yaw_joint", "wrist_pitch_joint", "wrist_roll_joint"] pose1 = [1.57079633, 0, 0, 0, 0, 0, 0.0] pose2 = [1.57079633, 0, -1.57079633, 0, -1.57079633, 0, 0.0] pose3 = [1.57079633, 1.57079633, -1.57079633, 0, -1.57079633, 1.57079633, 0.0] while not rospy.is_shutdown(): self.move_group.moveToJointPosition(joints, pose1, 0.02) self.move_group.moveToJointPosition(joints, pose2, 0.02) result = self.move_group.moveToJointPosition(joints, pose3, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class GripperInterfaceClient(): def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print '==========', goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0]+goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) # Try mesh representation #idx = -1 #for obj in find_result.objects: # idx += 1 # obj.object.name = "object%d"%idx # self.scene.addMesh(obj.object.name, # obj.object.mesh_poses[0], # obj.object.meshes[0], # wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0] + 0.1, 2.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size # if obj.object.primitives[0].dimensions[0] < 0.03 or \ # obj.object.primitives[0].dimensions[0] > 0.04 or \ # obj.object.primitives[0].dimensions[0] < 0.07 or \ # obj.object.primitives[0].dimensions[0] > 0.09 or \ # obj.object.primitives[2].dimensions[2] < 0.19 or \ # obj.object.primitives[2].dimensions[2] > 0.20: # continue # has to be on table #if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def plan_pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, allow_gripper_support_collision=False, planner_id = 'PRMkConfigDefault', retries = 2, scene=self.scene) #self.pick_result = pick_result if success: return pick_result.trajectory_stages else: rospy.loginfo("Planning Failed.") return None
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient( find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the postrection m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class Grasping(object): def __init__(self): self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.scene = PlanningSceneInterface("base_link") self.move_group = MoveGroupInterface("arm", "base_link") find_objects = "basic_grasping_perception/find_objects" self.find_client = actionlib.SimpleActionClient( find_objects, FindGraspableObjectsAction) self.find_client.wait_for_server() def pickup(self, block, grasps): rospy.sleep(1) success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success #swaps two blocks positions. def swapBlockPos(self, block1Pos, block2Pos): #intermediate point for movement self.armIntermediatePose() #intermediate positon on the table, used for short term storage of the first block manipulated in the sorting. posIntermediate = np.array([0.725, 0]) self.armIntermediatePose() # Get block to pick while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(block1Pos) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") #self.tuck() #Place the block while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, posIntermediate): break rospy.logwarn("Placing failed.") #place block 2 in block 1's self.armIntermediatePose() # Get block to pick while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(block2Pos) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, block1Pos): break rospy.logwarn("Placing failed.") #place block1 in block 2's spot self.armIntermediatePose() while not rospy.is_shutdown(): rospy.loginfo("Picking object...") self.updateScene() cube, grasps = self.getGraspableObject(posIntermediate) if cube == None: rospy.logwarn("Perception failed.") continue # Pick the block if self.pickup(cube, grasps): break rospy.logwarn("Grasping failed.") #self.tuck() self.armIntermediatePose() while not rospy.is_shutdown(): rospy.loginfo("Placing object...") pose = PoseStamped() pose.pose = cube.primitive_poses[0] pose.pose.position.z += 0.05 pose.header.frame_id = cube.header.frame_id if self.place(cube, pose, block2Pos): break rospy.logwarn("Placing failed.") return def place(self, block, pose_stamped, placePos): rospy.sleep(1) #creates a list of place positons for the block, with a specified x, y, and z. places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = "base_link" l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat #this x and y value are input as placePos through the function call. l.place_pose.pose.position.x = placePos[0] l.place_pose.pose.position.y = placePos[1] places.append(copy.deepcopy(l)) #success = self.pickplace.place_with_retry(block.name, places, scene = self.scene) #return success ## create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m - 1): l.place_pose.pose = rotate_pose_msg_by_euler_angles( l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry( block.name, places, scene=self.scene) return success def updateScene(self): rospy.sleep(1) # find objectsw goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], use_service=False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], use_service=True) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def getGraspableObject(self, pos): graspable = None for obj in self.objects: if len(obj.grasps) < 1: continue if (obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[1] < 0.05 or \ obj.object.primitives[0].dimensions[1] > 0.07 or \ obj.object.primitives[0].dimensions[2] < 0.05 or \ obj.object.primitives[0].dimensions[2] > 0.07): continue if (obj.object.primitive_poses[0].position.z < 0.3) or \ (obj.object.primitive_poses[0].position.y > pos[1]+0.05) or \ (obj.object.primitive_poses[0].position.y < pos[1]-0.05) or \ (obj.object.primitive_poses[0].position.x > pos[0]+0.05) or \ (obj.object.primitive_poses[0].position.x < pos[0]-0.05): continue return obj.object, obj.grasps return None, None def armToXYZ(self, x, y, z): rospy.sleep(1) #new pose_stamped of the end effector that moves the arm out of the way of the vision for planning. intermediatePose = PoseStamped() intermediatePose.header.frame_id = 'base_link' #position intermediatePose.pose.position.x = x intermediatePose.pose.position.y = y intermediatePose.pose.position.z = z #quaternion for the end position intermediatePose.pose.orientation.w = 1 while not rospy.is_shutdown(): result = self.move_group.moveToPose(intermediatePose, "wrist_roll_link") if result.error_code.val == MoveItErrorCodes.SUCCESS: return def armIntermediatePose(self): self.armToXYZ(0.1, -0.7, 0.9) def tuck(self): joints = [ "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.robot = moveit_commander.RobotCommander() # self.model_pose = [0.0,0.2,0.2 , 0,0,0,1] self.model_pose = [0.0, 0.3, 0.1, 0, 0, 0, 1] self.obstacle_pose = [0.0, 0.0, 0.0] self.set_forbidden() self.set_init_pose() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_ground") self.planning_scene.addCube("my_ground", 2, 0, 0, -1.04) self.planning_scene.addBox("obstacle", 0.1, 0.5, 1, -0.3, 0, 0.5) self.planning_scene.addBox("obstacle2", 0.5, 0.1, 1, 0, -0.3, 0.5) #self.planning_scene.addCube("demo_cube", 0.06,0,0.3,0) print dir(self.planning_scene) import inspect print "addBox's variable is ", inspect.getargspec( self.planning_scene.addBox) print "attachBox's variable is ", inspect.getargspec( self.planning_scene.attachBox) print "addCube's variable is ", inspect.getargspec( self.planning_scene.addCube) print "setColor's variable is ", inspect.getargspec( self.planning_scene.setColor) def set_init_pose(sefl): #set init pose move_group = MoveGroupInterface("manipulator", "base_link") planning_scene = PlanningSceneInterface("base_link") joint_names = [ "shoulder_pan_joint", "shoulder_lift_joint", "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint" ] pose = [-1.26, -0.64, -2.44, -0.66, 1.56, 0.007] move_group.moveToJointPosition(joint_names, pose, wait=False) move_group.get_move_action().wait_for_result() result = move_group.get_move_action().get_result() move_group.get_move_action().cancel_all_goals() def callback(self, data): x = data.pose[1].position.x y = data.pose[1].position.y z = data.pose[1].position.z def start_subscriber(self): rospy.Subscriber("/gazebo/model_states", ModelStates, self.callback) def reaching_pose(self): print self.model_pose group = moveit_commander.MoveGroupCommander("manipulator") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = self.model_pose[3] pose_target.orientation.y = self.model_pose[4] pose_target.orientation.z = self.model_pose[5] pose_target.orientation.w = self.model_pose[6] pose_target.position.x = self.model_pose[0] pose_target.position.y = self.model_pose[1] pose_target.position.z = self.model_pose[2] group.set_pose_target(pose_target) group.go() #raw_input("Enter -->") def gripper_close(self): print("Gripper_Close") gripper_pose = Char() gripper_pose = 255 pub.publish(gripper_pose) #raw_input("Enter -->") def gripper_open(self): print("Gripper_Open") gripper_pose = Char() gripper_pose = 0 pub.publish(gripper_pose) #raw_input("Enter -->") def move_45(self): rate = rospy.Rate(1) rospy.sleep(1) a = 0.065 b = 0.040 c = 0.012 pattern_45 = [ [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'c', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B #[0.0 ,0.31 ,0.15 , 0,0,0,1], #A90 [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795], 'o', [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], #[0.0 ,0.31 ,0.1 , 0,0,0,1], #A90 [-0.10, 0.401, 0.041, 0, 0, 0, 1], 'c', [-0.10, 0.401, 0.100, 0, 0, 0, 1], [0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795], [0, 0.5 - b, 0.037 + b, 0.3826834, 0, 0, 0.9238795], [0, 0.5 - c, 0.037 + c, 0.3826834, 0, 0, 0.9238795], 'o', 'c', [0, 0.5 - a, 0.037 + a, 0.3826834, 0, 0, 0.9238795], [-0.10, 0.401, 0.100, 0, 0, 0, 1], [-0.10, 0.401, 0.050, 0, 0, 0, 1], 'o', [-0.10, 0.401, 0.100, 0, 0, 0, 1], [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [0, 0.5, 0.0365, 0.3826834, 0, 0, 0.9238795], 'c', [0, 0.5, 0.081, 0.3826834, 0, 0, 0.9238795], [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'o', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B ] while not rospy.is_shutdown(): for pose in pattern_45: if pose == 'c': self.gripper_close() rospy.sleep(1) elif pose == 'o': self.gripper_open() rospy.sleep(1) else: self.model_pose = pose self.reaching_pose() if rospy.is_shutdown(): return 0 def move_90(self): rate = rospy.Rate(1) rospy.sleep(1) a = 0.065 b = 0.040 c = 0.012 pattern_45 = [ [-0.10, 0.31, 0.015, 0, 0, 0, 1], #B 'c', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [0.0, 0.315, 0.0358, 0, 0, 0, 1], #A90 'o', [0.0, 0.315, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.40, 0.041, 0, 0, 0, 1], #negi 'c', [-0.10, 0.40, 0.1, 0, 0, 0, 1], [-0.001, 0.316, 0.1, 0, 0, 0, 1], #A90 [-0.001, 0.316, 0.053, 0, 0, 0, 1], #A90 'o', 'c', [0.0, 0.314, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.40, 0.1, 0, 0, 0, 1], [-0.10, 0.40, 0.05, 0, 0, 0, 1], #negi 'o', #[-0.10,0.40 ,0.1 , 0,0,0,1], [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [0.0, 0.315, 0.036, 0, 0, 0, 1], #A90 'c', [0.0, 0.300, 0.1, 0, 0, 0, 1], #A90 [-0.10, 0.320, 0.1, 0, 0, 0, 1], #B [-0.10, 0.31, 0.0153, 0, 0, 0, 1], #B 'o', [-0.10, 0.31, 0.1, 0, 0, 0, 1], #B ] while not rospy.is_shutdown(): for pose in pattern_45: if pose == 'c': self.gripper_close() rospy.sleep(1) elif pose == 'o': self.gripper_open() rospy.sleep(1) else: self.model_pose = pose self.reaching_pose() if rospy.is_shutdown(): return 0
class GripperInterfaceClient: def __init__(self): self.joint_names = ["l_gripper_finger_joint", "r_gripper_finger_joint"] self.client = actionlib.SimpleActionClient("gripper_controller/gripper_action", GripperCommandAction) rospy.loginfo("Waiting for gripper_controller...") self.client.wait_for_server() self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True, plan_only=True) find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def execute_trajectory(self, trajectory): goal_position = trajectory.points[-1].positions print "==========", goal_position command_goal = GripperCommandGoal() command_goal.command.position = goal_position[0] + goal_position[1] command_goal.command.max_effort = 50.0 # Placeholder. TODO Figure out a better way to compute this self.client.send_goal(command_goal) self.client.wait_for_result() def move_to(self, goal_position, max_effort): command_goal = GripperCommandGoal() command_goal.command.position = goal_position command_goal.command.max_effort = max_effort self.client.send_goal(command_goal) self.client.wait_for_result() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d" % idx self.scene.addSolidPrimitive( obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait=False ) # Try mesh representation # idx = -1 # for obj in find_result.objects: # idx += 1 # obj.object.name = "object%d"%idx # self.scene.addMesh(obj.object.name, # obj.object.mesh_poses[0], # obj.object.meshes[0], # wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [ obj.primitives[0].dimensions[0] + 0.1, 2.5, # wider obj.primitives[0].dimensions[2] + height, ] obj.primitive_poses[0].position.z += -height / 2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait=False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size # if obj.object.primitives[0].dimensions[0] < 0.03 or \ # obj.object.primitives[0].dimensions[0] > 0.04 or \ # obj.object.primitives[0].dimensions[0] < 0.07 or \ # obj.object.primitives[0].dimensions[0] > 0.09 or \ # obj.object.primitives[2].dimensions[2] < 0.19 or \ # obj.object.primitives[2].dimensions[2] > 0.20: # continue # has to be on table # if obj.object.primitive_poses[0].position.z < 0.5: # continue return obj.object, obj.grasps # nothing detected return None, None def plan_pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry( block.name, grasps, support_name=block.support_surface, allow_gripper_support_collision=False, planner_id="PRMkConfigDefault", retries=2, scene=self.scene, ) # self.pick_result = pick_result if success: return pick_result.trajectory_stages else: rospy.loginfo("Planning Failed.") return None
class Reaching: def __init__(self): rospy.init_node('reaching', anonymous=True) self.model_pose = [0.0, 0.0, 0.0] self.set_init_pose() #self.set_forbidden() def set_forbidden(self): #set forbidden erea self.planning_scene = PlanningSceneInterface("base_link") self.planning_scene.removeCollisionObject("my_front_ground") self.planning_scene.removeCollisionObject("my_back_ground") self.planning_scene.removeCollisionObject("my_right_ground") self.planning_scene.removeCollisionObject("my_left_ground") self.planning_scene.addCube("my_front_ground", 2, 1.1, 0.0, -1.0) self.planning_scene.addCube("my_back_ground", 2, -1.2, 0.0, -1.0) self.planning_scene.addCube("my_left_ground", 2, 0.0, 1.2, -1.0) self.planning_scene.addCube("my_right_ground", 2, 0.0, -1.2, -1.0) self.planning_scene.removeCollisionObject("demo_cube") def set_init_pose(self): #set init pose move_group = MoveGroupInterface("arm_with_torso", "base_link") #set arm initial position joints = [ "torso_lift_joint", "shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint" ] #pose = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] pose = [0.385, -1.0, -1.0, 0.0, 1.0, 0.0, 1.5, 0.0] #pose = [0.385, 1.32, 0.7, 0.0, -2.0, 0.0, -0.57, 0.0] result = move_group.moveToJointPosition(joints, pose, 0.02) def target(self): rate = rospy.Rate(1) rospy.sleep(1) ls = tf.TransformListener() while not rospy.is_shutdown(): print self.model_pose try: (trans, rot) = ls.lookupTransform('/base_link', '/object', rospy.Time(0)) print "tf base_link to test", trans group = moveit_commander.MoveGroupCommander("arm_with_torso") #print group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose = group.get_current_pose().pose pose_target = geometry_msgs.msg.Pose() pose_target.orientation.x = pose.orientation.x pose_target.orientation.y = pose.orientation.y pose_target.orientation.z = pose.orientation.z pose_target.orientation.w = pose.orientation.w pose_target.position.x = trans[0] pose_target.position.y = trans[1] pose_target.position.z = trans[2] + 0.3 group.set_pose_target(pose_target) group.go() except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): print "e" rospy.sleep(1)
class World: def __init__(self, tower_array, disk_array, config, debug=0): print "[INFO] Initialise World" self.DEBUG = debug # initialise arrays self.tower_array = tower_array self.disk_array = disk_array # configs self.max_gripper = config.get_max_gripper() self.disk_height = config.disk_height self.tower_positions = config.tower_positions self.planning_scene_interface = PlanningSceneInterface(REFERENCE_FRAME) # Create a scene publisher to push changes to the scene self.scene_pub = rospy.Publisher('planning_scene', PlanningScene, queue_size=10) """Calls a method to display the collision objects. """ def create_planning_scene(self): print "[INFO] Create_planning_scene" self.display_towers_and_disks() """This method collects all needed information and publish them to other methods. """ def display_towers_and_disks(self): print "[INFO] Display towers and disks" for tower in self.tower_array: # call method to publish new tower self.publish_scene(tower.get_position(), tower) # set color by name self.planning_scene_interface.setColor(tower.get_name(), 1.0, 1.0, 1.0) # publish color self.planning_scene_interface.sendColors() for disk in self.disk_array: self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors() # wait for sync after publishing collision objects self.planning_scene_interface.waitForSync(5.0) rospy.sleep(5) """This method creates a box or a cylinder with methods of the planning scene interface. :param position: the position of the new collision object :param tower: the tower object :param disk: the disk object """ def publish_scene(self, position, tower=None, disk=None): if tower is not None: self.planning_scene_interface.addBox( tower.get_name(), self.max_gripper / 100.0, self.max_gripper / 100.0, (self.tower_positions[tower.get_id() - 1][2] * 2), position[0], position[1], position[2]) else: self.planning_scene_interface.addCylinder(disk.get_id(), self.disk_height, disk.get_diameter() / 2, position[0], position[1], position[2]) """This method cleans the planning scene. :param close: with this flag a new planning scene objects will be removed in sync otherwise the object will be deleted without syncing the scene """ def clean_up(self, close=False): if close: # get the actual list after game self.planning_scene_interface = PlanningSceneInterface( REFERENCE_FRAME) for name in self.planning_scene_interface.getKnownCollisionObjects(): if self.DEBUG is 1: print("[DEBUG] Removing object %s" % name) self.planning_scene_interface.removeCollisionObject(name, False) for name in self.planning_scene_interface.getKnownAttachedObjects(): if self.DEBUG is 1: print("[DEBUG] Removing attached object %s" % name) self.planning_scene_interface.removeAttachedObject(name, False) if close: self.planning_scene_interface.waitForSync(5.0) pass """This method corrects the position of the moved disk. :param tower: parent tower of the disk """ def refresh_disk_pose(self, tower): print "[INFO] Refresh disk pose" disk = tower.get_last() if self.DEBUG is 1: print "[DEBUG] Refresh", disk.get_id(), "pose:", disk.get_position(), "tower size", tower.get_size(),\ "tower pose", tower.get_position() # remove the disk from planning scene self.planning_scene_interface.removeCollisionObject( disk.get_id(), False) # publish collision object and set old color self.publish_scene(disk.get_position(), None, disk) self.planning_scene_interface.setColor(disk.get_id(), disk.get_color()[0], disk.get_color()[1], disk.get_color()[2], disk.get_color()[3]) self.planning_scene_interface.sendColors()
class FetchRobotApi: def __init__(self): self.moving_mode = False self.waldo_mode = False self.prev_joy_buttons = 0 # See http://docs.fetchrobotics.com/robot_hardware.html#naming-conventions self.joint_names = [ #'torso_lift_joint', #'bellows_joint', #'head_pan_joint', #'head_tilt_joint', 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint', 'l_gripper_finger_joint', #'r_gripper_finger_joint', ] self.joint_name_map = dict([(name, index) for index, name in enumerate(self.joint_names)]) self.cur_joint_pos = np.zeros([len(self.joint_names)]) self.cur_joint_vel = np.zeros([len(self.joint_names)]) self.cur_joint_effort = np.zeros([len(self.joint_names)]) self.cur_base_scan = np.zeros([662], dtype=np.float32) self.cur_head_camera_depth_image = np.zeros([480,640], dtype=np.float32) self.cur_head_camera_rgb_image = np.zeros([480,640,3], dtype=np.uint8) self.timeseq = None self.timeseq_mutex = threading.Lock() self.image_queue = Queue.Queue() self.image_compress_thread = threading.Thread(target=self.image_compress_main) self.image_compress_thread.daemon = True self.image_compress_thread.start() logger.info('Subscribing...') self.subs = [] if 1: self.subs.append(rospy.Subscriber('/joint_states', JointState, self.joint_states_cb)) if 0: self.subs.append(rospy.Subscriber('/base_scan', LaserScan, self.base_scan_cb)) if 1: self.subs.append(rospy.Subscriber('/head_camera/depth/image', numpy_msg(Image), self.head_camera_depth_image_cb)) if 1: self.subs.append(rospy.Subscriber('/head_camera/rgb/image_raw', numpy_msg(Image), self.head_camera_rgb_image_raw_cb)) if 1: self.subs.append(rospy.Subscriber('/spacenav/joy', Joy, self.spacenav_joy_cb)) logger.info('Subscribed') self.arm_effort_pub = rospy.Publisher('/arm_controller/weightless_torque/command', JointTrajectory, queue_size=2) #self.head_goal_pub = rospy.Publisher('/head_controller/point_head/goal', PointHeadActionGoal, queue_size=2) self.gripper_client = actionlib.SimpleActionClient('gripper_controller/gripper_action', GripperCommandAction) self.arm_cartesian_twist_pub = rospy.Publisher('/arm_controller/cartesian_twist/command', Twist, queue_size=2) self.head_point_client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction) self.arm_move_group = MoveGroupInterface("arm", "base_link", plan_only = True) self.arm_trajectory_client = actionlib.SimpleActionClient("arm_controller/follow_joint_trajectory", FollowJointTrajectoryAction) self.arm_trajectory_client.wait_for_server() if 0: logger.info('Creating MoveGroupInterface...') self.move_group = MoveGroupInterface('arm_with_torso', 'base_link', plan_only = True) logger.info('Created MoveGroupInterface') if 0: logger.info('Creating PlanningSceneInterface...') self.planning_scene = PlanningSceneInterface('base_link') self.planning_scene.removeCollisionObject('my_front_ground') self.planning_scene.removeCollisionObject('my_back_ground') self.planning_scene.removeCollisionObject('my_right_ground') self.planning_scene.removeCollisionObject('my_left_ground') self.planning_scene.addCube('my_front_ground', 2, 1.1, 0.0, -1.0) self.planning_scene.addCube('my_back_ground', 2, -1.2, 0.0, -1.0) self.planning_scene.addCube('my_left_ground', 2, 0.0, 1.2, -1.0) self.planning_scene.addCube('my_right_ground', 2, 0.0, -1.2, -1.0) logger.warn('FetchRobotGymEnv ROS node running') self.head_point_client.wait_for_server() logger.warn('FetchRobotGymEnv ROS node connected') def start_timeseq(self, script): timeseq = TimeseqWriter.create(script) timeseq.add_channel('robot_joints', 'FetchRobotJoints') timeseq.add_channel('gripper_joints', 'FetchGripperJoints') timeseq.add_schema('FetchRobotJoints', ('torso_lift_joint', 'JointState'), ('head_pan_joint', 'JointState'), ('head_tilt_joint', 'JointState'), ('shoulder_pan_joint', 'JointState'), ('shoulder_lift_joint', 'JointState'), ('upperarm_roll_joint', 'JointState'), ('elbow_flex_joint', 'JointState'), ('forearm_roll_joint', 'JointState'), ('wrist_flex_joint', 'JointState'), ('wrist_roll_joint', 'JointState'), ) timeseq.add_schema('FetchGripperJoints', ('l_gripper_finger_joint', 'JointState'), ) timeseq.add_schema('JointState', ('pos', 'double'), ('vel', 'double'), ('effort', 'double'), ) timeseq.add_channel('arm_action', 'FetchArmWeightlessTorqueAction') timeseq.add_schema('FetchArmWeightlessTorqueAction', ('shoulder_pan_joint', 'WeightlessTorqueAction'), ('shoulder_lift_joint', 'WeightlessTorqueAction'), ('upperarm_roll_joint', 'WeightlessTorqueAction'), ('elbow_flex_joint', 'WeightlessTorqueAction'), ('forearm_roll_joint', 'WeightlessTorqueAction'), ('wrist_flex_joint', 'WeightlessTorqueAction'), ('wrist_roll_joint', 'WeightlessTorqueAction'), ) timeseq.add_schema('WeightlessTorqueAction', ('action', 'double'), ('effort', 'double'), ) timeseq.add_channel('gripper_action', 'FetchGripperAction') timeseq.add_schema('FetchGripperAction', ('action', 'double'), ('effort', 'double'), ('pos', 'double'), ) timeseq.add_channel('head_camera_rgb', 'VideoFrame') timeseq.add_schema('VideoFrame', ('url', 'string'), ) with self.timeseq_mutex: if self.timeseq: self.timeseq.close() self.timeseq = timeseq def close_timeseq(self): with self.timeseq_mutex: if self.timeseq is not None: self.timeseq.close() threading.Thread(target=self.timeseq.upload_s3).start() self.timeseq = None def start_axis_video(self): cameras = rospy.get_param('/axis_cameras') if cameras and len(cameras): with self.timeseq_mutex: if self.timeseq: for name, info in cameras.items(): logging.info('Camera %s: %s', name, repr(info)) self.timeseq.start_axis_video(timeseq_name = name, auth_header=info.get('auth_header'), daemon_endpoint=info.get('daemon_endpoint'), ipaddr=info.get('ipaddr'), local_link_prefix=info.get('local_link_prefix'), remote_traces_dir=info.get('remote_traces_dir'), resolution=info.get('resolution')) def stop_axis_video(self): with self.timeseq_mutex: if self.timeseq: self.timeseq.stop_axis_video() def base_scan_cb(self, data): # fmin replaces nans with 15 self.cur_base_scan = np.fmin(np.array(data.ranges), 15.0) def head_camera_depth_image_cb(self, data): shape = [data.height, data.width] dtype = np.dtype(np.float32) npdata = np.fromstring(data.data, dtype=dtype).reshape(shape) npdata.strides = (data.step, dtype.itemsize) self.cur_head_camera_depth_image = np.fmin(npdata, 5.0) def head_camera_rgb_image_raw_cb(self, data): shape = [data.height, data.width, 3] dtype = np.dtype(np.uint8) npdata = np.fromstring(data.data, dtype=dtype).reshape(shape) npdata.strides = (data.step, dtype.itemsize*3, 1) self.cur_head_camera_rgb_image = npdata if self.timeseq: self.image_queue.put(('head_camera_rgb', data)) def joint_states_cb(self, data): """ Handle a joint_states message. Messages can have subsets of the joints, hopefully non-overlapping. """ t0 = time.time() for i in range(len(data.name)): name = data.name[i] jni = self.joint_name_map.get(name, -1) if jni >= 0: self.cur_joint_pos[jni] = data.position[i] self.cur_joint_vel[jni] = data.velocity[i] self.cur_joint_effort[jni] = data.effort[i] with self.timeseq_mutex: if self.timeseq: channel, channel_type = ((data.name[0] == 'l_wheel_joint' and ('robot_joints', 'FetchRobotJoints')) or (data.name[0] == 'l_gripper_finger_joint' and ('gripper_joints', 'FetchGripperJoints')) or (None, None)) if channel: state = dict([(jn, { '__type': 'JointState', 'pos': data.position[i], 'vel': data.velocity[i], 'effort': data.effort[i], }) for i, jn in enumerate(data.name)]) state['__type'] = channel_type state['rxtime'] = t0 self.timeseq.add(data.header.stamp.to_sec(), channel, state) if 0: t1 = time.time() print '%0.6f+%0.6f stamp=%0.6f (age %0.6f) from %s' % (t0, t1-t0, data.header.stamp.to_sec(), data.header.stamp.to_sec()-t0, channel) def image_compress_main(self): while True: channel, data = self.image_queue.get() if 0: print 'len(data.data)=%d data.width=%d data.height=%d' % (len(data.data), data.width, data.height) im = PIL.Image.frombytes('RGB', (data.width, data.height), data.data, 'raw') jpeg_writer = StringIO() im.save(jpeg_writer, 'jpeg') im_url = 'data:image/jpeg;base64,' + base64.b64encode(jpeg_writer.getvalue()) with self.timeseq_mutex: if self.timeseq: self.timeseq.add(data.header.stamp.to_sec(), channel, { '__type': 'VideoFrame', 'url': im_url, }) def move_to_start(self): self.moving_mode = True # Look down goal = PointHeadGoal() goal.target.header.stamp = rospy.Time.now() goal.target.header.frame_id = '/base_link' goal.target.point.x = 1.5 goal.target.point.y = 0.0 goal.target.point.z = -0.2 goal.min_duration = rospy.Duration(0.5) if 0: logger.info('Point head to %s...', goal); self.head_point_client.send_goal(goal) if 0: logger.info('Point head sent') goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.1 self.gripper_client.send_goal(goal) joints = [ 'shoulder_pan_joint', 'shoulder_lift_joint', 'upperarm_roll_joint', 'elbow_flex_joint', 'forearm_roll_joint', 'wrist_flex_joint', 'wrist_roll_joint', ] pose = [0.0, +0.8, 0.0, -0.8, 0.0, 0.0, 0.0] result = self.arm_move_group.moveToJointPosition(joints, pose, plan_only=True) if result.error_code.val == MoveItErrorCodes.SUCCESS: if 0: logger.info('Got trajectory %s', result.planned_trajectory) follow_goal = FollowJointTrajectoryGoal() follow_goal.trajectory = result.planned_trajectory.joint_trajectory logger.info('sending trajectory to arm...') self.arm_trajectory_client.send_goal(follow_goal) result = self.arm_trajectory_client.wait_for_result() logger.info('arm followed trajectory %s', result) else: logger.warn('moveToJointPosition returned %s', result) return result = self.head_point_client.wait_for_result() logger.info('head followed trajectory %s', result) logger.info('sending empty arm goal') empty_goal = FollowJointTrajectoryGoal() self.arm_trajectory_client.send_goal(empty_goal) logger.info('Point head done') self.moving_mode = False def set_arm_action(self, action): arm_joints = [ ('shoulder_pan_joint', 1.57, 33.82), ('shoulder_lift_joint', 1.57, 131.76), ('upperarm_roll_joint', 1.57, 76.94), ('elbow_flex_joint', 1.57, 66.18), ('forearm_roll_joint', 1.57, 29.35), ('wrist_flex_joint', 2.26, 25.70), ('wrist_roll_joint', 2.26, 7.36), ] arm_efforts = [min(1.0, max(-1.0, action[self.joint_name_map.get(name)])) * torque_scale * 0.75 for name, vel_scale, torque_scale in arm_joints] arm_joint_names = [name for name, vel_scale, torque_scale in arm_joints] if 1: arm_joint_names.append('gravity_compensation') arm_efforts.append(1.0) arm_msg = JointTrajectory(joint_names=arm_joint_names, points=[JointTrajectoryPoint(effort = arm_efforts)]) self.arm_effort_pub.publish(arm_msg) with self.timeseq_mutex: if self.timeseq: state = dict([(jn, { '__type': 'WeightlessTorqueAction', 'action': action[self.joint_name_map.get(jn)], 'effort': arm_efforts[i], }) for i, jn in enumerate(arm_joint_names)]) state['__type'] = 'FetchArmWeightlessTorqueAction' self.timeseq.add(time.time(), 'arm_action', state) def set_gripper_action(self, action): grip = action[self.joint_name_map.get('l_gripper_finger_joint')] goal = GripperCommandGoal() if grip > 0: goal.command.max_effort = 60*min(1.0, grip) goal.command.position = 0.0 else: goal.command.max_effort = 60 goal.command.position = 0.1 self.gripper_client.send_goal(goal) with self.timeseq_mutex: if self.timeseq: state = { '__type': 'FetchGripperAction', 'action': grip, 'effort': goal.command.max_effort, 'pos': goal.command.position, } self.timeseq.add(time.time(), 'gripper_action', state) def set_waldo_action(self, joy): twist = Twist() twist.linear.x = joy.axes[0] twist.linear.y = joy.axes[1] twist.linear.z = joy.axes[2] twist.angular.x = +3.0*joy.axes[3] twist.angular.y = +3.0*joy.axes[4] twist.angular.z = +2.0*joy.axes[5] self.arm_cartesian_twist_pub.publish(twist) if joy.buttons[1] and not self.prev_joy_buttons[1]: goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.0 self.gripper_client.send_goal(goal) elif not joy.buttons[1] and self.prev_joy_buttons[1]: goal = GripperCommandGoal() goal.command.max_effort = 60 goal.command.position = 0.1 self.gripper_client.send_goal(goal) def spacenav_joy_cb(self, joy): if 0: logger.info('joy %s', str(joy)) if joy.buttons[0] and not self.prev_joy_buttons[0]: if self.waldo_mode: self.stop_waldo_mode() elif not self.moving_mode: self.start_waldo_mode() if self.waldo_mode and not self.moving_mode: self.set_waldo_action(joy) self.prev_joy_buttons = joy.buttons def start_waldo_mode(self): logger.info('Start waldo mode'); self.waldo_mode = True self.start_timeseq('fetchwaldo') self.start_axis_video() def stop_waldo_mode(self): logger.info('Stop waldo mode'); self.waldo_mode = False timeseq_url = None if self.timeseq: timeseq_url = self.timeseq.get_url() logger.info('save_logs url=%s', timeseq_url) self.stop_axis_video() self.close_timeseq() mts_thread = threading.Thread(target=self.move_to_start) mts_thread.daemon = True mts_thread.start()
class GraspingClient(object): def __init__(self): self.scene = PlanningSceneInterface("base_link") self.pickplace = PickPlaceInterface("arm", "gripper", verbose=True) self.move_group = MoveGroupInterface("arm", "base_link") find_topic = "basic_grasping_perception/find_objects" rospy.loginfo("Waiting for %s..." % find_topic) self.find_client = actionlib.SimpleActionClient(find_topic, FindGraspableObjectsAction) self.find_client.wait_for_server() def updateScene(self): # find objects goal = FindGraspableObjectsGoal() goal.plan_grasps = True self.find_client.send_goal(goal) self.find_client.wait_for_result(rospy.Duration(5.0)) find_result = self.find_client.get_result() # remove previous objects for name in self.scene.getKnownCollisionObjects(): self.scene.removeCollisionObject(name, False) for name in self.scene.getKnownAttachedObjects(): self.scene.removeAttachedObject(name, False) self.scene.waitForSync() # insert objects to scene idx = -1 for obj in find_result.objects: idx += 1 obj.object.name = "object%d"%idx self.scene.addSolidPrimitive(obj.object.name, obj.object.primitives[0], obj.object.primitive_poses[0], wait = False) for obj in find_result.support_surfaces: # extend surface to floor, and make wider since we have narrow field of view height = obj.primitive_poses[0].position.z obj.primitives[0].dimensions = [obj.primitives[0].dimensions[0], 1.5, # wider obj.primitives[0].dimensions[2] + height] obj.primitive_poses[0].position.z += -height/2.0 # add to scene self.scene.addSolidPrimitive(obj.name, obj.primitives[0], obj.primitive_poses[0], wait = False) self.scene.waitForSync() # store for grasping self.objects = find_result.objects self.surfaces = find_result.support_surfaces def getGraspableCube(self): graspable = None for obj in self.objects: # need grasps if len(obj.grasps) < 1: continue # check size if obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07 or \ obj.object.primitives[0].dimensions[0] < 0.05 or \ obj.object.primitives[0].dimensions[0] > 0.07: continue # has to be on table if obj.object.primitive_poses[0].position.z < 0.5: continue return obj.object, obj.grasps # nothing detected return None, None def getSupportSurface(self, name): for surface in self.support_surfaces: if surface.name == name: return surface return None def getPlaceLocation(self): pass def pick(self, block, grasps): success, pick_result = self.pickplace.pick_with_retry(block.name, grasps, support_name=block.support_surface, scene=self.scene) self.pick_result = pick_result return success def place(self, block, pose_stamped): places = list() l = PlaceLocation() l.place_pose.pose = pose_stamped.pose l.place_pose.header.frame_id = pose_stamped.header.frame_id # copy the posture, approach and retreat from the grasp used l.post_place_posture = self.pick_result.grasp.pre_grasp_posture l.pre_place_approach = self.pick_result.grasp.pre_grasp_approach l.post_place_retreat = self.pick_result.grasp.post_grasp_retreat places.append(copy.deepcopy(l)) # create another several places, rotate each by 360/m degrees in yaw direction m = 16 # number of possible place poses pi = 3.141592653589 for i in range(0, m-1): l.place_pose.pose = rotate_pose_msg_by_euler_angles(l.place_pose.pose, 0, 0, 2 * pi / m) places.append(copy.deepcopy(l)) success, place_result = self.pickplace.place_with_retry(block.name, places, scene=self.scene) return success def tuck(self): joints = ["shoulder_pan_joint", "shoulder_lift_joint", "upperarm_roll_joint", "elbow_flex_joint", "forearm_roll_joint", "wrist_flex_joint", "wrist_roll_joint"] pose = [1.32, 1.40, -0.2, 1.72, 0.0, 1.66, 0.0] while not rospy.is_shutdown(): result = self.move_group.moveToJointPosition(joints, pose, 0.02) if result.error_code.val == MoveItErrorCodes.SUCCESS: return
description="Remove objects from the MoveIt planning scene.") parser.add_argument("name", nargs="?", help="Name of object to remove") parser.add_argument("--all", help="Remove all objects.", action="store_true") parser.add_argument("--attached", help="Remove an attached object.", action="store_true") args = parser.parse_args() if args.all: rospy.init_node("remove_objects") scene = PlanningSceneInterface("base_link") for name in scene.getKnownCollisionObjects(): print("Removing %s" % name) scene.removeCollisionObject(name, use_service=False) scene.waitForSync() elif args.name: rospy.init_node("remove_objects") scene = PlanningSceneInterface("base_link") print("Removing %s" % args.name) if args.attached: scene.removeAttachedObject(args.name) else: scene.removeCollisionObject(args.name) else: parser.print_help()