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
class MultiSuctionPoint2D(object):
"""Multi-Cup Suction grasp in image space.
Equivalent to projecting a 6D pose to image space.
Attributes
----------
pose : :obj:`autolab_core.RigidTransform`
Pose in 3D camera space.
camera_intr : :obj:`perception.CameraIntrinsics`
Frame of reference for camera that the suction point corresponds to.
"""
def __init__(self, pose, camera_intr=None):
self._pose = pose
# TODO(vsatish): Confirm that this is really not needed.
# frame = "image"
# if camera_intr is not None:
# frame = camera_intr.frame
# 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 pose(self):
return self._pose
@property
def frame(self):
"""The name of the frame of reference for the grasp."""
if self.camera_intr is None:
raise ValueError("Must specify camera intrinsics")
return self.camera_intr.frame
@property
def center(self):
center_camera = Point(self._pose.translation,
frame=self.camera_intr.frame)
center_px = self.camera_intr.project(center_camera)
return center_px
@property
def axis(self):
return self._pose.x_axis
@property
def approach_axis(self):
return self.axis
@property
def approach_angle(self):
"""The angle between the grasp approach axis and camera optical axis.
"""
dot = max(min(self.axis.dot(np.array([0, 0, 1])), 1.0), -1.0)
return np.arccos(dot)
@property
def angle(self):
g_axis = self._pose.y_axis
g_axis_im = np.array([g_axis[0], g_axis[1], 0])
if np.linalg.norm(g_axis_im) == 0:
return 0
theta = np.arctan2(g_axis[1], g_axis[0])
return theta
@property
def depth(self):
return self._pose.translation[2]
@property
def orientation(self):
x_axis = self.axis
y_axis = np.array([x_axis[1], -x_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
delta_R = R.T.dot(self._pose.rotation)
orientation = np.arccos(delta_R[1, 1])
if delta_R[1, 2] > 0:
orientation = 2 * np.pi - orientation
return orientation
@property
def feature_vec(self):
"""Returns the feature vector for the suction point.
Note
----
`v = [center, axis, depth]`
"""
return np.r_[self.center.data,
np.cos(self.orientation),
np.sin(self.orientation)]
@staticmethod
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)
@staticmethod
def image_dist(g1, g2, alpha=1.0):
"""Computes the distance between grasps in image space.
Uses Euclidean distance with alpha weighting of angles.
Parameters
----------
g1 : :obj:`SuctionPoint2D`
First suction point.
g2 : :obj:`SuctionPoint2D`
Second suction point.
alpha : float
Weight of angle distance (rad to meters).
Returns
-------
float
Distance between grasps.
"""
# Point to point distances.
point_dist = np.linalg.norm(g1.center.data - g2.center.data)
# Axis distances.
dot = max(min(np.abs(g1.axis.dot(g2.axis)), 1.0), -1.0)
axis_dist = np.arccos(dot)
return point_dist + alpha * axis_dist
class Grasp2D(object):
"""Parallel-jaw grasp in image space.
Attributes
----------
center : :obj:`autolab_core.Point`
Point in image space.
angle : float
Grasp axis angle with the camera x-axis.
depth : float
Depth of the grasp center in 3D space.
width : float
Distance between the jaws in meters.
camera_intr : :obj:`perception.CameraIntrinsics`
Frame of reference for camera that the grasp corresponds to.
contact_points : list of :obj:`numpy.ndarray`
Pair of contact points in image space.
contact_normals : list of :obj:`numpy.ndarray`
Pair of contact normals in image space.
"""
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)
@property
def axis(self):
"""Returns the grasp axis."""
return np.array([np.cos(self.angle), np.sin(self.angle)])
@property
def approach_axis(self):
return np.array([0, 0, 1])
@property
def approach_angle(self):
"""The angle between the grasp approach axis and camera optical axis.
"""
return 0.0
@property
def frame(self):
"""The name of the frame of reference for the grasp."""
if self.camera_intr is None:
raise ValueError("Must specify camera intrinsics")
return self.camera_intr.frame
@property
def width_px(self):
"""Returns the width in pixels."""
if self.camera_intr is None:
missing_camera_intr_msg = ("Must specify camera intrinsics to"
" compute gripper width in 3D space.")
raise ValueError(missing_camera_intr_msg)
# 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)
@property
def endpoints(self):
"""Returns the grasp endpoints."""
p1 = self.center.data - (self.width_px / 2) * self.axis
p2 = self.center.data + (self.width_px / 2) * self.axis
return p1, p2
@property
def feature_vec(self):
"""Returns the feature vector for the grasp.
`v = [p1, p2, depth]` where `p1` and `p2` are the jaw locations in
image space.
"""
p1, p2 = self.endpoints
return np.r_[p1, p2, self.depth]
@staticmethod
def from_feature_vec(v, width=0.0, camera_intr=None):
"""Creates a `Grasp2D` instance 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 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
@staticmethod
def image_dist(g1, g2, alpha=1.0):
"""Computes the distance between grasps in image space.
Uses Euclidean distance with alpha weighting of angles
Parameters
----------
g1 : :obj:`Grasp2D`
First grasp.
g2 : :obj:`Grasp2D`
Second grasp.
alpha : float
Weight of angle distance (rad to meters).
Returns
-------
float
Distance between grasps.
"""
# Point to point distances.
point_dist = np.linalg.norm(g1.center.data - g2.center.data)
# Axis distances.
dot = max(min(np.abs(g1.axis.dot(g2.axis)), 1.0), -1.0)
axis_dist = np.arccos(dot)
return point_dist + alpha * axis_dist
class Grasp2D(object):
"""
Parallel-jaw grasp in image space.
Attributes
----------
center : :obj:`autolab_core.Point`
point in image space
angle : float
grasp axis angle with the camera x-axis
depth : float
depth of the grasp center in 3D space
width : float
distance between the jaws in meters
camera_intr : :obj:`perception.CameraIntrinsics`
frame of reference for camera that the grasp corresponds to
"""
def __init__(self, center, angle, depth, width=0.0, camera_intr=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
@property
def axis(self):
""" Returns the grasp axis. """
return np.array([np.cos(self.angle), np.sin(self.angle)])
@property
def frame(self):
""" The name of the frame of reference for the grasp. """
if self.camera_intr is None:
raise ValueError('Must specify camera intrinsics')
return self.camera_intr.frame
@property
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)
@property
def endpoints(self):
""" Returns the grasp endpoints """
p1 = self.center.data - (float(self.width_px) / 2) * self.axis
p2 = self.center.data + (float(self.width_px) / 2) * self.axis
return p1, p2
@property
def feature_vec(self):
""" Returns the feature vector for the grasp.
v = [p1, p2, depth]
where p1 and p2 are the jaw locations in image space
"""
p1, p2 = self.endpoints
return np.r_[p1, p2, self.depth]
@staticmethod
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 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]) # aligned with camera Z axis
grasp_y_camera = grasp_axis_camera
grasp_z_camera = np.cross(grasp_x_camera, grasp_y_camera)
grasp_x_camera = np.cross(grasp_z_camera, grasp_y_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 possible 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
@staticmethod
def image_dist(g1, g2, alpha=1.0):
""" Computes the distance between grasps in image space.
Euclidean distance with alpha weighting of angles
Parameters
----------
g1 : :obj:`Grasp2D`
first grasp
g2 : :obj:`Grasp2D`
second grasp
alpha : float
weight of angle distance (rad to meters)
Returns
-------
float
distance between grasps
"""
# point to point distances
point_dist = np.linalg.norm(g1.center.data - g2.center.data)
# axis distances
axis_dist = np.arccos(np.abs(g1.axis.dot(g2.axis)))
return point_dist + alpha * axis_dist
