print('New target orientation: ', rand_orient)
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
# get arm feedback
feedback = interface.get_feedback()
hand_xyz = robot_config.Tx('EE', feedback['q'])
# set our target to our ee_xyz since we are only focussing on orinetation
interface.set_mocap_xyz('target_orientation', hand_xyz)
interface.set_mocap_orientation('target_orientation', rand_orient)
target = np.hstack([
interface.get_mocap_xyz('target_orientation'),
transformations.euler_from_quaternion(
interface.get_mocap_orientation('target_orientation'), 'rxyz')
])
rc_matrix = robot_config.R('EE', feedback['q'])
rc_angles = transformations.euler_from_matrix(rc_matrix, axes='rxyz')
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target=target,
)
# apply the control signal, step the sim forward
interface.send_forces(u)
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target=np.hstack((target, np.zeros(3))),
#target_vel=np.hstack((target_vel, np.zeros(3))),
ref_frame=link_name)
# apply the control signal, step the sim forward
interface.send_forces(u)
if count % 500 == 0:
print('ee_xyz: ', hand_xyz)
print('target_xyz', target_xyz)
ee_track.append(np.copy(hand_xyz))
target_track.append(interface.get_mocap_xyz(link_name))
count += 1
time_elapsed += timeit.default_timer() - start
finally:
interface.disconnect()
ee_track = np.array(ee_track)
target_track = np.array(target_track)
if show_plot:
plt.figure()
plt.subplot(1, 2, 1, projection='3d')
plt.plot(ee_track[:, 0], ee_track[:, 1], ee_track[:, 2])
plt.plot(target_track[:, 0],
# make the target offset from that start position
target_xyz = start + np.array([0.2, -0.2, -0.3])
interface.set_mocap_xyz(name='target', xyz=target_xyz)
count = 0.0
print('\nSimulation starting...\n')
while count < 1500:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
# get joint angle and velocity feedback
feedback = interface.get_feedback()
target = np.hstack([
interface.get_mocap_xyz('target'),
transformations.euler_from_quaternion(
interface.get_mocap_orientation('target'), 'rxyz')
])
# calculate the control signal
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target=target,
)
# send forces into Mujoco, step the sim forward
interface.send_forces(u)
# calculate end-effector position
