Exemple #1
0
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)
Exemple #2
0
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)
Exemple #3
0
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)