def velramp(t, velabs, xy0, xyg, tr): d = xyg - xy0 vel = velabs * kguseful.safe_div(d, abs(d)) # avoid zero division stability a = vel / tr tv = kguseful.safe_div((d - tr * vel), vel) if tv > 0: if t <= tr: veldes = t * a xydes = 0.5 * veldes * t + xy0 elif t > tr and t < tr + tv: veldes = vel xydes = 0.5 * vel * tr + (t - tr) * vel + xy0 elif t > tr + tv and t < 2 * tr + tv: veldes = vel - (t - tr - tv) * a xydes = 0.5 * vel * tr + tv * vel + veldes * ( t - tr - tv) + 0.5 * (vel - veldes) * (t - tr - tv) + xy0 else: veldes = 0 xydes = xyg elif tv <= 0: tg = math.sqrt((4 * d) / a) if t <= 0.5 * tg: veldes = a * t xydes = 0.5 * veldes * t + xy0 elif t > 0.5 * tg and t < tg: veldes = 0.5 * tg * a - (t - 0.5 * tg) * a vm2 = 0.5 * tg * a xydes = vm2 * 0.5 * tg - 0.5 * (tg - t) * vm2 + xy0 else: veldes = 0 xydes = xyg return xydes, veldes
def velramp(t, velabs, xy0, xyg, tr,name): print_results = False # set to True if you want to print d = xyg-xy0 vel = velabs*kguseful.safe_div(d, abs(d)) # avoid zero division stability a = vel/tr # Can it reach the user-specified velocity? Shape: ramp or triangle. tv = kguseful.safe_div((d-tr*vel), vel) if tv > 0: # where(timewise) robot is on ramp if t <= tr: ades = a veldes = t*a xydes = 0.5*veldes*t + xy0 if (name == "x" and print_results) : print(name + "-ramp acc is +a") elif t > tr and t < tr+tv: ades = 0 veldes = vel xydes = 0.5*vel*tr + (t-tr)*vel + xy0 if (name == "x" and print_results) : print(name + "-ramp acc is 0") elif t > tr + tv and t < 2*tr + tv: ades = -a veldes = vel-(t-tr-tv)*a xydes = 0.5*vel*tr + tv*vel + veldes*(t-tr-tv) + 0.5*(vel-veldes)*(t-tr-tv) + xy0 if (name == "x" and print_results) : print(name + "-ramp acc is -a") else: ades = 0 veldes = 0 xydes = xyg if (name == "x" and print_results) : print(name + "-ramp. Outside of ramp, velocity and acc is 0!") elif tv <= 0: # calculate tg insteadof tr. Fixes the error when Mallard traverses sidways, y-direction for instance. # Thenk velocity, acceleration and distance is in x-direction is 0. if (a == 0): tg = 0 # tg = math.sqrt((4*d)/0.001) if (name == "x" and print_results) : print (name + "-triangle. Distance and acceleration is 0!") else: tg = math.sqrt((4*d)/a) if (name == "x" and print_results) : print(name + "-triangle. Standard computation") # where(timewise) robot is on triangle ramp if t <= 0.5*tg: ades = a veldes = a*t xydes = 0.5*veldes*t + xy0 if (name == "x" and print_results) : print(name + "-triangle acc is +a") elif t > 0.5*tg and t < tg: ades = -a veldes = 0.5*tg*a - (t - 0.5*tg)*a vm2 = 0.5*tg*a xydes = vm2*0.5*tg - 0.5*(tg-t)*vm2 + xy0 if (name == "x" and print_results) : print(name + "-triangle acc is -a") else: ades = 0 veldes = 0 xydes = xyg if (name == "x" and print_results) : print(name + "-tirnagle. Outside of ramp, velocity and acc is 0!") return xydes, veldes,ades
def despsi_fun(goal, t_gpsi, q0f, t_nowf): if t_nowf < t_gpsi: ratio = kguseful.safe_div(t_nowf, t_gpsi) des = tft.quaternion_slerp(q0f, goal, ratio) else: des = goal return des
def cont_fun(xy, des, vel, veldes, kp, kd, lim): fp = (des - xy) * kp fd = (veldes - vel) * kd f_nav = fp + fd if abs(f_nav) > lim: f_nav = kguseful.safe_div(f_nav, abs(f_nav)) * lim print('xy limit hit') return f_nav
def contpsi_fun(q, des, vel, veldes, kp, kd, lim): err_psi = kguseful.err_psi_fun(q, des) fp = err_psi * kp fd = (veldes - vel) * kd f_nav = fp + fd if abs(f_nav) > lim: f_nav = kguseful.safe_div(f_nav, abs(f_nav)) * lim print('psi limit hit') return f_nav
def callback(data, paramf): global dtv, dxv, dyv, tp, xp, yp, qp, ed global flag_first, flag_goal_met, flag_end, n_safe, n_goals global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array pub = rospy.Publisher('/mallard/cmd_vel', Twist, queue_size=10) twist = Twist() q_now = [data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w] # if it's the first run then zero is current position if flag_first: x0 = data.pose.position.x y0 = data.pose.position.y q0 = q_now x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) # if a goal has been met then increment the goal if flag_goal_met: x0 = goal_array[n_goals, 0] y0 = goal_array[n_goals, 1] q0 = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) n_goals = n_goals + 1 x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) # work out time it will take to get to new goal, xy and psi if flag_first or flag_goal_met: t0 = data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001 dist = math.sqrt(pow((x_goal - x0), 2) + pow((y_goal - y0), 2)) t_goal = kguseful.safe_div(dist, paramf['vel']) # avoid zero division stability ed = kguseful.err_psi_fun(q0, q_goal) t_goal_psi = abs(kguseful.safe_div(ed, paramf['psivel'])) flag_first = False flag_goal_met = False rospy.loginfo("t_goal_psi: %s, t_goal: %s", t_goal_psi, t_goal) rospy.loginfo("x: %s, y: %s", data.pose.position.x, data.pose.position.y) rospy.loginfo("xg: %s, yg: %s", x_goal, y_goal) rospy.loginfo("goal number: %s, end goal: %s", n_goals + 1, goal_array.shape[0]) # build difference history buffers and calculate velocities t_now = (data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001) - t0 # time since start of goal dtv.appendleft(t_now-tp) # time difference vector dxv.appendleft(data.pose.position.x-xp) # x difference vector dyv.appendleft(data.pose.position.y-yp) # y difference vector dpsi = kguseful.err_psi_fun(qp, q_now) dpsiv.appendleft(dpsi) # psi difference vector xvel = kglocal.vel_fun(list(dxv), list(dtv)) # velocity vectors x, y and psi yvel = kglocal.vel_fun(list(dyv), list(dtv)) psivel = kglocal.vel_fun(list(dpsiv), list(dtv)) # get current desired positions and velocities xdes = kglocal.desxy_fun(x_goal, x0, t_goal, t_now) ydes = kglocal.desxy_fun(y_goal, y0, t_goal, t_now) qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, t_now) xveldes = kglocal.desvel_fun(x_goal, x0, t_goal, t_now) yveldes = kglocal.desvel_fun(y_goal, y0, t_goal, t_now) psiveldes = kglocal.desvelpsi_fun(ed, t_goal_psi, t_now, paramf['psivel']) # Get forces in nav frame using PD controller xf_nav = kglocal.cont_fun(data.pose.position.x, xdes, xvel, xveldes, paramf['kp'], paramf['kd'], paramf['lim']) yf_nav = kglocal.cont_fun(data.pose.position.y, ydes, yvel, yveldes, paramf['kp'], paramf['kd'], paramf['lim']) psif_nav = kglocal.contpsi_fun(q_now, qdes, psivel, psiveldes, paramf['kp_psi'], paramf['kd_psi'], paramf['lim_psi']) # put xy forces into body frame f_body = kguseful.quat_rot([xf_nav, yf_nav, 0], [-q_now[0], -q_now[1], -q_now[2], q_now[3]]) # put forces into twist structure and publish twist.linear.x = f_body[0] twist.linear.y = f_body[1] twist.angular.z = -psif_nav # minus is a fix to account for wrong direction on el_mal if n_safe > paramf['nv'] + 5: # stop output while deque buffers are filling pub.publish(twist) # publish twist command n_safe = n_safe + 1 # if goal is met then move to next goal if abs(x_goal - data.pose.position.x) <= paramf['goal_tol']: if abs(y_goal - data.pose.position.y) <= paramf['goal_tol']: e_psi = kguseful.err_psi_fun(q_now, q_goal) if abs(e_psi) <= paramf['goal_tol_psi']: if goal_array.shape[0] != n_goals + 1: # if there are more goals print 'goal met' flag_goal_met = True # set flag to move to next goal if goal_array.shape[0] == n_goals + 1: # if there are more goals print 'final goal met - holding position' # change current to previous values tp = t_now xp = data.pose.position.x yp = data.pose.position.y qp = q_now
def callback(data, paramf): global dtv, dxv, dyv, tp, xp, yp, qp, ed global flag_first, flag_goal_met, flag_end, n_safe, n_goals global x_goal, y_goal, q_goal, t_goal, t_goal_psi, x0, y0, q0, t0, goal_array global flag_obstacle_close, flag_obstacle_prev, fobs, t_stall # THIS CODE ADDED FOR RVIZ COVERAGE SELECTION # ------------------ # if no goal positions exist in goal_array, then exit this callback by using return # no further computation is undertaken if len(goal_array) == 0: return # ------------------ # pub = rospy.Publisher('/mallard/cmd_vel', Twist, queue_size=10) twist = Twist() q_now = [ data.pose.orientation.x, data.pose.orientation.y, data.pose.orientation.z, data.pose.orientation.w ] ##### code added for obstacle avoidance global param_obs, lidar_data if lidar_data is not None: flag_obstacle_close, fobs = detect_obstacle(lidar_data, param_obs) # rospy.loginfo("obstacle avoidance forces: %s", fobs) # rospy.loginfo("obstacle close flag: %s", flag_obstacle_close) else: flag_obstacle_close = False fobs = (0, 0) # Force obstacle avoidance to be null # --------------------------------- # Remove these two lines to use obstacle avoidance # flag_obstacle_close = False fobs = (0, 0) # ---------------------------------- # if it's the first run then zero is current position if flag_first: x0 = data.pose.position.x y0 = data.pose.position.y q0 = q_now x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) # if a obstacle is to close flag is set if flag_obstacle_close != flag_obstacle_prev: if flag_obstacle_close: t_stall = data.header.stamp.secs x0 = data.pose.position.x + 1e-9 y0 = data.pose.position.y + 1e-9 q0 = q_now x_goal = data.pose.position.x y_goal = data.pose.position.y q_goal = q_now print 'STOPPING!' else: t_stall = data.header.stamp.secs - t_stall x0 = data.pose.position.x y0 = data.pose.position.y q0 = q_now x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) print 'RESUMING' # if a goal has been met then increment the goal if flag_goal_met and not flag_obstacle_close: x0 = goal_array[n_goals, 0] y0 = goal_array[n_goals, 1] q0 = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) n_goals = n_goals + 1 x_goal = goal_array[n_goals, 0] y_goal = goal_array[n_goals, 1] q_goal = tft.quaternion_from_euler(0, 0, goal_array[n_goals, 2]) # work out time it will take to get to new goal, xy and psi if flag_first or flag_goal_met: t0 = data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001 dist = math.sqrt(pow((x_goal - x0), 2) + pow((y_goal - y0), 2)) t_goal = kguseful.safe_div( dist, paramf['vel']) # avoid zero division stability ed = kguseful.err_psi_fun(q0, q_goal) t_goal_psi = abs(kguseful.safe_div(ed, paramf['psivel'])) flag_first = False flag_goal_met = False rospy.loginfo("t_goal_psi: %s, t_goal: %s", t_goal_psi, t_goal) rospy.loginfo("x: %s, y: %s", data.pose.position.x, data.pose.position.y) rospy.loginfo("xg: %s, yg: %s", x_goal, y_goal) rospy.loginfo("goal number: %s, end goal: %s", n_goals + 1, goal_array.shape[0]) # build difference history buffers and calculate velocities t_now = (data.header.stamp.secs + data.header.stamp.nsecs * 0.000000001) - t0 # time since start of goal dtv.appendleft(t_now - tp) # time difference vector dxv.appendleft(data.pose.position.x - xp) # x difference vector dyv.appendleft(data.pose.position.y - yp) # y difference vector dpsi = kguseful.err_psi_fun(qp, q_now) dpsiv.appendleft(dpsi) # psi difference vector xvel = kglocal.vel_fun(list(dxv), list(dtv)) # velocity vectors x, y and psi yvel = kglocal.vel_fun(list(dyv), list(dtv)) psivel = kglocal.vel_fun(list(dpsiv), list(dtv)) # get current desired positions and velocities xdes = kglocal.desxy_fun(x_goal, x0, t_goal, t_now) ydes = kglocal.desxy_fun(y_goal, y0, t_goal, t_now) qdes = kglocal.despsi_fun(q_goal, t_goal_psi, q0, t_now) xveldes = kglocal.desvel_fun(x_goal, x0, t_goal, t_now) yveldes = kglocal.desvel_fun(y_goal, y0, t_goal, t_now) psiveldes = kglocal.desvelpsi_fun(ed, t_goal_psi, t_now, paramf['psivel']) # Get forces in nav frame using PD controller xf_nav = kglocal.cont_fun(data.pose.position.x, xdes, xvel, xveldes, paramf['kp'], paramf['kd'], paramf['lim']) yf_nav = kglocal.cont_fun(data.pose.position.y, ydes, yvel, yveldes, paramf['kp'], paramf['kd'], paramf['lim']) psif_nav = kglocal.contpsi_fun(q_now, qdes, psivel, psiveldes, paramf['kp_psi'], paramf['kd_psi'], paramf['lim_psi']) # put xy forces into body frame f_body = kguseful.quat_rot([xf_nav, yf_nav, 0], [-q_now[0], -q_now[1], -q_now[2], q_now[3]]) # print flag_obstacle_close, np.round(fobs,4) # put forces into twist structure and publish twist.linear.x = f_body[0] + fobs[0] twist.linear.y = f_body[1] + fobs[1] twist.angular.z = -psif_nav # minus is a fix to account for wrong direction on el_mal if n_safe > paramf['nv'] + 5: # stop output while deque buffers are filling pub.publish(twist) # publish twist command n_safe = n_safe + 1 # if goal is met then move to next goal if not flag_obstacle_close: if abs(x_goal - data.pose.position.x) <= paramf['goal_tol']: if abs(y_goal - data.pose.position.y) <= paramf['goal_tol']: e_psi = kguseful.err_psi_fun(q_now, q_goal) if abs(e_psi) <= paramf['goal_tol_psi']: if goal_array.shape[ 0] != n_goals + 1: # if there are more goals print 'goal met' flag_goal_met = True # set flag to move to next goal if goal_array.shape[ 0] == n_goals + 1: # if there are more goals print 'final goal met - holding position' # change current to previous values tp = t_now xp = data.pose.position.x yp = data.pose.position.y qp = q_now flag_obstacle_prev = flag_obstacle_close
def vel_fun(dv, dt): dvdt = [kguseful.safe_div(i, j) for i, j in zip(dv, dt)] vel = kguseful.safe_div(sum(dvdt), float(len(dvdt))) return vel
def desvel_fun(goal, zero, t_goalf, t_nowf): if t_nowf < t_goalf: des = kguseful.safe_div((goal - zero), t_goalf) else: des = 0 return des
def desxy_fun(goal, zero, t_goalf, t_nowf): if t_nowf < t_goalf: des = zero + (goal - zero) * (kguseful.safe_div(t_nowf, t_goalf)) else: des = goal return des