def test_position_waypoint_only_input(self):
waypoints = dict()
waypoints['x'] = np.array([1., 0.5, -0.5, -1.0])
waypoints['y'] = np.array([0.5, -0.5, 0.5, -0.5])
waypoints['z'] = np.array([-0.5, -0.5, 0.5, 0.5])
waypoints['yaw'] = np.array([0, 0, 0, 0])
traj = traj_qr.traj_qr(waypoints)
def test_full_waypoint_input(self):
waypoints = dict()
waypoints['x'] = np.zeros([5, 4])
waypoints['y'] = np.zeros([5, 4])
waypoints['z'] = np.zeros([5, 4])
waypoints['yaw'] = np.zeros([3, 4])
waypoints['x'][0, :] = np.array([1., 0.5, -0.5, -1.0])
waypoints['y'][0, :] = np.array([0.5, -0.5, 0.5, -0.5])
waypoints['z'][0, :] = np.array([-0.5, -0.5, 0.5, 0.5])
traj = traj_qr.traj_qr(waypoints)
def setUp(self):
waypoints = dict()
waypoints['x'] = np.zeros([5, 4])
waypoints['y'] = np.zeros([5, 4])
waypoints['z'] = np.zeros([5, 4])
waypoints['yaw'] = np.zeros([3, 4])
waypoints['x'][0, :] = np.array([1., 0.5, -0.5, -1.0])
waypoints['y'][0, :] = np.array([0.5, -0.5, 0.5, -0.5])
waypoints['z'][0, :] = np.array([-0.5, -0.5, 0.5, 0.5])
n_wayp = waypoints['x'].size
self.traj = traj_qr.traj_qr(waypoints)
self.traj.run_astro()
def setUp(self):
filename = '/home/torq-dev/catkin_ws/src/torq_gcs/trajectories/344_10_26/344_wayp_red_002.yaml'
in_waypoints = utils.load_waypoints(filename)
n_wayp = np.size(in_waypoints['x'])
""" Default waypoints"""
waypoints = dict()
waypoints['x'] = np.zeros([5, n_wayp])
waypoints['y'] = np.zeros([5, n_wayp])
waypoints['z'] = np.zeros([5, n_wayp])
waypoints['yaw'] = np.zeros([3, n_wayp])
waypoints['x'][0, :] = in_waypoints['x']
waypoints['y'][0, :] = in_waypoints['y']
waypoints['z'][0, :] = in_waypoints['z']
der_fixed = None
costs = dict()
costs['x'] = [0, 0, 0, 0, 1] # minimum snap
costs['y'] = [0, 0, 0, 0, 1] # minimum snap
costs['z'] = [0, 0, 0, 0, 1] # minimum snap
costs['yaw'] = [0, 0, 1] # minimum acceleration
order = dict(x=9, y=9, z=9, yaw=5)
# order=dict(x=12, y=12, z=12, yaw=5)
seed_times = np.ones(waypoints['x'].shape[1] - 1) * 1.0
self.traj = traj_qr.traj_qr(waypoints,
costs=costs,
order=order,
seed_times=seed_times,
curv_func=False,
der_fixed=der_fixed,
path_weight=None)
self.traj.run_astro()
def main():
from astro import traj_qr
waypoints = dict()
waypoints['x'] = np.zeros([5, 2])
waypoints['y'] = np.zeros([5, 2])
waypoints['z'] = np.zeros([5, 2])
waypoints['yaw'] = np.zeros([3, 2])
waypoints['x'][0, :] = np.array([-1.0, 1.0])
waypoints['y'][0, :] = np.array([0.4, 0.5])
waypoints['z'][0, :] = np.array([-0.6, -0.5])
traj = traj_qr.traj_qr(waypoints, seed_avg_vel=1.0)
# traj.initial_guess()
# traj.get_trajectory()
traj.run_astro()
weight = 1.0
x1 = np.array([0.0, 0.0, -0.5])
x2 = np.array([0.0, 1.0, -0.5])
r = 0.3
l = 0.01
x1 = np.array([-10.0, 0.0, -0.])
x2 = np.array([10.0, 0.0, -0.])
x1 = np.array([-10.0, -2.0, -4.])
x2 = np.array([10.0, 2.0, 4.])
r = 1.0
l = 0.01
keep_out = False
der = 0
constr = cylinder_constraint(weight,
keep_out,
der,
x1,
x2,
r,
l,
active_seg=0,
dynamic_weighting=False,
doCurv=True,
sum_func=False)
nsamp = 50
cost = np.zeros(nsamp)
state_test = dict()
for key in traj.state.keys():
state_test[key] = np.zeros((1, nsamp, 1))
state_test_store = np.array([
np.linspace(0.5, 0.5, nsamp),
np.linspace(-3., 3, nsamp),
np.linspace(-0.5, -0.9, nsamp)
])
for i in range(nsamp):
state_test['x'][0, :, 0] = state_test_store[0, i]
state_test['y'][0, :, 0] = state_test_store[1, i]
state_test['z'][0, :, 0] = state_test_store[2, i]
cost[i], grad, curv, max_ID = constr.cost_grad_curv(state_test,
0,
doGrad=True,
doCurv=True)
import matplotlib.pyplot as plt
import matplotlib as mpl
from mpl_toolkits.mplot3d import Axes3D
# PLOT FIGURE
fig = plt.figure(figsize=(10, 10))
fig.suptitle('Static 3D Plot', fontsize=20)
ax = fig.add_subplot(111, projection='rectilinear')
ax.set_xlabel('dist', fontsize=14)
ax.set_ylabel('cost', fontsize=14)
# plot Trajectory
ax.plot(state_test_store[1, :], cost)
# plot waypoints
plt.show()
import pdb
pdb.set_trace()
cost, grad, curv = constr.compute_constraint_cost_grad_curv(
traj.state, traj.poly.state_scaled, doGrad=True, doCurv=True)
import pdb
pdb.set_trace()
# import pdb; pdb.set_trace()
A = np.identity(3)
A[0, 0] = 1 / 0.2**2
A[1, 1] = 1 / 0.5**2
A[2, 2] = 1 / 0.3**2
weight = 1.0
x0 = np.array([0.0, 0.5, -0.5])
keep_out = True
der = 0
rot_mat = np.identity(3)
constr = ellipsoid_constraint(weight, keep_out, der, x0, A, rot_mat)
cost, grad, curv = constr.compute_constraint_cost_grad_curv(
traj.state, traj.poly.state_scaled, doGrad=True, doCurv=True)
import pdb
pdb.set_trace()
test_list = [
ellipsoid_constraint(weight, keep_out, der, x0, A),
ellipsoid_constraint(weight * 2.0, keep_out, der, x0 + 1.0, A)
]