Exemplo n.º 1
0
def PomdpFollower(x1, x2, dt, traj_duration, weight, agentID, H, theta_hat,
                  view_thr, thr, p1_goal, p2_goal, sm, v, ww, wc, wd, costmap,
                  costmap0s, cthr, cres, rr, r_m2o, t_m2o, wh):
    path_robot = []
    path_fScore = []
    path_intent = []
    pathx = []
    pathy = []
    pathx_ = []
    pathy_ = []
    robot_s_angz = []
    path_fScore = np.Inf
    current = []
    path_found = False
    initial_node = Nodes()
    initial_node.x_rob = x2
    initial_node.x_ped = x1
    initial_node.theta = theta_hat
    initial_node.theta_new = theta_hat
    initial_node.gScore_ = [0, 0]
    gScore = []
    gScore.append(0)
    gScore_ = []
    gScore_.append([0, 0])
    fScore = []
    fScore.append(np.Inf)
    fScore_ = []
    fScore_.append([np.Inf, np.Inf])
    totalNodes = []
    totalNodes.append(initial_node)
    closeSet = []
    openSet = []
    openSet.append(0)
    cameFrom = []
    cameFrom.append(-1)
    rob_hScore = np.Inf
    cn_node = []
    cn_node.append(initial_node)
    current = initial_node

    min_dist = 0.3
    min_vel = 0.3

    x_rob = initial_node.x_rob
    x_ped = initial_node.x_ped
    theta = initial_node.theta

    if agentID == 1:
        agentID_ = 2
    elif agentID == 2:
        agentID_ = 1

    belief_num = len(theta)
    r_path_found = []
    r_x = []
    r_y = []
    r_x_ = []
    r_y_ = []
    rob_s_angz = []

    rollout_path_found = []
    rollout_x = []
    rollout_x_ = []
    rollout_y = []
    rollout_y_ = []
    robot_s_angz = []

    fk_path_found = 1

    for i in range(belief_num):
        if theta[i] > thr:
            r_path_found, r_x, r_y, r_x_, r_y_, rob_s_angz, path_fScore, current_ = FullKnowledgeCollaborativePlanner(
                x_ped, x_rob, dt, traj_duration, weight, agentID_, H,
                p1_goal[i], p2_goal[i], sm, v, ww, costmap, costmap0s, cthr,
                cres, rr, r_m2o, t_m2o)
            if not r_path_found:
                fk_path_found = 0

        rollout_path_found.append(r_path_found)
        rollout_x.append(r_x)
        rollout_x_.append(r_x_)
        rollout_y.append(r_y)
        rollout_y_.append(r_y_)
        robot_s_angz.append(rob_s_angz)

    while len(openSet) > 0 and fk_path_found:
        current_fScores = [fScore[j] for j in openSet]
        min_fScore = min(current_fScores)
        current_node_id = fScore.index(min_fScore)
        if current_node_id > len(totalNodes):
            print 'cureent node id greater than total node size, error'
            break

        current = totalNodes[current_node_id]

        if (current.end_time > traj_duration * H) or (rob_hScore < 1.0):
            pathx, pathy, pathx_, pathy_, rob_s_angz, path_intent = ReconstructPathFromNode(
                cameFrom, current_node_id, totalNodes, agentID)
            path_found = True
            path_fScore = min_fScore
            pathx_ = r_x
            pathy_ = r_y
            #print 'rob_s_angz = {}'.format(rob_s_angz)
            #print 'rob_hScore = {}'.format(rob_hScore)
            #print 'pomdp path vel = {}'.format(np.sqrt((pathx[0]-pathx[1])**2+(pathy[0]-pathy[1])**2)/dt)
            break

        if current_node_id in openSet:
            openSet.pop(openSet.index(current_node_id))
        else:
            print 'current node id {} is not in list'.format(current_node_id)
            print 'totalNodes length = {}'.format(len(totalNodes))
            print 'closedSet = {}'.format(closeSet)
            print 'openSet = {}'.format(openSet)
            break

        closeSet.append(current_node_id)

        theta = current.theta_new
        num_ti = [i for i in range(len(theta)) if theta[i] > thr]

        ang_num = 1
        sp_num = 1
        H_ = max(1, H - 2)

        if current.end_time < (traj_duration * H_):
            sp_num = 1
        #print 'agent id = {}'.format(agentID)
        neighbors = NavigationActionSampling(current, dt, traj_duration,
                                             agentID, v, sm, p1_goal, p2_goal,
                                             ang_num, sp_num, len(num_ti))
        #print 'neighbor size = {}x{}'.format(len(neighbors),len(neighbors[0]))
        #print 'current node start time and end time = {},{}'.format(current.start_time,current.end_time)
        #print 'child node start time and end time = {},{}'.format(neighbors[0][0].start_time,neighbors[0][0].end_time)
        ped_travel_cost = traj_duration
        rob_travel_cost = traj_duration
        start_ind = int(np.floor(current.end_time / dt))
        num_traj = int(np.floor(traj_duration / dt))
        RolloutX = []
        RolloutY = []
        RolloutX_ = []
        RolloutY_ = []

        #print 'start ind, s+num = {},{}'.format(start_ind,start_ind+num_traj)
        #print 'neighbor size = {},{}'.format(len(neighbors),len(neighbors[0]))
        #print 'rollout length = {},{}'.format(len(rollout_x),len(rollout_x[0]))

        for j in num_ti:
            #print 'len rollout[j] = {}'.format(len(rollout_x[j]))
            #print 'start index = {}, end index = {}'.format(start_ind,start_ind+num_traj)
            RolloutX.append([
                rollout_x[j][k]
                for k in range(start_ind, start_ind + num_traj, 1)
            ])
            RolloutX_.append([
                rollout_x_[j][k]
                for k in range(start_ind, start_ind + num_traj, 1)
            ])
            RolloutY.append([
                rollout_y[j][k]
                for k in range(start_ind, start_ind + num_traj, 1)
            ])
            RolloutY_.append([
                rollout_y_[j][k]
                for k in range(start_ind, start_ind + num_traj, 1)
            ])

        Rollout = []
        Rollout.append(RolloutX)
        Rollout.append(RolloutY)

        for i in range(len(neighbors[0])):
            #nodes with the same sampled action
            cn_node = [neighbors[j][i] for j in range(len(neighbors))]
            traj = []
            vel = []  #agent velocity
            x = []  # agent position
            #print 'robot velocity = {}'.format(cn_node[0].robot_velx[-1]**2+cn_node[0].robot_vely[-1]**2)
            if agentID == 1:
                traj.append(
                    [cn_node[0].human_trajx[j] for j in range(num_traj)])
                traj.append(
                    [cn_node[0].human_trajy[j] for j in range(num_traj)])
                traj.append(
                    [cn_node[0].human_velx[j] for j in range(num_traj)])
                traj.append(
                    [cn_node[0].human_vely[j] for j in range(num_traj)])
                vel.append(cn_node[0].x_ped[2])
                vel.append(cn_node[0].x_ped[3])
                x = [cn_node[0].x_ped[0], cn_node[0].x_ped[1]]

            else:
                traj.append(
                    [cn_node[0].robot_trajx[j] for j in range(num_traj)])
                traj.append(
                    [cn_node[0].robot_trajy[j] for j in range(num_traj)])
                traj.append(
                    [cn_node[0].robot_velx[j] for j in range(num_traj)])
                traj.append(
                    [cn_node[0].robot_vely[j] for j in range(num_traj)])
                #print 'cn_node vel = {}'.format(cn_node[0].x_rob)
                vel.append(cn_node[0].x_rob[2])
                vel.append(cn_node[0].x_rob[3])
                x = [cn_node[0].x_rob[0], cn_node[0].x_rob[1]]

            #collision1 = Check(cn_node[0].robot_trajx,cn_node[0].robot_trajy,costmap,costmap0s,cthr,cres,rr,r_m2o,t_m2o)
            collision1 = Check(cn_node[0].robot_trajx, cn_node[0].robot_trajy,
                               costmap, costmap0s, cthr, cres, rr)

            collision2 = PartnerCollisionCheck(Rollout, traj, min_dist - 0.02,
                                               min_vel)
            if collision1 or collision2:
                #print 'collision 1,2 = {}{}'.format(collision1,collision2)
                #print 'Rollout = {}'.format(Rollout)
                #rint 'traj = {}'.format(traj)

                continue
            #print 'RolloutX_ = {}'.format(RolloutX_)
            #print 'vel = {}'.format(vel)
            wdist = 0.0
            x_ = []  #partner state
            x__ = []  #agent state appended
            for j in range(len(RolloutX_)):
                for k in range(len(RolloutX_[j])):
                    if np.isnan(RolloutX_[j][k]) or np.isnan(RolloutY_[j][k]):
                        print 'rollout is nan'
                    if np.isnan(traj[0][k]) or np.isnan(traj[1][k]):
                        print 'traj is nan'
                    wdist = wdist + theta[num_ti[j]] * np.sqrt(
                        (RolloutX_[j][k] - traj[0][j])**2 +
                        (RolloutY_[j][k] - traj[1][j])**2)
                x_.append([RolloutX[j][-1], RolloutY[j][-1]])
                x__.append(x)

            if agentID == 1:
                x_ped_ = x__
                x_rob_ = x_
            else:
                x_rob_ = x__
                x_ped_ = x_

            cdist = [wdist, wdist]
            cdist[agentID_] = 0.0
            cdist = [0, 0]  #TODO
            #chvel = [0,ww*((v-np.sqrt(cn_node[0].x_ped[2]**2+cn_node[0].x_ped[3]**2))**2)]
            chvel = [0, 0]
            gScore_new = [0, 0]
            #print 'gScorw = {}'.format(current.gScore_)
            gScore_new[
                0] = current.gScore_[0] + rob_travel_cost + chvel[0] + cdist[0]
            gScore_new[
                1] = current.gScore_[1] + ped_travel_cost + chvel[1] + cdist[1]
            gScoreNew = np.dot(gScore_new, weight)

            for j in num_ti:
                #bayesian belief update based on obs:traj_
                if len(num_ti) < belief_num:
                    # assuming only two states
                    theta = [0 for k in range(len(theta))]
                    theta[j] = 1.0
                else:
                    obs = x_[j]
                    distx = [
                        np.sqrt((x_[k][0] - obs[0])**2 +
                                (x_[k][1] - obs[1])**2) for k in range(len(x_))
                    ]
                    distx[j] = np.Inf
                    obs_x = [obs[0] - x[0], obs[1] - x[1]]
                    view = np.dot(vel, obs_x) / np.sqrt(np.dot(
                        obs_x, obs_x)) / np.sqrt(np.dot(vel, vel))
                    cn_node[j].view = view
                    if view > view_thr:
                        #belief updates
                        mindistx = min(distx)
                        minind = distx.index(mindistx)
                        #update: other
                        obs_s1 = ObsLikelihood(wd[j], mindistx)
                        obs_s = [1.0 - obs_s1 for k in range(belief_num)]
                        #update: self
                        obs_s[j] = obs_s1
                        s = theta  #??
                        o = np.dot(obs_s, s)
                        s_new = np.multiply(obs_s, s)
                        theta = [k / o for k in s_new]

                cn_ind = num_ti.index(j)
                cn_node[cn_ind].theta_new = theta
                cn_node[cn_ind].gScore_ = gScore_new
                #cost_to_go using the updated theta
                #print 'p1_goal and ped_pos = {},{}'.format(p1_goal,x_ped_)
                #print 'p2_goal and rob_pos = {},{}'.format(p2_goal,x_rob_)

                ped_dtgo = [
                    np.sqrt((p1_goal[num_ti[k]][0] - x_ped_[k][0])**2 +
                            (p1_goal[num_ti[k]][1] - x_ped_[k][1])**2) *
                    theta[num_ti[k]] / v for k in range(len(x_ped_))
                ]
                ped_hScore = np.sum(ped_dtgo)
                rob_dtgo = [
                    np.sqrt((p2_goal[num_ti[k]][0] - x_rob_[k][0])**2 +
                            (p2_goal[num_ti[k]][1] - x_rob_[k][1])**2) *
                    theta[num_ti[k]] / v for k in range(len(x_rob_))
                ]
                rob_hScore = np.sum(rob_dtgo)
                hScore = np.dot(weight, [rob_hScore, ped_hScore])
                gScore.append(gScoreNew)
                gScore_.append(gScore_new)
                fScore_.append(
                    np.add(gScore_new, [
                        wh * weight[0] * rob_hScore,
                        wh * weight[1] * ped_hScore
                    ]))
                fScore.append(gScoreNew + wh * hScore +
                              np.random.normal() * 0.01)

                #cost-to-go
                cameFrom.append(current_node_id)
                openSet.append(len(totalNodes))
                totalNodes.append(cn_node[cn_ind])

            if (len(totalNodes) - len(openSet)) > 400:
                print 'gScore_ = {}'.format(gScore_[-1])
                print 'gScoreNew = {}'.format(gScoreNew)
                print 'fScore = {}'.format(fScore[-1])
                #print 'too many nodes'
                break

    if not path_found:
        print 'pomdp path not found after openSet cleared'
        print 'total node length = {}'.format(len(totalNodes))
        print 'cn_node end time = {}'.format(cn_node[0].end_time)
        print 'current node time = {}'.format(current.end_time)
    #else:
    #    print 'total node length = {}'.format(len(totalNodes))

    return path_found, pathx, pathy, pathx_, pathy_, robot_s_angz, path_fScore, current