def __init__(self, limb, parameters, rate=100.0): self._param = parameters self._ns = '/sdk/robot/limb/' + limb + '/follow_joint_trajectory' self._server = actionlib.SimpleActionServer( self._ns, FollowJointTrajectoryAction, execute_cb=self._on_trajectory_action, auto_start=False) self._action_name = rospy.get_name() self._server.start() self._limb = baxter_interface.Limb(limb) # Controller parameters from arguments and dynamic reconfigure self._control_rate = rate # Hz self._control_joints = [] self._pid_gains = {'kp': {}, 'ki': {}, 'kd': {}} self._goal_time = 0.0 self._goal_error = {} self._error_threshold = {} self._dflt_vel = {} # Create our PID controllers self._pid = {} for joint in self._limb.joint_names(): self._pid[joint] = control.PID() # Set joint state publishing to specified control rate self._pub_rate = rospy.Publisher('/robot/joint_state_publish_rate', UInt16) self._pub_rate.publish(self._control_rate)
def runner(pid_params): process = MoveParticleProcess(particle=ctrl.Particle(x0=[0], v0=[0], inv_mass=1.), pid=ctrl.PID(**pid_params)) result = process.loop(tsim=50, dt=0.2) e = np.sum(np.square(result['e'])) return e
def turn(turn_deg, side, ir_values): global robot_dir, robot_pos, state, verified_grids, wheel_distance, x_gain, y_gain, ir_grid, ir_enabled st.set_speed(AUTO_MODE, 0) adjust_to_walls(ir_values) if ir_enabled: for i in range(4): while has_new_ir.value == 0: pass map_ir_neighbors(ir_grid, robot_pos, robot_dir, ir_values, tile_wall_count, has_new_ir) # Set turn direction if side == 'left' or side == LEFT: st.left() robot_dir = (robot_dir - (turn_deg//90)) % 4 elif side == 'right' or side == RIGHT: st.right() robot_dir = (robot_dir + (turn_deg//90)) % 4 else: return # Init turn regulator turn_pid = control.PID(turn_deg, 0.6, 0, 0) turn_pid.set_output_limits(70, 180) start_angle = gyro_value.value # Keep turning until the robot has reached 'turn_deg' angle while abs(gyro_value.value - start_angle) < turn_deg: turn_speed = turn_pid.compute(float(abs(gyro_value.value - start_angle))) st.set_speed(AUTO_MODE, turn_speed) # Break reset speed and driving direction st.set_speed(AUTO_MODE, 0) st.forward() st.steering_port.flushInput() time.sleep(0.1) wheel_distance = 0 if travel_commands: state = TRAVEL else: state = WALL
def run(): # Various PID controller parameters to run simulation with pid_params = [ dict(kp=0.1, ki=0., kd=0.), dict(kp=1.5, ki=0., kd=0.5), ] # Additionally tune PID parameters params = ctrl.tune_twiddle(params=dict(kp=0., ki=0., kd=0.), costfunction=runner, eps=0.001) pid_params.append(params) # Run simulation for each set of PID params handles = [] for idx, c in enumerate(pid_params): process = MoveParticleProcess(particle=ctrl.Particle(x0=[0], v0=[0], inv_mass=1.), pid=ctrl.PID(**c)) result = process.loop(tsim=100, dt=0.1) if idx == 0: fh, = plt.step(result['t'], result['y'], label='target') handles.append(fh) xh, = plt.plot(result['t'], result['x'], label='pid kp {:.2f} kd {:.2f} ki {:.2f}'.format( c['kp'], c['kd'], c['ki'])) handles.append(xh) plt.title('Particle trajectory') plt.legend(handles=handles, loc=1) plt.xlabel('Time $sec$') plt.ylabel('Position $m$') plt.show()
def __init__(self, particle=ctrl.Particle(), pid=ctrl.PID()): super(MoveParticleProcess, self).__init__() self.particle = particle self.pid = pid
def go_tiles(tiles, ir_values, shared_pos, ir_grid, enable_pid=True): """ Drives the robot n tiles forward. It uses two pid regulators. One for controlling the speed when there are no walls around and one for keeping a distance from walls. """ global robot_pos, robot_dir, wheel_distance, test_pos, tile_wall_count, update_ir_map, first_laser_read, ir_enabled prev_pos = int_robot_pos(robot_pos) st.set_speed(AUTO_MODE, 0) st.forward() adjust_to_walls(ir_values) compare_axis = None sign = 0 if robot_dir == NORTH: compare_axis = 1 sign = -1 elif robot_dir == EAST: compare_axis = 0 sign = 1 elif robot_dir == SOUTH: compare_axis = 1 sign = 1 elif robot_dir == WEST: compare_axis = 0 sign = -1 start = robot_pos[compare_axis] dest = 0 if sign == 1: dest = math.ceil(robot_pos[compare_axis]) + (sign * 0.5) elif sign == -1: dest = int(round(robot_pos[compare_axis],1)) + (sign * 0.5) to_drive = abs(round(dest - start, 1)) + (tiles - 1) # Distance to drive in tiles wheel_distance = 0 speed_pid = control.PID(to_drive * 40, 0.6, 0, 0) speed_pid.set_output_limits(st.speeds[st.DEFAULT], 125) print('Attempting to move %f from pos %s %f' % (to_drive, str(robot_pos), dest)) pid_side = None current_ir_values = [25 for i in range(6)] prev_ir_values = [25 for i in range(6)] # Clear distance inputs and set default speed st.steering_port.flushInput() st.set_speed(AUTO_MODE, st.speeds[st.DEFAULT]) # Move until correct number of tiles are traversed while wheel_distance < to_drive * 40: get_distance() # Update map when centered if has_new_ir.value == 1: with ir_values.get_lock(): current_ir_values = [ir for ir in ir_values] with has_new_ir.get_lock(): has_new_ir.value = 0 else: continue if check_centered() and ir_enabled: map_ir_neighbors(ir_grid, robot_pos, robot_dir, current_ir_values, tile_wall_count, has_new_ir) update_ir_map = True update_final_ir(grid, ir_grid) update_path(robot_pos, shared_pos) # Break if the robot is about to drive into a wall if current_ir_values[IR_ID[FORWARD]] < 8: st.set_speed(AUTO_MODE, 0) _x, _y = GRID_INDEX_VALUES[robot_dir][0] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, WALL_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,100,0] st.backward() while has_new_ir.value == 0: time.sleep(0.1) # Back while ir_values[IR_ID[FORWARD]] < 5: st.set_speed(AUTO_MODE, 60) time.sleep(0.03) st.set_speed(AUTO_MODE, 0) while has_new_ir.value == 0: time.sleep(0.01) has_new_ir.value = 0 st.set_speed(AUTO_MODE, 0) st.steering_port.flushInput() st.forward() break # Enable pid if the robot is driving close to a wall pid_side = detect_walls(current_ir_values, pid_side) if enable_pid and pid_side != None: #st.set_speed(AUTO_MODE, st.speeds[st.DEFAULT]) if pid_side == LEFT: pid.reversed = True if abs(current_ir_values[IR_ID[FORWARD_LEFT]] - prev_ir_values[IR_ID[FORWARD_LEFT]]) > 4: prev_ir_values = [ir for ir in current_ir_values] continue elif pid_side == RIGHT: pid.reversed = False if abs(current_ir_values[IR_ID[FORWARD_RIGHT]] - prev_ir_values[IR_ID[FORWARD_RIGHT]]) > 4: prev_ir_values = [ir for ir in current_ir_values] continue pid.enabled = True turn_factor = pid.compute(current_ir_values) if st.speeds[st.LEFT] != abs(int(st.speeds[st.DEFAULT] - turn_factor)): st.set_speed(AUTO_MODE, abs(st.speeds[st.DEFAULT] - turn_factor), "left") if st.speeds[st.RIGHT] != abs(int(st.speeds[st.DEFAULT] + turn_factor)): st.set_speed(AUTO_MODE, abs(st.speeds[st.DEFAULT] + turn_factor), "right") # No walls nearby. Use pid to determine forward speed else: pid.enabled = False speed = speed_pid.compute(float(wheel_distance)) st.set_speed(AUTO_MODE, speed) # Update driven distance get_distance() prev_ir_values = [ir for ir in current_ir_values] pid.enabled = False if ir_values[IR_ID[FORWARD]] < EMPTY_CHECK and ir_values[IR_ID[FORWARD]] > 12: st.set_speed(AUTO_MODE, 80) _x, _y = GRID_INDEX_VALUES[robot_dir][0] set_grid_value_2d(int(robot_pos[0])+_x, int(robot_pos[1])+_y, WALL_TILE, ir_grid) tile_wall_count[(int(robot_pos[0]) + _x, int(robot_pos[1]) + _y)] = [100,100,0] while ir_values[IR_ID[FORWARD]] > 12: pass # The robot has reached its target, set speed to 0 st.set_speed(AUTO_MODE, 0)
travel_commands = [] tile_wall_count = {} state = MANUAL wheel_distance = 0 left_count = 0 right_count = 0 x_gain = 0 y_gain = 0 test_pos = [15.5, 15.5] # Init pid regulator for keeping a distance from walls pid = control.PID(10, 10, 0, 5) pid.set_output_limits(-st.speeds[st.DEFAULT], st.speeds[st.DEFAULT]) def adjust_to_walls(ir_values): wall_side = detect_walls(ir_values) if wall_side == LEFT: while ir_values[IR_ID[FORWARD_LEFT]] - ir_values[IR_ID[BACKWARD_LEFT]] > 0: st.left() st.set_speed(AUTO_MODE, 80) time.sleep(0.03) st.set_speed(AUTO_MODE, 0) while has_new_ir.value == 0: time.sleep(0.01) has_new_ir.value = 0
def run(): # Various PID controller parameters to run simulation with pid_params = [dict(kp=0.,ki=0.,kd=0.)] #Running an ACE iteration to get the best set of parameters before we params = ace_tune(costfunction=runner,number_sim=1000, jobname='pidtuner-lghoo') pid_params.append(params) # Run simulation for each set of PID params handles = [] for idx, c in enumerate(pid_params): process = MoveParticleProcess(particle=ctrl.Particle(x0=[0], v0=[0], inv_mass=1.), pid=ctrl.PID(**c)) result = process.loop(tsim=100, dt=0.1) if idx == 0: fh, = plt.step(result['t'], result['y'], label='target') handles.append(fh) xh, = plt.plot(result['t'], result['x'], label='pid kp {:.2f} kd {:.2f} ki {:.2f}'.format(c['kp'], c['kd'], c['ki'])) handles.append(xh) plt.title('Particle trajectory') plt.legend(handles=handles, loc=1) plt.xlabel('Time $sec$') plt.ylabel('Position $m$') plt.show()
print "new start?: " + str(start_q) print "target_xy: " + str(target_xy) print "target_q: " + str(target_q) # setup controller kp = 40 ki = 0 kd = 5 print "kp: " + str(kp) + ", ki: " + str(ki) + ", kd: " + str(kd) start_q = arm.q grav_comp = True alpha = 1.0 controller = control.PID(kp, ki, kd, start_q, target_q, grav_comp, alpha=alpha) trajplanner = planner.Planner(start_q, target_q, alpha=alpha) # start simulator sim_flag = sys.argv[1] iactive = True sim = Simulator(dt=dt, sim_type=sim_flag) sim.run(arm, controller, trajplanner, target_xy, target_q) sim.show()