def send_forces(self, u):
""" Apply the specified torque to the robot joints
Apply the specified torque to the robot joints, move the simulation
one time step forward, and update the position of the hand object.
Parameters
----------
u : np.array
the torques to apply to the robot
"""
# invert because CoppeliaSim torque directions are opposite of expected
u *= -1
for ii, joint_handle in enumerate(self.joint_handles):
# get the current joint torque
_, torque = sim.simxGetJointForce(self.clientID, joint_handle,
self.blocking)
if _ != 0:
raise Exception(
"Error retrieving joint torque, " + "return code ", _)
# if force has changed signs,
# we need to change the target velocity sign
if np.sign(torque) * np.sign(u[ii]) <= 0:
self.joint_target_velocities[
ii] = self.joint_target_velocities[ii] * -1
_ = sim.simxSetJointTargetVelocity(
self.clientID,
joint_handle,
self.joint_target_velocities[ii],
self.set_opmode,
)
# and now modulate the force
_ = sim.simxSetJointForce(
self.clientID,
joint_handle,
abs(u[ii]), # force to apply
self.set_opmode,
)
# Update position of hand object
hand_xyz = self.robot_config.Tx(name="EE", q=self.q)
self.set_xyz("hand", hand_xyz)
# Update orientation of hand object
angles = transformations.euler_from_matrix(self.robot_config.R(
"EE", q=self.q),
axes="rxyz")
self.set_orientation("hand", angles)
# move simulation ahead one time step
sim.simxSynchronousTrigger(self.clientID)
self.count += self.dt
def on_keypress(self, key):
if key == pygame.K_LEFT:
self.theta += np.pi / 10
if key == pygame.K_RIGHT:
self.theta -= np.pi / 10
print('theta: ', self.theta)
# set the target orientation to be the initial EE
# orientation rotated by theta
R_theta = np.array([
[np.cos(interface.theta), -np.sin(interface.theta), 0],
[np.sin(interface.theta), np.cos(interface.theta), 0],
[0, 0, 1]])
R_target = np.dot(R_theta, R)
self.target_angles = transformations.euler_from_matrix(
R_target, axes='sxyz')
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target=target,
#target_vel=np.hstack([vel, np.zeros(3)])
)
# apply the control signal, step the sim forward
interface.send_forces(u)
# track data
ee_track.append(np.copy(hand_xyz))
ee_angles_track.append(
transformations.euler_from_matrix(robot_config.R(
'EE', feedback['q']),
axes='rxyz'))
target_track.append(np.copy(target[:3]))
target_angles_track.append(np.copy(target[3:]))
count += 1
finally:
# stop and reset the simulation
interface.disconnect()
print('Simulation terminated...')
ee_track = np.array(ee_track).T
ee_angles_track = np.array(ee_angles_track).T
target_track = np.array(target_track).T
target_angles_track = np.array(target_angles_track).T
count = 0
while 1:
if interface.viewer.exit:
glfw.destroy_window(interface.viewer.window)
break
if count % n_timesteps == 0:
feedback = interface.get_feedback()
target_xyz = np.array([
np.random.random() * 0.5 - 0.25,
np.random.random() * 0.5 - 0.25,
np.random.random() * 0.5 + 0.5,
])
R = robot_config.R("EE", q=feedback["q"])
target_orientation = transformations.euler_from_matrix(R, "sxyz")
# update the position of the target
interface.set_mocap_xyz("target", target_xyz)
# can use 3 different methods to calculate inverse kinematics
# see inverse_kinematics.py file for details
path_planner.generate_path(
position=feedback["q"],
target_position=np.hstack([target_xyz, target_orientation]),
method=3,
dt=0.005,
n_timesteps=n_timesteps,
plot=False,
)
# returns desired [position, velocity]
q=feedback["q"],
dq=feedback["dq"],
target=target,
)
# add gripper forces
u = np.hstack((u, np.zeros(robot_config.N_GRIPPER_JOINTS)))
# apply the control signal, step the sim forward
interface.send_forces(u)
# track data
ee_track.append(np.copy(hand_xyz))
ee_angles_track.append(
transformations.euler_from_matrix(robot_config.R(
"EE", feedback["q"]),
axes="rxyz"))
target_track.append(np.copy(target[:3]))
target_angles_track.append(np.copy(target[3:]))
count += 1
finally:
# stop and reset the simulation
interface.disconnect()
print("Simulation terminated...")
ee_track = np.array(ee_track)
ee_angles_track = np.array(ee_angles_track)
target_track = np.array(target_track)
target_angles_track = np.array(target_angles_track)
if (count % 200) == 0:
# change target once every simulated second
if index >= len(orientations):
break
interface.set_orientation('target', orientations[index])
interface.set_xyz('target', positions[index])
index += 1
target = np.hstack([
interface.get_xyz('target'),
interface.get_orientation('target')])
# set the block to be the same orientation as end-effector
R_e = robot_config.R('EE', feedback['q'])
EA_e = transformations.euler_from_matrix(R_e, axes='rxyz')
interface.set_orientation('object', EA_e)
# calculate the Jacobian for the end effectora
# and isolate relevate dimensions
J = robot_config.J('EE', q=feedback['q'])[ctrlr_dof]
# calculate the inertia matrix in task space
M = robot_config.M(q=feedback['q'])
# calculate the inertia matrix in task space
M_inv = np.linalg.inv(M)
Mx_inv = np.dot(J, np.dot(M_inv, J.T))
if np.linalg.det(Mx_inv) != 0:
# do the linalg inverse if matrix is non-singular
# because it's faster and more accurate
interface.get_mocap_xyz('target_orientation'),
transformations.euler_from_quaternion(
interface.get_mocap_orientation('target_orientation'), 'rxyz')])
u = ctrlr.generate(
q=feedback['q'],
dq=feedback['dq'],
target=target,
)
# apply the control signal, step the sim forward
interface.send_forces(u)
# track data
ee_track.append(np.copy(hand_xyz))
ee_angles_track.append(transformations.euler_from_matrix(
robot_config.R('EE', feedback['q']), axes='rxyz'))
target_track.append(np.copy(target[:3]))
target_angles_track.append(np.copy(target[3:]))
count += 1
finally:
# stop and reset the simulation
interface.disconnect()
print('Simulation terminated...')
ee_track = np.array(ee_track)
ee_angles_track = np.array(ee_angles_track)
target_track = np.array(target_track)
target_angles_track = np.array(target_angles_track)
hand_xyz = robot_config.Tx('EE', feedback['q'])
if (count % 200) == 0:
# change target once every simulated second
if index >= len(orientations):
break
interface.set_orientation('target', orientations[index])
index += 1
target = np.hstack([
interface.get_xyz('target'),
interface.get_orientation('target')])
# set the block to be the same orientation as end-effector
R_e = robot_config.R('EE', feedback['q'])
EA_e = transformations.euler_from_matrix(R_e, axes='rxyz')
interface.set_orientation('object', EA_e)
# calculate the Jacobian for the end effectora
# and isolate relevate dimensions
J = robot_config.J('EE', q=feedback['q'])[ctrlr_dof]
# calculate the inertia matrix in task space
M = robot_config.M(q=feedback['q'])
# calculate the inertia matrix in task space
M_inv = np.linalg.inv(M)
Mx_inv = np.dot(J, np.dot(M_inv, J.T))
if np.linalg.det(Mx_inv) != 0:
# do the linalg inverse if matrix is non-singular
# because it's faster and more accurate
