def sample_model_material(self):
r, g, b = np.random.rand(), np.random.rand(), np.random.rand()
k_a = sample_value(0.25, 0.35)
k_d = sample_value(0.45, 0.55)
k_s = sample_value(0.15, 0.25) #limited (not get to glossy)
ks = np.array([k_a, k_d, k_s])
ks = ks / np.sum(ks)
alpha = sample_value(0.5, 1.5) #mirror
model_material = MaterialProperties(color=np.array([b, g, r]),
k_a=k_a,
k_d=k_d,
k_s=k_s,
alpha=alpha,
smooth=True,
wireframe=False)
return model_material
def plot3d(points,
color=(0.5, 0.5, 0.5),
tube_radius=0.005,
n_components=30,
name=None):
"""Plot a 3d curve through a set of points using tubes.
Parameters
----------
points : (n,3) float
A series of 3D points that define a curve in space.
color : (3,) float
The color of the tube.
tube_radius : float
Radius of tube representing curve.
n_components : int
The number of edges in each polygon representing the tube.
name : str
A name for the object to be added.
"""
points = np.asanyarray(points)
mp = MaterialProperties(color=np.array(color),
k_a=0.5,
k_d=0.3,
k_s=0.0,
alpha=10.0,
smooth=True)
# Generate circular polygon
vec = np.array([0, 1]) * tube_radius
angle = np.pi * 2.0 / n_components
rotmat = np.array([[np.cos(angle), -np.sin(angle)],
[np.sin(angle), np.cos(angle)]])
perim = []
for i in range(n_components):
perim.append(vec)
vec = np.dot(rotmat, vec)
poly = Polygon(perim)
# Sweep it out along the path
mesh = trimesh.creation.sweep_polygon(poly, points)
obj = SceneObject(mesh, material=mp)
if name is None:
name = str(uuid.uuid4())
Visualizer3D._scene.add_object(name, obj)
def sample_ground_material(self):
r, g, b = sample_value(0.7,
0.9), sample_value(0.0, 0.25), sample_value(
0.0, 0.25) #red-ish box
k_a = sample_value(0.3, 0.6)
k_d = sample_value(0.4, 0.7)
k_s = sample_value(0.0, 0.05)
ks = np.array([k_a, k_d, k_s])
ks = ks / np.sum(ks)
alpha = 0.0 #mirror
ground_material = MaterialProperties(color=np.array([b, g, r]),
k_a=k_a,
k_d=k_d,
k_s=k_s,
alpha=alpha,
smooth=True,
wireframe=False)
return ground_material
def create_scene(camera, workspace_objects):
# Start with an empty scene
scene = Scene()
# Create a VirtualCamera
virt_cam = VirtualCamera(camera.intrinsics, camera.pose)
# Add the camera to the scene
scene.camera = virt_cam
mp = MaterialProperties(
color=np.array([0.3,0.3,0.3]),
k_a=0.5, k_d=0.3, k_s=0.0, alpha=10.0
)
if camera.geometry is not None:
so = SceneObject(camera.geometry, camera.pose.copy(), mp)
scene.add_object(camera.name, so)
return scene
def get_sim_point_cloud(scene, grasp_obj):
# Remove old objects
scene_objs = scene.objects.copy()
if 'obj' in scene_objs:
scene.remove_object('obj')
# Get graspable object material properties and add to scene
mp = hasattr(grasp_obj, 'material_properties')
if not mp:
mp = MaterialProperties(
color=np.random.uniform(0.0, 1.0, size=3),
k_a=0.5, k_d=0.3, k_s=0.0, alpha=10.0
)
so = SceneObject(grasp_obj.mesh, grasp_obj.T_obj_world.copy(), mp)
scene.add_object(grasp_obj.key, so)
# Create simulated pointcloud for ICP matching
wrapped_depth = depth_scene.wrapped_render([RenderMode.DEPTH])
sim_point_cloud = phoxi_tf*phoxi.intrinsics.deproject(wrapped_depth[0])
sim_point_cloud_masked, _ = sim_point_cloud.box_mask(mask_box)
return sim_point_cloud_masked
def mesh(mesh,
T_mesh_world=RigidTransform(from_frame='obj', to_frame='world'),
style='surface',
smooth=False,
color=(0.5, 0.5, 0.5),
name=None):
"""Visualize a 3D triangular mesh.
Parameters
----------
mesh : trimesh.Trimesh
The mesh to visualize.
T_mesh_world : autolab_core.RigidTransform
The pose of the mesh, specified as a transformation from mesh frame to world frame.
style : str
Triangular mesh style, either 'surface' or 'wireframe'.
smooth : bool
If true, the mesh is smoothed before rendering.
color : 3-tuple
Color tuple.
name : str
A name for the object to be added.
"""
if not isinstance(mesh, trimesh.Trimesh):
raise ValueError('Must provide a trimesh.Trimesh object')
mp = MaterialProperties(color=np.array(color),
k_a=0.5,
k_d=0.3,
k_s=0.1,
alpha=10.0,
smooth=smooth,
wireframe=(style == 'wireframe'))
obj = SceneObject(mesh, T_mesh_world, mp)
if name is None:
name = str(uuid.uuid4())
Visualizer3D._scene.add_object(name, obj)
def load_3d_model(model_path):
# Start with an empty scene
scene = Scene()
# Add objects to the scene
# Begin by loading meshes
pawn_mesh = trimesh.load_mesh(model_path)
# Set up object's pose in the world
pawn_pose = RigidTransform(rotation=np.eye(3),
translation=np.array([0.0, 0.0, 0.0]),
from_frame='obj',
to_frame='world')
# Set up each object's material properties
pawn_material = MaterialProperties(color=np.array([1.0, 1.0, 1.0]),
k_a=1.0,
k_d=1.0,
k_s=0.0,
alpha=1.0,
smooth=False,
wireframe=False)
# Create SceneObjects for each object
pawn_obj = SceneObject(pawn_mesh, pawn_pose, pawn_material)
# Add the SceneObjects to the scene
scene.add_object('pawn', pawn_obj)
return scene, pawn_mesh
# Set up each object's pose in the world
pawn_pose = RigidTransform(rotation=np.eye(3),
translation=np.array([0.0, 0.0, 0.0]),
from_frame='obj',
to_frame='world')
bar_pose = RigidTransform(rotation=np.eye(3),
translation=np.array([0.1, 0.07, 0.00]),
from_frame='obj',
to_frame='world')
# Set up each object's material properties
pawn_material = MaterialProperties(color=5.0 * np.array([0.1, 0.1, 0.1]),
k_a=0.3,
k_d=0.5,
k_s=0.2,
alpha=10.0,
smooth=False,
wireframe=False)
#bar_material = MaterialProperties(
# color = 7.0*np.array([0.1, 0.1, 0.1]),
# k_a = 0.5,
# k_d = 0.3,
# k_s = 0.1,
# alpha = 10.0,
# smooth=False
#)
bar_material = pawn_material
# Create SceneObjects for each object
pawn_obj = SceneObject(pawn_mesh, pawn_pose, pawn_material)
plt.draw()
plt.pause(GUI_PAUSE)
# read workspace bounds
workspace_box = Box(np.array(workspace_config['min_pt']),
np.array(workspace_config['max_pt']),
frame='world')
# read workspace objects
workspace_objects = {}
for obj_key, obj_config in workspace_config['objects'].iteritems():
mesh_filename = obj_config['mesh_filename']
pose_filename = obj_config['pose_filename']
obj_mesh = trimesh.load_mesh(mesh_filename)
obj_pose = RigidTransform.load(pose_filename)
obj_mat_props = MaterialProperties(smooth=True,
wireframe=False)
scene_obj = SceneObject(obj_mesh, obj_pose, obj_mat_props)
workspace_objects[obj_key] = scene_obj
# setup each sensor
datasets = {}
sensors = {}
sensor_poses = {}
camera_intrs = {}
workspace_ims = {}
for sensor_name, sensor_config in sensor_configs.iteritems():
# read params
sensor_type = sensor_config['type']
sensor_frame = sensor_config['frame']
# read camera calib
translation=np.array([0.0, 0.0, 0.0]),
from_frame='obj',
to_frame='world'
)
sphere_pose = RigidTransform(
rotation=np.eye(3),
translation=np.array([1.0, 1.0, 0.0]),
from_frame='obj',
to_frame='world'
)
# Set up each object's material properties
cube_material = MaterialProperties(
color = np.array([0.1, 0.1, 0.5]),
k_a = 0.3,
k_d = 1.0,
k_s = 1.0,
alpha = 10.0,
smooth=False
)
sphere_material = MaterialProperties(
color = np.array([0.1, 0.1, 0.5]),
k_a = 0.3,
k_d = 1.0,
k_s = 1.0,
alpha = 10.0,
smooth=True
)
# Create SceneObjects for each object
cube_obj = SceneObject(cube_mesh, cube_pose, cube_material)
sphere_obj = SceneObject(sphere_mesh, sphere_pose, sphere_material)
# wrap the object to be rendered as a SceneObject
# rotation, translation = RigidTransform.rotation_and_translation_from_matrix(transforms[stable_pose])
# T_obj_table = RigidTransform(rotation=rotation, translation=translation)
# default pose
default_pose = RigidTransform(rotation=np.eye(3),
translation=np.array([0.0, 0.0, 0.0]),
from_frame='obj',
to_frame='world')
obj_material_properties = MaterialProperties(
color=np.array([66, 134, 244]) / 255.,
# color = 5.0*np.array([0.1, 0.1, 0.1]),
k_a=0.3,
k_d=0.5,
k_s=0.2,
alpha=10.0,
smooth=False,
wireframe=False)
obj = SceneObject(mesh, default_pose, obj_material_properties)
scene.add_object('to_render', obj)
print("ADDED OBJECT SUCCESSFULLY")
# table_obj_properties = MaterialProperties(
# color = np.array([0, 0, 0]),
# )
# wrap the table as a SceneObject
# table_mesh = trimesh.load(PLANE_MESH)
def points(points,
T_points_world=None,
color=np.array([0, 1, 0]),
scale=0.01,
n_cuts=20,
subsample=None,
random=False,
name=None):
"""Scatter a point cloud in pose T_points_world.
Parameters
----------
points : autolab_core.BagOfPoints or (n,3) float
The point set to visualize.
T_points_world : autolab_core.RigidTransform
Pose of points, specified as a transformation from point frame to world frame.
color : (3,) or (n,3) float
Color of whole cloud or per-point colors
scale : float
Radius of each point.
n_cuts : int
Number of longitude/latitude lines on sphere points.
subsample : int
Parameter of subsampling to display fewer points.
name : str
A name for the object to be added.
"""
if isinstance(points, BagOfPoints):
if points.dim != 3:
raise ValueError('BagOfPoints must have dimension 3xN!')
else:
if type(points) is not np.ndarray:
raise ValueError(
'Points visualizer expects BagOfPoints or numpy array!')
if len(points.shape) == 1:
points = points[:, np.newaxis].T
if len(points.shape) != 2 or points.shape[1] != 3:
raise ValueError(
'Numpy array of points must have dimension (N,3)')
frame = 'points'
if T_points_world:
frame = T_points_world.from_frame
points = PointCloud(points.T, frame=frame)
color = np.array(color)
if subsample is not None:
num_points = points.num_points
points, inds = points.subsample(subsample, random=random)
if color.shape[0] == num_points and color.shape[1] == 3:
color = color[inds, :]
# transform into world frame
if points.frame != 'world':
if T_points_world is None:
T_points_world = RigidTransform(from_frame=points.frame,
to_frame='world')
points_world = T_points_world * points
else:
points_world = points
point_data = points_world.data
if len(point_data.shape) == 1:
point_data = point_data[:, np.newaxis]
point_data = point_data.T
mpcolor = color
if len(color.shape) > 1:
mpcolor = color[0]
mp = MaterialProperties(color=np.array(mpcolor),
k_a=0.5,
k_d=0.3,
k_s=0.0,
alpha=10.0,
smooth=True)
# For each point, create a sphere of the specified color and size.
sphere = trimesh.creation.uv_sphere(scale, [n_cuts, n_cuts])
raw_pose_data = np.tile(np.eye(4), (points.num_points, 1))
raw_pose_data[3::4, :3] = points.data.T
instcolor = None
if color.shape[0] == points.num_points and color.shape[1] == 3:
instcolor = color
obj = InstancedSceneObject(sphere,
raw_pose_data=raw_pose_data,
colors=instcolor,
material=mp)
if name is None:
name = str(uuid.uuid4())
Visualizer3D._scene.add_object(name, obj)
def points(points,
T_points_world=None,
color=(0, 1, 0),
scale=0.01,
subsample=None,
random=False,
name=None):
"""Scatter a point cloud in pose T_points_world.
Parameters
----------
points : autolab_core.BagOfPoints or (n,3) float
The point set to visualize.
T_points_world : autolab_core.RigidTransform
Pose of points, specified as a transformation from point frame to world frame.
color : 3-tuple
Color tuple.
scale : float
Radius of each point.
subsample : int
Parameter of subsampling to display fewer points.
name : str
A name for the object to be added.
"""
if isinstance(points, BagOfPoints):
if points.dim != 3:
raise ValueError('BagOfPoints must have dimension 3xN!')
else:
if type(points) is not np.ndarray:
raise ValueError(
'Points visualizer expects BagOfPoints or numpy array!')
if len(points.shape) == 1:
points = points[:, np.newaxis].T
if len(points.shape) != 2 or points.shape[1] != 3:
raise ValueError(
'Numpy array of points must have dimension (N,3)')
frame = 'points'
if T_points_world:
frame = T_points_world.from_frame
points = PointCloud(points.T, frame=frame)
if subsample is not None:
points = points.subsample(subsample, random=random)
# transform into world frame
if points.frame != 'world':
if T_points_world is None:
T_points_world = RigidTransform(from_frame=points.frame,
to_frame='world')
points_world = T_points_world * points
else:
points_world = points
point_data = points_world.data
if len(point_data.shape) == 1:
point_data = point_data[:, np.newaxis]
point_data = point_data.T
mp = MaterialProperties(color=np.array(color),
k_a=0.5,
k_d=0.3,
k_s=0.0,
alpha=10.0,
smooth=True)
# For each point, create a sphere of the specified color and size.
sphere = trimesh.creation.uv_sphere(scale, [20, 20])
poses = []
for point in point_data:
poses.append(
RigidTransform(translation=point,
from_frame='obj',
to_frame='world'))
obj = InstancedSceneObject(sphere, poses, material=mp)
if name is None:
name = str(uuid.uuid4())
Visualizer3D._scene.add_object(name, obj)
