def visualize_scene(mesh, T_world_ar, T_ar_cam, T_cam_obj):
T_cam_cam = RigidTransform()
T_obj_cam = T_cam_obj.inverse()
T_cam_ar = T_ar_cam.inverse()
Twc = np.matmul(T_world_ar.matrix, T_ar_cam.matrix)
T_world_cam = RigidTransform(Twc[:3, :3], Twc[:3, 3])
T_cam_world = T_world_cam.inverse()
o = np.array([0., 0., 0.])
centroid_cam = apply_transform(o, T_cam_obj)
tag_cam = apply_transform(o, T_cam_ar)
robot_cam = apply_transform(o, T_cam_world)
# camera
vis3d.points(o, color=(0, 1, 0), scale=0.01)
vis3d.pose(T_cam_cam, alpha=0.05, tube_radius=0.005, center_scale=0.002)
# object
vis3d.mesh(mesh)
vis3d.points(centroid_cam, color=(0, 0, 0), scale=0.01)
vis3d.pose(T_cam_obj, alpha=0.05, tube_radius=0.005, center_scale=0.002)
# AR tag
vis3d.points(tag_cam, color=(1, 0, 1), scale=0.01)
vis3d.pose(T_cam_ar, alpha=0.05, tube_radius=0.005, center_scale=0.002)
vis3d.table(T_cam_ar, dim=0.074)
# robot
vis3d.points(robot_cam, color=(1, 0, 0), scale=0.01)
vis3d.pose(T_cam_world, alpha=0.05, tube_radius=0.005, center_scale=0.002)
vis3d.show()
if config['use_robot']:
# find the rightmost and further cb point in world frame
cb_points_world = T_camera_world * reg_result.cb_points_cam
cb_point_data_world = cb_points_world.data
dir_world = np.array([1.0, -1.0, 0])
dir_world = dir_world / np.linalg.norm(dir_world)
ip = dir_world.dot(cb_point_data_world)
if config['vis_cb_corners']:
_, depth_im, _ = sensor.frames()
points_world = T_camera_world * ir_intrinsics.deproject(depth_im)
vis3d.figure()
vis3d.points(cb_points_world, color=(0,0,1), scale=0.005)
vis3d.points(points_world, color=(0,1,0), subsample=10, random=True, scale=0.001)
vis3d.pose(T_camera_world)
vis3d.table(dim=0.5, T_table_world=T_cb_world)
vis3d.show()
# open interface to robot
y = YuMiRobot(tcp=YMC.TCP_SUCTION_STIFF)
y.reset_home()
time.sleep(1)
# choose target point #1
target_ind = np.where(ip == np.max(ip))[0]
target_pt_world = cb_points_world[target_ind[0]]
# create robot pose relative to target point
R_gripper_world = np.array([[1.0, 0, 0],
[0, -1.0, 0],
[0, 0, -1.0]])
def quality(self, state, actions, params=None):
"""Given a suction point, compute a score based on a best-fit 3D plane of the neighboring points.
Parameters
----------
state : :obj:`RgbdImageState`
An RgbdImageState instance that encapsulates rgbd_im, camera_intr, segmask, full_observed.
action: :obj:`SuctionPoint2D`
A suction grasp in image space that encapsulates center, approach direction, depth, camera_intr.
params: dict
Stores params used in computing suction quality.
Returns
-------
:obj:`numpy.ndarray`
Array of the quality for each grasp
"""
qualities = []
# deproject points
point_cloud_image = state.camera_intr.deproject_to_image(
state.rgbd_im.depth)
# compute negative SSE from the best fit plane for each grasp
for i, action in enumerate(actions):
if not isinstance(action, SuctionPoint2D):
raise ValueError(
'This function can only be used to evaluate suction quality'
)
points = self._points_in_window(
point_cloud_image, action,
segmask=state.segmask) # x,y in matrix A and z is vector z.
A, b = self._points_to_matrices(points)
w = self._action_to_plane(
point_cloud_image,
action) # vector w w/ a bias term represents a best-fit plane.
sse = self._sum_of_squared_residuals(w, A, b)
if params is not None and params['vis']['plane']:
from visualization import Visualizer2D as vis2d
from visualization import Visualizer3D as vis3d
mid_i = A.shape[0] / 2
pred_z = A.dot(w)
p0 = np.array([A[mid_i, 0], A[mid_i, 1], pred_z[mid_i]])
n = np.array([w[0], w[1], -1])
n = n / np.linalg.norm(n)
tx = np.array([n[1], -n[0], 0])
tx = tx / np.linalg.norm(tx)
ty = np.cross(n, tx)
R = np.array([tx, ty, n]).T
c = state.camera_intr.deproject_pixel(action.depth,
action.center)
d = Point(c.data - 0.01 * action.axis, frame=c.frame)
T_table_world = RigidTransform(rotation=R,
translation=p0,
from_frame='patch',
to_frame='world')
vis3d.figure()
vis3d.points(point_cloud_image.to_point_cloud(),
scale=0.0025,
subsample=10,
random=True,
color=(0, 0, 1))
vis3d.points(PointCloud(points.T),
scale=0.0025,
color=(1, 0, 0))
vis3d.points(c, scale=0.005, color=(1, 1, 0))
vis3d.points(d, scale=0.005, color=(1, 1, 0))
vis3d.table(T_table_world, dim=0.01)
vis3d.show()
vis2d.figure()
vis2d.imshow(state.rgbd_im.depth)
vis2d.scatter(action.center.x, action.center.y, s=50, c='b')
vis2d.show()
quality = np.exp(
-sse
) # evaluate how well best-fit plane describles all points in window.
qualities.append(quality)
return np.array(qualities)
