import rospy from MotionPlan import * from MoveArm import * from MoveHand import * from tf.transformations import * from copy import deepcopy rospy.init_node("test") from pr2_python.world_interface import WorldInterface wi = WorldInterface() grasp_pose = PoseStamped() #grasp_pose.header.frame_id = "base_link" grasp_pose.header.frame_id = "odom_combined" grasp_pose.header.stamp = rospy.Time() grasp_pose.pose = wi.collision_object('milk').poses[0] p = grasp_pose.pose.position #p.x, p.y, p.z = [-0.8,-0.2,0.9] p.x += 0.02 p.y += 0.02 p.z += 0.02 o = grasp_pose.pose.orientation o.x,o.y,o.z,o.w = quaternion_from_euler(-1.581, -0.019, 2.379) lift_pose = deepcopy(grasp_pose) p = lift_pose.pose.position p.x += 0.0 p.y += -0.05 p.z += 0.03 mp = MotionPlan()
def plate_main(): pub = rospy.Publisher('darrt_trajectory', MarkerArray) rospy.loginfo('Waiting for action') rospy.loginfo('Doing detection') wi = WorldInterface() psi = get_planning_scene_interface() detector = TablewareDetection() plate = wi.collision_object('plate') good_detection = False goal = DARRTGoal() if plate: rospy.loginfo('Use last detection?') good_detection = (raw_input() == 'y') ops = PoseStamped() ops.header = copy.deepcopy(plate.header) ops.pose = copy.deepcopy(plate.poses[0]) goal.pickup_goal = PickupGoal('right_arm', om.GraspableObject(reference_frame_id=ops.header.frame_id), object_pose_stamped=ops, collision_object_name='plate', collision_support_surface_name='serving_table') while not good_detection: det = detector.detect_objects(add_table_to_map=False, add_objects_as_mesh=False, table_name='serving_table') goal.pickup_goal = get_plate(det.pickup_goals, det.table.pose, wi) psi.reset() if not goal.pickup_goal: rospy.loginfo('Nothing to pick up!') rospy.loginfo('Good detection?') good_detection = (raw_input() == 'y') if not goal.pickup_goal: rospy.loginfo('Nothing to pick up!') return add_map_tables(wi) psi.reset() client = SimpleActionClient('/darrt_planning/darrt_action', DARRTAction) client.wait_for_server() goal.pickup_goal.arm_name = 'right_arm' goal.pickup_goal.desired_grasps = plate_grasps(goal.pickup_goal.object_pose_stamped, goal.pickup_goal.target.reference_frame_id) goal.pickup_goal.lift.desired_distance = 0.3 place_pose_stamped = PoseStamped() place_pose_stamped.header.frame_id = wi.world_frame # place_pose_stamped.pose.position.x = 1.3 # place_pose_stamped.pose.position.y = -0.3 # place_pose_stamped.pose.position.z = 1.01 place_pose_stamped.pose.position.x = 0.0 place_pose_stamped.pose.position.y = 0.0 place_pose_stamped.pose.position.z = 0.76 place_pose_stamped.pose.orientation.w = 1.0 # place_pose_stamped.pose.orientation.x = 0.1 # place_pose_stamped.pose.orientation.y = 0.1 # place_pose_stamped.pose.orientation.w = np.sqrt(0.98) #place_pose_stamped = copy.deepcopy(goal.pickup_goal.object_pose_stamped) #place_pose_stamped.pose.position.x -= 0.2 #place_pose_stamped.pose.position.y -= 0.2 #place_pose_stamped.pose.position.z += 0.2 goal.place_goal = PlaceGoal(goal.pickup_goal.arm_name, [place_pose_stamped], collision_support_surface_name = 'dirty_table', collision_object_name = goal.pickup_goal.collision_object_name) goal.place_goal.approach.direction.header.frame_id = wi.world_frame goal.place_goal.approach.direction.vector.x = np.sqrt(0.18) goal.place_goal.approach.direction.vector.y = -np.sqrt(0.18) goal.place_goal.approach.direction.vector.z = -0.8 goal.place_goal.approach.desired_distance = 0.2 goal.primitives = [goal.PICK, goal.PLACE, goal.PUSH, goal.BASE_TRANSIT] goal.min_grasp_distance_from_surface = 0.19 goal.object_sampling_fraction = 0.7 goal.retreat_distance = 0.3 goal.debug_level = 2 goal.do_pause = False goal.execute = False goal.planning_time = 6000 goal.tries = 1 goal.goal_bias = 0.2 rospy.loginfo('Sending goal') client.send_goal_and_wait(goal) rospy.loginfo('Returned') result = client.get_result() marray = MarkerArray() if result.error_code == result.SUCCESS: #pickle everything!! great excitement filename = 'trajectory_and_objects.pck' rospy.loginfo('Pickling to '+filename) #the last bit allows us to recreate the planning pickle.dump([client.get_result(), goal, wi.collision_objects(), wi.get_robot_state()], open(filename, 'wb')) rospy.loginfo('Successful write') for t in result.primitive_trajectories: marray.markers += vt.trajectory_markers(t, ns='trajectory', resolution=3).markers for (i, m) in enumerate(marray.markers): m.id = i (m.color.r, m.color.g, m.color.b) = vt.hsv_to_rgb(i/float(len(marray.markers))*300.0, 1, 1) while not rospy.is_shutdown(): pub.publish(marray) rospy.sleep(0.1)
import rospy from cob_arm_navigation_python.MotionPlan import * from cob_arm_navigation_python.MoveArm import * from cob_arm_navigation_python.MoveHand import * from tf.transformations import * from copy import deepcopy rospy.init_node("test") from pr2_python.world_interface import WorldInterface wi = WorldInterface() grasp_pose = PoseStamped() #grasp_pose.header.frame_id = "base_link" grasp_pose.header.frame_id = "odom_combined" grasp_pose.header.stamp = rospy.Time() grasp_pose.pose = wi.collision_object('milk').poses[0] p = grasp_pose.pose.position #p.x, p.y, p.z = [-0.8,-0.2,0.9] p.x += 0.02 p.y += 0.02 p.z += 0.02 o = grasp_pose.pose.orientation o.x, o.y, o.z, o.w = quaternion_from_euler(-1.581, -0.019, 2.379) lift_pose = deepcopy(grasp_pose) p = lift_pose.pose.position p.x += 0.0 p.y += -0.05 p.z += 0.03 mp = MotionPlan()