test_keyboard_twist.py

import time
import stretch_body.robot
from pynput import keyboard

keys = {'up': False, 'down': False, 'left': False, 'right': False, 'esc': False}

def on_press(key):
    if key == keyboard.Key.up:
        keys['up'] = True
    elif key == keyboard.Key.down:
        keys['down'] = True
    elif key == keyboard.Key.left:
        keys['left'] = True
    elif key == keyboard.Key.right:
        keys['right'] = True

def on_release(key):
    if key == keyboard.Key.up:
        keys['up'] = False
    elif key == keyboard.Key.down:
        keys['down'] = False
    elif key == keyboard.Key.left:
        keys['left'] = False
    elif key == keyboard.Key.right:
        keys['right'] = False
    elif key == keyboard.Key.esc:
        keys['esc'] = True

listener = keyboard.Listener(
    on_press=on_press,
    on_release=on_release,
    suppress=True)
listener.start()

r = stretch_body.robot.Robot()
r.startup()
#r.base.params['use_vel_traj'] = 1

apply_translation_vel = 0.12 # 0.25
apply_rotational_vel = 0.7 # 1.0
start = time.time()
zero_cmd_sent = False
while time.time() - start < 30.0 and not keys['esc']:
    translation_vel = apply_translation_vel * int(keys['up']) + -1.0 * apply_translation_vel * int(keys['down'])
    rotational_vel = apply_rotational_vel * int(keys['left']) + -1.0 * apply_rotational_vel * int(keys['right'])
    if translation_vel == 0.0 and rotational_vel == 0.0 and zero_cmd_sent:
        continue
    zero_cmd_sent = False
    print(translation_vel, rotational_vel)
    r.base.set_velocity(translation_vel, rotational_vel)
    r.push_command()
    if translation_vel == 0.0 and rotational_vel == 0.0:
        zero_cmd_sent = True
    time.sleep(0.033)

r.stop()
listener.stop()