def __init__(self, n, dt, lr_model=default_lr_model()):
self.lr_model = lr_model
self.n = n
self.dt = dt
# prep constants for calculations
alr_model = np.array(self.lr_model)
self.bhes = (dt * alr_model[0], dt * alr_model[1], dt * alr_model[2])
(_, _, qhx) = self.bhes[0]
(_, _, qhy) = self.bhes[1]
(_, _, qho) = self.bhes[2]
#print('(bhxl, bhxr, qhx): ' + estr((bhxl, bhxr, qhx)))
#print('(bhyl, bhyr, qhy): ' + estr((bhyl, bhyr, qhy)))
#print('(bhol, bhor, qho): ' + estr((bhol, bhor, qho)))
(alphax, alphay, alphao) = 1.0 - np.array((qhx, qhy, qho))
#print('(alphax, alphay, alphao):' + estr((alphax, alphay, alphao)))
# alpha ** j
alphaxj = [1.0]
alphayj = [1.0]
alphaoj = [1.0]
betaj = [dt]
for i in range(1, n):
alphaxj.append(alphaxj[i-1] * alphax)
alphayj.append(alphayj[i-1] * alphay)
alphaoj.append(alphaoj[i-1] * alphao)
betaj.append(betaj[i-1] + dt * alphaoj[i])
self.alphaxj = np.array(alphaxj)
self.alphayj = np.array(alphayj)
self.alphaoj = np.array(alphaoj)
self.betaj = np.array(betaj)
def __init__(self, start_pose, target_pose, dt, n,
start_twist=(0.0, 0.0, 0.0),
target_twist=(0.0, 0.0, 0.0),
start_lr = (0.0, 0.0),
lr_model=default_lr_model(),
mmax=1.0,
jerkweight=.002,
maxweight=.0002
):
self.start_pose=start_pose
self.target_pose = target_pose
self.dt = dt
self.start_twist = start_twist
self.target_twist = target_twist
self.start_lr = start_lr
self.lr_model = lr_model
self.mmax = mmax
self.jerkweight = jerkweight
self.maxweight = maxweight
(pxl, pxr, fx) = lr_model[0]
(pyl, pyr, fy) = lr_model[1]
(pol, por, fo) = lr_model[2]
phatss = ((dt * pxl, dt * pxr), (dt * pyl, dt * pyr), (dt * pol, dt * por)) # pphats[x|y|o] [l|r]
pphatsss = ( ([], []), ([], []), ([], []) )
print(pphatsss[0][0])
print(phatss[0][0])
for j in range(3):
for k in range(2):
pphatsss[j][k].append(phatss[j][k])
alphas = (1.0 - dt * fx, 1.0 - dt * fy, 1.0 - dt * fo)
alpha_prods = [1.0, 1.0, 1.0]
alpha_prodss = ( [alpha_prods[0]], [alpha_prods[1]], [alpha_prods[2]])
# alpha_prodss[x|y|o][n] = alpha_prods[x|y|o]**n
for i in range(1, n):
for j in range(3):
alpha_prods[j] *= alphas[j]
alpha_prodss[j].append(alpha_prods[j])
for alpha_prods in alpha_prodss:
print(fstr(alpha_prods))
# p_[x | y | o][left | right] * alpha[x | y | o] ** (-i)
for i in range(1, n):
for j in range(3):
for k in range(2):
pphatsss[j][k].append(phatss[j][k] / alpha_prodss[j][i])
self.tphat_xl = tf.constant(pphatsss[0][0])
self.tphat_xr = tf.constant(pphatsss[0][1])
self.tphat_yl = tf.constant(pphatsss[1][0])
self.tphat_yr = tf.constant(pphatsss[1][1])
self.tphat_ol = tf.constant(pphatsss[2][0])
self.tphat_or = tf.constant(pphatsss[2][1])
self.talpha_prodxs = tf.constant(alpha_prodss[0])
self.talpha_prodys = tf.constant(alpha_prodss[1])
self.talpha_prodos = tf.constant(alpha_prodss[2])
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)
if __name__ == '__main__':
dt = 0.02
start_pose = [0.0, 0.0, 0.0]
start_twist = [0.0, 0.0, 0.0]
target_pose = [.26, .52, -D_TO_R * 180]
target_twist = [0.0, 0.0, 0.0]
approach_rho = 0.30
min_rho = 0.05
cruise_v = 0.25
lr_start = (0.0, 0.0)
mmax = 1.0
u_time = 0.50
Qfact = [1, 1, 1, 1, 1, 1]
lr_model = default_lr_model()
# scale vy to match omega timing
#for i in range(3):
# lr_model[1][i] = lr_model[1][i] * lr_model[2][2] / lr_model[1][2]
print(gstr({'lr_model': lr_model}))
bad_lr_model = copy.deepcopy(lr_model)
bad_lr_model[0][1] *= .9
bad_lr_model[0][0] *= 1.0
bad_lr_model[2][1] *= 1.1
bad_lr_model[2][0] *= 1.0
# poles for state control model
fact = 0.90
base = -2.0
poles = []
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)
def poses(dt,
ls,
rs,
start_pose=(0.0, 0.0, 0.0),
start_twist=(0.0, 0.0, 0.0),
lr_model=default_lr_model()):
als = np.asarray(ls)
ars = np.asarray(rs)
# prep constants for calculations
alr_model = np.array(lr_model)
bhes = (dt * alr_model[0], dt * alr_model[1], dt * alr_model[2])
(bhxl, bhxr, qhx) = bhes[0]
(bhyl, bhyr, qhy) = bhes[1]
(bhol, bhor, qho) = bhes[2]
(alphax, alphay, alphao) = 1.0 - np.array((qhx, qhy, qho))
n = len(ls)
# alpha ** j
alphaxj = [1.0]
alphayj = [1.0]
alphaoj = [1.0]
betaj = [dt]
for i in range(1, n):
alphaxj.append(alphaxj[i - 1] * alphax)
alphayj.append(alphayj[i - 1] * alphay)
alphaoj.append(alphaoj[i - 1] * alphao)
betaj.append(betaj[i - 1] + dt * alphaoj[i])
#print('als:' + estr(als))
(px0, py0, theta0) = start_pose
(vxw0, vyw0, omega0) = start_twist
# initial robot velocities
vx0 = vxw0 * math.cos(theta0) + vyw0 * math.cos(theta0)
vy0 = -vxw0 * math.sin(theta0) + vyw0 * math.cos(theta0)
# twist
vxj = np.empty(n)
vyj = np.empty(n)
omegaj = np.empty(n)
vxj[0] = vx0
vyj[0] = vy0
omegaj[0] = omega0
bmotorxj = bhxl * als + bhxr * ars
bmotoryj = bhyl * als + bhyr * ars
bmotoroj = bhol * als + bhor * ars
for i in range(1, n):
vxj[i] = vx0 * alphaxj[i] + np.dot(alphaxj[i - 1::-1],
bmotorxj[1:i + 1])
vyj[i] = vy0 * alphayj[i] + np.dot(alphayj[i - 1::-1],
bmotoryj[1:i + 1])
omegaj[i] = omega0 * alphaoj[i] + np.dot(alphaoj[i - 1::-1],
bmotoroj[1:i + 1])
# pose
pxj = np.empty(n)
pyj = np.empty(n)
thetaj = np.empty(n)
pxj[0] = px0
pyj[0] = py0
thetaj[0] = theta0
for i in range(1, n):
thetaj[i] = theta0 + omega0 * (betaj[i] - dt) \
+ np.dot(betaj[i-1::-1], bmotoroj[1:i+1])
# intermediate values as vectors
cosj = np.cos(thetaj)
sinj = np.sin(thetaj)
vxcj = vxj * cosj
vxsj = vxj * sinj
vycj = vyj * cosj
vysj = vyj * sinj
vxwj = vxcj - vysj
vywj = vxsj + vycj
pxj[1:] = px0 + dt * np.cumsum(vxwj[1:])
pyj[1:] = py0 + dt * np.cumsum(vywj[1:])
return (pxj, pyj, thetaj, vxj, vyj, omegaj)
def __init__(self, lr_model=default_lr_model()):
self.lr_model = lr_model
motor = Motor()
speeds = []
while True:
ss = pp.v(tt)
ss['lr'] = motor(ss['v'], ss['omega'])
speeds.append(ss)
if tt > ss['time'] + over_t:
break # at end of plan
tt += dt
# apply the dynamic model to test accuracy
# add an error in dwheel to the dynamics
factor = 1.0
lr_model = list(default_lr_model())
lr_model[1] = list(lr_model[1])
lr_model[1][0] *= factor
lr_model[1][1] *= factor
dynamicStep = DynamicStep(lr_model)
# test of control strategy
tt = 0.0
evold = 0.0
sinpold = 0.0
# plot the movement of robot base
pose_m = pp.start_m[:]
start_vx_m = start_vx_r * cos(start_theta) - start_vy_r * sin(start_theta)
