Wheel Project
Setting up the Simulation Environment
Import necessary condynsate modules.
[1]:
from condynsate import Simulator as conSim
from condynsate import __assets__ as assets
Create a instance of condynsate.simulator that uses the keyboard, visualizer, and animator.
[2]:
simulator = conSim(keyboard = True,
visualization = True,
animation = True)
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7003/static/
Load the wheel .urdf file from condynsate’s default assests.
[3]:
# Load the wheel
wheel = simulator.load_urdf(urdf_path = assets['wheel'],
position = [0., 0., 0.],
fixed = True, # Make the base of the wheel fixed in space
update_vis = True)
Add some friction to the wheel’s axle.
[4]:
# Set joint damping
simulator.set_joint_damping(urdf_obj = wheel,
joint_name = "ground_to_axle",
damping = 0.01)
Setting up the Animator
Here we will add a set of plots the the animator to visualizer the error between a target wheel angle and the measured wheel angle as well as a set of control gains.
[5]:
# plot_1 is a line plot that shows the wheel angle against the target angle
plot_1, plot_1_artists = simulator.add_plot(n_artists = 2,
plot_type = 'line',
y_lim = [-0.6283, 6.9115],
title = "Angles vs Time",
x_label = "Time [Seconds]",
y_label = "Angles [Rad]",
color = ["k", "m"],
line_width = [2.5, 1.5],
line_style = ["-", "--"],
label = ["Measured", "Target"],
h_zero_line = True)
[6]:
# plot_2 is a bar chart that shows the current value of the control gains
plot_2, plot_2_artists = simulator.add_plot(n_artists = 2,
plot_type = 'bar',
x_lim = [0, 5.0],
title = "Control Gains",
x_label = "Set Value",
color = ["r", "b"],
label = ["Proportional", "Derivative"])
Here we make a tuple of the plots and artists for easy accessing later.
[7]:
plots = (plot_1, plot_2)
artists = (plot_1_artists, plot_2_artists)
The plots are now set up how we want, so we can open the animator GUI. Once opened, no additional plots can be added. Note: %%capture is required to supress the animator drawing the first frame to this notebook. Though it is not required, it is reccomended. It must be the first line in the cell in which it’s called.
[8]:
%%capture
simulator.start_animator()
Setting up the Simulation Loop
Below we will make a set of functions that will be used in the simulation loop.
[9]:
def detect_keypresses(simulator):
"""
Listens for keyboard presses and returns iteration values for
the target angle, proportional control gain, and derivative
control gain. Target angle is iterated up (increased) if 'e'
is pressed and iterated down (decreased) if 'q' is pressed.
Similarly for the proportional gain and 'r' and 'f'.
Similarly for the derivative gain and 't' and 'g'.
Parameters
----------
simulator: condynsate.Simulator
A member of the condynsate.Simulator class that is used
to listen for key presses.
Returns
-------
target_iter_val: float
The value by which the target angle should be iterated
during the current simulation step.
P_iter_val: float
The value by which the proportional gain should be iterated
during the current simulation step.
D_iter_val: float
The value by which the derivative gain should be iterated
during the current simulation step.
"""
# Target angle iteration
target_iter_val = 0.0
if simulator.is_pressed('q'): target_iter_val -= 0.02
if simulator.is_pressed('e'): target_iter_val += 0.02
# Proportional gain iteration
P_iter_val = 0.0
if simulator.is_pressed('f'): P_iter_val -= 0.02
if simulator.is_pressed('r'): P_iter_val += 0.02
# Derivative gain iteration
D_iter_val = 0.0
if simulator.is_pressed('g'): D_iter_val -= 0.02
if simulator.is_pressed('t'): D_iter_val += 0.02
return (target_iter_val, P_iter_val, D_iter_val)
[10]:
def get_target_error(simulator, wheel, target_angle):
"""
Calculates the current error between the current wheel
angle and the target wheel angle as well as the error
between the current wheel velocity and the target wheel
velocity (which is always 0.0).
Parameters
----------
simulator: condynsate.Simulator
The member of the condynsate.Simulator class that is
running the simulation
wheel: condynsate.core.simulator.URDF_Obj
The wheel URDF object whose state is measured.
target_angle: float
The target wheel angle in radians.
Returns
-------
angle_error: float
The error between the current wheel angle and the
target wheel angle in radians.
velocity_error: float
The error between the current wheel angular speed
and the target wheel angular speed (0.0 rad/s) in
radians per second.
"""
# Extract the current wheel speed and angle
wheel_state = simulator.get_joint_state(urdf_obj = wheel,
joint_name = "ground_to_axle")
wheel_angle = wheel_state['position']
wheel_velocity = wheel_state['velocity']
# Calculate the errors
angle_error = target_angle - wheel_angle
velocity_error = 0.0 - wheel_velocity
return (angle_error, velocity_error)
[11]:
def set_target_angle(simulator, wheel, error):
"""
Adjusts the "wheel_to_target" joint angle of the wheel
URDF object so it reflects the current target wheel angle.
Parameters
----------
simulator: condynsate.Simulator
The member of the condynsate.Simulator class that is
running the simulation
wheel: condynsate.core.simulator.URDF_Obj
The wheel URDF object whose joint angle is set.
error: tuple of floats
A tuple of floats with the form (angle_error, velocity_error)
where angle_error is the error between the current wheel angle
and the target wheel angle in radians and velocity_error is
the error between the current wheel angular speed
and the target wheel angular speed (0.0 rad/s) in
radians per second (as calculated by get_target_error).
Returns
-------
None.
"""
simulator.set_joint_position(urdf_obj = wheel,
joint_name = "wheel_to_target",
position = error[0])
[12]:
def set_torque(simulator, wheel, error, gains):
"""
Sets the torque applied to the wheel URDF object through
its axle based on the current control gains and the error
in the wheel's angle and angular speed.
Parameters
----------
simulator: condynsate.Simulator
The member of the condynsate.Simulator class that is
running the simulation
wheel: condynsate.core.simulator.URDF_Obj
The wheel URDF object whose torque is set.
error: tuple of floats
A tuple of floats with the form (angle_error, velocity_error)
where angle_error is the error between the current wheel angle
and the target wheel angle in radians and velocity_error is
the error between the current wheel angular speed
and the target wheel angular speed (0.0 rad/s) in
radians per second (as calculated by get_target_error).
gains: tuple of floats
A tuple of floats with the form (proportional gain, derivative gain).
Returns
-------
None.
"""
# Calculate the applied torque based on PD control
torque = gains[0] * error[0] + gains[1] * error[1]
simulator.set_joint_torque(urdf_obj = wheel,
joint_name = "ground_to_axle",
torque = torque)
[13]:
def update_angle_plot(simulator, plot, artists, target_angle, error):
"""
Updates the angle plot to reflect the current wheel angle and the
target wheel angle.
Parameters
----------
simulator: condynsate.Simulator
The member of the condynsate.Simulator class that hosts the
plot
plot: int
The plot descriptor of the line plot being updated.
artists: tuple of ints
A tuple of the two arist descriptors for each line in
the plot. Has the form (wheel angle artist, target angle artist).
target_angle: float
The target wheel angle in radians.
error: tuple of floats
A tuple of floats with the form (angle_error, velocity_error)
where angle_error is the error between the current wheel angle
and the target wheel angle in radians and velocity_error is
the error between the current wheel angular speed
and the target wheel angular speed (0.0 rad/s) in
radians per second (as calculated by get_target_error).
Returns
-------
None.
"""
time = simulator.time
wheel_angle = target_angle-error[0]
simulator.add_line_datum(plot, time, wheel_angle, artist_id=artists[0])
simulator.add_line_datum(plot, time, target_angle, artist_id=artists[1])
[14]:
def update_gain_plot(simulator, plot, artists, gains):
"""
Updates the control gain plot to reflect the current control gains
Parameters
----------
simulator: condynsate.Simulator
The member of the condynsate.Simulator class that hosts the
plot
plot: int
The plot descriptor of the bar plot being updated.
artists: tuple of ints
A tuple of the two arist descriptors for each bar in
the plot. Has the form (proportional artist, derivative artist).
gains: tuple of floats
A tuple of floats with the form (proportional gain, derivative gain).
Returns
-------
None.
"""
for a,g in zip(artists, gains):
simulator.set_bar_value(plot, g, artist_id=a)
[15]:
def step(simulator, plots, artists, target_angle, gains):
"""
This function is called each simulation step. It does five things:
1. Iterates the target angle and control gains based on key presses
2. Calculates the error between the current wheel angle and the
target wheel angle as well as the error between the current wheel
angular speed and the target wheel angular speed and sets torque
accordingly.
3. Updates the target angle indicator arrow to reflect the current
target angle.
4. Updates both the wheel angle line plot and the control gain
bar plot
5. Returns the iterated target wheel angle and the iterated control gains.
Parameters
----------
simulator: condynsate.Simulator
The member of the condynsate.Simulator class that is
running the simulation.
plots: tuple of ints
A tuple of plot descriptors of the form
(wheel angle plot, control gain plot).
artists: tuple of tuple of ints
A tuple of the artist descriptor tuples for each plot.
Has the form ((wheel angle artist, target angle artist),
(proportional gain artist, derivative gain artist)).
target_angle: float
The target wheel angle in radians.
gains: tuple of floats
A tuple of floats with the form (proportional gain, derivative gain).
Returns
-------
target_angle: float
The iterated target wheel angle in radians.
gains: tuple of floats
A tuple the iterated control gains with the form
(proportional gain, derivative gain).
"""
# Iterate the target angle and control gains based on key presses
target_iter_val, P_iter_val, D_iter_val = detect_keypresses(simulator)
target_angle = max(min(target_angle + target_iter_val, 6.2832), 0.0) # Clip to [0.0, 2pi]
gains[0] = max(min(gains[0] + P_iter_val, 5.0), 0.0) # Clip to [0.0, 5.0]
gains[1] = max(min(gains[1] + D_iter_val, 5.0), 0.0) # Clip to [0.0, 5.0]
# Set the torque via PD control
error = get_target_error(simulator, wheel, target_angle)
set_torque(simulator, wheel, error, gains)
# Update the target angle indicator arrow
set_target_angle(simulator, wheel, error)
# Update both plots
update_angle_plot(simulator, plots[0], artists[0], target_angle, error)
update_gain_plot(simulator, plots[1], artists[1], gains)
# Return the iterated target angle and control gains
return target_angle, gains
Running the Simulation Loop
Now that all the helper functions are defined, we can run the simulation loop. This simulation will start running when the user presses the ‘enter’ and continue running until the user presses the ‘esc’ key. During the simulation, the user can set the target angle of the wheel (indicated by the orange arrow) with the ‘q’ and ‘e’ keys. A PD controll will then apply torque to the wheel axle to attempt to enforce this angle based on a set of proportional and derivatve control gains. The proportional control gains can be adjusted by the user with the ‘f’ and ‘r’ keys and the derivative control gains can be adjusted with the ‘g’ and ‘t’ keys. The animator plots the target and current wheel angle is plotted as a function of time as well as the current values of the control gains.
[16]:
# Set the initial gains and target angle
target_angle = 0.0
gains = [0.0, 0.0]
# Reset before running a simulation loop
simulator.reset()
# Suspend execution until user presses enter key
simulator.await_keypress(key = 'enter')
# Run the simulation loop
while(not simulator.is_done):
# Step the sim
ret_code = simulator.step(real_time = True)
# If successful step was taken
if ret_code > 0:
target_angle, gains = step(simulator, plots, artists, target_angle, gains)
PRESS ENTER TO START SIMULATION.
PRESS ESC TO QUIT.
PRESS SPACE TO PAUSE/RESUME SIMULATION.
PRESS BACKSPACE TO RESET SIMULATION.
CONTINUING...
QUITTING...
Once the simulation is done, we terminate the animator.
[17]:
simulator.terminate_animator()
Termination command detected. Terminating keyboard listener. Goodbye