def __init__(self, workspace_limits, ip_vrep):
self.workspace_limits = workspace_limits
s = rospy.Service('cmd_gripper', GripperCmd, self.cmdGripper)
s = rospy.Service('robot_grasp', CoordAction, self.grasp)
s = rospy.Service('robot_push', CoordAction, self.push)
s = rospy.Service('restart_sim', Empty, self.restart_sim)
# Make sure to have the server side running in V-REP:
# in a child script of a V-REP scene, add following command
# to be executed just once, at simulation start:
#
# simExtRemoteApiStart(19999)
#
# then start simulation, and run this program.
#
# IMPORTANT: for each successful call to simxStart, there
# should be a corresponding call to simxFinish at the end!
# MODIFY remoteApiConnections.txt
# Connect to simulator
self.sim_client = vrep.simxStart(ip_vrep, 20003, True, True, 5000,
5) # Connect to V-REP on port 20003
if self.sim_client == -1:
print(
'Failed to connect to simulation (V-REP remote API server). Exiting.'
)
exit()
else:
print('Connected to simulation on port {}'.format(self.sim_client))
# Start simulation
sim_ret, self.UR5_target_handle = vrep.simxGetObjectHandle(
self.sim_client, 'UR5_target', vrep.simx_opmode_blocking)
sim_ret, self.cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_persp', vrep.simx_opmode_blocking)
vrep.simxSetObjectPosition(self.sim_client, self.UR5_target_handle, -1,
(-0.5, 0, 0.3), vrep.simx_opmode_blocking)
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 restart_sim(self, req):
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)
return EmptyResponse()
def gripper_fully_closed(self):
"""
Test if the gripper is closed
:return: True if the gripper is fully closed (so, no object is holded)
"""
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)
while gripper_joint_position > -0.044: # Block until gripper is fully closed
sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
if new_gripper_joint_position >= gripper_joint_position:
return False
gripper_joint_position = new_gripper_joint_position
return True
def __init__(self):
s = rospy.Service('get_color_depth_images', ColorDepthImages,
self.get_data_for_service)
s = rospy.Service('get_camera_informations', InfoCamera,
self.get_informations_for_service)
ipVREP = '127.0.0.1'
self.sim_client = vrep.simxStart(ipVREP, 20002, 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 on port {}'.format(self.sim_client))
# Get handle to camera
sim_ret, self.cam_handle = vrep.simxGetObjectHandle(
self.sim_client, 'Vision_sensor_persp', 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(self.euler2rotm(cam_orientation))
self.cam_pose = np.dot(
cam_trans, cam_rotm
) # Compute rigid transformation representation camera pose
self.cam_intrinsics = np.asarray([[618.62, 0, 320], [0, 618.62, 240],
[0, 0, 1]])
self.cam_depth_scale = 1.0
# Get background image
self.bg_color_img, self.bg_depth_img = self.get_camera_data()
print(self.bg_color_img.shape)
self.bg_depth_img = self.bg_depth_img * self.cam_depth_scale
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)
while gripper_joint_position > -0.044: # Block until gripper is fully closed
sim_ret, new_gripper_joint_position = vrep.simxGetJointPosition(
self.sim_client, RG2_gripper_handle, vrep.simx_opmode_blocking)
if new_gripper_joint_position >= gripper_joint_position:
return False
gripper_joint_position = new_gripper_joint_position
return True
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)
# ICAM 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 move_to(self, tool_position, tool_orientation):
sim_ret, UR5_target_position = vrep.simxGetObjectPosition(
self.sim_client, self.UR5_target_handle, -1,
vrep.simx_opmode_blocking)