# [-q_d[1], q_d[0], 0.0]])
#
# # calculate the difference between q_e and q_d
# # from (Nakanishi et al, 2008). NOTE: the sign of the last term
# # is different from (Yuan, 1998) to account for Euler angles in
# # world coordinates instead of local coordinates.
# # dq = (w_d * [x, y, z] - w * [x_d, y_d, z_d] -
# # [x_d, y_d, z_d]^x * [x, y, z])
# # the sign of quaternion that moves between q_e and q_d
# u_task[3:] = -(q_d[0] * q_e[1:] - q_e[0] * q_d[1:] +
# np.dot(S, q_e[1:]))
#
# ---------------------------------------------------------------------
u_task[3:] *= ko # scale orientation signal by orientation gain
# remove uncontrolled dimensions from u_task
u_task = u_task[ctrlr_dof]
# transform from operational space to torques
# add in velocity and gravity compensation in joint space
u = (np.dot(J.T, np.dot(Mx, u_task)) -
kv * np.dot(M, feedback['dq']) -
robot_config.g(q=feedback['q']))
# apply the control signal, step the sim forward
interface.send_forces(u)
count += 1
finally:
interface.disconnect()
interface.send_forces(u)
# calculate end-effector position
ee_xyz = robot_config.Tx('EE', q=feedback['q'])
# track data
ee_track.append(np.copy(ee_xyz))
target_track.append(np.copy(target[:3]))
count += 1
except:
print(traceback.format_exc())
finally:
# stop and reset the VREP simulation
interface.disconnect()
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 mpl_toolkits.mplot3d import axes3d # pylint: disable=W0611
fig = plt.figure(figsize=(8, 12))
ax1 = fig.add_subplot(211)
ax1.set_ylabel('Distance (m)')
ax1.set_xlabel('Time (ms)')
ax1.set_title('Distance to target')
ax1.plot(
class VREPEnv(object):
def __init__(self, direct=False):
self.robot_config = arm.Config(use_cython=False, hand_attached=True)
# Timestap is fixed and specified in .ttt file
self.dt = 0.005
self.direct = direct
if direct:
self.interface = VREP(robot_config=self.robot_config, dt=self.dt)
self.interface.connect()
else:
self.interface = None
self.requested_reset_q = None
self.requested_target_shadow_q = None
self.close_requested = False
@property
def u_dim(self):
return self.robot_config.N_JOINTS
@property
def x_dim(self):
return self.robot_config.N_JOINTS * 2
def reset_angles(self, q):
if self.direct:
self._reset_angles(q)
else:
self.requested_reset_q = q
def _reset_angles(self, q):
self.interface.send_target_angles(q)
def set_target_shadow(self, q, direct=False):
if self.direct:
self._set_target_shadow(q)
else:
self.requested_target_shadow_q = q
def _set_target_shadow(self, q):
names = [
'joint%i_shadow' % i for i in range(self.robot_config.N_JOINTS)
]
joint_handles = []
for name in names:
self.interface.get_xyz(name) # this loads in the joint handle
joint_handles.append(self.interface.misc_handles[name])
self.interface.send_target_angles(q, joint_handles)
def close(self, direct=False):
if self.direct:
self._close()
else:
self.close_requested = True
def _close(self):
self.interface.disconnect()
def step(self, action):
if self.interface is None:
self.interface = VREP(robot_config=self.robot_config, dt=self.dt)
self.interface.connect()
if self.requested_target_shadow_q is not None:
self._set_target_shadow(self.requested_target_shadow_q)
self.requested_target_shadow_q = None
if self.requested_reset_q is not None:
self._reset_angles(self.requested_reset_q)
self.requested_reset_q = None
# Send torq to VREP
self.interface.send_forces(action)
feedback = self.interface.get_feedback()
# Get joint angle and velocity feedback
q = feedback['q']
dq = feedback['dq']
# Concatenate q and dq
state = np.hstack((q, dq)) # (12,)
reward = 0.0
if self.close_requested:
self._close()
self.close_requested = False
return state, reward
