def make_and_execute_verb_traj(verb, arg_clouds):
if "expansion" in verb_info[verb]:
prim_verbs = verb_info[verb]["expansion"]
print "expanding into primitive actions: %s"%" ".join(prim_verbs)
for i in xrange(len(prim_verbs)):
exec_success = make_and_execute_verb_traj(prim_verbs[i], arg_clouds[i:i+1])
if exec_success == False:
rospy.logerr("stopping complex verb %s", verb)
return False
else:
if len(F[verb]["demos"]) == 0:
rospy.logerr("verb %s has no demonstrations!", verb)
return False
make_req = MakeTrajectoryRequest()
make_req.object_clouds = [xyz2pc(cloud, "base_footprint") for cloud in arg_clouds]
make_req.verb = verb
make_resp = make_svc.call(make_req)
approved = yes_or_no("execute trajectory?")
if approved:
exec_req = ExecTrajectoryRequest(make_resp.traj)
exec_resp = exec_svc.call(exec_req)
if exec_resp.success == False:
rospy.logerr("%s failed :(", verb)
return False
else:
rospy.loginfo("%s succeeded!", verb)
def make_and_execute_verb_traj(verb, arg_clouds):
if "expansion" in verb_info[verb]:
prim_verbs = verb_info[verb]["expansion"]
print "expanding into primitive actions: %s" % " ".join(prim_verbs)
for i in xrange(len(prim_verbs)):
exec_success = make_and_execute_verb_traj(prim_verbs[i],
arg_clouds[i:i + 1])
if exec_success == False:
rospy.logerr("stopping complex verb %s", verb)
return False
else:
if len(F[verb]["demos"]) == 0:
rospy.logerr("verb %s has no demonstrations!", verb)
return False
make_req = MakeTrajectoryRequest()
make_req.object_clouds = [
xyz2pc(cloud, "base_footprint") for cloud in arg_clouds
]
make_req.verb = verb
make_resp = make_svc.call(make_req)
approved = yes_or_no("execute trajectory?")
if approved:
exec_req = ExecTrajectoryRequest(make_resp.traj)
exec_resp = exec_svc.call(exec_req)
if exec_resp.success == False:
rospy.logerr("%s failed :(", verb)
return False
else:
rospy.loginfo("%s succeeded!", verb)
def filter_pc2(cloud_pc2):
cloud_xyz = (pc2xyzrgb(cloud_pc2)[0]).reshape(-1,3)
cloud_xyz_down = voxel_downsample(cloud_xyz, .02)
graph = ri.points_to_graph(cloud_xyz_down, .03)
cc = ri.largest_connected_component(graph)
good_xyzs = np.array([graph.node[node_id]["xyz"] for node_id in cc.nodes()])
pose_array = juc.array_to_pose_array(good_xyzs, "base_footprint")
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,1,0,1), type=Marker.CUBE_LIST, width=.001, ns="segmentation"))
raw_input("press enter when done looking")
del Globals.handles[:]
return xyz2pc(good_xyzs, cloud_pc2.header.frame_id)
for obj_name in demo_info["args"]:
print "select the %s" % obj_name
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in=pc_tf)).cloud_out
make_req.object_clouds.append(pc_sel)
else:
object_clouds = [
demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in demo_data["object_clouds"].keys()
]
for i in xrange(len(object_clouds)):
cloud = object_clouds[i].reshape(-1, 3)
#translation = np.random.randn(1,3) * np.array([[.1,.1,0]])
#cloud += translation
center = cloud.mean(axis=0)
from numpy import cos, sin, pi
rotation = np.array([[cos(pi / 4), sin(pi / 4), 0],
[-sin(pi / 4), cos(pi / 4), 0], [0, 0, 1]])
cloud = center[None, :] + np.dot(cloud - center[None, :], rotation.T)
make_req.object_clouds.append(ru.xyz2pc(cloud, 'base_footprint'))
make_resp = make_verb_traj.make_traj(make_req)
yn = yes_or_no("continue?")
if yn:
#exec_verb_traj.Globals.pr2.rarm.vel_limits *= .5
#exec_verb_traj.Globals.pr2.larm.vel_limits *= .5
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req)
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ru.transformPointCloud2(pc, exec_verb_traj.Globals.pr2.tf_listener, "base_footprint", pc.header.frame_id)
for obj_name in demo_info["args"]:
print "select the %s"%obj_name
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
make_req.object_clouds.append(pc_sel)
else:
object_clouds = [demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in demo_data["object_clouds"].keys()]
for i in xrange(len(object_clouds)):
cloud = object_clouds[i].reshape(-1,3)
#translation = np.random.randn(1,3) * np.array([[.1,.1,0]])
#cloud += translation
center = cloud.mean(axis=0)
from numpy import cos,sin,pi
rotation = np.array([[cos(pi/4), sin(pi/4), 0], [-sin(pi/4), cos(pi/4), 0], [0, 0, 1]])
cloud = center[None,:] + np.dot(cloud - center[None,:], rotation.T)
make_req.object_clouds.append(ru.xyz2pc(cloud,'base_footprint'))
make_resp = make_verb_traj.make_traj(make_req)
yn = yes_or_no("continue?")
if yn:
#exec_verb_traj.Globals.pr2.rarm.vel_limits *= .5
#exec_verb_traj.Globals.pr2.larm.vel_limits *= .5
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req)
