ctrlr.generate(q=zeros, dq=zeros, target_pos=target_xyz)
robot_config.orientation('EE', q=zeros)
print('\nSimulation starting...\n')
print('\nClick to move the obstacle.\n')
while 1:
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
# generate an operational space control signal
u = ctrlr.generate(q=feedback['q'],
dq=feedback['dq'],
target_pos=target_xyz)
# 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, .2]])
u += avoid.generate(q=feedback['q'])
# apply the control signal, step the sim forward
interface.send_forces(u)
# change target location once hand is within
# 5mm of the target
if (np.sqrt(np.sum((target_xyz - hand_xyz)**2)) < .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)
dq=feedback['dq'],
target_pos=target_xyz)
# if adaptation is on (toggled with space bar)
if interface.adaptation:
u += adapt.generate(input_signal=robot_config.scaledown(
'q', feedback['q']),
training_signal=ctrlr.training_signal)
fake_gravity = np.array([[0, -981, 0, 0, 0, 0]]).T
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)
# track data
ee_path.append(np.copy(hand_xyz))
target_path.append(np.copy(target_xyz))
finally:
# stop and reset the simulation
interface.disconnect()
