def grasp(self):
self.total_try += 1
self.before_grasp()
self.move_down()
# time.sleep(0.5)
self.close_gripper()
self.move_down(-1) # move up
time.sleep(1)
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)
if 0.055 >= gripper_joint_position >= -0.04:
success = True
else:
success = False
if success:
self.total_success += 1
accuracy = float(self.total_success) / self.total_try
info = {'grasp success rate': accuracy}
for tag, value in info.items():
self.logger.scalar_summary(tag, value, step=self.total_try)
return success
def open_griper(self):
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)
gripper_fully_opened = False
while gripper_joint_position < 0.0536: # Block until gripper is fully open
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 gripper_fully_opened
gripper_joint_position = new_gripper_joint_position
gripper_fully_opened = True
return gripper_fully_opened
def close_gripper(self):
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
gripper_fully_closed = True
return gripper_fully_closed
def grasp(self):
self.total_try += 1
self.move_down()
# time.sleep(0.5)
self.close_gripper()
self.move_down(-1) # move up
time.sleep(1)
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)
if 0.055 >= gripper_joint_position >= -0.04:
success = True
else:
success = False
if success:
self.total_success += 1
accuracy = float(self.total_success) / self.total_try
self.writer.add_scalar('grasp success rate', accuracy, self.total_try)
return success
