def grasp(grasp, T_obj_world=RigidTransform(from_frame='obj', to_frame='world'),
tube_radius=0.002, endpoint_color=(0,1,0),
endpoint_scale=0.004, grasp_axis_color=(0,1,0)):
""" Plots a grasp as an axis and center.
Parameters
----------
grasp : :obj:`dexnet.grasping.Grasp`
the grasp to plot
T_obj_world : :obj:`autolab_core.RigidTransform`
the pose of the object that the grasp is referencing in world frame
tube_radius : float
radius of the plotted grasp axis
endpoint_color : 3-tuple
color of the endpoints of the grasp axis
endpoint_scale : 3-tuple
scale of the plotted endpoints
grasp_axis_color : 3-tuple
color of the grasp axis
"""
g1, g2 = grasp.endpoints
center = grasp.center
g1 = Point(g1, 'obj')
g2 = Point(g2, 'obj')
center = Point(center, 'obj')
g1_tf = T_obj_world.apply(g1)
g2_tf = T_obj_world.apply(g2)
center_tf = T_obj_world.apply(center)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
mlab.points3d(g1_tf.data[0], g1_tf.data[1], g1_tf.data[2], color=endpoint_color, scale_factor=endpoint_scale)
mlab.points3d(g2_tf.data[0], g2_tf.data[1], g2_tf.data[2], color=endpoint_color, scale_factor=endpoint_scale)
mlab.plot3d(grasp_axis_tf[:,0], grasp_axis_tf[:,1], grasp_axis_tf[:,2], color=grasp_axis_color, tube_radius=tube_radius)
def vis_topple_probs(vertices, topple_probs):
vis3d.figure()
env.render_3d_scene()
for vertex, prob in zip(vertices, topple_probs):
color = [min(1, 2*(1-prob)), min(2*prob, 1), 0]
vis3d.points(Point(vertex, 'world'), scale=.001, color=color)
for bottom_point in policy.toppling_model.bottom_points:
vis3d.points(Point(bottom_point, 'world'), scale=.001, color=[0,0,0])
vis3d.show(starting_camera_pose=CAMERA_POSE)
def width_px(self, camera_intr, point1, point2):
"""Returns the width in pixels."""
# Form the jaw locations in 3D space at the given depth.
p1 = Point(point1, frame='camera')
p2 = Point(point2, frame='camera')
# Project into pixel space.
u1 = camera_intr.project(p1)
u2 = camera_intr.project(p2)
return np.linalg.norm(u1.data - u2.data)
def gripper(gripper, grasp, T_obj_world, color=(0.5, 0.5, 0.5)):
""" Plots a robotic gripper in a pose specified by a particular grasp object.
Parameters
----------
gripper : :obj:`dexnet.grasping.RobotGripper`
the gripper to plot
grasp : :obj:`dexnet.grasping.Grasp`
the grasp to plot the gripper performing
T_obj_world : :obj:`autolab_core.RigidTransform`
the pose of the object that the grasp is referencing in world frame
color : 3-tuple
color of the gripper mesh
"""
T_gripper_obj = grasp.gripper_pose(gripper)
T_gripper_world = T_obj_world * T_gripper_obj
T_mesh_world = T_gripper_world * gripper.T_mesh_gripper.inverse()
T_mesh_world = T_mesh_world.as_frames('obj', 'world')
Visualizer3D.mesh(gripper.mesh.trimesh,
T_mesh_world,
style='surface',
color=color)
g1, g2 = grasp.endpoints
center = grasp.center
g1 = Point(np.array([0.1, 0.0, 0.0]), 'obj')
g2 = Point(np.array([0.0, 0.0, 0.0]), 'obj')
center = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g1_tf = T_mesh_world.apply(g1)
g2_tf = T_mesh_world.apply(g2)
center_tf = T_mesh_world.apply(center)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
points = [(x[0], x[1], x[2]) for x in grasp_axis_tf]
Visualizer3D.plot3d(points, color=(0, 1, 0), tube_radius=0.004)
g1 = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g2 = Point(np.array([0.0, 0.0, 0.1]), 'obj')
center = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g1_tf = T_mesh_world.apply(g1)
g2_tf = T_mesh_world.apply(g2)
center_tf = T_mesh_world.apply(center)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
points = [(x[0], x[1], x[2]) for x in grasp_axis_tf]
Visualizer3D.plot3d(points, color=(1, 0, 0), tube_radius=0.004)
g1 = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g2 = Point(np.array([0.0, 0.1, 0.0]), 'obj')
center = Point(np.array([0.0, 0.0, 0.0]), 'obj')
g1_tf = T_mesh_world.apply(g1)
g2_tf = T_mesh_world.apply(g2)
center_tf = T_mesh_world.apply(center)
grasp_axis_tf = np.array([g1_tf.data, g2_tf.data])
points = [(x[0], x[1], x[2]) for x in grasp_axis_tf]
Visualizer3D.plot3d(points, color=(0, 0, 1), tube_radius=0.004)
def width_px(self):
""" Returns the width in pixels. """
if self.camera_intr is None:
raise ValueError('Must specify camera intrinsics to compute gripper width in 3D space')
# form the jaw locations in 3D space at the given depth
p1 = Point(np.array([0, 0, self.depth]), frame=self.frame)
p2 = Point(np.array([self.width, 0, self.depth]), frame=self.frame)
# project into pixel space
u1 = self.camera_intr.project(p1)
u2 = self.camera_intr.project(p2)
return np.linalg.norm(u1.data - u2.data)
def from_feature_vec(v, width=0.0, camera_intr=None):
""" Creates a Grasp2D obj from a feature vector and additional parameters.
Parameters
----------
v : :obj:`numpy.ndarray`
feature vector, see Grasp2D.feature_vec
width : float
grasp opening width, in meters
camera_intr : :obj:`perception.CameraIntrinsics`
frame of reference for camera that the grasp corresponds to
"""
# read feature vec
p1 = v[:2]
p2 = v[2:4]
depth = v[4]
# compute center and angle
center_px = (p1 + p2) / 2
center = Point(center_px, camera_intr.frame)
axis = p2 - p1
if np.linalg.norm(axis) > 0:
axis = axis / np.linalg.norm(axis)
if axis[1] > 0:
angle = np.arccos(axis[0])
else:
angle = -np.arccos(axis[0])
return Grasp2D(center, angle, depth, width=width, camera_intr=camera_intr)
def get_cuboid(object_pose, cuboid_dimensions, camera_intrinsics):
# cuboid centroid
cuboid_centroid = object_pose.translation
result_json = {'cuboid_centroid': cuboid_centroid.tolist()}
# cuboid
x = cuboid_dimensions[0] / 2
y = cuboid_dimensions[1] / 2
z = cuboid_dimensions[2] / 2
# colors in nvdu_viz: b b m m g g y y (b)lue, (m)agenta, (g)reen, (y)ellow
cuboid_corners = np.array([[x, -x, -x, x, x, -x, -x, x],
[-y, -y, y, y, -y, -y, y, y],
[z, z, z, z, -z, -z, -z, -z]])
cuboid_points_model = PointCloud(cuboid_corners, 'target_model')
cuboid_points_camera = object_pose.apply(cuboid_points_model)
result_json['cuboid'] = np.transpose(cuboid_points_camera.data).tolist()
# projected_cuboid_centroid
cuboid_centroid_image_coords = project_subpixel(
camera_intrinsics, Point(cuboid_centroid, 'camera'))
result_json[
'projected_cuboid_centroid'] = cuboid_centroid_image_coords.tolist()
# projected_cuboid
cuboid_image_coords = project_subpixel(camera_intrinsics,
cuboid_points_camera)
result_json['projected_cuboid'] = np.transpose(
cuboid_image_coords).tolist()
return result_json
def project(self, camera_intr, T_camera_world, gripper_width=0.05):
# Compute pose of grasp in camera frame.
T_grasp_camera = T_camera_world.inverse() * self.T_grasp_world
y_axis_camera = T_grasp_camera.y_axis[:2]
if np.linalg.norm(y_axis_camera) > 0:
y_axis_camera = y_axis_camera / np.linalg.norm(y_axis_camera)
# Compute grasp axis rotation in image space.
rot_grasp_camera = np.arccos(y_axis_camera[0])
if y_axis_camera[1] < 0:
rot_grasp_camera = -rot_grasp_camera
while rot_grasp_camera < 0:
rot_grasp_camera += 2 * np.pi
while rot_grasp_camera > 2 * np.pi:
rot_grasp_camera -= 2 * np.pi
# Compute grasp center in image space.
t_grasp_camera = T_grasp_camera.translation
p_grasp_camera = Point(t_grasp_camera, frame=camera_intr.frame)
u_grasp_camera = camera_intr.project(p_grasp_camera)
d_grasp_camera = t_grasp_camera[2]
return Grasp2D(u_grasp_camera,
rot_grasp_camera,
d_grasp_camera,
width=gripper_width,
camera_intr=camera_intr)
def project_camera(self, T_obj_camera, camera_intr):
""" Project a grasp for a given gripper into the camera specified by a set of intrinsics.
Parameters
----------
T_obj_camera : :obj:`autolab_core.RigidTransform`
rigid transformation from the object frame to the camera frame
camera_intr : :obj:`perception.CameraIntrinsics`
intrinsics of the camera to use
"""
# compute pose of grasp in camera frame
T_grasp_camera = T_obj_camera * self.T_grasp_obj
y_axis_camera = T_grasp_camera.y_axis[:2]
if np.linalg.norm(y_axis_camera) > 0:
y_axis_camera = y_axis_camera / np.linalg.norm(y_axis_camera)
# compute grasp axis rotation in image space
rot_z = np.arccos(y_axis_camera[0])
if y_axis_camera[1] < 0:
rot_z = -rot_z
while rot_z < 0:
rot_z += 2 * np.pi
while rot_z > 2 * np.pi:
rot_z -= 2 * np.pi
# compute grasp center in image space
t_grasp_camera = T_grasp_camera.translation
p_grasp_camera = Point(t_grasp_camera, frame=camera_intr.frame)
u_grasp_camera = camera_intr.project(p_grasp_camera)
d_grasp_camera = t_grasp_camera[2]
return Grasp2D(u_grasp_camera,
rot_z,
d_grasp_camera,
width=self.open_width,
camera_intr=camera_intr)
def _get_actions(self, preds, ind, images, depths, camera_intr,
num_actions):
"""Generate the actions to be returned."""
actions = []
# TODO(vsatish): These should use the max angle instead.
ang_bin_width = GeneralConstants.PI / preds.shape[-1]
for i in range(num_actions):
im_idx = ind[i, 0]
h_idx = ind[i, 1]
w_idx = ind[i, 2]
ang_idx = ind[i, 3]
center = Point(
np.asarray([
w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2,
h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2
]))
ang = GeneralConstants.PI / 2 - (ang_idx * ang_bin_width +
ang_bin_width / 2)
depth = depths[im_idx, 0]
grasp = Grasp2D(center,
ang,
depth,
width=self._gripper_width,
camera_intr=camera_intr)
grasp_action = GraspAction(grasp, preds[im_idx, h_idx, w_idx,
ang_idx],
DepthImage(images[im_idx]))
actions.append(grasp_action)
return actions
def test_similarity_transformation(self):
R_a_b = RigidTransform.random_rotation()
t_a_b = RigidTransform.random_translation()
s_a_b = 2 * np.random.rand()
R_b_c = RigidTransform.random_rotation()
t_b_c = RigidTransform.random_translation()
s_b_c = 2 * np.random.rand()
T_a_b = SimilarityTransform(R_a_b, t_a_b, s_a_b, "a", "b")
T_b_c = SimilarityTransform(R_b_c, t_b_c, s_b_c, "b", "c")
T_b_a = T_a_b.inverse()
x_a = np.random.rand(3)
p_a = Point(x_a, "a")
p_a2 = T_b_a * T_a_b * p_a
self.assertTrue(np.allclose(p_a.data, p_a2.data))
p_b = T_a_b * p_a
p_b2 = s_a_b * (R_a_b.dot(p_a.data)) + t_a_b
self.assertTrue(np.allclose(p_b.data, p_b2))
p_c = T_b_c * T_a_b * p_a
p_c2 = s_b_c * (R_b_c.dot(p_b2)) + t_b_c
self.assertTrue(np.allclose(p_c.data, p_c2))
v_a = np.random.rand(3)
v_a = v_a / np.linalg.norm(v_a)
v_a = Direction(v_a, "a")
v_b = T_a_b * v_a
v_b2 = R_a_b.dot(v_a.data)
self.assertTrue(np.allclose(v_b.data, v_b2))
def test_point_transformation(self):
R_a_b = RigidTransform.random_rotation()
t_a_b = RigidTransform.random_translation()
T_a_b = RigidTransform(R_a_b, t_a_b, "a", "b")
x_a = np.random.rand(3)
p_a = Point(x_a, "a")
# multiply with numpy arrays
x_b = R_a_b.dot(x_a) + t_a_b
# use multiplication operator
p_b = T_a_b * p_a
self.assertTrue(
np.sum(np.abs(p_b.vector - x_b)) < 1e-5,
msg="Point transformation incorrect: Expected {}, Got {}".format(
x_b, p_b.data
),
)
# check frames
self.assertEqual(
p_b.frame, "b", msg="Transformed point has incorrect frame"
)
def __init__(self,
center,
angle=0.0,
depth=1.0,
width=0.0,
camera_intr=None,
contact_points=None,
contact_normals=None):
self.center = center
self.angle = angle
self.depth = depth
self.width = width
# If `camera_intr` is none use default primesense camera intrinsics.
'''
if not camera_intr:
self.camera_intr = CameraIntrinsics("primesense_overhead",
fx=525,
fy=525,
cx=319.5,
cy=239.5,
width=640,
height=480)
else:
'''
self.camera_intr = camera_intr
self.contact_points = contact_points
self.contact_normals = contact_normals
#frame = "image"
#if camera_intr is not None:
frame = camera_intr.frame
if isinstance(center, np.ndarray):
self.center = Point(center, frame=frame)
def __init__(self, center, axis=None, depth=1.0, camera_intr=None):
if axis is None:
axis = np.array([0, 0, 1])
self.center = center
self.axis = axis
frame = "image"
if camera_intr is not None:
frame = camera_intr.frame
if isinstance(center, np.ndarray):
self.center = Point(center, frame=frame)
if isinstance(axis, list):
self.axis = np.array(axis)
if np.abs(np.linalg.norm(self.axis) - 1.0) > 1e-3:
raise ValueError("Illegal axis. Must be norm 1.")
self.depth = depth
# If `camera_intr` is `None` use default primesense camera intrinsics.
if not camera_intr:
self.camera_intr = CameraIntrinsics("primesense_overhead",
fx=525,
fy=525,
cx=319.5,
cy=239.5,
width=640,
height=480)
else:
self.camera_intr = camera_intr
def _get_actions(self, preds, ind, images, depths, camera_intr,
num_actions):
"""Generate the actions to be returned."""
depth_im = DepthImage(images[0], frame=camera_intr.frame)
point_cloud_im = camera_intr.deproject_to_image(depth_im)
normal_cloud_im = point_cloud_im.normal_cloud_im()
actions = []
for i in range(num_actions):
im_idx = ind[i, 0]
h_idx = ind[i, 1]
w_idx = ind[i, 2]
center = Point(
np.asarray([
w_idx * self._gqcnn_stride + self._gqcnn_recep_w // 2,
h_idx * self._gqcnn_stride + self._gqcnn_recep_h // 2
]))
axis = -normal_cloud_im[center.y, center.x]
if np.linalg.norm(axis) == 0:
continue
depth = depth_im[center.y, center.x, 0]
if depth == 0.0:
continue
grasp = SuctionPoint2D(center,
axis=axis,
depth=depth,
camera_intr=camera_intr)
grasp_action = GraspAction(grasp, preds[im_idx, h_idx, w_idx, 0],
DepthImage(images[im_idx]))
actions.append(grasp_action)
return actions
def showpoint(surface_point,
T_obj_world=RigidTransform(from_frame='obj',
to_frame='world'),
color=(0.5, 0.5, 0),
scale=0.001):
""" Plots a grasp as an axis and center.
Parameters
----------
grasp : :obj:`dexnet.grasping.Grasp`
the grasp to plot
T_obj_world : :obj:`autolab_core.RigidTransform`
the pose of the object that the grasp is referencing in world frame
tube_radius : float
radius of the plotted grasp axis
endpoint_color : 3-tuple
color of the endpoints of the grasp axis
endpoint_scale : 3-tuple
scale of the plotted endpoints
grasp_axis_color : 3-tuple
color of the grasp axis
"""
surface_point = Point(surface_point, 'obj')
surface_point_tf = T_obj_world.apply(surface_point)
#center_tf = T_obj_world.apply(center)
Visualizer3D.points(surface_point_tf.data, color=color, scale=scale)
def deproject_pixel(self, depth, pixel):
"""Deprojects a single pixel with a given depth into a 3D point.
Parameters
----------
depth : float
The depth value at the given pixel location.
pixel : :obj:`autolab_core.Point`
A 2D point representing the pixel's location in the camera image.
Returns
-------
:obj:`autolab_core.Point`
The projected 3D point.
Raises
------
ValueError
If pixel is not a valid 2D Point in the same reference frame
as the camera.
"""
if not isinstance(pixel, Point) and not pixel.dim == 2:
raise ValueError(
'Must provide 2D Point object for pixel projection')
if pixel.frame != self._frame:
raise ValueError(
'Cannot deproject pixel in frame %s from camera with frame %s'
% (pixel.frame, self._frame))
point_3d = depth * np.linalg.inv(self._K).dot(np.r_[pixel.data, 1.0])
return Point(data=point_3d, frame=self._frame)
def figure_0():
action = policy.action(env.state)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.001)
mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
mesh.fix_normals()
direction = normalize([0, -.04, 0])
origin = mesh.center_mass + np.array([0, .04, .09])
intersect, _, face_ind = mesh.ray.intersects_location([origin],
[direction],
multiple_hits=False)
normal = mesh.face_normals[face_ind[0]]
start_point = intersect[0] + .06 * normal
end_point = intersect[0]
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-normal)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-normal)
head_points = [end_point - 0.02 * h2, end_point, end_point - 0.02 * h1]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.002)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.002)
vis3d.points(Point(end_point), scale=.004, color=[0, 0, 0])
hand_pose = RigidTransform(rotation=policy.get_hand_pose(
start_point, end_point)[0].rotation,
translation=start_point,
from_frame='grasp',
to_frame='world')
orig_pose = env.state.obj.T_obj_world.copy()
env.state.obj.T_obj_world = policy.toppling_model.final_poses[1]
action = policy.grasping_policy.action(env.state)
env.state.obj.T_obj_world = orig_pose
gripper = env.gripper(action)
#vis3d.mesh(gripper.mesh, hand_pose * gripper.T_mesh_grasp, color=light_blue)
# mesh = trimesh.load('~/Downloads/chris.stl')
rot = np.array([[0, -1, 0], [1, 0, 0],
[0, 0,
1]]).dot(np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]))
T = RigidTransform(rotation=rot,
translation=np.array([0, -.09, .1]),
to_frame='mesh',
from_frame='mesh')
# vis3d.mesh(mesh, hand_pose * gripper.T_mesh_grasp * T, color=light_blue)
vis3d.mesh(gripper.mesh,
hand_pose * gripper.T_mesh_grasp * T,
color=light_blue)
vis3d.show(starting_camera_pose=CAMERA_POSE)
env.state.obj.T_obj_world = policy.toppling_model.final_poses[1]
action = policy.grasping_policy.action(env.state)
print 'q:', action.q_value
env.render_3d_scene()
vis3d.gripper(env.gripper(action),
action.grasp(env.gripper(action)),
color=light_blue)
vis3d.show(starting_camera_pose=CAMERA_POSE)
def from_feature_vec(v,
camera_intr=None,
angle=None,
depth=None,
axis=None):
"""Creates a `SuctionPoint2D` instance from a feature vector and
additional parameters.
Parameters
----------
v : :obj:`numpy.ndarray`
Feature vector, see `Grasp2D.feature_vec`.
camera_intr : :obj:`perception.CameraIntrinsics`
Frame of reference for camera that the grasp corresponds to.
depth : float
Hard-set the depth for the suction grasp.
axis : :obj:`numpy.ndarray`
Normalized 3-vector specifying the approach direction.
"""
# Read feature vec.
center_px = v[:2]
grasp_angle = 0
if v.shape[0] > 2 and angle is None:
# grasp_angle = v[2]
grasp_vec = v[2:]
grasp_vec = grasp_vec / np.linalg.norm(grasp_vec)
grasp_angle = np.arctan2(grasp_vec[1], grasp_vec[0])
elif angle is not None:
grasp_angle = angle
grasp_axis = np.array([1, 0, 0])
if axis is not None:
grasp_axis = axis
grasp_depth = 0.5
if depth is not None:
grasp_depth = depth
x_axis = grasp_axis
y_axis = np.array([grasp_axis[1], -grasp_axis[0], 0])
if np.linalg.norm(y_axis) == 0:
y_axis = np.array([1, 0, 0])
y_axis = y_axis / np.linalg.norm(y_axis)
z_axis = np.cross(x_axis, y_axis)
R = np.array([x_axis, y_axis, z_axis]).T
R = R.dot(RigidTransform.x_axis_rotation(grasp_angle))
t = camera_intr.deproject_pixel(
grasp_depth, Point(center_px, frame=camera_intr.frame)).data
T = RigidTransform(rotation=R,
translation=t,
from_frame="grasp",
to_frame=camera_intr.frame)
# Compute center and angle.
return MultiSuctionPoint2D(T, camera_intr=camera_intr)
def mesh_stable_pose(mesh,
T_obj_table,
T_table_world=RigidTransform(from_frame='table',
to_frame='world'),
style='wireframe',
smooth=False,
color=(0.5, 0.5, 0.5),
dim=0.15,
plot_table=True,
plot_com=False,
name=None):
"""Visualize a mesh in a stable pose.
Parameters
----------
mesh : trimesh.Trimesh
The mesh to visualize.
T_obj_table : autolab_core.RigidTransform
Pose of object relative to table.
T_table_world : autolab_core.RigidTransform
Pose of table relative to world.
style : str
Triangular mesh style, either 'surface' or 'wireframe'.
smooth : bool
If true, the mesh is smoothed before rendering.
color : 3-tuple
Color tuple.
dim : float
The side-length for the table.
plot_table : bool
If true, a table is visualized as well.
plot_com : bool
If true, a ball is visualized at the object's center of mass.
name : str
A name for the object to be added.
Returns
-------
autolab_core.RigidTransform
The pose of the mesh in world frame.
"""
T_obj_table = T_obj_table.as_frames('obj', 'table')
T_obj_world = T_table_world * T_obj_table
Visualizer3D.mesh(mesh,
T_obj_world,
style=style,
smooth=smooth,
color=color,
name=name)
if plot_table:
Visualizer3D.table(T_table_world, dim=dim)
if plot_com:
Visualizer3D.points(Point(np.array(mesh.center_mass), 'obj'),
T_obj_world,
scale=0.01)
return T_obj_world
def pose(self, grasp_approach_dir=None):
"""Computes the 3D pose of the grasp relative to the camera.
If an approach direction is not specified then the camera
optical axis is used.
Parameters
----------
grasp_approach_dir : :obj:`numpy.ndarray`
Approach direction for the grasp in camera basis (e.g. opposite to
table normal).
Returns
-------
:obj:`autolab_core.RigidTransform`
The transformation from the grasp to the camera frame of reference.
"""
# Check intrinsics.
if self.camera_intr is None:
raise ValueError(
"Must specify camera intrinsics to compute 3D grasp pose")
# Compute 3D grasp center in camera basis.
grasp_center_im = self.center.data
center_px_im = Point(grasp_center_im, frame=self.camera_intr.frame)
grasp_center_camera = self.camera_intr.deproject_pixel(
self.depth, center_px_im)
grasp_center_camera = grasp_center_camera.data
# Compute 3D grasp axis in camera basis.
grasp_axis_im = self.axis
grasp_axis_im = grasp_axis_im / np.linalg.norm(grasp_axis_im)
grasp_axis_camera = np.array([grasp_axis_im[0], grasp_axis_im[1], 0])
grasp_axis_camera = grasp_axis_camera / np.linalg.norm(
grasp_axis_camera)
# Convert to 3D pose.
grasp_rot_camera, _, _ = np.linalg.svd(grasp_axis_camera.reshape(3, 1))
grasp_x_camera = grasp_approach_dir
if grasp_approach_dir is None:
grasp_x_camera = np.array([0, 0, 1]) # Align with camera Z axis.
grasp_y_camera = grasp_axis_camera
grasp_z_camera = np.cross(grasp_x_camera, grasp_y_camera)
grasp_z_camera = grasp_z_camera / np.linalg.norm(grasp_z_camera)
grasp_y_camera = np.cross(grasp_z_camera, grasp_x_camera)
grasp_rot_camera = np.array(
[grasp_x_camera, grasp_y_camera, grasp_z_camera]).T
if np.linalg.det(grasp_rot_camera) < 0: # Fix reflections due to SVD.
grasp_rot_camera[:, 0] = -grasp_rot_camera[:, 0]
T_grasp_camera = RigidTransform(rotation=grasp_rot_camera,
translation=grasp_center_camera,
from_frame="grasp",
to_frame=self.camera_intr.frame)
return T_grasp_camera
def figure_1():
env.state.obj.T_obj_world.translation += np.array([-.01, -.05, .001])
action = policy.action(env.state)
env.render_3d_scene()
bottom_points = policy.toppling_model.bottom_points
vis3d.plot3d(bottom_points[:2], color=[0, 0, 0], tube_radius=.0005)
vis3d.points(Point(bottom_points[0]), color=[0, 0, 0], scale=.001)
vis3d.points(Point(bottom_points[1]), color=[0, 0, 0], scale=.001)
y_dir = normalize(bottom_points[1] - bottom_points[0])
origin = policy.toppling_model.com_projected_on_edges[0] - .005 * y_dir
vis_axes(origin, y_dir)
#mesh = env.state.mesh.copy().apply_transform(env.state.T_obj_world.matrix)
#mesh.fix_normals()
#direction = normalize([-.03, -.07, 0])
#intersect, _, face_ind = mesh.ray.intersects_location([[.02, -.005, .09]], [direction], multiple_hits=False)
#normal = mesh.face_normals[face_ind[0]]
#start_point = intersect[0] + .03*normal
#end_point = intersect[0]
#shaft_points = [start_point, end_point]
#h1 = np.array([[0.7071,-0.7071,0],[0.7071,0.7071,0],[0,0,1]]).dot(-normal)
#h2 = np.array([[0.7071,0.7071,0],[-0.7071,0.7071,0],[0,0,1]]).dot(-normal)
#head_points = [end_point - 0.01*h2, end_point, end_point - 0.01*h1]
#vis3d.plot3d(shaft_points, color=[1,0,0], tube_radius=.001)
#vis3d.plot3d(head_points, color=[1,0,0], tube_radius=.001)
#vis3d.points(Point(end_point), scale=.002, color=[0,0,0])
# Center of Mass
#start_point = env.state.T_obj_world.translation - .0025*y_dir - np.array([0,0,.005])
start_point = env.state.T_obj_world.translation
end_point = start_point - np.array([0, 0, .03])
vis3d.points(Point(start_point), scale=.002, color=[0, 0, 0])
shaft_points = [start_point, end_point]
h1 = np.array([[1, 0, 0], [0, 0.7071, -0.7071], [0, 0.7071,
0.7071]]).dot(-up)
h2 = np.array([[1, 0, 0], [0, 0.7071, 0.7071], [0, -0.7071,
0.7071]]).dot(-up)
head_points = [end_point - 0.01 * h2, end_point, end_point - 0.01 * h1]
vis3d.plot3d(shaft_points, color=[1, 0, 0], tube_radius=.001)
vis3d.plot3d(head_points, color=[1, 0, 0], tube_radius=.001)
vis3d.show(starting_camera_pose=CAMERA_POSE)
def project(self, camera_intr, T_camera_world):
# Compute pose of grasp in camera frame.
T_grasp_camera = T_camera_world.inverse() * self.T_grasp_world
x_axis_camera = T_grasp_camera.x_axis
# Compute grasp center in image space.
t_grasp_camera = T_grasp_camera.translation
p_grasp_camera = Point(t_grasp_camera, frame=camera_intr.frame)
u_grasp_camera = camera_intr.project(p_grasp_camera)
d_grasp_camera = t_grasp_camera[2]
return SuctionPoint2D(u_grasp_camera,
x_axis_camera,
d_grasp_camera,
camera_intr=camera_intr)
def shownormals(normal,
surface_point,
color=(1, 0, 0),
tube_radius=0.001,
T_obj_world=RigidTransform(from_frame='obj',
to_frame='world')):
""" Plots a grasp as an axis and center.
Parameters
----------
grasp : :obj:`dexnet.grasping.Grasp`
the grasp to plot
T_obj_world : :obj:`autolab_core.RigidTransform`
the pose of the object that the grasp is referencing in world frame
tube_radius : float
radius of the plotted grasp axis
endpoint_color : 3-tuple
color of the endpoints of the grasp axis
endpoint_scale : 3-tuple
scale of the plotted endpoints
grasp_axis_color : 3-tuple
color of the grasp axis
"""
normalpoint = surface_point + 0.02 * normal
normalpoint = Point(normalpoint, 'obj')
surface_point = Point(surface_point, 'obj')
normal_tf = T_obj_world.apply(normalpoint)
surface_point_tf = T_obj_world.apply(surface_point)
normal_axis_tf = np.array([normal_tf.data, surface_point_tf.data])
# print [(x[0], x[1], x[2]) for x in normal_axis_tf]
points = [(x[0], x[1], x[2]) for x in normal_axis_tf]
Visualizer3D.plot3d(points, color=color, tube_radius=tube_radius)
def sample(self, size=1):
""" Sample random variables from the model.
Parameters
----------
size : int
number of sample to take
Returns
-------
:obj:`list` of :obj:`GraspableObject3D`
sampled graspable objects from the pose random variable
"""
samples = []
for i in range(size):
num_consecutive_failures = 0
prev_len = len(samples)
while len(samples) == prev_len:
try:
# sample random pose
obj_copy = copy.deepcopy(self.obj_)
xi = self.r_xi_rv_.rvs(size=1)
S_xi = skew(xi)
R = self.R_sample_sigma_.dot(
scipy.linalg.expm(S_xi).dot(
self.R_sample_sigma_.T.dot(
self.mean_T_obj_world_.rotation)))
s = max(self.s_rv_.rvs(size=1)[0], 0)
t = self.R_sample_sigma_.dot(self.t_rv_.rvs(size=1).T).T
z = self.R_sample_sigma_.dot(self.com_rv_.rvs(size=1))
sample_tf = SimilarityTransform(rotation=R.T,
translation=t,
scale=s)
z_tf = sample_tf * Point(z, frame=sample_tf.from_frame)
z_tf = z_tf.data
# transform object by pose
obj_sample = obj_copy.transform(sample_tf)
obj_sample.mesh.center_of_mass = z_tf
samples.append(obj_sample)
except Exception as e:
num_consecutive_failures += 1
if num_consecutive_failures > 3:
raise
# not a list if only 1 sample
if size == 1 and len(samples) > 0:
return samples[0]
return samples
def project(self, point_cloud, round_px=True):
"""Projects a point cloud onto the camera image plane.
Parameters
----------
point_cloud : :obj:`autolab_core.PointCloud` or :obj:`autolab_core.Point`
A PointCloud or Point to project onto the camera image plane.
round_px : bool
If True, projections are rounded to the nearest pixel.
Returns
-------
:obj:`autolab_core.ImageCoords` or :obj:`autolab_core.Point`
A corresponding set of image coordinates representing the given
PointCloud's projections onto the camera image plane. If the input
was a single Point, returns a 2D Point in the camera plane.
Raises
------
ValueError
If the input is not a PointCloud or Point in the same reference
frame as the camera.
"""
if not isinstance(point_cloud, PointCloud) and not (isinstance(
point_cloud, Point) and point_cloud.dim == 3):
raise ValueError(
'Must provide PointCloud or 3D Point object for projection')
if point_cloud.frame != self._frame:
raise ValueError(
'Cannot project points in frame %s into camera with frame %s' %
(point_cloud.frame, self._frame))
points_proj = self._K.dot(point_cloud.data)
if len(points_proj.shape) == 1:
points_proj = points_proj[:, np.newaxis]
point_depths = np.tile(points_proj[2, :], [3, 1])
points_proj = np.divide(points_proj, point_depths)
if round_px:
points_proj = np.round(points_proj)
if isinstance(point_cloud, Point):
return Point(data=points_proj[:2, :].astype(np.int16),
frame=self._frame)
return ImageCoords(data=points_proj[:2, :].astype(np.int16),
frame=self._frame)
def finger_slips(self, push_direction, normal, finger_friction_noise,
tipping_point_rotation):
def finger_slip_helper(normal):
parallel_component = push_direction.dot(-normal)
#perpend_component = np.linalg.norm(push_direction - normal*parallel_component)
perpend_component = math.sqrt(1 - min(parallel_component**2, 1))
# Finger slips before even starting a rotation
#return parallel_component / (perpend_component + 1e-5) <= 1/(finger_friction_noise * self.finger_friction_coeff)
return perpend_component / (
parallel_component +
1e-5) >= finger_friction_noise * self.finger_friction_coeff
if finger_slip_helper(normal):
return True
new_normal = (tipping_point_rotation * Point(normal, 'world')).data
if finger_slip_helper(new_normal):
return True
return False
def camera_intrinsics(self, T_camera_obj, f, cx, cy):
""" Generate shifted camera intrinsics to simulate cropping """
# form intrinsics
camera_intr = CameraIntrinsics(self.frame,
fx=f,
fy=f,
cx=cx,
cy=cy,
skew=0.0,
height=self.im_height,
width=self.im_width)
# compute new camera center by projecting object 0,0,0 into the camera
center_obj_obj = Point(np.zeros(3), frame='obj')
center_obj_camera = T_camera_obj * center_obj_obj
u_center_obj = camera_intr.project(center_obj_camera)
camera_shifted_intr = copy.deepcopy(camera_intr)
camera_shifted_intr.cx = 2 * camera_intr.cx - float(u_center_obj.x)
camera_shifted_intr.cy = 2 * camera_intr.cy - float(u_center_obj.y)
return camera_shifted_intr
def test_bad_transformation(self, num_points=10):
R_a_b = RigidTransform.random_rotation()
t_a_b = RigidTransform.random_translation()
T_a_b = RigidTransform(R_a_b, t_a_b, "a", "b")
# bad point frame
caught_bad_frame = False
try:
x_c = np.random.rand(3)
p_c = Point(x_c, "c")
T_a_b * p_c
except ValueError:
caught_bad_frame = True
self.assertTrue(
caught_bad_frame, msg="Failed to catch bad point frame"
)
# bad point cloud frame
caught_bad_frame = False
try:
x_c = np.random.rand(3, num_points)
pc_c = PointCloud(x_c, "c")
T_a_b * pc_c
except ValueError:
caught_bad_frame = True
self.assertTrue(
caught_bad_frame, msg="Failed to catch bad point cloud frame"
)
# illegal input
caught_bad_input = False
try:
x_a = np.random.rand(3, num_points)
T_a_b * x_a
except ValueError:
caught_bad_input = True
self.assertTrue(
caught_bad_input, msg="Failed to catch numpy array input"
)
def pose(self):
"""Computes the 3D pose of the grasp relative to the camera.
Returns
-------
:obj:`autolab_core.RigidTransform`
The transformation from the grasp to the camera frame of reference.
"""
# Check intrinsics.
if self.camera_intr is None:
raise ValueError(
"Must specify camera intrinsics to compute 3D grasp pose")
# Compute 3D grasp center in camera basis.
suction_center_im = self.center.data
center_px_im = Point(suction_center_im, frame=self.camera_intr.frame)
suction_center_camera = self.camera_intr.deproject_pixel(
self.depth, center_px_im)
suction_center_camera = suction_center_camera.data
# Compute 3D grasp axis in camera basis.
suction_axis_camera = self.axis
# Convert to 3D pose.
suction_x_camera = suction_axis_camera
suction_z_camera = np.array(
[-suction_x_camera[1], suction_x_camera[0], 0])
if np.linalg.norm(suction_z_camera) < 1e-12:
suction_z_camera = np.array([1.0, 0.0, 0.0])
suction_z_camera = suction_z_camera / np.linalg.norm(suction_z_camera)
suction_y_camera = np.cross(suction_z_camera, suction_x_camera)
suction_rot_camera = np.c_[suction_x_camera, suction_y_camera,
suction_z_camera]
T_suction_camera = RigidTransform(rotation=suction_rot_camera,
translation=suction_center_camera,
from_frame="grasp",
to_frame=self.camera_intr.frame)
return T_suction_camera