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 SuctionPoint2D(object):
"""Suction grasp in image space.
Attributes
----------
center : :obj:`autolab_core.Point`
Point in image space.
axis : :obj:`numpy.ndarray`
Dormalized 3-vector representing the direction of the suction tip.
depth : float
Depth of the suction point in 3D space.
camera_intr : :obj:`perception.CameraIntrinsics`
Frame of reference for camera that the suction point corresponds to.
"""
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
@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 angle(self):
"""The angle that the grasp pivot axis makes in image space."""
rotation_axis = np.cross(self.axis, np.array([0, 0, 1]))
rotation_axis_image = np.array([rotation_axis[0], rotation_axis[1]])
angle = 0
if np.linalg.norm(rotation_axis) > 0:
rotation_axis_image = rotation_axis_image / np.linalg.norm(
rotation_axis_image)
angle = np.arccos(rotation_axis_image[0])
if rotation_axis[1] < 0:
angle = -angle
return angle
@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 approach_axis(self):
return self.axis
@property
def feature_vec(self):
"""Returns the feature vector for the suction point.
Note
----
`v = [center, axis, depth]`
"""
return self.center.data
@staticmethod
def from_feature_vec(v, camera_intr=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_axis = np.array([0, 0, -1])
if v.shape[0] > 2 and axis is None:
grasp_axis = v[2:5]
grasp_axis = grasp_axis / np.linalg.norm(grasp_axis)
elif axis is not None:
grasp_axis = axis
grasp_depth = 0.5
if v.shape[0] > 5 and depth is None:
grasp_depth = v[5]
elif depth is not None:
grasp_depth = depth
# Compute center and angle.
center = Point(center_px, camera_intr.frame)
return SuctionPoint2D(center,
grasp_axis,
grasp_depth,
camera_intr=camera_intr)
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
@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
"""
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
