def __init__(self,
sim_client,
open_force=20.,
open_velocity=0.5,
close_force=100.,
close_velocity=-0.5):
'''
VRep RG2 gripper class.
Currently only does torque control.
Args:
- sim_client: VRep client object to communicate with simulator over
- open_force: Force to apply when opening gripper
- open_velocity: Target velocity when opening gripper
- close_force: Force to apply when closing gripper
- close_velocity: Target velocity when closing gripper
'''
self.sim_client = sim_client
# Set gripper params
self.open_force = open_force
self.open_velocity = open_velocity
self.close_force = close_force
self.close_velocity = close_velocity
# Get gripper actuation joint and target dummy object
sim_ret, self.gripper_joint = utils.getObjectHandle(
self.sim_client, 'RG2_openCloseJoint')
sim_ret, self.gripper_tip = utils.getObjectHandle(
self.sim_client, 'UR5_tip')
def getReward(self):
sim_ret, narrow_tip = utils.getObjectHandle(self.sim_client, 'narrow_tip')
sim_ret, cube_bottom = utils.getObjectHandle(self.sim_client, 'cube_bottom')
sim_ret, tip_position = utils.getObjectPosition(self.sim_client, narrow_tip)
sim_ret, bottom_position = utils.getObjectPosition(self.sim_client, cube_bottom)
sim_ret, cube_orientation = utils.getObjectOrientation(self.sim_client, self.cube)
return -np.linalg.norm(tip_position-bottom_position) + (-10 * cube_orientation[0])
def __init__(self, sim_client, gripper):
self.sim_client = sim_client
self.gripper = gripper
# Create handles to the UR5 target and tip which control IK control
sim_ret, self.UR5_target = utils.getObjectHandle(
self.sim_client, 'UR5_target')
sim_ret, self.gripper_tip = utils.getObjectHandle(
self.sim_client, 'UR5_tip')
def reset(self):
"""
reset the environment
:return: the observation, List[List[float], List[float]]
"""
vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube')
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.08])
dy = 0.3 * np.random.random()
# dy = 0
# dz = 0.1 * np.random.random() - 0.05
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
target_pose[1, 3] += dy
# target_pose[2, 3] += dz
self.rdd.setFingerPos(-0.1)
self.ur5.moveTo(target_pose)
self.narrow_p = []
self.target_position = None
while self.target_position is None:
time.sleep(0.1)
return [0. for _ in range(20)]
def reset(self):
"""
reset the environment
:return: the observation, List[List[float], List[float]]
"""
vrep.simxStopSimulation(self.sim_client, utils.VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, utils.VREP_BLOCKING)
time.sleep(1)
sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube')
self.rdd.setFingerPos(-0.1)
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.08])
# utils.setObjectPosition(self.sim_client, self.ur5.UR5_target, [-0.2, 0.6, 0.15])
dy = 0.3 * np.random.random()
# dz = 0.1 * np.random.random() - 0.05
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
target_pose[1, 3] += dy
# target_pose[2, 3] += dz
self.sendClearSignal()
self.ur5.moveTo(target_pose)
self.rdd.setFingerPos()
return self.getObs()
def reset(self):
vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
# Generate a cube
# position = self.cube_start_position
# orientation = [np.radians(90), 0, np.radians(90)]
# # orientation = [0, 0, 0]
# size = self.cube_size
# mass = 0.1
# color = [255, 0, 0]
# self.cube = utils.importShape(self.sim_client, 'cube', CUBE_MESH, position, orientation, color)
# self.cube = utils.generateShape(self.sim_client, 'cube', 0, size, position, orientation, mass, color)
# time.sleep(1)
sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube')
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target,
[-0.2, 0.6, 0.07])
dy = 0.3 * np.random.random()
# dy = 0.3
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
target_pose[1, 3] += dy
self.ur5.moveTo(target_pose)
# time.sleep(0.5)
self.rdd.openFinger(RDD.NARROW)
return self.rdd.getFingerPosition(RDD.NARROW)
def reset(self):
vrep.simxStopSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
vrep.simxStartSimulation(self.sim_client, VREP_BLOCKING)
time.sleep(1)
sim_ret, self.cube = utils.getObjectHandle(self.sim_client, 'cube')
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target,
[-0.2, 0.6, 0.08])
dy = 0.3 * np.random.random()
# dy = 0
# dz = 0.1 * np.random.random() - 0.05
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
target_pose[1, 3] += dy
# target_pose[2, 3] += dz
self.ur5.moveTo(target_pose)
# self.rdd.open(self.open_position)
self.target_position = None
while self.target_position is None:
time.sleep(0.1)
return self.state
def __init__(self,
sim_client,
sensor_name,
workspace,
intrinsics,
get_rgb=True,
get_depth=True,
z_near=0.01,
z_far=10):
'''
VRep vision sensor class.
Args:
- sim_client: VRep client object to communicate with simulator over
- sensor_name: Sensor name in simulator
- workspace: Workspace for the vision sensor in robot coordinates
- intrinsics: sensor intrinsics
- get_rgb: Should the sensor get the rgb image
- get_depth: Should the sensor get the depth image
- z_near: Minimum distance for depth sensing
- z_far: Maximum distance for depth sensing
'''
self.sim_client = sim_client
self.workspace = workspace
self.intrinsics = intrinsics
self.get_rgb = get_rgb
self.get_depth = get_depth
# Setup sensor and default sensor values
sim_ret, self.sensor = utils.getObjectHandle(self.sim_client,
sensor_name)
sim_ret, pos = utils.getObjectPosition(self.sim_client, self.sensor)
sim_ret, rot = utils.getObjectOrientation(self.sim_client, self.sensor)
cam_trans = np.eye(4, 4)
cam_trans[:3, 3] = np.asarray(pos)
rotm = np.linalg.inv(
transformations.euler_matrix(-rot[0], -rot[1], -rot[2]))
self.pose = np.dot(cam_trans, rotm)
self.z_near = z_near
self.z_far = z_far