###########################################
# 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)))
