def close_RG2_gripper(self, default_vel=-0.1, motor_force=50):
# RG2 gripper function
gripper_motor_velocity = default_vel
if self.is_insert_task:
gripper_motor_force = 200
else:
gripper_motor_force = motor_force
_, gripper_handle = vrep.simxGetObjectHandle(self.sim_client,
'RG2_openCloseJoint',
vrep.simx_opmode_blocking)
_, gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, gripper_handle, vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, gripper_handle,
gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, gripper_handle,
gripper_motor_velocity,
vrep.simx_opmode_blocking)
gripper_fully_closed = False
close_gripper_count = 0
while gripper_joint_position > -0.04: # Block until gripper is fully closed
_, new_gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, gripper_handle, vrep.simx_opmode_blocking)
close_gripper_count += 1
if new_gripper_joint_position < gripper_joint_position:
close_gripper_count = 0
gripper_joint_position = new_gripper_joint_position
if close_gripper_count > 1:
return gripper_fully_closed
gripper_fully_closed = True
return gripper_fully_closed
def open_gripper(self, isAsync=False):
gripper_motor_velocity = 0.5
gripper_motor_force = 20
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint',
vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle,
vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity,
vrep.simx_opmode_blocking)
while gripper_joint_position < 0.0536: # Block until gripper is fully open
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle,
vrep.simx_opmode_blocking)
def close_gripper(self, isAsync=False):
gripper_motor_velocity = -0.5
gripper_motor_force = 100
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(self.sim_client, 'RG2_openCloseJoint',
vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle,
vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle, gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle, gripper_motor_velocity,
vrep.simx_opmode_blocking)
gripper_fully_closed = False
while gripper_joint_position > -0.047: # Block until gripper is fully closed
sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(self.sim_client, RG2_gripper_handle,
vrep.simx_opmode_blocking)
print(gripper_joint_position)
if new_gripper_joint_position >= gripper_joint_position:
return gripper_fully_closed
gripper_joint_position = new_gripper_joint_position
def checkgrasp(self):
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(
self.clientID, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(
self.clientID, RG2_gripper_handle, vrep.simx_opmode_blocking)
if gripper_joint_position > -0.047:
self.remove_height_object()
return True
else:
return False
def open_RG2_gripper(self, default_vel=0.5, motor_force=100):
# RG2 Gripper
gripper_motor_velocity = default_vel
gripper_motor_force = motor_force
sim_ret, gripper_handle = vrep.simxGetObjectHandle(
self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
_, _ = vrep.simxGetJointPosition(self.sim_client, gripper_handle,
vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, gripper_handle,
gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, gripper_handle,
gripper_motor_velocity,
vrep.simx_opmode_blocking)
class RobotSim(object):
def __init__(self,
obj_mesh_dir,
workspace_limits,
is_testing,
ip_vrep='127.0.0.1'):
self.workspace_limits = workspace_limits
# Read files in object mesh directory
self.obj_mesh_dir = obj_mesh_dir
self.mesh_list = os.listdir(self.obj_mesh_dir)
vrep.simxFinish(-1) # Just in case, close all opened connections
self.sim_client = vrep.simxStart(ip_vrep, 19997, True, True, 5000,
5) # Connect to V-REP on port 19997
if self.sim_client == -1:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
else:
print('Connected to simulation.')
self.restart_sim()
# Setup virtual camera in simulation
self.setup_sim_camera()
self.is_testing = is_testing
def check_sim(self):
# Check if simulation is stable by checking if gripper is within workspace
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
sim_ok = gripper_position[0] > self.workspace_limits[0][
0] - 0.1 and gripper_position[0] < self.workspace_limits[0][
1] + 0.1 and gripper_position[1] > self.workspace_limits[1][
0] - 0.1 and gripper_position[1] < self.workspace_limits[
1][1] + 0.1 and gripper_position[
2] > self.workspace_limits[2][
0] and gripper_position[
2] < self.workspace_limits[2][1]
if not sim_ok:
print('Simulation unstable. Restarting environment.')
self.restart_sim()
def restart_sim(self):
sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1,
(-0.5, 0, 0.3), vrep.simx_opmode_blocking)
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(1)
sim_ret, self.RG2_tip_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking)
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
while gripper_position[
2] > 0.4: # V-REP bug requiring multiple starts and stops to restart
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(self.sim_client,
vrep.simx_opmode_blocking)
time.sleep(1)
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
def move_to(self, tool_position):
# sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(self.sim_client,'UR5_target',vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
move_direction = np.asarray([
tool_position[0] - UR5_target_position[0],
tool_position[1] - UR5_target_position[1],
tool_position[2] - UR5_target_position[2]
])
move_magnitude = np.linalg.norm(move_direction)
move_step = 0.02 * move_direction / move_magnitude
num_move_steps = int(np.floor(move_magnitude / 0.02))
for step_iter in range(num_move_steps):
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle,
-1,
(UR5_target_position[0] + move_step[0],
UR5_target_position[1] + move_step[1],
UR5_target_position[2] + move_step[2]),
vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
(tool_position[0], tool_position[1], tool_position[2]),
vrep.simx_opmode_blocking)
def open_gripper(self, async=False):
gripper_motor_velocity = 0.5
gripper_motor_force = 20
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(
self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle,
gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle,
gripper_motor_velocity,
vrep.simx_opmode_blocking)
while gripper_joint_position < 0.0536: # Block until gripper is fully open
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle,
gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle,
gripper_motor_velocity,
vrep.simx_opmode_blocking)
while gripper_joint_position < 0.0536: # Block until gripper is fully open
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
def close_gripper(self, async=False):
gripper_motor_velocity = -0.5
gripper_motor_force = 100
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(
self.sim_client, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
sim_ret, gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.sim_client, RG2_gripper_handle,
gripper_motor_force, vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.sim_client, RG2_gripper_handle,
gripper_motor_velocity,
vrep.simx_opmode_blocking)
gripper_fully_closed = False
while gripper_joint_position > -0.047: # Block until gripper is fully closed
sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
print(gripper_joint_position)
if new_gripper_joint_position >= gripper_joint_position:
return gripper_fully_closed
gripper_joint_position = new_gripper_joint_position
def get_camera_data(self):
def getJointPosition(sim_client, joint):
sim_ret, position = vrep.simxGetJointPosition(sim_client, joint,
VREP_BLOCKING)
return sim_ret, position