angles = gaitsequence[:, 1:]
period = np.sum(dts[:-1])
times = np.zeros(len(angles))
times[1:] = np.cumsum(dts[:-1])
is_first = True
# append the last one to the start
N = len(gaitsequence)
t = np.arange(N)
tinterp = np.arange((N - 1) * 100) / 100
angle_for = interp1d(times, angles.T)
with Robot.connect() as r, ui.basic(r) as gui:
r.target_joint_angle = angles[0]
time.sleep(2)
logger = Logger(r)
start_time = time.time()
while True:
t = time.time() - start_time
t_offset = t % period
target = np.radians(angle_for(t_offset))
r.target_joint_angle = target
time.sleep(0.02)
logger.update()
if not gui.open:
break
"""
Demo moving the end link back and forth sinusoidally
"""
import time
import numpy as np
import ui
from robot import Robot
# Uncomment the following for simulation
from robot import SimulatedRobot as Robot
from trajectories import qheart
with Robot.connect() as r:
gui = ui.basic(r)
@gui.with_
def _(ui):
while gui.open:
print('started')
for i in range(qheart.shape[0]):
print(i)
r.servo_angle = qheart[i]
print(i)
time.sleep(0.1)