def static_plan(
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),
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,
u=0.50,
details=False,
max_segments=3,
vhat_start=None):
if details:
print(
fstr({
's_pose': start_pose,
't_pose': target_pose,
's_twist': start_twist,
't_twist': target_twist
}))
# estimate left, right to achieve the path
pp = PathPlan(approach_rho=approach_rho, min_rho=min_rho)
pp.start2(start_pose, target_pose, max_segments=max_segments)
# calculate start vhat from start_twist
if vhat_start is None:
vhat_start = abs(start_twist[0]) + abs(start_twist[2] * pp.rhohat)
pp.speedPlan(vhat_start, cruise_v, target_twist[0], u=u)
return pp
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)
min_rho = 0.005
cruise_v = 0.3
lr_start = (0.0, 0.0)
gauss_iters = 0
nr_iters = 1
Wmax = dt * 1.e-3
#Wmax = 0.0
Wjerk = dt * 1.e-3
Wback = 1.0
#Wback = 0.0
NRstart = 1.0
NRfact = 2
maxSlew = 1.0
testNR = False
pp = PathPlan(approach_rho=approach_rho, min_rho=min_rho)
pathPlan = pp.start2(start_pose, target_pose)
print('path_plan:')
for segment in pathPlan:
print(fstr(segment, fmat='10.7f'))
# estimate left, right to achieve the path
speedPlan = pp.speedPlan(start_twist[0], cruise_v, target_twist[0], u=0.10)
print('speed_plan:')
for segment in speedPlan:
print(fstr(segment, fmat='10.7f'))
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])
lr_model = default_lr_model()
#lr_model = ((1.0, 1.0, 10.0), (-1.0, 1.0, 10.0), (-1.0, 10.0, 10.0))
start_pose = [0.0, 0.0, 0.0]
start_twist = [0.0, 0.0, 0.0]
target_pose = [0.10, 0.05, 0.0]
target_twist = [0.0, 0.0, 0.0]
cruise_v = 0.3
lr_start = (0.0, 0.0)
gauss_iters = 1
nr_iters = 100
Wmax = 1.e-3
Wjerk = 1.e-4
NRstart = 0.01
NRfact = 1.1
pp = PathPlan()
pathPlan = pp.start2(start_pose, target_pose)
print('path_plan:')
for segment in pathPlan:
print(fstr(segment))
# estimate left, right to achieve the path
speedPlan = pp.speedPlan(start_twist[0], cruise_v, target_twist[0], u=0.25)
print('speed_plan:')
for segment in speedPlan:
print(fstr(segment))
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]
# development of a path planning class. import matplotlib.pyplot as plt from math import cos, sin from bdbd_common.utils import fstr from bdbd_common.pathPlan2 import PathPlan from bdbd_common.geometry import D_TO_R, Motor, pose2to3, transform2d, DynamicStep, default_lr_model ### MAIN PROGRAM ### pp = PathPlan(approach_rho=0.20, min_rho=0.05, rhohat=0.2) start_x = 0.0 start_y = 0.0 start_theta = 0.0 start_vx = 0.0 start_vy = 0.0 start_omega = 0.0 #start_theta_degrees = 0.0 #start_theta = start_theta_degrees * D_TO_R end_theta_degrees = 30.0 end_theta = end_theta_degrees * D_TO_R end_x = .4 end_y = .1 start_omega = 0.0 start_vx_r = 0.0 start_vy_r = pp.dwheel * start_omega end_vx_r = 0.0 over_t = 1.0 vhatcruise = 0.25
tomegab.read(i).numpy())
})
t += dt
return path
target_pose = [0.3, 0.1, 0.0] # x, y, theta
target_twist = [0.0, 0.0, 0.0] # vx, vy, omega
start_pose = [0.0, 0.0, 0.0]
start_v = 0.2
cruise_v = 0.33
start_twist = [start_v, 0.0, 0.0]
pp = PathPlan()
print(fstr({'dwheel': pp.dwheel}))
path = pp.start2(start_pose, target_pose)
print('path_plan:')
for segment in path:
print(fstr(segment))
# estimate left, right to achieve the path
s_plan = pp.speedPlan(start_v, cruise_v, 0.0)
print('speed_plan:')
for segment in s_plan:
print(fstr(segment))
print('estimate lr without dynamics')
dt = 0.02
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)
# scratchpad for various simple experiments
from bdbd_common.geometry import pose3to2, pose2to3, D_TO_R, transform2d
from bdbd_common.utils import fstr
from bdbd_common.pathPlan2 import PathPlan
startPose3 = pose2to3((0.0, 0.0, 0.0))
endPose3 = pose2to3((.4, .1, 30. * D_TO_R))
pp = PathPlan()
pp.start(startPose3, endPose3)
for seg in pp.path:
print(fstr(seg, '8.5f'))
for seg in pp.path:
# The control model operates in the robot frame, relative to the plan. So we need the deviation
# of the actual location from the plan location, in the robot frame.
near_wheel_m = transform2d(seg['end'], pp.frame_p, pp.frame_m)
# near_wheel_m is the current origin of the wheel frame for the nearest point
near_robot_m = transform2d(pp.robot_w, near_wheel_m, pp.frame_m)
print(fstr(near_robot_m, '8.5f'))
#print(fstr(pose3to2(pose3), '10.7f'))
def setup(self, start_pose, start_twist, target_pose, target_twist,
cruise_v, dt):
self.start_pose = start_pose
self.start_twist = start_twist
self.target_pose = target_pose
self.target_twist = target_twist
(xb, yb, thetab) = start_pose
(vxb, vyb, omegab) = start_twist
self.epoch = 0
self.dt = dt
# model is in what we will call here 'base' frame
pp = PathPlan()
self.pp = pp
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')
tt = 0.0
motor = Motor()
lrs = []
self.lrs = lrs
self.vvs = []
while True:
vv = self.pp.v(tt)
self.vvs.append(vv)
lr = motor(vv['v'], vv['omega'])
lrs.append({'t': tt, 'left': lr[0], 'right': lr[1]})
print(fstr(lrs[-1]) + fstr(vv))
if vv['fraction'] > 0.9999:
break
tt += dt
# copy trainable estimates into tensors
tleft = []
tright = []
for i in range(len(lrs)):
tleft.append(tf.Variable(lrs[i]['left'], name='left' + str(i)))
tright.append(tf.Variable(lrs[i]['right'], name='right' + str(i)))
# accumulating variables
tomegab = tf.TensorArray(dtype=tf.float32,
size=len(lrs),
dynamic_size=False,
clear_after_read=False,
name='omegab')
self.tomegab = tomegab.write(0, omegab)
tvxb = tf.TensorArray(dtype=tf.float32,
size=len(lrs),
dynamic_size=False,
clear_after_read=False,
name='vxb')
self.tvxb = tvxb.write(0, vxb)
tvyb = tf.TensorArray(dtype=tf.float32,
size=len(lrs),
dynamic_size=False,
clear_after_read=False,
name='vyb')
self.tvyb = tvyb.write(0, vyb)
tthetab = tf.TensorArray(dtype=tf.float32,
size=len(lrs),
dynamic_size=False,
clear_after_read=False,
name='theta')
self.tthetab = tthetab.write(0, thetab)
txb = tf.TensorArray(dtype=tf.float32,
size=len(lrs),
dynamic_size=False,
clear_after_read=False,
name='txb')
self.txb = txb.write(0, xb)
tyb = tf.TensorArray(dtype=tf.float32,
size=len(lrs),
dynamic_size=False,
clear_after_read=False,
name='tyb')
self.tyb = tyb.write(0, yb)
self.opt2 = keras.optimizers.Adam(learning_rate=.02)
self.opt1 = keras.optimizers.SGD(learning_rate=0.2)
self.tleft = tleft
self.tright = tright
