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