def run_thread(pr2):
joints = joints_from_names(pr2, PR2_GROUPS['base'])
dx = dy = dz = dth = 0
speed, turn = 0.5, 1.0
settings = termios.tcgetattr(sys.stdin)
try:
print(HELP_MSG)
print_velocities(speed, turn)
while True:
key = get_key(settings) # Waits until a key is read
if key in MOVE_BINDINGS:
dx, dy, dz, dth = MOVE_BINDINGS[key]
elif key in SPEED_BINDINGS:
mspeed, mturn = SPEED_BINDINGS[key]
speed *= mspeed
turn *= mturn
print_velocities(speed, turn)
else: # When it receives another key
dx = dy = dz = dth = 0
if key == ESCAPE:
break
# twist.linear.dz = dz * speed
velocities = [dx * speed, dy * speed, dth * turn]
velocity_control_joints(pr2, joints, velocities)
except Exception as e:
print(e)
finally:
termios.tcsetattr(sys.stdin, termios.TCSADRAIN, settings)
def run_simulate(pr2):
joints = joints_from_names(pr2, PR2_GROUPS['base'])
dx = dy = dz = dth = 1
speed, turn = 0.5, 1.0
while True:
velocities = [dx * speed, dy * speed, dth * turn]
velocity_control_joints(pr2, joints, velocities)
def test_door(door):
door_joints = get_movable_joints(door)
sample_fn = get_sample_fn(door, door_joints)
set_joint_positions(door, door_joints, sample_fn())
while True:
positions = sample_fn()
set_joint_positions(door, door_joints, positions)
wait_if_gui()
lower, upper = get_joint_intervals(door, door_joints)
control_joints(door, door_joints, positions=lower)
velocity_control_joints(door, door_joints,
velocities=[PI / 4]) # Able to exceed limits