import math
from time import process_time
import matplotlib.pyplot as plt
import numpy as np
from dynopy.data_objects.node_traj import Node
from dynopy.motion_planning.tree_analysis import plot_tree
from config.config import get_parameters
from tree_analysis import plot_tree
cfg_ws = get_parameters()
def update_information(V, E, epsilon_0, I_0, fused, channel_list, cfg):
"""
:param V: list of nodes
:param E: list of edges
:param epsilon_0: current pdf
:param I_0: information gained so far
:param fused: dict of information fused on each channel
:param channel_list: dict of channels and their path
:param cfg: configuration of agent
:return: list of nodes with updated information values
"""
# TODO: could probably save time here by not going through the full tree, just new nodes
ol = [V[0]] # open list
bl = [Node(None, None, 0, I_0, 0, 0,
fused)] # branch list for visited nodes
cl = [] # closed list
# -*- coding: utf-8 -*-
from math import sqrt, cos, sin, trunc, pi
from time import process_time
import matplotlib.pyplot as plt
import numpy as np
import scipy.stats as stats
from config import config
from dynopy.data_objects.node import Node
from tree_analysis import plot_tree
# ================================
# THIS VERSION HAS BEEN DEPRECATED
# ================================
cfg = config.get_parameters()
def RIG_tree(d, B, X_all, X_free, epsilon, x_0, R, t_limit=0.1):
"""
:param d: single dynamic step
:param B: budget
:param X_all: workspace
:param X_free: free space
:param epsilon: environment
:param x_0: initial state
:param R: nearest neighbor radius
:param t_limit: time expansion can run for
:return: a list of nodes and edges
"""
# !/usr/bin/env python
# -*- coding: utf-8 -*-
import os
import matplotlib.pyplot as plt
from config import config
from dynopy.motion_planning.RIG_tree_R_based import RIG_tree
import dynopy.tools.initialize as init
cfg_ws = config.get_parameters()
def main():
cfg = config.load_agent_parameters("Blinky")
run("workspace_sandbox.txt", cfg.get("lambda"), cfg.get("budget"), cfg.get("t_limit"), cfg.get("gamma"), 1, 1)
def run(file_ws, lamb, budget, t_limit, gamma, n_agents, plot=True, plot_full=False):
ws = init.load_workspace(file_ws, os.path.dirname(__file__))
volunteer = init.load_volunteer(config, lamb, budget, t_limit, gamma, plot_full)
agents_dedicated = init.load_agents(n_agents, config)
agents_all = agents_dedicated.copy()
agents_all.append(volunteer)
for agent in agents_all:
n = config.get_cfg_number(agent.get_name())
waypoints = ws.get_default_waypoints(n)
agent.set_state(waypoints[0])