print('\nSimulation starting...\n')
while 1:
# get joint angle and velocity feedback
feedback = interface.get_feedback()
# calculate the control signal
u = ctrlr.generate(q=feedback['q'], dq=feedback['dq'])
# send forces into VREP
interface.send_forces(u)
# calculate the position of the hand
hand_xyz = robot_config.Tx('EE', q=feedback['q'])
# track end effector position
ee_track.append(hand_xyz)
except:
print(traceback.format_exc())
finally:
# close the connection to the arm
interface.disconnect()
print('Simulation terminated...')
ee_track = np.array(ee_track)
if ee_track.shape[0] > 0:
import matplotlib.pyplot as plt
from abr_control.utils.plotting import plot_3D
plot_3D(ee_track)
plt.show()
count += 1
except:
print(traceback.format_exc())
finally:
# stop and reset the VREP simulation
interface.disconnect()
print('Simulation terminated...')
ee_track = np.array(ee_track)
target_track = np.array(target_track)
if ee_track.shape[0] > 0:
# plot distance from target and 3D trajectory
import matplotlib.pyplot as plt
from abr_control.utils.plotting import plot_3D
plt.figure()
plt.plot(
np.sqrt(
np.sum((np.array(target_track) - np.array(ee_track))**2,
axis=1)))
plt.ylabel('Distance (m)')
plt.xlabel('Time (ms)')
plt.title('Distance to target')
plot_3D(ee_track, target_track)
plt.show()