def nextLR(dt,
callback_dt,
last_lr,
now_twist_robot,
sum_epsv,
sum_epso,
pp,
lr_model=default_lr_model(),
mmax=1.0):
# Calculate next left, right from velocity model with integral error correction
tt = 0.0
tees = [tt]
last_vx = now_twist_robot[0] - sum_epsv
last_omega = now_twist_robot[2] - sum_epso
vxes = [last_vx]
omegas = [last_omega]
lefts = [last_lr[0]]
rights = [last_lr[1]]
while True:
tt += dt
vv = pp.v(tt)
v = vv['v'] - sum_epsv
omega = vv['omega'] - sum_epso
vxes.append(v)
omegas.append(omega)
print(fstr(vv))
(left, right, last_vx, last_omega) = lr_est(v,
omega,
last_vx,
last_omega,
dt,
lr_model=lr_model,
mmax=mmax)
lefts.append(left)
rights.append(right)
if tt > callback_dt:
break
new_left = average(callback_dt, tees, lefts)
new_right = average(callback_dt, tees, rights)
print(gstr((new_left, new_right)))
return (new_left, new_right)
def static_plan(
dt,
start_pose=(0.0, 0.0, 0.0),
start_twist=(0.0, 0.0, 0.0),
target_pose=(0.0, 0.0, 0.0),
target_twist=(0.0, 0.0, 0.0),
lr_model=default_lr_model(),
mmax=1.0,
approach_rho=0.08, # radius of planned approach
min_rho=0.02, # the smallest radius we allow in a path plan,
cruise_v=0.25,
lr_start=(0.0, 0.0),
u=0.50,
details=False,
max_segments=3,
vhat_start=0.0):
if details:
print(gstr({'start_pose': start_pose, 'target_pose': target_pose}))
# estimate left, right to achieve the path
pp = PathPlan(approach_rho=approach_rho, min_rho=min_rho)
pathPlan = pp.start2(start_pose, target_pose, max_segments=max_segments)
#print(dt, start_twist[0], cruise_v, target_twist[0], u)
speedPlan = pp.speedPlan(vhat_start, cruise_v, target_twist[0], u=u)
for seg in speedPlan:
print(gstr(seg))
total_time = 0.0
for seg in speedPlan:
total_time += seg['time']
vxr0 = start_twist[0] * math.cos(
start_pose[2]) + start_twist[1] * math.sin(start_pose[2])
vyr0 = -start_twist[0] * math.sin(
start_pose[2]) + start_twist[1] * math.cos(start_pose[2])
last_vx = vxr0
last_omega = start_twist[2]
vxres = [vxr0]
vyres = [vyr0]
omegas = [start_twist[2]]
vvs = [pp.v(0.0)]
vvs[0]['left'] = lr_start[0]
vvs[0]['right'] = lr_start[1]
lefts = [lr_start[0]]
rights = [lr_start[1]]
tt = 0.0
tees = [tt]
poses = [(0., 0., 0.)]
vv = None
while tt < total_time:
tt += dt
vv = pp.v(tt)
vvs.append(vv)
# vv gives vhat is in wheel frame. We need to convert to robot frame.
vxres.append(vv['v'])
vyres.append(vv['omega'] * pp.dwheel)
omegas.append(vv['omega'])
if tt < 4.00:
print(fstr(vv))
(left, right, last_vx, last_omega) = lr_est(vv['v'],
vv['omega'],
last_vx,
last_omega,
dt,
lr_model=lr_model,
mmax=mmax)
#if tt < 0.10:
# print("(left, right, last_vx, last_omega):" + fstr((left, right, last_vx, last_omega)))
lefts.append(left)
rights.append(right)
tees.append(tt)
poses.append(vv['point'])
vv['left'] = left
vv['right'] = right
if details:
if vv:
print('vv:' + gstr(vv))
print('pathPlan:')
for segment in pathPlan:
print(fstr(segment, fmat='7.4f'))
print('speedPlan')
for segment in speedPlan:
print(fstr(segment, fmat='7.4f'))
num_segments = len(pathPlan)
return (tees, lefts, rights, num_segments, pp, total_time, vxres, omegas,
poses)
def plan_arrays(pp,
dt,
start_pose=(0.0, 0.0, 0.0),
start_twist=(0.0, 0.0, 0.0),
lr_model=default_lr_model(),
mmax=1.0,
lr_start=(0.0, 0.0),
details=False,
details_v=False):
total_time = 0.0
for seg in pp.s_plan:
total_time += seg['time']
vxr0 = start_twist[0] * math.cos(
start_pose[2]) + start_twist[1] * math.sin(start_pose[2])
vyr0 = -start_twist[0] * math.sin(
start_pose[2]) + start_twist[1] * math.cos(start_pose[2])
last_vx = vxr0
last_omega = start_twist[2]
vxres = [vxr0]
vyres = [vyr0]
omegas = [start_twist[2]]
vvs = [pp.v(0.0)]
vvs[0]['left'] = lr_start[0]
vvs[0]['right'] = lr_start[1]
lefts = [lr_start[0]]
rights = [lr_start[1]]
tt = 0.0
tees = [tt]
poses = [(0., 0., 0.)]
vv = None
while tt < total_time:
tt += dt
vv = pp.v(tt)
vvs.append(vv)
# vv gives vhat is in wheel frame. We need to convert to robot frame.
vxres.append(vv['v'])
vyres.append(vv['omega'] * pp.dwheel)
omegas.append(vv['omega'])
if details_v:
print(fstr(vv))
(left, right, last_vx, last_omega) = lr_est(vv['v'],
vv['omega'],
last_vx,
last_omega,
dt,
lr_model=lr_model,
mmax=mmax)
lefts.append(left)
rights.append(right)
tees.append(tt)
poses.append(vv['point'])
vv['left'] = left
vv['right'] = right
if details:
if vv:
print('vv at end:' + fstr(vv))
print('pathPlan:')
for segment in pp.path:
print(fstr(segment, fmat='7.4f'))
print('speedPlan')
for segment in pp.s_plan:
print(fstr(segment, fmat='7.4f'))
num_segments = len(pp.plan)
return (tees, lefts, rights, num_segments, total_time, vxres, omegas,
poses)
vvs[0]['left'] = lr_start[0]
vvs[0]['right'] = lr_start[1]
lefts = [lr_start[0]]
rights = [lr_start[1]]
tt = 0.0
tees = [tt]
while True:
tt += dt
vv = pp.v(tt)
vvs.append(vv)
# vv gives vhat is in wheel frame. We need to convert to robot frame.
vxres.append(vv['v'])
vyres.append(vv['omega'] * pp.dwheel)
omegas.append(vv['omega'])
(left, right, last_vx, last_omega) = lr_est(vv['v'], vv['omega'], last_vx, last_omega, dt)
lefts.append(left)
rights.append(right)
tees.append(tt)
vv['left'] = left
vv['right'] = right
if vv['fraction'] > 0.9999:
break
for seg in vvs:
print(estr(seg))
#lefts = lefts[:10]
#rights = rights[:10]
#tees = tees[:10]
n = len(lefts)
def setup(self,
start_pose,
start_twist,
target_pose,
target_twist,
cruise_v,
dt,
lr_start=(0.0, 0.0),
lr_end=(0.0, 0.0)):
self.start_pose = start_pose
self.start_twist = start_twist
self.target_pose = target_pose
self.target_twist = target_twist
self.lr_start = lr_start
self.lr_end = lr_end
self.epoch = 0
self.dt = dt
# model is in what we will call here 'base' frame
pp = PathPlan()
self.pp = pp
print(fstr({'target_pose': target_pose, 'target_twist': target_twist}))
self.pathPlan = pp.start2(start_pose, target_pose)
print('path_plan:')
for segment in self.pathPlan:
print(fstr(segment))
# estimate left, right to achieve the path
self.speedPlan = pp.speedPlan(start_twist[0], cruise_v,
target_twist[0])
print('speed_plan:')
for segment in self.speedPlan:
print(fstr(segment))
print('estimate lr without dynamics')
vxr0 = start_twist[0] * math.cos(
start_pose[2]) + start_twist[1] * math.sin(start_pose[2])
vyr0 = -start_twist[0] * math.sin(
start_pose[2]) + start_twist[1] * math.cos(start_pose[2])
last_vx = vxr0
last_omega = start_twist[2]
lefts = [lr_start[0]]
rights = [lr_start[1]]
vxres = [vxr0]
vyres = [vyr0]
omegas = [self.start_twist[2]]
self.vvs = [self.pp.v(0.0)]
tt = 0.0
while True:
tt += dt
vv = self.pp.v(tt)
self.vvs.append(vv)
# vv gives vhat is in wheel frame. We need to convert to robot frame.
vxres.append(vv['v'])
vyres.append(vv['omega'] * pp.dwheel)
omegas.append(vv['omega'])
(left, right, last_vx,
last_omega) = lr_est(vv['v'], vv['omega'], last_vx, last_omega,
dt)
lefts.append(left)
rights.append(right)
if vv['fraction'] > 0.9999:
break
'''
self.tleft = tf.concat(
( tf.constant([lefts[0]]),
tf.Variable(lefts[1:], trainable=True, name='left_var')
), axis=0, name='tleft')
self.tright = tf.concat(
( tf.constant([rights[0]]),
tf.Variable(rights[1:], trainable=True, name='right_var')
), axis=0, name='tright')
'''
self.tleft = tf.Variable(lefts, trainable=True, name='left_var')
self.tright = tf.Variable(rights, trainable=True, name='right_var')
self.tvxr = tf.constant(vxres, name='vxr')
self.tvyr = tf.constant(vyres, name='vyr')
self.tomega = tf.constant(omegas, name='omega')
self.opt2 = keras.optimizers.Adam(learning_rate=.000000002)
self.opt1 = keras.optimizers.SGD(learning_rate=0.000002)