# if adaptation is on (toggled with space bar)
if interface.adaptation:
u += adapt.generate(input_signal=feedback["q"],
training_signal=ctrlr.training_signal)
fake_gravity = np.array([[0, -9.81, 0, 0, 0, 0]]).T * 10.0
g = np.zeros((robot_config.N_JOINTS, 1))
for ii in range(robot_config.N_LINKS):
pars = tuple(feedback["q"]) + tuple([0, 0, 0])
g += np.dot(J_links[ii](*pars).T, fake_gravity)
u += g.squeeze()
new_target = interface.get_mousexy()
if new_target is not None:
target_xyz[:2] = new_target
interface.set_target(target_xyz)
# apply the control signal, step the sim forward
interface.send_forces(u,
update_display=True if count %
20 == 0 else False)
count += 1
finally:
# stop and reset the simulation
interface.disconnect()
print("Simulation terminated...")
count = 1
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
error = np.sqrt(np.sum((target_xyz - hand_xyz)**2))
target = np.hstack([target_xyz, target_angles])
# generate an operational space control signal
u = ctrlr.generate(
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
# get the next point in the target trajectory from the dmp
target_xyz[0], target_xyz[1] = dmps.step(error=error * 1e2)[0]
interface.set_target(target_xyz)
# apply the control signal, step the sim forward
interface.send_forces(u, update_display=(count % 20 == 0))
count += 1
finally:
# stop and reset the simulation
interface.disconnect()
print("Simulation terminated...")
