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)