def done_cb(self, state, r):
rospy.loginfo(
gstr({
'done state': GoalStatus.to_string(state),
'elapsed time': time.time() - self.start,
'loss': r.loss
}))
if state == GoalStatus.SUCCEEDED:
self.queue.put(r)
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 init(self, n, dt, pose0s=(0.0, 0.0, 0.0), twist0s=(0.0, 0.0, 0.0)):
self.n = n
self.dt = dt
self.pose0s = pose0s
self.twist0s = twist0s
# 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])
(bhxl, bhxr, qhx) = self.bhes[0]
(bhyl, bhyr, qhy) = self.bhes[1]
(bhol, bhor, qho) = self.bhes[2]
print('(bhxl, bhxr, qhx): ' + gstr((bhxl, bhxr, qhx)))
print('(bhyl, bhyr, qhy): ' + gstr((bhyl, bhyr, qhy)))
print('(bhol, bhor, qho): ' + gstr((bhol, bhor, qho)))
(alphax, alphay, alphao) = 1.0 - np.array((qhx, qhy, qho))
print('(alphax, alphay, alphao):' + gstr((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)
print('alphaxj:' + gstr(self.alphaxj, fmat='15.12f'))
print('alphayj:' + gstr(self.alphayj, fmat='15.12f'))
print('alphaoj:' + gstr(self.alphaoj, fmat='15.12f'))
print('betaj:' + gstr(self.betaj, fmat='15.12f'))
# first derivatives (Jacobians)
'''
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)
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 = []
for i in range(6):
poles.append(base)
base = base * fact
np_poles = np.array(poles)
start_twist = [0.0, 0.0, 0.0]
target_pose = [.3, -.05, -D_TO_R * 180]
target_twist = [0.0, 0.0, 0.0]
approach_rho = 0.30
min_rho = 0.10
cruise_v = 0.25
lr_start = (0.0, 0.0)
mmax = 1.0
u_time = 0.50
Qfact = [1, 1, 1, 1, 1, 1]
replan_rate = 10000
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] *= .8
bad_lr_model[0][0] *= 1.0
bad_lr_model[2][1] *= 1.2
bad_lr_model[2][0] *= 1.0
# poles for state control model
fact = 0.9
base = -2.0
max_err = .10
poles = []
for i in range(6):
poles.append(base)
base = base * fact
source = Object()
source.tees = tees
source.pxj = pxj
source.pyj = pyj
source.thetaj = thetaj
source.lefts = lefts
source.rights = rights
source.now_pose_map = now_pose_map
source.now_pose_wheel = now_pose_wheel
#pathPlot(dt, source)
times.append(time.time())
deltas = []
for i in range(1, len(times)):
deltas.append(times[i] - times[i - 1])
print('deltas:' + gstr(deltas) + ' total:' +
gstr(times[-1] - times[0]))
print('rospy shut down')
motor_pub.publish(0.0, 0.0)
odom.odom_sub.unregister()
rospy.sleep(.1)
plt.waitforbuttonpress()
exit(0)
pathPlot = PathPlot(history_rate=3)
# send to C++ node for processing
lroClient = LrOptimizeClient(Wmax=Wmax,
Wjerk=Wjerk,
Wback=Wback,
mmax=mmax)
cruise_v = 0.3
dt = 0.05
frac = 0.0
tt = 0.0
vps = []
#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))
nr = NewRaph(lr_model)
lefts = [1.0, 1.0, 1.0, 1.0]
rights = [1.0, 1.0, 1.0, 1.0]
n = len(lefts)
nr.init(n, dt, pose0s=start_pose, twist0s=start_twist)
(pxj, pyj, thetaj, vxj, vyj, omegaj) = nr.poses(lefts, rights)
print('vxj:' + gstr(vxj, fmat='15.12f'))
print('vyj:' + gstr(vyj, fmat='15.12f'))
print('omegaj:' + gstr(omegaj, fmat='15.12f'))
print('thetaj:' + fstr(thetaj, fmat='15.12f'))
print('pxj:' + fstr(pxj, fmat='15.12f'))
print('pyj:' + fstr(pyj, fmat='15.12f'))
print('sin(theta):' + fstr(np.sin(thetaj), fmat='15.12f'))
print('cos(theta):' + fstr(np.cos(thetaj), fmat='15.12f'))
print('vxw:' +
fstr(vxj * np.cos(thetaj) - vyj * np.sin(thetaj), fmat='15.12f'))
print('vyw:' +
fstr(vxj * np.sin(thetaj) + vyj * np.cos(thetaj), fmat='15.12f'))
print('dpxdr:' + gstr(nr.dpxdr, fmat='15.12f', n_per_line=18))
print('dpxdr type:' + fstr(nr.dpxdr.dtype))
print(' ')
C = np.identity(6)
#D = ((0,0), (0,0), (0,0), (0,0), (0,0), (0,0))
D = np.zeros((6,2))
sys = control.ss(A, B, C, D)
w, v = eig(A)
print(gstr((w, v)))
'''
fact = 0.95
base = -2.0
poles = []
for i in range(6):
poles.append(base)
base = base * fact
np_poles = np.array(poles)
#K = scipy.signal.place_poles(A, B, np_poles,
# maxiter=100, method='YT')
K = control.place_varga(A, B, np_poles)
print(gstr({'K': K}))
T = A - B * np.asmatrix(K)
#print(gstr({'K': K.gain_matrix, 'rtol': K.rtol, 'iters': K.nb_iter, 'T': T}))
w, v = eig(T)
print(gstr({'w': w, 'v': v}))
'''
Q = 10.0 * np.identity(6)
R = np.identity(2)
Kr, S, E = control.lqr(A, B, Q, R)
print(gstr({'Kr': Kr, 'S': S, 'E': E}))
T = A - B * Kr
'''
def poses(self, ls, rs):
als = np.asarray(ls)
ars = np.asarray(rs)
#print('als:' + estr(als))
(px0, py0, theta0) = self.pose0s
(bhxl, bhxr, _) = self.bhes[0]
(bhyl, bhyr, _) = self.bhes[1]
(bhol, bhor, _) = self.bhes[2]
(vxw0, vyw0, omega0) = self.twist0s
n = self.n
dt = self.dt
alphaxj = self.alphaxj
alphayj = self.alphayj
alphaoj = self.alphaoj
betaj = self.betaj
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 * (self.betaj[i] - dt) \
+ np.dot(self.betaj[i-1::-1], bmotoroj[1:i+1])
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:])
# gradients
dpxdl = np.zeros((n, n))
dpydl = np.zeros((n, n))
dpxdr = np.zeros((n, n))
dpydr = np.zeros((n, n))
elapsed_times = []
for i in range(1, n):
start = time.time()
for k in range(1, i + 1):
doto = np.dot((-vxsj[k:i + 1] - vycj[k:i + 1]),
betaj[:i + 1 - k])
dotx = np.dot(cosj[k:i + 1], alphaxj[:i + 1 - k])
doty = np.dot(-sinj[k:i + 1], alphayj[:i + 1 - k])
dpxdl[i, k] = dt * (+bhol * doto + bhxl * dotx + bhyl * doty)
dpxdr[i, k] = dt * (+bhor * doto + bhxr * dotx + bhyr * doty)
doto = np.dot((vxcj[k:i + 1] - vysj[k:i + 1]),
betaj[:i + 1 - k])
dotx = np.dot(sinj[k:i + 1], alphaxj[:i + 1 - k])
doty = np.dot(cosj[k:i + 1], alphayj[:i + 1 - k])
dpydl[i, k] = dt * (+bhol * doto + bhxl * dotx + bhyl * doty)
dpydr[i, k] = dt * (+bhor * doto + bhxr * dotx + bhyr * doty)
elapsed_times.append(time.time() - start)
print('elapsed: ' + gstr(elapsed_times))
self.vxj = vxj
self.vyj = vyj
self.omegaj = omegaj
self.pxj = pxj
self.pyj = pyj
self.thetaj = thetaj
self.dpxdl = dpxdl
self.dpydl = dpydl
self.dpxdr = dpxdr
self.dpydr = dpydr
return (pxj, pyj, thetaj, vxj, vyj, omegaj)
dr = .1
dt = 0.05
steps = 3
# prep constants for calculations
lr_model = ((1.0, 1.0, 10.0), (-1.0, 1.0, 10.0), (-1.0, 10.0, 10.0))
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]
alphas = 1.0 - np.array((qhx, qhy, qho))
(alphax, alphay, alphao) = alphas
print('(alphax, alphay, alphao):' + gstr((alphax, alphay, alphao)))
print('bhes:' + gstr(bhes))
eps = .1
lefts = steps * [1.0]
rights = steps * [1.0]
(px1s, py1s, vx1s, vy1s, theta1s) = pes(lefts, rights)
for count in range(6):
lefts = steps * [1.0]
rights = steps * [1.0]
rights[1] += eps
(px2s, py2s, vx2s, vy2s, theta2s) = pes(lefts, rights)
dPX2dR1 = (px2s[1] - px1s[1]) / eps
print(estr({'eps': eps, 'dPX2DR1': dPX2dR1}))
eps /= 10
'rho': rho,
'vxw_n': vyw_n
}))
omega0 = vv['omega']
s0 = vv['v']
# Note that "rho" is always positive, but "kappa" has the same sign as omega.
if vv['kappa'] is None:
sdot = 0.0
odot = vv['direction'] * vv['d_vhat_dt'] / pp.rhohat
else:
sdot = vv['d_vhat_dt'] / (1.0 + pp.rhohat * abs(vv['kappa']))
odot = vv['kappa'] * sdot
s0 = s0 if s0 else 0.001
A = np.array(
((-qx, 0, 0, 0, 0, -omega0 * s0), (omega0, 0, s0, 0, 0,
-qx * s0), (0, 0, -qo, 0, 0, 0),
(1, 0, 0, 0, 0, 0), (0, 1, 0, 0, 0, 0), (0, 0, 1, 0, 0, 0)))
Kr = control.place_varga(A, B, np_poles)
print(gstr({'Kr': Kr}))
# debug
#print(gstr({'eps * Kr': np.squeeze(eps) * np.asarray(Kr)}))
lrs = -Kr * eps
print({'lrs': np.squeeze(lrs)})
corr_left = lrs[0][0]
corr_right = lrs[1][0]
print('all done')
plt.waitforbuttonpress()
eps *= 0.5
for i in range(1, n):
lefts[i] = last_lefts[i] - eps * last_dlefts[i]
rights[i] = last_rights[i] - eps * last_drights[i]
plt.waitforbuttonpress()
print('pxj:' + estr(pxj))
print('pyj:' + estr(pyj))
print('thetaj:' + estr(thetaj))
print('vxj:' + estr(vxj))
print('vyj:' + estr(vyj))
print('omegaj:' + estr(omegaj))
print('lefts:' + estr(lefts))
print('rights:' + estr(rights))
print('dpxdl:' + gstr(nr.dpxdl, fmat='25.22f', n_per_line=18))
print('dpxdr:' + gstr(nr.dpxdr, fmat='25.22f', n_per_line=18))
print('dpydl:' + gstr(nr.dpydl, fmat='25.22f', n_per_line=18))
print('dpydr:' + gstr(nr.dpydr, fmat='25.22f', n_per_line=18))
print('d2pxdldl' + gstr(nr.d2pxdldl, fmat='25.22f', n_per_line=18))
print('d2pxdldr' + gstr(nr.d2pxdldr, fmat='25.22f', n_per_line=18))
print('d2pxdrdr' + gstr(nr.d2pxdrdr, fmat='25.22f', n_per_line=18))
print('d2pydldl' + gstr(nr.d2pydldl, fmat='25.22f', n_per_line=18))
print('d2pydldr' + gstr(nr.d2pydldr, fmat='25.22f', n_per_line=18))
print('d2prdrdr' + gstr(nr.d2pydrdr, fmat='25.22f', n_per_line=18))
print('sin(theta):' + fstr(np.sin(thetaj), fmat='25.22f'))
print('cos(theta):' + fstr(np.cos(thetaj), fmat='25.22f'))
print('vxw:' +
fstr(vxj * np.cos(thetaj) - vyj * np.sin(thetaj), fmat='25.22f'))
print('vyw:' +
fstr(vxj * np.sin(thetaj) + vyj * np.cos(thetaj), fmat='25.22f'))