def _setResolution(self, resolution):
"""
INTERNAL METHOD, sets the resolution for the camera. The fov of the
camera has to be setted beforehand
Parameters:
resolution - The resolution of the camera (CameraResolution type)
"""
try:
with self.resolution_lock:
assert isinstance(resolution, CameraResolution)
assert self.hfov is not None and self.vfov is not None
self.resolution = resolution
self.projection_matrix = pybullet.computeProjectionMatrix(
left=-math.tan(math.pi * self.hfov / 360.0) *
self.near_plane,
right=math.tan(math.pi * self.hfov / 360.0) *
self.near_plane,
bottom=-math.tan(math.pi * self.vfov / 360.0) *
self.near_plane,
top=math.tan(math.pi * self.vfov / 360.0) *
self.near_plane,
nearVal=self.near_plane,
farVal=self.far_plane,
physicsClientId=self.physics_client)
except AssertionError as e:
print("Cannot set camera resolution: " + str(e))
def get_camera_image(self):
"""Get the camera image of the scene
Returns
-------
tuple
Three arrays corresponding to rgb, depth, and segmentation image.
"""
upAxisIndex = 2
camDistance = 500
pixelWidth = 350
pixelHeight = 700
camTargetPos = [0, 80, 0]
far = camDistance
near = -far
view_matrix = p.computeViewMatrixFromYawPitchRoll(
camTargetPos, camDistance, 0, 90, 0, upAxisIndex)
projection_matrix = p.computeProjectionMatrix(-90, 60, 150, -150, near,
far)
# Get depth values using the OpenGL renderer
width, height, rgbImg, depthImg, segImg = p.getCameraImage(
pixelWidth,
pixelHeight,
view_matrix,
projection_matrix,
renderer=p.ER_BULLET_HARDWARE_OPENGL)
return rgbImg, depthImg, segImg
def __init__(self, viewMatrixList=((0,0,3), (0,0,0), (0,1,0)), projectionMatrixList=(45.0, 1.0, 0.1, 3.1), width=224, height=224):
self._viewMatrixList = viewMatrixList
self._projectionMatrixList = projectionMatrixList
self._width = width
self._height = height
self._viewMatrix = p.computeViewMatrix(self._viewMatrixList[0], self._viewMatrixList[1],self._viewMatrixList[2],) # cameraEyePosition=[0, 0, 3], cameraTargetPosition=[0, 0, 0], cameraUpVector=[0, 1, 0]
#self._projectionMatrix = p.computeProjectionMatrixFOV(self._projectionMatrixList[0], self._projectionMatrixList[1], self._projectionMatrixList[2], self._projectionMatrixList[3]) # fov=45.0,aspect=1.0, nearVal=0.1, farVal=3.1
self._projectionMatrix = p.computeProjectionMatrix(-0.5,0.5,-0.5,0.5,0.1,40.0) # Screen left, right, bottom, top, near plane distance, far plane distance // Screen LRTB is for image size from origin (ie. -x,+x,-y,+y)
def take_1photo(): imageFrame = [] width = 3000 height = 3000 view_matrix = p.computeViewMatrix([0, 0, 50], [0, 0, 0.0], [1, 0, 0]) projection_matrix = p.computeProjectionMatrix(12, -12, 12, -12, 48.0, 51.1) width, height, rgbaImg, depthImg, segImg = p.getCameraImage(width, height, view_matrix, projection_matrix, shadow=True, renderer=p.ER_BULLET_HARDWARE_OPENGL) img, cubePositions, patchPositions = boxDetection2411(rgbaImg,width,height) imageFrame.append(img) return imageFrame, cubePositions, patchPositions
def _projection_matrix(self):
width = self.width
height = self.height
near = self.near
far = self.far
fx = self.fx
fy = self.fy
cx = self.cx
cy = self.cy
# TODO(yl): verify this
left = cx * near / -fx
top = cy * near / fy
right = -(width - cx) * near / -fx
bottom = -(height - cy) * near / fy
# Note pybullet returns the projection matrix in column major
# To convert it into the convention used in Python, do
# np.asarray(projectionMatrix).reshape((4,4)).T
projectionMatrix = pybullet.computeProjectionMatrix(
left, right, bottom, top, near, far
)
return projectionMatrix
def _setResolution(self, resolution):
"""
INTERNAL METHOD, sets the resolution for the camera. The fov of the
camera has to be setted beforehand
Parameters:
resolution - The resolution of the camera (CameraResolution type)
"""
try:
with self.resolution_lock:
assert isinstance(resolution, CameraResolution)
assert self.hfov is not None and self.vfov is not None
self.resolution = resolution
self.projection_matrix = pybullet.computeProjectionMatrix(
left=-math.tan(math.pi*self.hfov/360.0)*self.near_plane,
right=math.tan(math.pi*self.hfov/360.0)*self.near_plane,
bottom=-math.tan(math.pi*self.vfov/360.0)*self.near_plane,
top=math.tan(math.pi*self.vfov/360.0)*self.near_plane,
nearVal=self.near_plane,
farVal=self.far_plane,
physicsClientId=self.getPhysicsClientId())
self.intrinsic_matrix = [
self.projection_matrix[0]*self.resolution.width/2.0,
0.0,
(1-self.projection_matrix[8])*self.resolution.width/2.0,
0.0,
self.projection_matrix[5]*self.resolution.height/2.0,
(1-self.projection_matrix[9])*self.resolution.height/2.0,
0.0,
0.0,
1.0]
except AssertionError as e:
raise pybullet.error("Cannot set camera resolution: " + str(e))
def kinova_pusher_camera(camera):
camera.CameraViewMatrix = p.computeViewMatrixFromYawPitchRoll(
[0.7, 0.1, -0.00], 1, 90, -45, 0, 2)
camera.CameraProjMatrix = p.computeProjectionMatrix(
-0.5000, 0.5000, -0.5000, 1.5000, 1.0000, 6.0000)
def __init__(self, config):
if 'robot' not in config:
config['robot'] = 'ur5'
if 'pos_candidate' not in config:
config['pos_candidate'] = None
if 'perfect_grasp' not in config:
config['perfect_grasp'] = False
if 'perfect_place' not in config:
config['perfect_place'] = False
if 'workspace_check' not in config:
config['workspace_check'] = 'box'
if 'in_hand_size' not in config:
config['in_hand_size'] = 24
if 'in_hand_mode' not in config:
config['in_hand_mode'] = 'sub'
if 'num_random_objects' not in config:
config['num_random_objects'] = 0
if 'check_random_obj_valid' not in config:
config['check_random_obj_valid'] = False
seed = config['seed']
workspace = config['workspace']
max_steps = config['max_steps']
obs_size = config['obs_size']
fast_mode = config['fast_mode']
render = config['render']
action_sequence = config['action_sequence']
simulate_grasp = config['simulate_grasp']
pos_candidate = config['pos_candidate']
perfect_grasp = config['perfect_grasp']
perfect_place = config['perfect_place']
robot = config['robot']
workspace_check = config['workspace_check']
in_hand_size = config['in_hand_size']
in_hand_mode = config['in_hand_mode']
num_random_objects = config['num_random_objects']
check_random_obj_valid = config['check_random_obj_valid']
super(PyBulletEnv, self).__init__(seed, workspace, max_steps, obs_size,
action_sequence, pos_candidate,
in_hand_size, in_hand_mode)
# Connect to pybullet and add data files to path
if render:
self.client = pb.connect(pb.GUI)
else:
self.client = pb.connect(pb.DIRECT)
pb.setAdditionalSearchPath(pybullet_data.getDataPath())
self.dynamic = not fast_mode
# Environment specific variables
self._timestep = 1. / 240.
if robot == 'ur5':
self.robot = UR5_Simple()
elif robot == 'ur5_robotiq':
self.robot = UR5_Robotiq()
elif robot == 'kuka':
self.robot = Kuka()
else:
raise NotImplementedError
# TODO: Move this somewhere it makes sense
self.block_original_size = 0.05
self.block_scale_range = (0.6, 0.7)
self.min_block_size = self.block_original_size * self.block_scale_range[
0]
self.max_block_size = self.block_original_size * self.block_scale_range[
1]
self.pick_pre_offset = 0.1
self.pick_offset = 0.005
self.place_pre_offset = 0.1
self.place_offset = self.block_scale_range[1] * self.block_original_size
# Setup camera parameters
workspace_x_offset = (workspace[0][1] - workspace[0][0]) / 2
workspace_y_offset = (workspace[1][1] - workspace[1][0]) / 2
self.view_matrix = pb.computeViewMatrixFromYawPitchRoll(
[workspace[0].mean(), workspace[1].mean(), 0], 10, -90, -90, 0, 2)
self.proj_matrix = pb.computeProjectionMatrix(-workspace_x_offset,
workspace_x_offset,
-workspace_y_offset,
workspace_y_offset,
-10.0, 100.0)
# self.view_matrix = pb.computeViewMatrixFromYawPitchRoll([workspace[0].mean(), workspace[1].mean(), 0], 3, -90, -90, 0, 2)
# self.proj_matrix = pb.computeProjectionMatrixFOV(5.7, 1, 2, 3.01)
# Rest pose for arm
rot = pb.getQuaternionFromEuler([0, np.pi, 0])
self.rest_pose = [[0.0, 0.5, 0.5], rot]
self.objects = list()
self.object_types = {}
self.simulate_grasp = simulate_grasp
self.perfect_grasp = perfect_grasp
self.perfect_place = perfect_place
self.workspace_check = workspace_check
self.table_id = None
self.heightmap = None
self.current_episode_steps = 1
self.last_action = None
self.last_obj = None
self.state = {}
self.pb_state = None
self.initialize()
self.num_random_objects = num_random_objects
self.check_random_obj_valid = check_random_obj_valid
self.episode_count = 0