target=target,
)
# 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()
try:
print("\nSimulation starting...")
print("Click to move the obstacle.\n")
count = 0
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx("EE", feedback["q"])
target = np.hstack([target_xyz, target_angles])
# generate an operational space control signal
u = ctrlr.generate(q=feedback["q"], dq=feedback["dq"], target=target,)
# add in obstacle avoidance
obs_xy = interface.get_mousexy()
if obs_xy is not None:
avoid.set_obstacles(obstacles=[[obs_xy[0], obs_xy[1], 0, 0.2]])
# apply the control signal, step the sim forward
interface.send_forces(u, update_display=True if count % 20 == 0 else False)
# change target location once hand is within
# 5mm of the target
if np.sqrt(np.sum((target_xyz - hand_xyz) ** 2)) < 0.005:
target_xyz = np.array(
[np.random.random() * 2 - 1, np.random.random() * 2 + 1, 0]
)
# update the position of the target
interface.set_target(target_xyz)
