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()
