def make_small2_path():
_dir = '/home/poine/work/two_d_guidance/paths/demo_z'
x0, x1 = -0.75, 5
c1, r = [x1, 0], 1.75
line1 = tdg.make_line_path([x0, r], [x1, r], n_pt=100)
circle1 = tdg.make_circle_path(c1, -r, np.pi / 2, np.pi, n_pt=180)
r2 = 0.9575
x2 = 4 * r2
line2 = tdg.make_line_path([x1, -r], [x2, -r], n_pt=50)
circle2 = tdg.make_circle_path([x2, -(r - r2)],
-r2,
-np.pi / 2,
np.pi / 2,
n_pt=90)
x3 = 2 * r2
circle3 = tdg.make_circle_path([x3, -(r - r2)], r2, 0, np.pi, n_pt=180)
circle4 = tdg.make_circle_path([0, -(r - r2)], -r2, 0, np.pi, n_pt=180)
circle5 = tdg.make_circle_path([-x3, -(r - r2)], r2, 0, np.pi, n_pt=180)
circle6 = tdg.make_circle_path([-x2, -(r - r2)],
-r2,
0,
np.pi / 2,
n_pt=90)
line3 = tdg.make_line_path([-x2, -r], [-x1, -r], n_pt=50)
circle7 = tdg.make_circle_path([-x1, 0], -r, -np.pi / 2, np.pi, n_pt=180)
line4 = tdg.make_line_path([-x2, r], [x0, r], n_pt=50)
line1.append([
circle1, line2, circle2, circle3, circle4, circle5, circle6, line3,
circle7, line4
])
p = line1
fname = os.path.join(_dir, 'track_trr.npz')
p.save(fname)
return fname, p
def make_z_room_path(res=0.01):
#x0, x1 = -0.75, 2.25
x0, x1 = 0., 2.25 # moved start of path to center
c1, r = [x1, 0], 1.25
line1 = tdg.make_line_path([x0, r], [x1, r], res=res)
circle1 = tdg.make_circle_path(c1, -r, np.pi / 2, np.pi, res=res)
x2, y2 = 1.581, -0.35
line2 = tdg.make_line_path([x1, -r], [x2, -r], res=0.01)
c2, r2, th2 = [x2, y2], 0.9, np.deg2rad(61.8)
circle2 = tdg.make_circle_path(c2, -r2, -np.pi / 2, th2, res=res)
c3, r3 = [0, -1.21], 0.9
circle3 = tdg.make_circle_path(c3, r3, np.pi / 2 - th2, 2 * th2, res=res)
c4, r4 = [-x2, y2], 0.9
circle4 = tdg.make_circle_path(c4, -r4, -(np.pi / 2 - th2), th2, res=res)
line3 = tdg.make_line_path([-x2, -r], [-x1, -r], res=res)
circle5 = tdg.make_circle_path([-x1, 0], -r, -np.pi / 2, np.pi, res=res)
line4 = tdg.make_line_path([-x1, r], [x0, r], res=res)
line1.append(
[circle1, line2, circle2, circle3, circle4, line3, circle5, line4])
p = line1
fname = '/tmp/path_tmp.npz'
p.save(fname)
# add landmarks and velocity profile
p1 = trr_u.TrrPath(fname, v=1.)
p1.lm_s = [
p.dists[-1] - 1.25, 1.25
] # start and finish are located -1.25 and 1.25 from path origin
vel_prof_id = 0
vels = [[1., 1., 1., 1., 1., 1., 1., 1., 1.],
[1.2, 1., 1., 1., 1., 1., 1., 1., 1.2],
[1.4, 1.2, 1.1, 1., 1., 1., 1.1, 1.2, 1.4],
[1.5, 1.3, 1.2, 1.0, 1.0, 1.0, 1.2, 1.3, 1.5],
[1.75, 1.4, 1.2, 1.0, 1.0, 1.0, 1.2, 1.4, 1.75],
[2.0, 1.4, 1.4, 1., 1., 1., 1.4, 1.4, 2.0]]
make_vel_profile(p1,
vels[vel_prof_id],
a_max=0,
omega_ref=5.5,
braking_delay=100)
p1.report()
tdg_dir = rospkg.RosPack().get_path('two_d_guidance')
#fname = os.path.join(tdg_dir, 'paths/demo_z/track_trr_sim_{}.npz'.format(vel_prof_id))
fname = os.path.join(
tdg_dir, 'paths/demo_z/track_trr_real_{}.npz'.format(vel_prof_id))
p1.save(fname)
return fname, p1
def make_oval(res=0.01):
_dir = tdg.tdg_dir() + '/paths/roboteck'
r, m = 0.25, 0.125 # radius, margin
dx, dy = [1.1, 0.7]
xc, yc = [dx / 2, dy / 2]
dxl = dx - 2 * (r + m)
x10, y10, x11, y11 = xc - dxl / 2, yc - r, xc + dxl / 2, yc - r
line1 = tdg.make_line_path([x10, y10], [x11, y11], res=res)
c1 = [xc + dxl / 2, yc]
circle1 = tdg.make_circle_path(c1, r, -np.pi / 2, np.pi, res=res)
x20, y20, x21, y21 = xc + dxl / 2, yc + r, xc - dxl / 2, yc + r
line2 = tdg.make_line_path([x20, y20], [x21, y21], res=res)
c2 = [xc - dxl / 2, yc]
circle2 = tdg.make_circle_path(c2, r, np.pi / 2, np.pi, res=res)
line1.append([circle1, line2, circle2])
p = line1
fname = os.path.join(_dir, 'oval_1.npz')
p.save(fname)
return fname, p
def make_real_path(res=0.01):
_dir = '/home/poine/work/two_d_guidance/paths/demo_z'
#x0, x1 = -0.75, 2.25
x0, x1 = 0., 2.25 # moved start of path to center
c1, r = [x1, 0], 1.25
line1 = tdg.make_line_path([x0, r], [x1, r], res=res)
circle1 = tdg.make_circle_path(c1, -r, np.pi / 2, np.pi, res=res)
x2, y2 = 1.581, -0.35
line2 = tdg.make_line_path([x1, -r], [x2, -r], res=0.01)
c2, r2, th2 = [x2, y2], 0.9, np.deg2rad(61.8)
circle2 = tdg.make_circle_path(c2, r2, -np.pi / 2, -th2, res=res)
c3, r3 = [0, -1.21], 0.9
circle3 = tdg.make_circle_path(c3, r3, np.pi / 2 - th2, 2 * th2, res=res)
c4, r4 = [-x2, y2], 0.9
circle4 = tdg.make_circle_path(c4, r4, -(np.pi / 2 - th2), -th2, res=res)
line3 = tdg.make_line_path([-x2, -r], [-x1, -r], res=res)
circle5 = tdg.make_circle_path([-x1, 0], -r, -np.pi / 2, np.pi, res=res)
line4 = tdg.make_line_path([-x1, r], [x0, r], res=res)
line1.append(
[circle1, line2, circle2, circle3, circle4, line3, circle5, line4])
p = line1
fname = os.path.join(_dir, 'track_trr_real.npz')
p.save(fname)
return fname, p
def make_square(res=0.01):
_dir = tdg.tdg_dir() + '/paths/roboteck'
dx, dy, r, m = 1.040, 0.64, 0.16, 0.04
x10, y10, x11, y11 = m, r + m, m, dy - r - m
line1 = tdg.make_line_path([x10, y10], [x11, y11], res=res)
c1 = [r + m, dy - r - m]
circle1 = tdg.make_circle_path(c1, -r, -np.pi, np.pi / 2, res=res)
x20, y20, x21, y21 = r + m, dy - m, dx - r - m, dy - m
line2 = tdg.make_line_path([x20, y20], [x21, y21], res=res)
c2 = [dx - r - m, dy - r - m]
circle2 = tdg.make_circle_path(c2, -r, np.pi / 2, np.pi / 2, res=res)
x30, y30, x31, y31 = dx - m, dy - r - m, dx - m, r + m
line3 = tdg.make_line_path([x30, y30], [x31, y31], res=res)
c3 = [dx - r - m, r + m]
circle3 = tdg.make_circle_path(c3, -r, 0, np.pi / 2, res=res)
x40, y40, x41, y41 = dx - r - m, m, r + m, m
line4 = tdg.make_line_path([x40, y40], [x41, y41], res=res)
c4 = [r + m, r + m]
circle4 = tdg.make_circle_path(c4, -r, -np.pi / 2, np.pi / 2, res=res)
line1.append([circle1, line2, circle2, line3, circle3, line4, circle4])
p = line1
fname = os.path.join(_dir, 'square_1.npz')
p.save(fname)
return fname, p
def run(which='line_01'):
paths = {
# ccw 1m circle
'circle_02':
lambda: tdg.make_circle_path([2, 1.8], 1., 0, 2 * np.pi, 360),
# cw 1m circle
'circle_03':
lambda: tdg.make_circle_path([2, 1.8], -1., 0, 2 * np.pi, 360),
# figure of eight
'fig_of_eight_01': lambda: tdg.make_fig_of_height_path2(0.7),
# oval
'oval_01': lambda: tdg.make_oval_path([-1.25, 0.], [1., 0.], 1.5),
'oval_02': lambda: tdg.make_oval_path([-2.5, 0.], [2.5, 0.], 1.75),
# line 1m
'line_01': lambda: tdg.make_line_path([-2, -1.8], [0., -0.8])
}
_dir = '/home/poine/work/two_d_guidance/paths/demo_z'
p = paths[which]()
p.transform([2, 1.8])
fname = os.path.join(_dir, which + '.npz')
p.save(fname)
return fname, p
def make_path(res=0.01):
_dir = '/home/poine/work/two_d_guidance/paths/roboteck'
r = 0.16
x10, y10, x11, y11 = 0., 0., 0., 0.48 # moved start of path to center
line1 = tdg.make_line_path([x10, y10], [x11, y11], res=res)
c1 = [0.16, 0.48]
circle1 = tdg.make_circle_path(c1, -r, -np.pi, np.pi, res=res)
x2, y2, x3, y3 = 0.32, 0.48, 0.32, 0.32
line2 = tdg.make_line_path([x2, y2], [x3, y3], res=res)
c2 = [0.48, 0.32]
circle2 = tdg.make_circle_path(c2, r, -np.pi, np.pi, res=res)
x4, y4, x5, y5 = 0.64, 0.32, 0.64, 0.48
line3 = tdg.make_line_path([x4, y4], [x5, y5], res=res)
c3 = [0.8, 0.48]
circle3 = tdg.make_circle_path(c3, -r, -np.pi, np.pi / 2, res=res)
x6, y6, x7, y7 = 0.8, 0.64, 0.88, 0.64
line4 = tdg.make_line_path([x6, y6], [x7, y7], res=res)
c4 = [0.88, 0.48]
circle4 = tdg.make_circle_path(c4, -r, np.pi / 2, np.pi / 2, res=res)
x8, y8, x9, y9 = 1.04, 0.48, 1.04, 0.16
line5 = tdg.make_line_path([x8, y8], [x9, y9], res=res)
c5 = [0.88, 0.16]
circle5 = tdg.make_circle_path(c5, -r, 0, np.pi / 2, res=res)
x60, y60, x61, y61 = 0.88, 0., 0.32, 0.
line6 = tdg.make_line_path([x60, y60], [x61, y61], res=res)
c6 = [0.32, 0.16]
circle6 = tdg.make_circle_path(c6, -r, -np.pi / 2, np.pi / 2, res=res)
x70, y70, x71, y71 = 0.16, 0.16, 0.16, 0.48
line7 = tdg.make_line_path([x70, y70], [x71, y71], res=res)
line1.append([
circle1, line2, circle2, line3, circle3, line4, circle4, line5,
circle5, line6, circle6, line7
])
p = line1
fname = os.path.join(_dir, 'track.npz')
p.save(fname)
return fname, p
def make_vedrines_path(res=0.01):
# origin of path is at the center of the long straight line
lw = 1.5 # lane width
x0, x1 = 0., 22.5 # start and end of first line (step0)
c1, r1 = [x1, 0], 1.75 # center and radius of first circle (step1)
line1 = tdg.make_line_path([x0, r1], [x1, r1], res=res)
circle1 = tdg.make_circle_path(c1, -r1, np.pi / 2, np.pi, res=res)
d1 = 3.83
r2 = 2
x2, y2 = 2 * d1, r2 - r1
line2 = tdg.make_line_path([x1, -r1], [x2, -r1], res=0.01)
c2, th2 = [x2, y2], np.deg2rad(51.1) # 51 < th2 < 51.15
circle2 = tdg.make_circle_path(c2, -r2, -np.pi / 2, th2, res=res)
r3 = 2.927
x3, y3 = d1, -r1 - 1.1
c3, th3, dth3 = [x3, y3], np.pi / 2 - th2, 2 * th2
circle3 = tdg.make_circle_path(c3, r3, th3, dth3, res=res)
r4, c4, th4, dth4 = r2, [0, y2], -np.pi / 2 + th2, 2 * th2
circle4 = tdg.make_circle_path(c4, -r4, th4, dth4, res=res)
r5, c5, th5, dth5 = r3, [-x3, y3], th3, dth3
circle5 = tdg.make_circle_path(c5, r5, th5, dth5, res=res)
r6, c6, th6, dth6 = r2, [-x2, y2], -np.pi / 2 + th2, th2
circle6 = tdg.make_circle_path(c6, -r6, th6, dth6, res=res)
line3 = tdg.make_line_path([-x2, -r1], [-x1, -r1], res=0.01)
r7, c7, th7, dth7 = r1, [-x1, 0], -np.pi / 2, np.pi
circle7 = tdg.make_circle_path(c7, -r7, th7, dth7, res=res)
line4 = tdg.make_line_path([-x1, r1], [-x0, r1], res=res)
line1.append([
circle1, line2, circle2, circle3, circle4, circle5, circle6, line3,
circle7, line4
])
p = line1
fname = '/tmp/path_tmp.npz'
p.save(fname)
# add landmarks and velocity profile
p1 = trr_u.TrrPath(fname, v=1.)
p1.lm_s = [
0., 16.66
] # start and finish are located at 0 and 16.66 m from path origin
idx_velp = 1
vels = [[1, 1, 1, 1, 1, 1, 1, 1, 1, 1,
1], [3, 2, 3, 2, 2, 2, 2, 2, 3, 2, 3],
[4, 2, 4, 2, 2, 2, 2, 2, 4, 2, 4],
[6, 2.5, 6, 2, 2.5, 2, 2.5, 2, 6, 2.5, 6],
[7, 2.5, 7, 2, 2.5, 2, 2.5, 2, 7, 2.5, 7],
[8, 2.5, 8, 2, 2.5, 2, 2.5, 2, 8, 2.5, 8]]
for idx_velp in range(len(vels)):
make_vel_profile(p1,
vels[idx_velp],
a_max=0,
omega_ref=3.,
braking_delay=100)
p1.report()
tdg_dir = rospkg.RosPack().get_path('two_d_guidance')
fname = os.path.join(
tdg_dir, 'paths/vedrines/track_trr_{}.npz'.format(idx_velp))
p1.save(fname)
return fname, p1