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()
