def step(self, a):
if a in [self.N, self.S, self.W, self.E]:
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
if a == self.N:
target_pose[0, 3] += 0.01
elif a == self.S:
target_pose[0, 3] -= 0.01
elif a == self.W:
target_pose[1, 3] += 0.01
elif a == self.E:
target_pose[1, 3] -= 0.01
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target,
target_pose[:3, 3])
elif a is self.CLOSE:
closed = self.rdd.close()
if not closed:
sim_ret, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
sim_ret, tip_position = utils.getObjectPosition(
self.sim_client, self.ur5.gripper_tip)
if np.all(tip_position > (np.array(cube_position) - np.array(self.cube_size))) and \
np.all(tip_position < (np.array(cube_position) + np.array(self.cube_size))):
return None, 1, True, None
self.rdd.open(self.open_position)
return self.getState(), 0, False, None
def print_joint_pos(joint):
while True:
# print utils.getJointPosition(sim_client, joint)
res, cube_pose = utils.getObjectPosition(sim_client, cube)
while any(np.isnan(cube_pose)):
res, cube_pose = utils.getObjectPosition(sim_client, cube)
print cube_pose
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 step(self, a):
"""
take a step
:param a: action, int
:return: observation, reward, done, info
"""
self.sendClearSignal()
sim_ret, target_position = utils.getObjectPosition(
self.sim_client, self.ur5.UR5_target)
sim_ret, target_orientation = utils.getObjectOrientation(
self.sim_client, self.ur5.UR5_target)
target_pose = transformations.euler_matrix(target_orientation[0],
target_orientation[1],
target_orientation[2])
target_pose[:3, -1] = target_position
if a == self.RIGHT:
target_pose[1, 3] -= 0.05
elif a == self.LEFT:
target_pose[1, 3] += 0.05
elif a == self.UP:
target_pose[2, 3] += 0.03
elif a == self.DOWN:
target_pose[2, 3] -= 0.03
self.ur5.moveTo(target_pose)
sim_ret, cube_orientation = utils.getObjectOrientation(
self.sim_client, self.cube)
sim_ret, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
sim_ret, target_position = utils.getObjectPosition(
self.sim_client, self.ur5.UR5_target)
# arm is in wrong pose
# sim_ret, target_position = utils.getObjectPosition(self.sim_client, self.ur5.UR5_target)
if target_position[1] < 0.42 or target_position[
1] > 0.95 or target_position[2] < 0 or target_position[2] > 0.2:
# print 'Wrong arm position: ', target_position
return None, -1, True, None
# cube in wrong position
while any(np.isnan(cube_position)):
res, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
if cube_position[0] < self.cube_start_position[0] - self.cube_size[0] or \
cube_position[0] > self.cube_start_position[0] + self.cube_size[0] or \
cube_position[1] < self.cube_start_position[1] - self.cube_size[1] or \
cube_position[1] > self.cube_start_position[1] + self.cube_size[1]:
# print 'Wrong cube position: ', cube_position
return None, 0, True, None
# cube is lifted
if cube_orientation[0] < -0.05:
return None, 1, True, None
# cube is not lifted
return self.getObs(), 0, False, None
def step(self, a):
if a in [self.RIGHT, self.LEFT]:
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
if a == self.RIGHT:
target_pose[1, 3] -= 0.01
elif a == self.LEFT:
target_pose[1, 3] += 0.01
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target,
target_pose[:3, 3])
elif a == self.CLOSE:
self.rdd.close()
sim_ret, cube_orientation = utils.getObjectOrientation(
self.sim_client, self.cube)
sim_ret, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
sim_ret, tip_position = utils.getObjectPosition(
self.sim_client, self.ur5.gripper_tip)
if np.all(tip_position > (np.array(cube_position) - np.array(self.cube_size))) and \
np.all(tip_position < (np.array(cube_position) + np.array(self.cube_size))) and \
(cube_orientation[0] < -0.02 or cube_position[2] > self.cube_start_position[2] + 0.005):
return None, 1, True, None
self.rdd.open(self.open_position)
# arm is in wrong pose
sim_ret, target_position = utils.getObjectPosition(
self.sim_client, self.ur5.UR5_target)
if target_position[1] < 0.42 or target_position[
1] > 0.95 or target_position[2] < 0 or target_position[2] > 0.2:
print 'Wrong arm position: ', target_position
return None, -1, True, None
else:
# cube in wrong position
sim_ret, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
while any(np.isnan(cube_position)):
res, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
if cube_position[0] < self.cube_start_position[0] - self.cube_size[0] or \
cube_position[0] > self.cube_start_position[0] + self.cube_size[0] or \
cube_position[1] < self.cube_start_position[1] - self.cube_size[1] or \
cube_position[1] > self.cube_start_position[1] + self.cube_size[1]:
print 'Wrong cube position: ', cube_position
return None, 0, True, None
# cube is not lifted
return self.state, 0, False, None
def step(self, a):
"""
take a step
:param a: action, int
:return: observation, reward, done, info
"""
if a in [self.RIGHT, self.LEFT]:
current_position = self.target_position
target_pose = transformations.euler_matrix(
self.target_orientation[0], self.target_orientation[1],
self.target_orientation[2])
target_pose[:3, -1] = current_position
if a == self.RIGHT:
target_pose[1, 3] -= 0.01
elif a == self.LEFT:
target_pose[1, 3] += 0.01
utils.setObjectPositionOneShot(self.sim_client,
self.ur5.UR5_target, target_pose[:3,
3])
elif a == self.CLOSE:
self.rdd.setFingerPos(-0.1)
elif a == self.OPEN:
self.rdd.setFingerPos()
cube_orientation = self.cube_orientation
cube_position = self.cube_position
tip_position = self.tip_position
narrow_position = self.narrow_position
target_position = self.target_position
# arm is in wrong pose
# sim_ret, target_position = utils.getObjectPosition(self.sim_client, self.ur5.UR5_target)
if target_position[1] < 0.42 or target_position[
1] > 0.95 or target_position[2] < 0 or target_position[2] > 0.2:
print 'Wrong arm position: ', target_position
return None, -1, True, None
# cube in wrong position
while any(np.isnan(cube_position)):
res, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
if cube_position[0] < self.cube_start_position[0] - self.cube_size[0] or \
cube_position[0] > self.cube_start_position[0] + self.cube_size[0] or \
cube_position[1] < self.cube_start_position[1] - self.cube_size[1] or \
cube_position[1] > self.cube_start_position[1] + self.cube_size[1]:
print 'Wrong cube position: ', cube_position
return None, 0, True, None
# cube is lifted
if np.all(tip_position > (np.array(cube_position) - np.array(self.cube_size))) and \
np.all(tip_position < (np.array(cube_position) + np.array(self.cube_size))) and \
cube_orientation[0] < -0.02 and \
narrow_position > -0.5:
return None, 1, True, None
# cube is not lifted
return self.getObs(), 0, False, None
def step(self, a):
current_pose = self.ur5.getEndEffectorPose()
target_pose = current_pose.copy()
if a == 0:
target_pose[1, 3] -= 0.02
else:
target_pose[1, 3] += 0.02
utils.setObjectPosition(self.sim_client, self.ur5.UR5_target,
target_pose[:3, 3])
# utils.setObjectOrientation(self.sim_client, self.ur5.UR5_target, target_pose.flatten()[:-4])
# self.ur5.moveTo(target_pose)
# time.sleep(0.5)
# arm is in wrong pose
sim_ret, tip_position = utils.getObjectPosition(
self.sim_client, self.ur5.gripper_tip)
# if np.linalg.norm(tip_position - target_pose[:3, -1]) > 0.05:
# print 'Wrong position, dist: ', np.linalg.norm(tip_position - target_pose[:3, -1])
# return None, -1, True, None
if tip_position[1] < 0.42 or tip_position[1] > 0.95:
print 'Wrong arm position: ', tip_position
return None, -1, True, None
else:
# cube is lifted
sim_ret, cube_orientation = utils.getObjectOrientation(
self.sim_client, self.cube)
if cube_orientation[0] < -0.02:
return None, 1, True, None
# cube in wrong position
sim_ret, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
while any(np.isnan(cube_position)):
res, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
# if np.any(np.array(cube_position) < np.array(self.cube_start_position) - 0.5 * np.array(self.cube_size))\
# or np.any(np.array(cube_position) > np.array(self.cube_start_position) + 0.5 * np.array(self.cube_size)):
if cube_position[0] < self.cube_start_position[0] - self.cube_size[0] or \
cube_position[0] > self.cube_start_position[0] + self.cube_size[0] or\
cube_position[1] < self.cube_start_position[1] - self.cube_size[1] or\
cube_position[1] > self.cube_start_position[1] + self.cube_size[1]:
print 'Wrong cube position: ', cube_position
return None, 0, True, None
# cube is not lifted
return self.rdd.getFingerPosition(RDD.NARROW), 0, False, None
def moveTo(self, pose, move_step_size=0.01, single_step=False):
'''
Moves the tip of the gripper to the target pose
Args:
- pose: 4D target pose
'''
# Get current position and orientation of UR5 target
sim_ret, UR5_target_position = utils.getObjectPosition(
self.sim_client, self.UR5_target)
sim_ret, UR5_target_orientation = utils.getObjectOrientation(
self.sim_client, self.UR5_target)
# Calculate the movement increments
move_direction = pose[:3, -1] - UR5_target_position
move_magnitude = np.linalg.norm(move_direction)
move_step = move_step_size * move_direction / move_magnitude
num_move_steps = int(np.ceil(move_magnitude / move_step_size))
# Calculate the rotation increments
rotation = np.asarray(transformations.euler_from_matrix(pose))
rotation_step = rotation - UR5_target_orientation
rotation_step[rotation >= 0] = 0.1
rotation_step[rotation < 0] = -0.1
num_rotation_steps = np.ceil(
(rotation - UR5_target_orientation) / rotation_step).astype(np.int)
# Move and rotate to the target pose
if not single_step:
for i in range(max(num_move_steps, np.max(num_rotation_steps))):
pos = UR5_target_position + move_step * min(i, num_move_steps)
rot = [
UR5_target_orientation[0] +
rotation_step[0] * min(i, num_rotation_steps[0]),
UR5_target_orientation[1] +
rotation_step[1] * min(i, num_rotation_steps[1]),
UR5_target_orientation[2] +
rotation_step[2] * min(i, num_rotation_steps[2])
]
utils.setObjectPosition(self.sim_client, self.UR5_target, pos)
utils.setObjectOrientation(self.sim_client, self.UR5_target,
rot)
utils.setObjectPosition(self.sim_client, self.UR5_target, pose[:3, -1])
utils.setObjectOrientation(self.sim_client, self.UR5_target, rotation)
def planAction(self):
sim_ret, target_position = utils.getObjectPosition(self.sim_client, self.ur5.UR5_target)
# step 1: move to bottom
if target_position[2] > 0.06:
return self.DOWN
elif target_position[2] < 0.05:
return self.UP
# step 2: move to left
elif target_position[1] < 0.79:
return self.LEFT
elif target_position[1] > 0.8:
return self.RIGHT
# step 3: close
else:
return self.CLOSE
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
def getObs(self):
sim_ret, theta = utils.getJointPosition(self.sim_client, self.rdd.finger_joint_narrow)
sim_ret, tip_position = utils.getObjectPosition(self.sim_client, self.ur5.gripper_tip)
return np.concatenate((tip_position, [theta]))
else:
# cube in wrong position
sim_ret, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
while any(np.isnan(cube_position)):
res, cube_position = utils.getObjectPosition(
self.sim_client, self.cube)
if cube_position[0] < self.cube_start_position[0] - self.cube_size[0] or \
cube_position[0] > self.cube_start_position[0] + self.cube_size[0] or \
cube_position[1] < self.cube_start_position[1] - self.cube_size[1] or \
cube_position[1] > self.cube_start_position[1] + self.cube_size[1]:
print 'Wrong cube position: ', cube_position
return None, 0, True, None
# cube is not lifted
return self.state, 0, False, None
if __name__ == '__main__':
env = ScoopEnv(port=19997)
env.reset()
while True:
a = input('input action')
s_, r, done, info = env.step(int(a))
sim_ret, tip_position = utils.getObjectPosition(
env.sim_client, env.ur5.gripper_tip)
print s_, r, done
if done:
break
def getPosition(self):
'''
Get position of the top of the gripper
'''
return utils.getObjectPosition(self.sim_client, self.gripper_tip)
