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)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(0.5)
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)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client,
vrep.simx_opmode_blocking)
time.sleep(0.5)
sim_ret, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
def setup_sim_camera(self, resolution_x=1024., resolution_y=1024.):
# Get handle to camera
perspectiveAngle = np.deg2rad(54.70)
self.cam_intrinsics = np.asarray(
[[
resolution_x / (2 * np.tan(perspectiveAngle / 2)), 0,
resolution_x / 2
],
[
0, resolution_y / (2 * np.tan(perspectiveAngle / 2)),
resolution_y / 2
], [0, 0, 1]])
_, self.cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_persp_0',
vrep.simx_opmode_blocking)
# Get camera pose and intrinsics in simulation
_, cam_position = vrep.simxGetObjectPosition(self.sim_client,
self.cam_handle, -1,
vrep.simx_opmode_blocking)
_, cam_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking)
cam_trans = np.eye(4, 4)
cam_trans[0:3, 3] = np.asarray(cam_position)
cam_orientation = [
cam_orientation[0], cam_orientation[1], cam_orientation[2]
]
cam_rotm = np.eye(4, 4)
cam_rotm[0:3, 0:3] = utils.euler2rotm(cam_orientation)
self.cam_pose = np.dot(
cam_trans, cam_rotm
) # Compute rigid transformation representating camera pose
self.cam_depth_scale = 1
def setup_sim_camera(self):
# Get handle to camera
sim_ret, self.cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_ortho', vrep.simx_opmode_blocking)
# Get camera pose and intrinsics in simulation
sim_ret, cam_position = vrep.simxGetObjectPosition(
self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking)
sim_ret, cam_orientation = vrep.simxGetObjectOrientation(
self.sim_client, self.cam_handle, -1, vrep.simx_opmode_blocking)
cam_trans = np.eye(4, 4)
cam_trans[0:3, 3] = np.asarray(cam_position)
cam_orientation = [
-cam_orientation[0], -cam_orientation[1], -cam_orientation[2]
]
cam_rotm = np.eye(4, 4)
cam_rotm[0:3, 0:3] = np.linalg.inv(utils.euler2rotm(cam_orientation))
self.cam_pose = np.dot(
cam_trans, cam_rotm
) # Compute rigid transformation representating camera pose
self.cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240],
[0, 0, 1]])
self.cam_depth_scale = 1
# Get background image
self.bg_color_img, self.bg_depth_img = self.get_camera_data()
self.bg_depth_img = self.bg_depth_img * self.cam_depth_scale
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 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 close_gripper(self):
motorVelocity = -0.5 # m / s
motorForce = 200 # N
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(
self.clientID, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
vrep.simxSetJointForce(self.clientID, RG2_gripper_handle, motorForce,
vrep.simx_opmode_blocking)
vrep.simxSetJointTargetVelocity(self.clientID, RG2_gripper_handle,
motorVelocity,
vrep.simx_opmode_blocking)
time.sleep(0.2)
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 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)
def restart_sim(self):
_, self.UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
_, self.UR5_tip_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1,
self.gripper_home_pos,
vrep.simx_opmode_blocking)
vrep.simxSetObjectOrientation(self.sim_client, self.UR5_target_handle,
-1, self.gripper_home_ori,
vrep.simx_opmode_blocking)
vrep.simxStopSimulation(self.sim_client, vrep.simx_opmode_blocking)
vrep.simxStartSimulation(self.sim_client, vrep.simx_opmode_blocking)
time.sleep(0.3)
_, self.RG2_tip_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_tip', vrep.simx_opmode_blocking)
_, gripper_position = vrep.simxGetObjectPosition(
self.sim_client, self.RG2_tip_handle, -1,
vrep.simx_opmode_blocking)
while gripper_position[2] > self.gripper_home_pos[
2] + 0.01: # 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)
self.open_RG2_gripper()
self.obj_target_handles = []
if self.is_insert_task:
self.hole_handles = []
self.add_hole()
self.add_objects()
time.sleep(0.8)
def env_prepare(self):
sim_ret, self.UR3_target_handle = vrep.simxGetObjectHandle(
self.clientID, 'UR3_Target', vrep.simx_opmode_blocking)
sim_ret, self.WorkSpace_handle = vrep.simxGetObjectHandle(
self.clientID, 'WorkSpace', vrep.simx_opmode_blocking)
sim_ret, self.WorkSpace_position = vrep.simxGetObjectPosition(
self.clientID, self.WorkSpace_handle, -1,
vrep.simx_opmode_blocking)
print("WorkSpace_position", self.WorkSpace_position)
self.WorkSpace_size = 0.34 # 0.36*0.36
self.workspace_limits = np.array(
[[
self.WorkSpace_position[0] - self.WorkSpace_size / 2,
self.WorkSpace_position[0] + self.WorkSpace_size / 2
],
[
self.WorkSpace_position[1] - self.WorkSpace_size / 2,
self.WorkSpace_position[1] + self.WorkSpace_size / 2
]])
self.move_abs_ownframe(
[self.WorkSpace_size / 2, self.WorkSpace_size / 2, 0.3])
self.setangle_360(0)
self.setVisionSensor()
self.color_space = np.asarray([
[78.0, 121.0, 167.0], # blue
[89.0, 161.0, 79.0], # green
[156, 117, 95], # brown
[242, 142, 43], # orange
[237.0, 201.0, 72.0], # yellow
[186, 176, 172], # gray
[255.0, 87.0, 89.0], # red
[176, 122, 161], # purple
[118, 183, 178], # cyan
[255, 157, 167]
]) / 255 # pink
def setVisionSensor(self):
sim_ret, self.VS_left_handle = vrep.simxGetObjectHandle(
self.clientID, 'Vision_sensor_left', vrep.simx_opmode_blocking)
ret, self.VS_left_position = vrep.simxGetObjectPosition(
self.clientID, self.VS_left_handle, -1, vrep.simx_opmode_blocking)
ret, self.VS_left_orientation = vrep.simxGetObjectOrientation(
self.clientID, self.VS_left_handle, -1, vrep.simx_opmode_blocking)
self.left_cam_intrinsics = np.asarray([[561.82, 0, 320],
[0, 561.82, 240], [0, 0,
1]]) # 内部参数
self.leftmat = utils.Creat_posemat(self.VS_left_orientation,
self.VS_left_position)
sim_ret, self.VS_right_handle = vrep.simxGetObjectHandle(
self.clientID, 'Vision_sensor_right', vrep.simx_opmode_blocking)
ret, self.VS_right_position = vrep.simxGetObjectPosition(
self.clientID, self.VS_right_handle, -1, vrep.simx_opmode_blocking)
ret, self.VS_right_orientation = vrep.simxGetObjectOrientation(
self.clientID, self.VS_right_handle, -1, vrep.simx_opmode_blocking)
self.right_cam_intrinsics = np.asarray([[561.82, 0, 320],
[0, 561.82, 240], [0, 0,
1]]) # 内部参数
self.rightmat = utils.Creat_posemat(self.VS_right_orientation,
self.VS_right_position)
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 checkState(self):
sim_ret, UR3_target_position = vrep.simxGetObjectPosition(
self.clientID, self.UR3_target_handle, -1,
vrep.simx_opmode_blocking)
sim_ret, RG2_gripper_handle = vrep.simxGetObjectHandle(
self.clientID, 'RG2_openCloseJoint', vrep.simx_opmode_blocking)
sim_ret, RG2_gripper_position = vrep.simxGetObjectPosition(
self.clientID, RG2_gripper_handle, -1, vrep.simx_opmode_blocking)
#print("===================================================================")
#print(UR3_target_position)
#print(RG2_gripper_position)
dist=pow(pow(UR3_target_position[0]-RG2_gripper_position[0],2)+\
pow(UR3_target_position[1] - RG2_gripper_position[1], 2) +\
pow(UR3_target_position[2] - RG2_gripper_position[2], 2),0.5)
if dist > 0.1:
return True #异常状态
else:
return False
import time
import random
import copy
vrep.simxFinish(-1)
clientID = vrep.simxStart('127.0.0.1', 19997, True, True, 5000, 5)
if clientID == -1:
raise Exception('Failed connecting to remote API server')
## define Baxter's joints
# define the body joints
bodyJoints = np.zeros(3)
bodyError = np.zeros(3)
vertError, vertJoint = vrep.simxGetObjectHandle(clientID,
'Baxter_verticalJoint',
vrep.simx_opmode_blocking)
rotError, rotJoint = vrep.simxGetObjectHandle(clientID, 'Baxter_rotationJoint',
vrep.simx_opmode_blocking)
monitorError, monitorJoint = vrep.simxGetObjectHandle(
clientID, 'Baxter_monitorJoint', vrep.simx_opmode_blocking)
block0err, block0 = vrep.simxGetObjectHandle(clientID, "cube0",
vrep.simx_opmode_blocking)
# block2err, block2 = vrep.simxGetObjectHandle(clientID, "cube2", vrep.simx_opmode_blocking)
# block1err, block1 = vrep.simxGetObjectHandle(clientID, "cube1", vrep.simx_opmode_blocking)
#block3err, block3 = vrep.simxGetObjectHandle(clientID, "cube3", vrep.simx_opmode_blocking)
# fireerr, fire = vrep.simxGetObjectHandle(clientID, "fire", vrep.simx_opmode_blocking)
# smoke0err, smoke0 = vrep.simxGetObjectHandle(clientID, "smoke#0", vrep.simx_opmode_blocking)
# smoke1err, smoke1 = vrep.simxGetObjectHandle(clientID, "smoke#1", vrep.simx_opmode_blocking)
from PIL import Image
import detect
#Setup
is_sim = True
workspace_limits = np.asarray([
[-0.724, -0.276], [-0.224, 0.224], [-0.0001, 0.4]
]) # Cols: min max, Rows: x y z (define workspace limits in robot coordinates)
robot = Robot(is_sim, workspace_limits)
#Variable Creation
heightmap_rotation_angle = 0 #needed for grasp function
workspace_center = [-.5, 0, .03] #where the objects will be placed
#Handle of UR5_target, needed as a home for the robotic arm
sim_ret, UR5_target_handle = vrep.simxGetObjectHandle(
robot.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
robot.sim_client, UR5_target_handle, -1, vrep.simx_opmode_blocking)
#Camera With YOLOv5
robot.setup_sim_camera()
rgb, depth = robot.get_camera_data()
workspace_picture = Image.fromarray(rgb)
workspace_picture.save(
"/home/saeid/Downloads/CoppeliaSim_Edu_V4_1_0_Ubuntu18_04/programming/remoteApiBindings/python/python/data/images/workspace.jpg"
)
#Detect number of objects
objectsPresent = detect.detect(False) #filled with objects detected by cameras
objectsPresent = int(objectsPresent)
print("Number of Objects detected: ", objectsPresent)
def getObjectHandle(sim_client, obj_name):
return vrep.simxGetObjectHandle(sim_client, obj_name, VREP_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, 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
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)
