Animator
Setting up the Simulation Environment
Import necessary condynsate modules. Also import numpy for array management.
[1]:
from condynsate import Simulator as conSim
from condynsate import __assets__ as assets
import numpy as np
Create a instance of condynsate.simulator that does not use the keyboard but does use the animator and the visualizer.
[2]:
simulator = conSim(keyboard = False,
visualization = True,
animation = True)
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7000/static/
Load three large planes into the simulator to represent the ground and walls. Set fixed = True so that they are fixed in space, and set update_vis = False. Because the ground and walls will not move, there is no need to send updates to the visualizer. This saves resources. Further, adjust the position and rotation of the walls so they look good.
[3]:
# Load the ground
ground = simulator.load_urdf(urdf_path = assets['plane_big'],
fixed = True,
update_vis = False)
# Load the walls
left_wall = simulator.load_urdf(urdf_path = assets['plane_big'],
tex_path = assets['concrete_img'], # Choose a default texture for appearance
position = [0., -5., 0.],
roll = -1.5707,
fixed = True,
update_vis = False)
right_wall = simulator.load_urdf(urdf_path = assets['plane_big'],
tex_path = assets['concrete_img'], # Choose a default texture for appearance
position = [0., 5., 0.],
roll = 1.5707,
fixed = True,
update_vis = False)
Load the cart into the simulator. Set fixed = False so that the cart is able to freely move. Set update_vis = True so that its position updates are reflected in the visualizer. Additionally, set the position to [0.0, 0.0, 0.25] so that its wheels rest on the ground plane, and set yaw to 90 degrees so that is moves in the Y plane.
[4]:
cart = simulator.load_urdf(urdf_path = assets['cart'],
position = [0., 0., 0.25],
yaw = 1.5707,
fixed = False,
update_vis = True)
Set the each wheel joint’s and the pendulum arm joint’s friction to some small value.
[5]:
# Set joint damping
joint_names = ('chassis_to_wheel_1',
'chassis_to_wheel_2',
'chassis_to_wheel_3',
'chassis_to_wheel_4',
'chassis_to_arm',)
for joint_name in joint_names:
simulator.set_joint_damping(urdf_obj = cart,
joint_name = joint_name,
damping = 0.01)
Set the initial angle of the pendulum. We denote that this is an initial condition with the initial_cond = True flag and we make the change instantaneously by using the physics = False flag.
[6]:
initial_angle = 0.1745
simulator.set_joint_position(urdf_obj = cart,
joint_name = 'chassis_to_arm',
position = initial_angle, # For a rotational joint, this value is in radians
initial_cond = True,
physics = False)
Setting up the Animator
Because we set animator = True in the simulator initialization, the animator is ready to add new plots to it. To do this we use the condynsate.Simulator.add_plot function.
In the first plot, we want to show the mean wheel torque as a function of time. To do so, we create a simple line plot. This is done with the argument plot_type = 'line'. Further we add a title and axis labels with the title = "Torque vs Time", x_label = "Time [Seconds]", and y_label = "Torque [Nm]", respectively. Next, to format the line itself, we set its line width to 2.5 with line_width = 2.5. Finally, we limit the y-axis view between -0.8 and 0.8 with
y_lim = [-0.80, 0.80] and add a thin horizontal line across y=0 with h_zero_line = True. The return value of condynsate.Simulator.add_plot is the identifier of the plot just added. It will be used to later to tell the animator which plot to update when adding data points.
[7]:
plot_1 = simulator.add_plot(plot_type = 'line',
title = "Torque vs Time",
x_label = "Time [Seconds]",
y_label = "Torque [Nm]",
line_width = 2.5,
y_lim = [-0.80, 0.80],
h_zero_line = True)
Now we want another line plot that shows both the pendulum angle and mean wheel angle simultaneously. Because we want two lines on this plot, we call for 2 artists, one for each line. All other arguments are similar aside for labels = ["Pendulum", "Wheel"] which tells the animator what each arist should be called for the legend. Further, when multiple artists are used color, line_width, and line_style may either be a single argument (applied to all artists) or a list of arguemnts
with length equal to number of artists. When n_artists > 1, the return value of condynsate.Simulator.add_plot is the identifier of the plot just added and a tuple of artist identifiers (one for each artist). These will also be used to later to tell the animator which plot adn artist to update when adding data points.
[8]:
plot_2, plot_2_artists = simulator.add_plot(n_artists = 2,
plot_type = 'line',
title = "Angles vs Time",
x_label = "Time [Seconds]",
y_label = "Angles [Rad]",
color = ["m", "c"], # One color for each artist
line_width = 2.5, # Single value to give all artists same line_width
line_style = "-", # Single value to give all artists same line_style
label = ["Pendulum", "Wheel"], # One label for each artist
h_zero_line = True)
Finally, we would like a bar plot that shows each wheel’s deviation from the mean wheel angle. To do so, we add a new plot of type 'bar' with 4 artists. This time, we also add a thin verticle line along x=0 so we can see if the wheel deviation is leading or lagging the mean. Also, we fix the x axis limits so it is easier to read.
[9]:
plot_3, plot_3_artists = simulator.add_plot(n_artists = 4,
plot_type = 'bar',
x_lim = [-0.4, 0.4],
title = "Wheel Deviation from Mean",
x_label = "Deviation [Rad]",
y_label = "Wheel Number",
color = "b",
label = ["W1", "W2", "W3", "W4"],
v_zero_line = True)
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.
[10]:
%%capture
simulator.start_animator()
As expected, a figure with our three plots opens.
Running the Simulation
Run the simulation in a loop. Begin by resetting the simulation using condynsate.Simulator.reset to reset the simulation to an initial state. Then we create a simulation loop. The loop terminates when the flag condynsate.Simulator.is_done flips to True. In each time step of the simulation loop we do four things:
Take a simulation time step.
If a successful simulation step was taken, read the wheel angles, wheel angular velocities, pendulum angle, and pendulum angular velocity. Successful simulation steps are indicated by
condynsate.Simulator.stephaving aret_codegreater than0.If a successful simulation step was taken, apply torques to wheels based on the mean wheel angle, mean wheel angular velocity, pendulum angle, and pendulum angular velocity.
If a successful simulation step was taken, update the animator.
Let’s start by making a function that reads each angle and angular velocity we want.
[11]:
def read_states(simulator, cart):
# Define which joints to read
joint_names = ('chassis_to_wheel_1',
'chassis_to_wheel_2',
'chassis_to_wheel_3',
'chassis_to_wheel_4',
'chassis_to_arm',)
# Loop over each joint and read the state extracting angle and angular vel.
angles = []
angular_vels = []
for joint_name in joint_names:
state = simulator.get_joint_state(urdf_obj = cart,
joint_name = joint_name)
angles.append(state['position'])
angular_vels.append(state['velocity'])
# For the wheels, return the mean angle and angular vel.
mean_wheel_angle = np.mean(angles[:-1])
mean_wheel_ang_vel = np.mean(angular_vels[:-1])
# Additionally, calculate the wheel deviation from mean.
wheel_deviations = []
for wheel_angle in angles[:-1]:
wheel_deviations.append(wheel_angle - mean_wheel_angle)
return (mean_wheel_angle, mean_wheel_ang_vel), (angles[-1], angular_vels[-1]), wheel_deviations
Next we make a function that applies torque based on the state we read.
[12]:
def apply_torque(simulator, cart, mean_wheel_state, pendulum_state):
# Calculate the torque via magic
K = np.array([[ -2. , -0.1, -10. , -0.1]])
m_e = np.zeros((4,1))
n_e = np.zeros((1,1))
m = np.array([[pendulum_state[1]],
[mean_wheel_state[1]],
[pendulum_state[0]],
[mean_wheel_state[0]]])
torque = (-K@(m - m_e) + n_e).flatten()[0]
torque = np.clip(torque, -0.75, 0.75)
# Define which joints we apply torque to
joint_names = ('chassis_to_wheel_1',
'chassis_to_wheel_2',
'chassis_to_wheel_3',
'chassis_to_wheel_4',)
# Apply the torque we calculated
for joint_name in joint_names:
simulator.set_joint_torque(urdf_obj = cart,
joint_name = joint_name,
torque = torque,
show_arrow = True, # Define arrow params to visualize the torque
arrow_scale = 0.25,
arrow_offset = 0.025)
return torque
Next we want to build a function that updates the first line plot. We want this function to add a single datum to the end of the data list being plotted as a line. To do this, we will use the condynsate.Simulator.add_line_datum function. The arguments to this function are the plot to which the point is added and the (x,y) coordinates of the point. The x coordinate will be simulation time which we extract with condynsate.Simulator.time, and the y coordinate is the torque we calculated in
the previous function. After condynsate.Simulator.add_line_datum is called, the animator automatically updates to add the point at the specified frame rate.
[13]:
def add_torque_point(simulator, plot_id, torque):
simulator.add_line_datum(plot_id = plot_id,
x = simulator.time,
y = torque)
We build a similar function for the wheel and pendulum angles, but this time the plot has 2 artists. Therefore, we need to specify to which artist we are adding the data points. This is done with the artist_id argument.
[14]:
def add_state_points(simulator, plot, plot_artists, mean_wheel_state, pendulum_state):
simulator.add_line_datum(plot_id = plot,
artist_id = plot_artists[0], # Draw to the first artist (whose label is "Pendulum")
x = simulator.time,
y = pendulum_state[0])
simulator.add_line_datum(plot_id = plot,
artist_id = plot_artists[1], # Draw to the second artist (whose label is "Wheel")
x = simulator.time,
y = mean_wheel_state[0])
Now we want a function to edit the bar chart. This is very similar to add_state_points, however we use the function ‘condynsate.Simulator.set_bar_value’ to edit a bar chart’s values.
[15]:
def set_deviation_values(simulator, plot, plot_artists, wheel_deviations):
for plot_artist, wheel_deviation in zip(plot_artists, wheel_deviations):
simulator.set_bar_value(plot_id = plot,
artist_id = plot_artist,
value = wheel_deviation)
Finally, we build our simulation loop. By setting the real_time = True flag and max_time = 10.0 in condynsate.Simulator.step, we tell the simulation to attempt to run in real time for 5 seconds.
[16]:
# Reset before running a simulation loop
simulator.reset()
# Run the simulation loop
while(not simulator.is_done):
# Step the sim
ret_code = simulator.step(real_time = True,
max_time = 10.0)
# If successful step was taken
if ret_code > 0:
# Read the states and apply the torque
mean_wheel_state, pendulum_state, wheel_deviations = read_states(simulator, cart)
torque = apply_torque(simulator, cart, mean_wheel_state, pendulum_state)
# Add the new data points to the animator
add_torque_point(simulator, plot_1, torque)
add_state_points(simulator, plot_2, plot_2_artists, mean_wheel_state, pendulum_state)
set_deviation_values(simulator, plot_3, plot_3_artists, wheel_deviations)
Watch as the pendulum magically balances while the torque and angles are plotted in real time on the animator GUI.
Once the simulation is done, we terminate the animator
[17]:
simulator.terminate_animator()