def _reset_scene(self, scale_factor=1.0):
"""Resets the scene.
Parameters
----------
scale_factor : float
optional scale factor to apply to the image dimensions
"""
# delete scene
if self._scene is not None:
self._scene.clear()
del self._scene
# create scene
scene = Scene()
# setup camera
camera = IntrinsicsCamera(
self.camera.intrinsics.fx,
self.camera.intrinsics.fy,
self.camera.intrinsics.cx,
self.camera.intrinsics.cy,
)
pose_m = self.camera.pose.matrix.copy()
pose_m[:, 1:3] *= -1.0
scene.add(camera, pose=pose_m, name=self.camera.frame)
scene.main_camera_node = next(
iter(scene.get_nodes(name=self.camera.frame))
)
material = MetallicRoughnessMaterial(
baseColorFactor=np.array([1, 1, 1, 1.0]),
metallicFactor=0.2,
roughnessFactor=0.8,
)
# add workspace objects
for obj_key in self.state.workspace_keys:
obj_state = self.state[obj_key]
obj_mesh = Mesh.from_trimesh(obj_state.mesh, material=material)
T_obj_world = obj_state.pose.matrix
scene.add(obj_mesh, pose=T_obj_world, name=obj_key)
# add scene objects
for obj_key in self.state.obj_keys:
obj_state = self.state[obj_key]
obj_mesh = Mesh.from_trimesh(obj_state.mesh, material=material)
T_obj_world = obj_state.pose.matrix
scene.add(obj_mesh, pose=T_obj_world, name=obj_key)
# add light (for color rendering)
light = DirectionalLight(color=np.ones(3), intensity=1.0)
scene.add(light, pose=np.eye(4))
ray_light_nodes = self._create_raymond_lights()
[scene.add_node(rln) for rln in ray_light_nodes]
self._scene = scene
def _reset_scene(self, scale_factor=1.0):
""" Resets the scene.
Parameters
----------
scale_factor : float
optional scale factor to apply to the image dimensions
"""
# delete scene
if self._scene is not None:
self._scene.clear()
del self._scene
# create scene
scene = Scene()
# setup camera
camera = IntrinsicsCamera(self.camera.intrinsics.fx, self.camera.intrinsics.fy,
self.camera.intrinsics.cx, self.camera.intrinsics.cy)
pose_m = self.camera.pose.matrix.copy()
pose_m[:,1:3] *= -1.0
scene.add(camera, pose=pose_m, name=self.camera.frame)
scene.main_camera_node = next(iter(scene.get_nodes(name=self.camera.frame)))
# add workspace objects
for obj_key in self.state.workspace_keys:
obj_state = self.state[obj_key]
obj_mesh = Mesh.from_trimesh(obj_state.mesh)
T_obj_world = obj_state.pose.matrix
scene.add(obj_mesh, pose=T_obj_world, name=obj_key)
# add scene objects
for obj_key in self.state.obj_keys:
obj_state = self.state[obj_key]
obj_mesh = Mesh.from_trimesh(obj_state.mesh)
T_obj_world = obj_state.pose.matrix
scene.add(obj_mesh, pose=T_obj_world, name=obj_key)
self._scene = scene
def test_scenes():
# Basics
s = Scene()
assert np.allclose(s.bg_color, np.ones(4))
assert np.allclose(s.ambient_light, np.zeros(3))
assert len(s.nodes) == 0
assert s.name is None
s.name = 'asdf'
s.bg_color = None
s.ambient_light = None
assert np.allclose(s.bg_color, np.ones(4))
assert np.allclose(s.ambient_light, np.zeros(3))
assert s.nodes == set()
assert s.cameras == set()
assert s.lights == set()
assert s.point_lights == set()
assert s.spot_lights == set()
assert s.directional_lights == set()
assert s.meshes == set()
assert s.camera_nodes == set()
assert s.light_nodes == set()
assert s.point_light_nodes == set()
assert s.spot_light_nodes == set()
assert s.directional_light_nodes == set()
assert s.mesh_nodes == set()
assert s.main_camera_node is None
assert np.all(s.bounds == 0)
assert np.all(s.centroid == 0)
assert np.all(s.extents == 0)
assert np.all(s.scale == 0)
# From trimesh scene
tms = trimesh.load('tests/data/WaterBottle.glb')
s = Scene.from_trimesh_scene(tms)
assert len(s.meshes) == 1
assert len(s.mesh_nodes) == 1
# Test bg color formatting
s = Scene(bg_color=[0, 1.0, 0])
assert np.allclose(s.bg_color, np.array([0.0, 1.0, 0.0, 1.0]))
# Test constructor for nodes
n1 = Node()
n2 = Node()
n3 = Node()
nodes = [n1, n2, n3]
s = Scene(nodes=nodes)
n1.children.append(n2)
s = Scene(nodes=nodes)
n3.children.append(n2)
with pytest.raises(ValueError):
s = Scene(nodes=nodes)
n3.children = []
n2.children.append(n3)
n3.children.append(n2)
with pytest.raises(ValueError):
s = Scene(nodes=nodes)
# Test node accessors
n1 = Node()
n2 = Node()
n3 = Node()
nodes = [n1, n2]
s = Scene(nodes=nodes)
assert s.has_node(n1)
assert s.has_node(n2)
assert not s.has_node(n3)
# Test node poses
for n in nodes:
assert np.allclose(s.get_pose(n), np.eye(4))
with pytest.raises(ValueError):
s.get_pose(n3)
with pytest.raises(ValueError):
s.set_pose(n3, np.eye(4))
tf = np.eye(4)
tf[:3, 3] = np.ones(3)
s.set_pose(n1, tf)
assert np.allclose(s.get_pose(n1), tf)
assert np.allclose(s.get_pose(n2), np.eye(4))
nodes = [n1, n2, n3]
tf2 = np.eye(4)
tf2[:3, :3] = np.diag([-1, -1, 1])
n1.children.append(n2)
n1.matrix = tf
n2.matrix = tf2
s = Scene(nodes=nodes)
assert np.allclose(s.get_pose(n1), tf)
assert np.allclose(s.get_pose(n2), tf.dot(tf2))
assert np.allclose(s.get_pose(n3), np.eye(4))
n1 = Node()
n2 = Node()
n3 = Node()
n1.children.append(n2)
s = Scene()
s.add_node(n1)
with pytest.raises(ValueError):
s.add_node(n2)
s.set_pose(n1, tf)
assert np.allclose(s.get_pose(n1), tf)
assert np.allclose(s.get_pose(n2), tf)
s.set_pose(n2, tf2)
assert np.allclose(s.get_pose(n2), tf.dot(tf2))
# Test node removal
n1 = Node()
n2 = Node()
n3 = Node()
n1.children.append(n2)
n2.children.append(n3)
s = Scene(nodes=[n1, n2, n3])
s.remove_node(n2)
assert len(s.nodes) == 1
assert n1 in s.nodes
assert len(n1.children) == 0
assert len(n2.children) == 1
s.add_node(n2, parent_node=n1)
assert len(n1.children) == 1
n1.matrix = tf
n3.matrix = tf2
assert np.allclose(s.get_pose(n3), tf.dot(tf2))
# Now test ADD function
s = Scene()
m = Mesh([], name='m')
cp = PerspectiveCamera(yfov=2.0)
co = OrthographicCamera(xmag=1.0, ymag=1.0)
dl = DirectionalLight()
pl = PointLight()
sl = SpotLight()
n1 = s.add(m, name='mn')
assert n1.mesh == m
assert len(s.nodes) == 1
assert len(s.mesh_nodes) == 1
assert n1 in s.mesh_nodes
assert len(s.meshes) == 1
assert m in s.meshes
assert len(s.get_nodes(node=n2)) == 0
n2 = s.add(m, pose=tf)
assert len(s.nodes) == len(s.mesh_nodes) == 2
assert len(s.meshes) == 1
assert len(s.get_nodes(node=n1)) == 1
assert len(s.get_nodes(node=n1, name='mn')) == 1
assert len(s.get_nodes(name='mn')) == 1
assert len(s.get_nodes(obj=m)) == 2
assert len(s.get_nodes(obj=m, obj_name='m')) == 2
assert len(s.get_nodes(obj=co)) == 0
nsl = s.add(sl, name='sln')
npl = s.add(pl, parent_name='sln')
assert nsl.children[0] == npl
ndl = s.add(dl, parent_node=npl)
assert npl.children[0] == ndl
nco = s.add(co)
ncp = s.add(cp)
assert len(s.light_nodes) == len(s.lights) == 3
assert len(s.point_light_nodes) == len(s.point_lights) == 1
assert npl in s.point_light_nodes
assert len(s.spot_light_nodes) == len(s.spot_lights) == 1
assert nsl in s.spot_light_nodes
assert len(s.directional_light_nodes) == len(s.directional_lights) == 1
assert ndl in s.directional_light_nodes
assert len(s.cameras) == len(s.camera_nodes) == 2
assert s.main_camera_node == nco
s.main_camera_node = ncp
s.remove_node(ncp)
assert len(s.cameras) == len(s.camera_nodes) == 1
assert s.main_camera_node == nco
s.remove_node(n2)
assert len(s.meshes) == 1
s.remove_node(n1)
assert len(s.meshes) == 0
s.remove_node(nsl)
assert len(s.lights) == 0
s.remove_node(nco)
assert s.main_camera_node is None
s.add_node(n1)
s.clear()
assert len(s.nodes) == 0
# Trigger final errors
with pytest.raises(ValueError):
s.main_camera_node = None
with pytest.raises(ValueError):
s.main_camera_node = ncp
with pytest.raises(ValueError):
s.add(m, parent_node=n1)
with pytest.raises(ValueError):
s.add(m, name='asdf')
s.add(m, name='asdf')
s.add(m, parent_name='asdf')
with pytest.raises(ValueError):
s.add(m, parent_name='asfd')
with pytest.raises(TypeError):
s.add(None)
s.clear()
# Test bounds
m1 = Mesh.from_trimesh(trimesh.creation.box())
m2 = Mesh.from_trimesh(trimesh.creation.box())
m3 = Mesh.from_trimesh(trimesh.creation.box())
n1 = Node(mesh=m1)
n2 = Node(mesh=m2, translation=[1.0, 0.0, 0.0])
n3 = Node(mesh=m3, translation=[0.5, 0.0, 1.0])
s.add_node(n1)
s.add_node(n2)
s.add_node(n3)
assert np.allclose(s.bounds, [[-0.5, -0.5, -0.5], [1.5, 0.5, 1.5]])
s.clear()
s.add_node(n1)
s.add_node(n2, parent_node=n1)
s.add_node(n3, parent_node=n2)
assert np.allclose(s.bounds, [[-0.5, -0.5, -0.5], [2.0, 0.5, 1.5]])
tf = np.eye(4)
tf[:3, 3] = np.ones(3)
s.set_pose(n3, tf)
assert np.allclose(s.bounds, [[-0.5, -0.5, -0.5], [2.5, 1.5, 1.5]])
s.remove_node(n2)
assert np.allclose(s.bounds, [[-0.5, -0.5, -0.5], [0.5, 0.5, 0.5]])
s.clear()
assert np.allclose(s.bounds, 0.0)
class PybulletPhysicsEngine(PhysicsEngine):
"""Wrapper for pybullet physics engine that is tied to a single ID"""
def __init__(self, urdf_cache_dir, debug=False):
PhysicsEngine.__init__(self)
self._physics_client = None
self._debug = debug
self._urdf_cache_dir = urdf_cache_dir
if not os.path.isabs(self._urdf_cache_dir):
self._urdf_cache_dir = os.path.join(
os.getcwd(), self._urdf_cache_dir
)
if not os.path.exists(os.path.join(self._urdf_cache_dir, "plane")):
os.makedirs(os.path.join(self._urdf_cache_dir, "plane"))
shutil.copy(
pkg_resources.resource_filename(
"sd_maskrcnn", "data/plane/plane.urdf"
),
os.path.join(self._urdf_cache_dir, "plane", "plane.urdf"),
)
shutil.copy(
pkg_resources.resource_filename(
"sd_maskrcnn", "data/plane/plane_convex_piece_0.obj"
),
os.path.join(
self._urdf_cache_dir, "plane", "plane_convex_piece_0.obj"
),
)
def add(self, obj, static=False):
# create URDF
urdf_filename = os.path.join(
self._urdf_cache_dir,
KEY_SEP_TOKEN.join(obj.key.split(KEY_SEP_TOKEN)[:-1]),
"{}.urdf".format(
KEY_SEP_TOKEN.join(obj.key.split(KEY_SEP_TOKEN)[:-1])
),
)
urdf_dir = os.path.dirname(urdf_filename)
if not os.path.exists(urdf_filename):
try:
os.makedirs(urdf_dir)
except:
self._logger.warning(
"Failed to create dir %s. The object may have been created simultaneously by another process"
% (urdf_dir)
)
self._logger.info(
"Exporting URDF for object {}".format(
KEY_SEP_TOKEN.join(obj.key.split(KEY_SEP_TOKEN)[:-1])
)
)
# Fix center of mass (for rendering) and density and export
geometry = obj.mesh.copy()
geometry.apply_translation(-obj.mesh.center_mass)
trimesh.exchange.export.export_urdf(geometry, urdf_dir)
com = obj.mesh.center_mass
pose = self._convert_pose(obj.pose, com)
obj_t = pose.translation
obj_q_wxyz = pose.quaternion
obj_q_xyzw = np.roll(obj_q_wxyz, -1)
try:
obj_id = pybullet.loadURDF(
urdf_filename,
obj_t,
obj_q_xyzw,
useFixedBase=static,
physicsClientId=self._physics_client,
)
except:
raise Exception("Failed to load %s" % (urdf_filename))
if self._debug:
self._add_to_scene(obj)
self._key_to_id[obj.key] = obj_id
self._key_to_com[obj.key] = com
def get_velocity(self, key):
obj_id = self._key_to_id[key]
return pybullet.getBaseVelocity(
obj_id, physicsClientId=self._physics_client
)
def get_pose(self, key):
obj_id = self._key_to_id[key]
obj_t, obj_q_xyzw = pybullet.getBasePositionAndOrientation(
obj_id, physicsClientId=self._physics_client
)
obj_q_wxyz = np.roll(obj_q_xyzw, 1)
pose = RigidTransform(
rotation=obj_q_wxyz,
translation=obj_t,
from_frame="obj",
to_frame="world",
)
pose = self._deconvert_pose(pose, self._key_to_com[key])
return pose
def remove(self, key):
obj_id = self._key_to_id[key]
pybullet.removeBody(obj_id, physicsClientId=self._physics_client)
self._key_to_id.pop(key)
self._key_to_com.pop(key)
if self._debug:
self._remove_from_scene(key)
def step(self):
pybullet.stepSimulation(physicsClientId=self._physics_client)
if self._debug:
time.sleep(0.04)
self._update_scene()
def reset(self):
if self._physics_client is not None:
self.stop()
self.start()
def start(self):
if self._physics_client is None:
self._physics_client = pybullet.connect(pybullet.DIRECT)
pybullet.setGravity(
0, 0, -GRAVITY_ACCEL, physicsClientId=self._physics_client
)
self._key_to_id = {}
self._key_to_com = {}
if self._debug:
self._create_scene()
self._viewer = Viewer(
self._scene, use_raymond_lighting=True, run_in_thread=True
)
def stop(self):
if self._physics_client is not None:
pybullet.disconnect(self._physics_client)
self._physics_client = None
if self._debug:
self._scene = None
self._viewer.close_external()
while self._viewer.is_active:
pass
def __del__(self):
self.stop()
del self
def _convert_pose(self, pose, com):
new_pose = pose.copy()
new_pose.translation = pose.rotation.dot(com) + pose.translation
return new_pose
def _deconvert_pose(self, pose, com):
new_pose = pose.copy()
new_pose.translation = pose.rotation.dot(-com) + pose.translation
return new_pose
def _create_scene(self):
self._scene = Scene()
camera = PerspectiveCamera(
yfov=0.833, znear=0.05, zfar=3.0, aspectRatio=1.0
)
cn = Node()
cn.camera = camera
pose_m = np.array(
[
[0.0, 1.0, 0.0, 0.0],
[1.0, 0.0, 0.0, 0.0],
[0.0, 0.0, -1.0, 0.88],
[0.0, 0.0, 0.0, 1.0],
]
)
pose_m[:, 1:3] *= -1.0
cn.matrix = pose_m
self._scene.add_node(cn)
self._scene.main_camera_node = cn
def _add_to_scene(self, obj):
self._viewer.render_lock.acquire()
n = Node(
mesh=Mesh.from_trimesh(obj.mesh),
matrix=obj.pose.matrix,
name=obj.key,
)
self._scene.add_node(n)
self._viewer.render_lock.release()
def _remove_from_scene(self, key):
self._viewer.render_lock.acquire()
if self._scene.get_nodes(name=key):
self._scene.remove_node(
next(iter(self._scene.get_nodes(name=key)))
)
self._viewer.render_lock.release()
def _update_scene(self):
self._viewer.render_lock.acquire()
for key in self._key_to_id.keys():
obj_pose = self.get_pose(key).matrix
if self._scene.get_nodes(name=key):
next(iter(self._scene.get_nodes(name=key))).matrix = obj_pose
self._viewer.render_lock.release()
