########################################### # compute trajectory to ck using basis function from utils import convert_traj2ck ck = convert_traj2ck(basis, xt) print('ck: ') print(ck) ########################################### # barrier function test from barrier import Barrier # define the Barrier object barrier = Barrier(explr_space) # test cost function print(barrier.cost(explr_space.sample() - size)) # test derivative of cost function wrt to pose print(barrier.dx(explr_space.sample() - size)) ########################################### # target distribution test from target_dist import TargetDist from utils import convert_phi2phik, convert_phik2phi import matplotlib.pyplot as plt # define a target distribution object t_dist = TargetDist( ) #TargetDist(size=size, means=[[1.0,1.0],[3.0,3.0]], cov=0.1) # plot first fig, original target dist fig1 = plt.figure() ax1_1 = fig1.add_subplot(121)
########################################### # compute trajectory to ck using basis function from utils import convert_traj2ck ck = convert_traj2ck(basis, xt) print('ck: ') print(ck) ########################################### # barrier function test from barrier import Barrier # define the Barrier object barrier = Barrier(explr_space) # test cost function print(barrier.cost(explr_space.sample() - 1.0)) # test derivative of cost function wrt to pose print(barrier.dx(explr_space.sample() - 1.0)) ########################################### # target distribution test from target_dist import TargetDist from utils import convert_phi2phik, convert_phik2phi import matplotlib.pyplot as plt # define a target distribution object t_dist = TargetDist() # plot first fig, original target dist fig1 = plt.figure() ax1_1 = fig1.add_subplot(121) ax1_1.set_aspect('equal')
import numpy as np from gym.spaces import Box import sys sys.path.append('../rt_erg_lib') from barrier import Barrier size = 4.0 explr_space = Box(np.array([0.0, 0.0]), np.array([size, size]), dtype=np.float32) barrier = Barrier(explr_space) loc = np.array([4.0, 2.0]) print('cost at {} is: {}'.format(loc, barrier.cost(loc))) obstacles = [[1.5, 2.5]] barrier.update_obstacles(obstacles) loc = np.array([1.45, 2.47]) print('cost at {} is: {}'.format(loc, barrier.cost(loc)))