Keyboard Interactivity
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 does not use the animator but does use the keyboard and the visualizer.
[2]:
simulator = conSim(keyboard = True,
visualization = True,
animation = False)
You can open the visualizer by visiting the following URL:
http://127.0.0.1:7034/static/
Load a large plane into the simulator to represent the ground. Set fixed = True so that it is fixed in space, and set update_vis = False. Because the ground will not move, there is no need to send updates to the visualizer. This saves resources.
[3]:
# Load the ground
ground = simulator.load_urdf(urdf_path = assets['plane_big'],
fixed = True,
update_vis = False)
Load the gyro into the simulator. Set fixed = True so that the base link of the gyro is fixed in space. Set update_vis = True so that its position updates are reflected in the visualizer. Additionally, set the position to [0.0, 0.0, 0.05] so that the base of the pendulum rests on the ground plane.
[4]:
# Load the gyro
gyro = simulator.load_urdf(urdf_path = assets['gyro'],
fixed = True,
update_vis = True,
position = [0.,0.,0.05])
Set the "base_to_outer", "outer_to_r2", and "r2_to_r1" joint frictions to some small value. The "base_to_outer" joint is the rotational joint between the base of the pendulum and the outer gimbal. The "outer_to_r2" joint is the rotational joint between the outer gimbal and the second gimbal of the gyro. The "r2_to_r1" joint is the rotational joint between the second gimbal and the inner gimbal of the gyro.
[5]:
# Set joint damping
simulator.set_joint_damping(urdf_obj = gyro,
joint_name = 'base_to_outer',
damping = 0.025)
simulator.set_joint_damping(urdf_obj = gyro,
joint_name = 'outer_to_r2',
damping = 0.025)
simulator.set_joint_damping(urdf_obj = gyro,
joint_name = 'r2_to_r1',
damping = 0.025)
Set the initial speed of the wheel of the gyro to 10 rad/sec. 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. Further, we would like to color the wheel based on its speed. We do this by passing three additional arguments:
color = True: tells the simulator to color the child link of the joint (in this case the child link of"'r1_to_core'"is the wheel) based on a minimum speed, maximum speed, and current speed. At minimum speed, the child link will be blue, at maximum speed, it will be red, and half way between the minimum and maximum it will be white.min_vel = wheel_min_vel: tells the simulator what the minimum allowed velocity is. If thevelocityargument is less thanmin_vel,velocitywill be clipped.max_vel = wheel_max_vel: tells the simulator what the minimum allowed velocity is. If thevelocityargument is greater thanmax_vel,velocitywill be clipped.
[6]:
wheel_min_vel = -10.0
wheel_max_vel = 10.0
simulator.set_joint_velocity(urdf_obj = gyro,
joint_name = 'r1_to_core',
velocity = 5.0,
initial_cond = True,
physics = False,
color = True,
min_vel = wheel_min_vel,
max_vel = wheel_max_vel)
Enabling Keyboard Interactivity
In this simulation, we want the user to be able to apply torques to each gimbal joint by using keypresses. To enable this, we will create a function that listens for keypresses, converts them to gimbal joint torque commands, and returns these values for the simulator to apply.
[7]:
def read_torque_keypresses(simulator):
# Determine what torques to apply based on key presses
tau0 = 0.0 # Torque applied to outer gimbal
tau2 = 0.0 # Torque applied to second gimbal
tau1 = 0.0 # Torque applied to inner gimbal
if simulator.is_pressed('q'): tau0 -= 0.1
if simulator.is_pressed('e'): tau0 += 0.1
if simulator.is_pressed('w'): tau1 -= 0.1
if simulator.is_pressed('s'): tau1 += 0.1
if simulator.is_pressed('a'): tau2 -= 0.1
if simulator.is_pressed('d'): tau2 += 0.1
return (tau0, tau1, tau2)
Next we create a function that applies these torques. This is done with the condynsate.Simulator.set_joint_torque function. Further, we would like to visualize these torque inputs. We do this by setting the show_arrow = True flag and adjusting the arrow_offset and arrow_scale until the drawn arrows look like we want.
[8]:
def apply_torques(simulator, gyro, torques):
simulator.set_joint_torque(urdf_obj = gyro,
joint_name = "base_to_outer",
torque = torques[0],
show_arrow = True,
arrow_offset = -1.3,
arrow_scale = 8.)
simulator.set_joint_torque(urdf_obj = gyro,
joint_name = "outer_to_r2",
torque = torques[1],
show_arrow = True,
arrow_offset = 1.1,
arrow_scale = 8.)
simulator.set_joint_torque(urdf_obj = gyro,
joint_name = "r2_to_r1",
torque = torques[2],
show_arrow = True,
arrow_offset = -0.85,
arrow_scale = 8.)
Finally, we would also like to allow the user the adjust the wheel speed based on keyboard inputs as well. We do this in a similar way. First we listen for keypresses and convert them to a wheel speed iteration value:
[9]:
def read_wheel_vel_keypresses(simulator):
iter_val = 0.0
if simulator.is_pressed('f'): iter_val -= 0.1
if simulator.is_pressed('r'): iter_val += 0.1
return iter_val
Then we apply the wheel speed iteration by reading the current wheel speed and then iterating its value, remembering to upate the color of the wheel as well.
[10]:
def apply_wheel_vel(simulator, gyro, iter_val, wheel_min_vel, wheel_max_vel):
wheel_state = simulator.get_joint_state(urdf_obj = gyro,
joint_name = 'r1_to_core')
wheel_vel = wheel_state['velocity']
simulator.set_joint_velocity(urdf_obj = gyro,
joint_name = 'r1_to_core',
velocity = wheel_vel + iter_val,
physics = False,
color = True,
min_vel = wheel_min_vel,
max_vel = wheel_max_vel)
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 use condynsate.Simulator.await_keypress to suspend further code execution until the user presses the desired key. Finally 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 three things:
Take a simulation time step.
Apply torques to the
"base_to_outer","outer_to_r2", and"r2_to_r1"joint based on keyboard inputs if a successful simulation step is taken. Successful simulation steps are indicated bycondynsate.Simulator.stephaving aret_codegreater than0.Apply wheel velocity iterations to
"r1_to_core"based on keyboard inputs if a successful simulation step is taken.
By setting the real_time = True flag and max_time = None in condynsate.Simulator.step, we tell the simulation to attempt to run in real time until the user presses the esc key.
[11]:
# 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,
max_time = None) # Max time set to None means the sim runs until user presses ESC.
# If successful step was taken
if ret_code > 0:
# Apply torques based on key presses
torques = read_torque_keypresses(simulator)
apply_torques(simulator, gyro, torques)
# Iterate wheel speed based on keypresses
iter_val = read_wheel_vel_keypresses(simulator)
apply_wheel_vel(simulator, gyro, iter_val, wheel_min_vel, wheel_max_vel)
PRESS ENTER TO START SIMULATION.
PRESS ESC TO QUIT.
PRESS SPACE TO PAUSE/RESUME SIMULATION.
PRESS BACKSPACE TO RESET SIMULATION.
CONTINUING...
QUITTING...
Termination command detected. Terminating keyboard listener. Goodbye