grasping¶
Classes for parallel-jaw grasping and robust grasp quality evaluation.
-
class
dexnet.grasping.
Contact3D
(graspable, contact_point, in_direction=None)¶ 3D contact points.
-
graspable
¶ GraspableObject3D
– object to use to get contact information
-
contact_point
¶ 3x1
numpy.ndarray
– point of contact on the object
-
in_direction
¶ 3x1
numpy.ndarray
– direction along which contact was made
-
normal
¶ normalized 3x1
numpy.ndarray
– surface normal at the contact point
-
friction_cone
(num_cone_faces=8, friction_coef=0.5)¶ Computes the friction cone and normal for a contact point.
Parameters: - num_cone_faces (int) – number of cone faces to use in discretization
- friction_coef (float) – coefficient of friction at contact point
Returns: - success (bool) – False when cone can’t be computed
- cone_support (
numpy.ndarray
) – array where each column is a vector on the boundary of the cone - normal (normalized 3x1
numpy.ndarray
) – outward facing surface normal
-
normal_force_magnitude
()¶ Returns the component of the force that the contact would apply along the normal direction.
Returns: magnitude of force along object surface normal Return type: float
-
reference_frame
(align_axes=True)¶ Returns the local reference frame of the contact. Z axis in the in direction (or surface normal if not specified) X and Y axes in the tangent plane to the direction
Parameters: align_axes (bool) – whether or not to align to the object axes Returns: rigid transformation from contact frame to object frame Return type: RigidTransform
-
surface_information
(width, num_steps, sigma_range=0.1, sigma_spatial=1, back_up=0.0, max_projection=0.1, direction=None, debug_objs=None, samples_per_grid=2)¶ Returns the local surface window, gradient, and curvature for a single contact.
Parameters: - width (float) – width of surface window in object frame
- num_steps (int) – number of steps to use along the in direction
- sigma_range (float) – bandwidth of bilateral range filter on window
- sigma_spatial (float) – bandwidth of gaussian spatial filter of bilateral filter
- back_up (float) – amount to back up before finding a contact in meters
- max_projection (float) – maximum amount to search forward for a contact (meters)
- direction (3x1
numpy.ndarray
) – direction along width to render the window - debug_objs (
list
) – list to put debugging info into - samples_per_grid (float) – number of samples per grid when finding contacts
Returns: surface_window – window information for local surface patch of contact on the given object
Return type: SurfaceWindow
-
surface_window_projection
(width=0.01, num_steps=21, max_projection=0.1, back_up=0.0, samples_per_grid=2.0, sigma_range=0.1, sigma_spatial=1, direction=None, compute_pca=False, vis=False, debug_objs=None)¶ Projects the local surface onto the tangent plane at a contact point.
Parameters: - width (float) – width of the window in obj frame
- num_steps (int) – number of steps to use along the in direction
- max_projection (float) – maximum amount to search forward for a contact (meters)
- back_up (float) – amount to back up before finding a contact in meters
- samples_per_grid (float) – number of samples per grid when finding contacts
- sigma_range (float) – bandwidth of bilateral range filter on window
- sigma_spatial (float) – bandwidth of gaussian spatial filter of bilateral filter
- direction (3x1
numpy.ndarray
) – dir to do the projection along
Returns: window – array of distances from tangent plane to obj, False if surface window can’t be computed
Return type: NUM_STEPSxNUM_STEPS
numpy.ndarray
-
surface_window_projection_unaligned
(width=0.01, num_steps=21, max_projection=0.1, back_up=0.0, samples_per_grid=2.0, sigma=1.5, direction=None, vis=False)¶ Projects the local surface onto the tangent plane at a contact point. Deprecated.
-
surface_window_sdf
(width=0.01, num_steps=21)¶ Returns a window of SDF values on the tangent plane at a contact point. Used for patch computation.
Parameters: - width (float) – width of the window in obj frame
- num_steps (int) – number of steps to use along the contact in direction
Returns: window – array of distances from tangent plane to obj along in direction, False if surface window can’t be computed
Return type: NUM_STEPSxNUM_STEPS
numpy.ndarray
-
tangents
(direction=None, align_axes=True, max_samples=1000)¶ Returns the direction vector and tangent vectors at a contact point. The direction vector defaults to the inward-facing normal vector at this contact. The direction and tangent vectors for a right handed coordinate frame.
Parameters: - direction (3x1
numpy.ndarray
) – direction to find orthogonal plane for - align_axes (bool) – whether or not to align the tangent plane to the object reference frame
- max_samples (int) – number of samples to use in discrete optimization for alignment of reference frame
Returns: - direction (normalized 3x1
numpy.ndarray
) – direction to find orthogonal plane for - t1 (normalized 3x1
numpy.ndarray
) – first tangent vector, x axis - t2 (normalized 3x1
numpy.ndarray
) – second tangent vector, y axis
- direction (3x1
-
torques
(forces)¶ Get the torques that can be applied by a set of force vectors at the contact point.
Parameters: forces (3xN numpy.ndarray
) – the forces applied at the contactReturns: - success (bool) – whether or not computation was successful
- torques (3xN
numpy.ndarray
) – the torques that can be applied by given forces at the contact
-
-
class
dexnet.grasping.
GraspableObject
(sdf, mesh, key=”, model_name=”, mass=1.0, convex_pieces=None)¶ Encapsulates geometric structures for computing contact in grasping.
-
sdf
¶ Sdf3D
– signed distance field, for quickly computing contact points
-
mesh
¶ Mesh3D
– 3D triangular mesh to specify object geometry, should match SDF
-
key
¶ str
– object identifier, usually given from the database
-
model_name
¶ str
– name of the object mesh as a .obj file, for use in collision checking
-
mass
¶ float – mass of the object
-
convex_pieces
¶ list
ofMesh3D
– convex decomposition of the object geom for collision checking
-
-
class
dexnet.grasping.
GraspableObject3D
(sdf, mesh, key=”, model_name=”, mass=1.0, convex_pieces=None)¶ 3D Graspable object for computing contact in grasping.
-
sdf
¶ Sdf3D
– signed distance field, for quickly computing contact points
-
mesh
¶ Mesh3D
– 3D triangular mesh to specify object geometry, should match SDF
-
key
¶ str
– object identifier, usually given from the database
-
model_name
¶ str
– name of the object mesh as a .obj file, for use in collision checking
-
mass
¶ float – mass of the object
-
convex_pieces
¶ list
ofMesh3D
– convex decomposition of the object geom for collision checking
-
moment_arm
(x)¶ Computes the moment arm to a point x.
Parameters: x (3x1 numpy.ndarray
) – point to get moment arm forReturns: Return type: 3x1 numpy.ndarray
-
rescale
(scale)¶ Rescales uniformly by a given factor.
Parameters: scale (float) – the amount to scale the object Returns: the graspable object rescaled by the given factor Return type: GraspableObject3D
-
surface_information
(grasp, width, num_steps, plot=False, direction1=None, direction2=None)¶ Returns the patches on this object for a given grasp.
Parameters: - grasp (
ParallelJawPtGrasp3D
) – grasp to get the patch information for - width (float) – width of jaw opening
- num_steps (int) – number of steps
- plot (bool) – whether to plot the intermediate computation, for debugging
- direction1 (normalized 3x1
numpy.ndarray
) – direction along which to compute the surface information for the first jaw, if None then defaults to grasp axis - direction2 (normalized 3x1
numpy.ndarray
) – direction along which to compute the surface information for the second jaw, if None then defaults to grasp axis
Returns: surface patches, one for each contact
Return type: list
ofSurfaceWindow
- grasp (
-
transform
(delta_T)¶ Transform by a delta transform.
Parameters: delta_T ( RigidTransform
) – the transformation from the current reference frame to the alternate reference frameReturns: graspable object trasnformed by the delta Return type: GraspableObject3D
-
-
class
dexnet.grasping.
ParallelJawPtGrasp3D
(configuration, frame=’object’, grasp_id=None)¶ Parallel Jaw point grasps in 3D space.
-
T_grasp_obj
¶ Rigid transformation from grasp frame to object frame. Rotation matrix is X-axis along approach direction, Y axis pointing between the jaws, and Z-axis orthogonal. Translation vector is the grasp center.
Returns: transformation from grasp to object coordinates Return type: RigidTransform
-
approach_angle
¶ float – approach angle of the grasp
-
axis
¶ numpy.ndarray
– normalized 3-vector specifying the line between the jaws
-
static
axis_from_endpoints
(g1, g2)¶ Normalized axis of grasp from endpoints as np 3-arrays
-
center
¶ numpy.ndarray
– 3-vector specifying the center of the jaws
-
static
center_from_endpoints
(g1, g2)¶ Grasp center from endpoints as np 3-arrays
-
close_fingers
(obj, vis=False, check_approach=True, approach_dist=0.2)¶ Steps along grasp axis to find the locations of contact with an object
Parameters: - obj (
GraspableObject3D
) – object to close fingers on - vis (bool) – whether or not to plot the line of action and contact points
- check_approach (bool) – whether or not to check if the contact points can be reached
- approach_dist (float) – how far back to check the approach distance, only if checking the approach is set
Returns: - obj (
-
close_width
¶ float – minimum opening width of the jaws
-
configuration
¶ numpy.ndarray
– vector specifying the parameters of the grasp as follows (grasp_center, grasp_axis, grasp_angle, grasp_width, jaw_width)
-
static
configuration_from_params
(center, axis, width, angle=0, jaw_width=0, min_width=0)¶ Converts grasp parameters to a configuration vector.
-
static
create_line_of_action
(g, axis, width, obj, num_samples, min_width=0, convert_grid=True)¶ Creates a straight line of action, or list of grid points, from a given point and direction in world or grid coords
Parameters: - g (3x1
numpy.ndarray
) – start point to create the line of action - axis (normalized 3x1
numpy.ndarray
) – normalized numpy 3 array of grasp direction - width (float) – the grasp width
- num_samples (int) – number of discrete points along the line of action
- convert_grid (bool) – whether or not the points are specified in world coords
Returns: line_of_action – coordinates to pass through in 3D space for contact checking
Return type: list
of 3x1numpy.ndarrays
- g (3x1
-
static
distance
(g1, g2, alpha=0.05)¶ Evaluates the distance between two grasps.
Parameters: - g1 (
ParallelJawPtGrasp3D
) – the first grasp to use - g2 (
ParallelJawPtGrasp3D
) – the second grasp to use - alpha (float) – parameter weighting rotational versus spatial distance
Returns: distance between grasps g1 and g2
Return type: float
- g1 (
-
endpoints
¶ returns: location of jaws in 3D space at max opening width :rtype:
numpy.ndarray
-
static
find_contact
(line_of_action, obj, vis=True)¶ Find the point at which a point traveling along a given line of action hits a surface.
Parameters: - line_of_action (
list
of 3x1numpy.ndarray
) – the points visited as the fingers close (grid coords) - obj (
GraspableObject3D
) – to check contacts on - vis (bool) – whether or not to display the contact check (for debugging)
Returns: - contact_found (bool) – whether or not the point contacts the object surface
- contact (
Contact3D
) – found along line of action (None if contact not found)
- line_of_action (
-
frame
¶ str
– name of grasp reference frame
-
grasp_angles_from_stp_z
(stable_pose)¶ Get angles of the the grasp from the table plane: 1) the angle between the grasp axis and table normal 2) the angle between the grasp approach axis and the table normal
Parameters: stable_pose ( StablePose
orRigidTransform
) – the stable pose to compute the angles forReturns: - psi (float) – grasp y axis rotation from z axis in stable pose
- phi (float) – grasp x axis rotation from z axis in stable pose
-
static
grasp_from_contact_and_axis_on_grid
(obj, grasp_c1_world, grasp_axis_world, grasp_width_world, grasp_angle=0, jaw_width_world=0, min_grasp_width_world=0, vis=False, backup=0.5)¶ Creates a grasp from a single contact point in grid coordinates and direction in grid coordinates.
Parameters: - obj (
GraspableObject3D
) – object to create grasp for - grasp_c1_grid (3x1
numpy.ndarray
) – contact point 1 in world - grasp_axis (normalized 3x1
numpy.ndarray
) – normalized direction of the grasp in world - grasp_width_world (float) – grasp_width in world coords
- jaw_width_world (float) – width of jaws in world coords
- min_grasp_width_world (float) – min closing width of jaws
- vis (bool) – whether or not to visualize the grasp
Returns: - obj (
-
static
grasp_from_endpoints
(g1, g2, width=None, approach_angle=0, close_width=0)¶ Create a grasp from given endpoints in 3D space, making the axis the line between the points.
Parameters: - g1 (
numpy.ndarray
) – location of the first jaw - g2 (
numpy.ndarray
) – location of the second jaw - width (float) – maximum opening width of jaws
- approach_angle (float) – approach angle of grasp
- close_width (float) – width of gripper when fully closed
- g1 (
-
grasp_y_axis_offset
(theta)¶ Return a new grasp with the given approach angle.
Parameters: theta (float) – approach angle for the new grasp Returns: grasp with the given approach angle Return type: ParallelJawPtGrasp3D
-
gripper_pose
(gripper=None)¶ Returns the RigidTransformation from the gripper frame to the object frame when the gripper is executing the given grasp. Differs from the grasp reference frame because different robots use different conventions for the gripper reference frame.
Parameters: gripper ( RobotGripper
) – gripper to get the pose forReturns: transformation from gripper frame to object frame Return type: RigidTransform
-
id
¶ int – id of grasp
-
jaw_width
¶ float – width of the jaws in the tangent plane to the grasp axis
-
open_width
¶ float – maximum opening width of the jaws
-
parallel_table
(stable_pose)¶ Returns a grasp with approach_angle set to be perpendicular to the table normal specified in the given stable pose.
Parameters: stable_pose ( StablePose
) – the pose specifying the tableReturns: aligned grasp Return type: ParallelJawPtGrasp3D
-
static
params_from_configuration
(configuration)¶ Converts configuration vector into grasp parameters.
Returns: - grasp_center (
numpy.ndarray
) – center of grasp in 3D space - grasp_axis (
numpy.ndarray
) – normalized axis of grasp in 3D space - max_width (float) – maximum opening width of jaws
- angle (float) – approach angle
- jaw_width (float) – width of jaws
- min_width (float) – minimum closing width of jaws
- grasp_center (
-
perpendicular_table
(stable_pose)¶ Returns a grasp with approach_angle set to be aligned width the table normal specified in the given stable pose.
Parameters: stable_pose ( StablePose
orRigidTransform
) – the pose specifying the orientation of the tableReturns: aligned grasp Return type: ParallelJawPtGrasp3D
-
project_camera
(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 (
autolab_core.RigidTransform
) – rigid transformation from the object frame to the camera frame - camera_intr (
perception.CameraIntrinsics
) – intrinsics of the camera to use
- T_obj_camera (
-
rotated_full_axis
¶ Rotation matrix from canonical grasp reference frame to object reference frame. X axis points out of the gripper palm along the grasp approach angle, Y axis points between the jaws, and the Z axs is orthogonal.
Returns: rotation matrix of grasp Return type: numpy.ndarray
-
surface_information
(graspable, width=0.02, num_steps=21, direction=None)¶ Return the patch surface information at the contacts that this grasp makes on a graspable.
Parameters: - graspable (
GraspableObject3D
) – object to get surface information for - width (float) – width of the window in obj frame
- num_steps (int) – number of steps
Returns: surface patches, one for each contact
Return type: list
ofSurfaceWindow
- graspable (
-
unrotated_full_axis
¶ Rotation matrix from canonical grasp reference frame to object reference frame. X axis points out of the gripper palm along the 0-degree approach direction, Y axis points between the jaws, and the Z axs is orthogonal.
Returns: rotation matrix of grasp Return type: numpy.ndarray
-
static
width_from_endpoints
(g1, g2)¶ Width of grasp from endpoints
-
-
class
dexnet.grasping.
Grasp
¶ Abstract grasp class.
-
configuration
¶ numpy.ndarray
– vector specifying the parameters of the grasp (e.g. hand pose, opening width, joint angles, etc)
-
frame
¶ str
– string name of grasp reference frame (defaults to obj)
-
close_fingers
(obj)¶ Finds the contact points by closing on the given object.
Parameters: obj ( GraspableObject3D
) – object to find contacts on
-
configuration
() Returns the numpy array representing the hand configuration
-
static
configuration_from_params
(*params)¶ Convert param list to a configuration vector for the class
-
frame
() Returns the string name of the grasp reference frame
-
static
params_from_configuration
(configuration)¶ Convert configuration vector to a set of params for the class
-
-
class
dexnet.grasping.
PointGrasp
¶ Abstract grasp class for grasps with a point contact model.
-
configuration
¶ numpy.ndarray
– vector specifying the parameters of the grasp (e.g. hand pose, opening width, joint angles, etc)
-
frame
¶ str
– string name of grasp reference frame (defaults to obj)
-
create_line_of_action
(g, axis, width, obj, num_samples)¶ Creates a line of action, or the points in space that the grasp traces out, from a point g in world coordinates on an object.
Returns: - bool – whether or not successful
list
ofnumpy.ndarray
– points in 3D space along the line of action
-
-
class
dexnet.grasping.
RobotGripper
(name, mesh, mesh_filename, params, T_mesh_gripper, T_grasp_gripper)¶ Robot gripper wrapper for collision checking and encapsulation of grasp parameters (e.g. width, finger radius, etc) Note: The gripper frame should be the frame used to command the physical robot
-
name
¶ str
– name of gripper
-
mesh
¶ Mesh3D
– 3D triangular mesh specifying the geometry of the gripper
-
params
¶ dict
– set of parameters for the gripper, at minimum (finger_radius and grasp_width)
-
T_mesh_gripper
¶ RigidTransform
– transform from mesh frame to gripper frame (for rendering)
-
T_grasp_gripper
¶ RigidTransform
– transform from gripper frame to the grasp canonical frame (y-axis = grasp axis, x-axis = palm axis)
-
collides_with_table
(grasp, stable_pose, clearance=0.0)¶ Checks whether or not the gripper collides with the table in the stable pose. No longer necessary with CollisionChecker.
Parameters: - grasp (
ParallelJawPtGrasp3D
) – grasp parameterizing the pose of the gripper - stable_pose (
StablePose
) – specifies the pose of the table - clearance (float) – min distance from the table
Returns: True if collision, False otherwise
Return type: bool
- grasp (
-
static
load
(gripper_name, gripper_dir=’data/grippers’)¶ Load the gripper specified by gripper_name.
Parameters: - gripper_name (
str
) – name of the gripper to load - gripper_dir (
str
) – directory where the gripper files are stored
Returns: loaded gripper objects
Return type: - gripper_name (
-
-
class
dexnet.grasping.
PointGraspMetrics3D
¶ Class to wrap functions for quasistatic point grasp quality metrics.
-
static
ferrari_canny_L1
(forces, torques, normals, soft_fingers=False, params=None, wrench_norm_thresh=0.001, wrench_regularizer=1e-10)¶ Ferrari & Canny’s L1 metric. Also known as the epsilon metric.
Parameters: - forces (3xN
numpy.ndarray
) – set of forces on object in object basis - torques (3xN
numpy.ndarray
) – set of torques on object in object basis - normals (3xN
numpy.ndarray
) – surface normals at the contact points - soft_fingers (bool) – whether or not to use the soft finger contact model
- params (
GraspQualityConfig
) – set of parameters for grasp matrix and contact model - wrench_norm_thresh (float) – threshold to use to determine equivalence of target wrenches
- wrench_regularizer (float) – small float to make quadratic program positive semidefinite
Returns: float
Return type: value of metric
- forces (3xN
-
static
force_closure
(c1, c2, friction_coef, use_abs_value=True)¶ ” Checks force closure using the antipodality trick.
Parameters: Returns: int
Return type: 1 if in force closure, 0 otherwise
-
static
force_closure_qp
(forces, torques, normals, soft_fingers=False, wrench_norm_thresh=0.001, wrench_regularizer=1e-10, params=None)¶ Checks force closure by solving a quadratic program (whether or not zero is in the convex hull)
Parameters: - forces (3xN
numpy.ndarray
) – set of forces on object in object basis - torques (3xN
numpy.ndarray
) – set of torques on object in object basis - normals (3xN
numpy.ndarray
) – surface normals at the contact points - soft_fingers (bool) – whether or not to use the soft finger contact model
- wrench_norm_thresh (float) – threshold to use to determine equivalence of target wrenches
- wrench_regularizer (float) – small float to make quadratic program positive semidefinite
- params (
GraspQualityConfig
) – set of parameters for grasp matrix and contact model
Returns: int
Return type: 1 if in force closure, 0 otherwise
- forces (3xN
-
static
grasp_isotropy
(forces, torques, normals, soft_fingers=False, params=None)¶ Condition number of grasp matrix - ratio of “weakest” wrench that the grasp can exert to the “strongest” one.
Parameters: - forces (3xN
numpy.ndarray
) – set of forces on object in object basis - torques (3xN
numpy.ndarray
) – set of torques on object in object basis - normals (3xN
numpy.ndarray
) – surface normals at the contact points - soft_fingers (bool) – whether or not to use the soft finger contact model
- params (
GraspQualityConfig
) – set of parameters for grasp matrix and contact model
Returns: float
Return type: value of grasp isotropy metric
- forces (3xN
-
static
grasp_matrix
(forces, torques, normals, soft_fingers=False, finger_radius=0.005, params=None)¶ Computes the grasp map between contact forces and wrenchs on the object in its reference frame.
Parameters: - forces (3xN
numpy.ndarray
) – set of forces on object in object basis - torques (3xN
numpy.ndarray
) – set of torques on object in object basis - normals (3xN
numpy.ndarray
) – surface normals at the contact points - soft_fingers (bool) – whether or not to use the soft finger contact model
- finger_radius (float) – the radius of the fingers to use
- params (
GraspQualityConfig
) – set of parameters for grasp matrix and contact model
Returns: G – grasp map
Return type: 6xM
numpy.ndarray
- forces (3xN
-
static
grasp_quality
(grasp, obj, params, vis=False)¶ Computes the quality of a two-finger point grasps on a given object using a quasi-static model.
Parameters: - grasp (
ParallelJawPtGrasp3D
) – grasp to evaluate - obj (
GraspableObject3D
) – object to evaluate quality on - params (
GraspQualityConfig
) – parameters of grasp quality function
- grasp (
-
static
min_norm_vector_in_facet
(facet, wrench_regularizer=1e-10)¶ Finds the minimum norm point in the convex hull of a given facet (aka simplex) by solving a QP.
Parameters: - facet (6xN
numpy.ndarray
) – vectors forming the facet - wrench_regularizer (float) – small float to make quadratic program positive semidefinite
Returns: - float – minimum norm of any point in the convex hull of the facet
- Nx1
numpy.ndarray
– vector of coefficients that achieves the minimum
- facet (6xN
-
static
min_singular
(forces, torques, normals, soft_fingers=False, params=None)¶ Min singular value of grasp matrix - measure of wrench that grasp is “weakest” at resisting.
Parameters: - forces (3xN
numpy.ndarray
) – set of forces on object in object basis - torques (3xN
numpy.ndarray
) – set of torques on object in object basis - normals (3xN
numpy.ndarray
) – surface normals at the contact points - soft_fingers (bool) – whether or not to use the soft finger contact model
- params (
GraspQualityConfig
) – set of parameters for grasp matrix and contact model
Returns: float
Return type: value of smallest singular value
- forces (3xN
-
static
partial_closure
(forces, torques, normals, soft_fingers=False, wrench_norm_thresh=0.001, wrench_regularizer=1e-10, params=None)¶ Evalutes partial closure: whether or not the forces and torques can resist a specific wrench. Estimates resistance by sollving a quadratic program (whether or not the target wrench is in the convex hull).
Parameters: - forces (3xN
numpy.ndarray
) – set of forces on object in object basis - torques (3xN
numpy.ndarray
) – set of torques on object in object basis - normals (3xN
numpy.ndarray
) – surface normals at the contact points - soft_fingers (bool) – whether or not to use the soft finger contact model
- wrench_norm_thresh (float) – threshold to use to determine equivalence of target wrenches
- wrench_regularizer (float) – small float to make quadratic program positive semidefinite
- params (
GraspQualityConfig
) – set of parameters for grasp matrix and contact model
Returns: int
Return type: 1 if in partial closure, 0 otherwise
- forces (3xN
-
static
wrench_in_positive_span
(wrench_basis, target_wrench, force_limit, num_fingers=1, wrench_norm_thresh=0.0001, wrench_regularizer=1e-10)¶ Check whether a target can be exerted by positive combinations of wrenches in a given basis with L1 norm fonger force limit limit.
Parameters: - wrench_basis (6xN
numpy.ndarray
) – basis for the wrench space - target_wrench (6x1
numpy.ndarray
) – target wrench to resist - force_limit (float) – L1 upper bound on the forces per finger (aka contact point)
- num_fingers (int) – number of contacts, used to enforce L1 finger constraint
- wrench_norm_thresh (float) – threshold to use to determine equivalence of target wrenches
- wrench_regularizer (float) – small float to make quadratic program positive semidefinite
Returns: - int – whether or not wrench can be resisted
- float – minimum norm of the finger forces required to resist the wrench
- wrench_basis (6xN
-
static
wrench_resistance
(forces, torques, normals, soft_fingers=False, wrench_norm_thresh=0.001, wrench_regularizer=1e-10, finger_force_eps=1e-09, params=None)¶ Evalutes wrench resistance: the inverse norm of the contact forces required to resist a target wrench Estimates resistance by sollving a quadratic program (min normal contact forces to produce a wrench).
Parameters: - forces (3xN
numpy.ndarray
) – set of forces on object in object basis - torques (3xN
numpy.ndarray
) – set of torques on object in object basis - normals (3xN
numpy.ndarray
) – surface normals at the contact points - soft_fingers (bool) – whether or not to use the soft finger contact model
- wrench_norm_thresh (float) – threshold to use to determine equivalence of target wrenches
- wrench_regularizer (float) – small float to make quadratic program positive semidefinite
- finger_force_eps (float) – small float to prevent numeric issues in wrench resistance metric
- params (
GraspQualityConfig
) – set of parameters for grasp matrix and contact model
Returns: float
Return type: value of wrench resistance metric
- forces (3xN
-
static
wrench_volume
(forces, torques, normals, soft_fingers=False, params=None)¶ Volume of grasp matrix singular values - score of all wrenches that the grasp can resist.
Parameters: - forces (3xN
numpy.ndarray
) – set of forces on object in object basis - torques (3xN
numpy.ndarray
) – set of torques on object in object basis - normals (3xN
numpy.ndarray
) – surface normals at the contact points - soft_fingers (bool) – whether or not to use the soft finger contact model
- params (
GraspQualityConfig
) – set of parameters for grasp matrix and contact model
Returns: float
Return type: value of wrench volume
- forces (3xN
-
static
-
class
dexnet.grasping.
GraspQualityConfig
(config)¶ Base wrapper class for parameters used in grasp quality computation. Used to elegantly enforce existence and type of required parameters.
-
config
¶ dict
– dictionary mapping parameter names to parameter values
-
check_valid
(config)¶ Raise an exception if the config is missing required keys
-
contains
(key)¶ Checks whether or not the key is supported
-
-
class
dexnet.grasping.
QuasiStaticGraspQualityConfig
(config)¶ Parameters for quasi-static grasp quality computation.
-
config
¶ dict
– dictionary mapping parameter names to parameter values
Notes
Required configuration key-value pairs in Other Parameters.
Other Parameters: - quality_method (
str
) – string name of quasi-static quality metric - friction_coef (float) – coefficient of friction at contact point
- num_cone_faces (int) – number of faces to use in friction cone approximation
- soft_fingers (bool) – whether to use a soft finger model
- quality_type (
str
) – string name of grasp quality type (e.g. quasi-static, robust quasi-static) - check_approach (bool) – whether or not to check the approach direction
-
-
class
dexnet.grasping.
RobustQuasiStaticGraspQualityConfig
(config)¶ Parameters for quasi-static grasp quality computation.
-
config
¶ dict
– dictionary mapping parameter names to parameter values
Notes
Required configuration key-value pairs in Other Parameters.
Other Parameters: - quality_method (
str
) – string name of quasi-static quality metric - friction_coef (float) – coefficient of friction at contact point
- num_cone_faces (int) – number of faces to use in friction cone approximation
- soft_fingers (bool) – whether to use a soft finger model
- quality_type (
str
) – string name of grasp quality type (e.g. quasi-static, robust quasi-static) - check_approach (bool) – whether or not to check the approach direction
- num_quality_samples (int) – number of samples to use
-
-
class
dexnet.grasping.
GraspQualityConfigFactory
¶ Helper class to automatically create grasp quality configurations of different types.
-
static
create_config
(config)¶ Automatically create a quality config from a dictionary.
Parameters: config ( dict
) – dictionary mapping parameter names to parameter values
-
static
-
class
dexnet.grasping.
GraspSampler
(gripper, config)¶ Base class for various methods to sample a number of grasps on an object. Should not be instantiated directly.
-
gripper
¶ RobotGripper
– the gripper to compute grasps for
-
config
¶ YamlConfig
– configuration for the grasp sampler
-
generate_grasps
(graspable, target_num_grasps=None, grasp_gen_mult=5, max_iter=3, sample_approach_angles=False, vis=False, **kwargs)¶ Samples a set of grasps for an object.
Parameters: - graspable (
GraspableObject3D
) – the object to grasp - target_num_grasps (int) – number of grasps to return, defualts to self.target_num_grasps
- grasp_gen_mult (int) – number of additional grasps to generate
- max_iter (int) – number of attempts to return an exact number of grasps before giving up
- sample_approach_angles (bool) – whether or not to sample approach angles
Returns: list of generated grasps
Return type: list
ofParallelJawPtGrasp3D
- graspable (
-
generate_grasps_stable_poses
(graspable, stable_poses, target_num_grasps=None, grasp_gen_mult=5, max_iter=3, sample_approach_angles=False, vis=False, **kwargs)¶ Samples a set of grasps for an object, aligning the approach angles to the object stable poses.
Parameters: - graspable (
GraspableObject3D
) – the object to grasp - stable_poses (
list
ofmeshpy.StablePose
) – list of stable poses for the object with ids read from the database - target_num_grasps (int) – number of grasps to return, defualts to self.target_num_grasps
- grasp_gen_mult (int) – number of additional grasps to generate
- max_iter (int) – number of attempts to return an exact number of grasps before giving up
- sample_approach_angles (bool) – whether or not to sample approach angles
Returns: list of generated grasps
Return type: list
ofParallelJawPtGrasp3D
- graspable (
-
sample_grasps
(graspable)¶ Create a list of candidate grasps for a given object. Must be implemented for all grasp sampler classes.
Parameters: graspable ( GraspableObject3D
) – object to sample grasps on
-
-
class
dexnet.grasping.
UniformGraspSampler
(gripper, config)¶ Sample grasps by sampling pairs of points on the object surface uniformly at random.
-
sample_grasps
(graspable, num_grasps, vis=False, max_num_samples=1000)¶ Returns a list of candidate grasps for graspable object using uniform point pairs from the SDF
Parameters: - graspable (
GraspableObject3D
) – the object to grasp - num_grasps (int) – the number of grasps to generate
Returns: list of generated grasps
Return type: list
ofParallelJawPtGrasp3D
- graspable (
-
-
class
dexnet.grasping.
GaussianGraspSampler
(gripper, config)¶ Sample grasps by sampling a center from a gaussian with mean at the object center of mass and grasp axis by sampling the spherical angles uniformly at random.
-
sample_grasps
(graspable, num_grasps, vis=False, sigma_scale=2.5)¶ Returns a list of candidate grasps for graspable object by Gaussian with variance specified by principal dimensions.
Parameters: - graspable (
GraspableObject3D
) – the object to grasp - num_grasps (int) – the number of grasps to generate
- sigma_scale (float) – the number of sigmas on the tails of the Gaussian for each dimension
Returns: :obj:`list` of obj – list of generated grasps
Return type: ParallelJawPtGrasp3D
- graspable (
-
-
class
dexnet.grasping.
AntipodalGraspSampler
(gripper, config)¶ Samples antipodal pairs using rejection sampling. The proposal sampling ditribution is to choose a random point on the object surface, then sample random directions within the friction cone, then form a grasp axis along the direction, close the fingers, and keep the grasp if the other contact point is also in the friction cone.
-
perturb_point
(x, scale)¶ Uniform random perturbations to a point
-
sample_from_cone
(n, tx, ty, num_samples=1)¶ Samples directoins from within the friction cone using uniform sampling.
Parameters: - n (3x1 normalized
numpy.ndarray
) – surface normal - tx (3x1 normalized
numpy.ndarray
) – tangent x vector - ty (3x1 normalized
numpy.ndarray
) – tangent y vector - num_samples (int) – number of directions to sample
Returns: v_samples – sampled directions in the friction cone
Return type: list
of 3x1numpy.ndarray
- n (3x1 normalized
-
sample_grasps
(graspable, num_grasps, vis=False)¶ Returns a list of candidate grasps for graspable object.
Parameters: - graspable (
GraspableObject3D
) – the object to grasp - num_grasps (int) – number of grasps to sample
- vis (bool) – whether or not to visualize progress, for debugging
Returns: the sampled grasps
Return type: list
ofParallelJawPtGrasp3D
- graspable (
-
within_cone
(cone, n, v)¶ Checks whether or not a direction is in the friction cone. This is equivalent to whether a grasp will slip using a point contact model.
Parameters: - cone (3xN
numpy.ndarray
) – supporting vectors of the friction cone - n (3x1
numpy.ndarray
) – outward pointing surface normal vector at c1 - v (3x1
numpy.ndarray
) – direction vector
Returns: - in_cone (bool) – True if alpha is within the cone
- alpha (float) – the angle between the normal and v
- cone (3xN
-
-
class
dexnet.grasping.
GraspableObjectPoseGaussianRV
(obj, mean_T_obj_world, config)¶ Random variable for sampling graspable objects in different poses, to model uncertainty in object registration.x
-
s_rv
¶ scipy.stats.norm
– Gaussian random variable for object scale
-
t_rv
¶ scipy.stats.multivariate_normal
– multivariate Gaussian random variable for object translation
-
r_xi_rv
¶ scipy.stats.multivariate_normal
– multivariate Gaussian random variable of object rotations over the Lie Algebra
-
R_sample_sigma
¶ 3x3
numpy.ndarray
– rotation from the sampling reference frame to the random variable reference frame (e.g. for use with uncertainty only in the plane of the table)
-
sample
(size=1)¶ Sample random variables from the model.
Parameters: size (int) – number of sample to take Returns: sampled graspable objects from the pose random variable Return type: list
ofGraspableObject3D
-
-
class
dexnet.grasping.
ParallelJawGraspPoseGaussianRV
(grasp, config)¶ Random variable for sampling grasps in different poses, to model uncertainty in robot repeatability
-
t_rv
¶ scipy.stats.multivariate_normal
– multivariate Gaussian random variable for grasp translation
-
r_xi_rv
¶ scipy.stats.multivariate_normal
– multivariate Gaussian random variable of grasp rotations over the Lie Algebra
-
R_sample_sigma
¶ 3x3
numpy.ndarray
– rotation from the sampling reference frame to the random variable reference frame (e.g. for use with uncertainty only in the plane of the table)
-
sample
(size=1)¶ Sample random variables from the model.
Parameters: size (int) – number of sample to take Returns: sampled grasps in various poses Return type: list
ofParallelJawPtGrasp3D
-
-
class
dexnet.grasping.
ParamsGaussianRV
(params, u_config)¶ Random variable for sampling a Gaussian set of parameters.
-
rvs
¶ dict
mapping string paramter names toscipy.stats.multivariate_normal
– multivariate Gaussian random variables of different paramters
-
sample
(size=1)¶ Sample random variables from the model.
Parameters: size (int) – number of sample to take Returns: list of sampled dictionaries of parameters Return type: list
ofdict
-
-
class
dexnet.grasping.
QuasiStaticGraspQualityRV
(grasp_rv, obj_rv, params_rv, quality_config)¶ RV class for grasp quality on an object.
-
grasp_rv
¶ ParallelJawGraspPoseGaussianRV
– random variable for gripper pose
-
obj_rv
¶ GraspableObjectPoseGaussianRV
– random variable for object pose
-
params_rv
¶ ParamsGaussianRV
– random variable for a set of grasp quality parameters
-
quality_config
¶ GraspQualityConfig
– parameters for grasp quality computation
-
sample
(size=1)¶ Samples deterministic quasi-static point grasp quality metrics.
Parameters: size (int) – number of samples to take
-
-
class
dexnet.grasping.
RobustPointGraspMetrics3D
¶ Class to wrap functions for robust quasistatic point grasp quality metrics.
-
static
expected_quality
(grasp_rv, graspable_rv, params_rv, quality_config)¶ Compute robustness, or the expected grasp quality wrt given random variables.
Parameters: - grasp_rv (
ParallelJawGraspPoseGaussianRV
) – random variable for gripper pose - obj_rv (
GraspableObjectPoseGaussianRV
) – random variable for object pose - params_rv (
ParamsGaussianRV
) – random variable for a set of grasp quality parameters - quality_config (
GraspQualityConfig
) – parameters for grasp quality computation
Returns: - float – mean quality
- float – variance of quality samples
- grasp_rv (
-
static
-
class
dexnet.grasping.
GraspQualityResult
(quality, uncertainty=0.0, quality_config=None)¶ Stores the results of grasp quality computation.
-
quality
¶ float – value of quality
-
uncertainty
¶ float – uncertainty estimate of the quality value
-
quality_config
¶
-
-
class
dexnet.grasping.
GraspQualityFunction
(graspable, quality_config)¶ Abstraction for grasp quality functions to make scripts for labeling with quality functions simple and readable.
-
graspable
¶ GraspableObject3D
– object to evaluate grasp quality on
-
quality_config
¶ GraspQualityConfig
– set of parameters to evaluate grasp quality
-
quality
(grasp)¶ Compute grasp quality.
Parameters: grasp ( GraspableObject3D
) – grasp to quality quality onReturns: result of quality computation Return type: GraspQualityResult
-
-
class
dexnet.grasping.
QuasiStaticQualityFunction
(graspable, quality_config)¶ Grasp quality metric using a quasi-static model.
-
quality
(grasp)¶ Compute grasp quality using a quasistatic method.
Parameters: grasp ( GraspableObject3D
) – grasp to quality quality onReturns: result of quality computation Return type: GraspQualityResult
-
-
class
dexnet.grasping.
RobustQuasiStaticQualityFunction
(graspable, quality_config, T_obj_world=RigidTransform(rotation=[[ 1. 0. 0.] [ 0. 1. 0.] [ 0. 0. 1.]], translation=[ 0. 0. 0.], from_frame=obj, to_frame=world))¶ Grasp quality metric using a robust quasi-static model (average over random perturbations)
-
quality
(grasp)¶ Compute grasp quality using a robust quasistatic method.
Parameters: grasp ( GraspableObject3D
) – grasp to quality quality onReturns: result of quality computation Return type: GraspQualityResult
-
-
class
dexnet.grasping.
OpenRaveCollisionChecker
(env=None, view=False, win_height=1200, win_width=1200, cam_dist=0.5)¶ Wrapper for collision checking with OpenRAVE
-
in_collision
(names=None)¶ Checks whether there are any pairwise collisions between objects in the environment.
Parameters: names ( list
ofstr
) – names of target objects to check collisions withReturns: True if a collision occurs, False otherwise Return type: bool
-
in_collision_single
(target_name, names=None)¶ Checks whether a target object collides with a given set of objects in the environment.
Parameters: - target_name (
str
) – name of target object to check collisions for - names (
list
ofstr
) – names of target objects to check collisions with
Returns: True if a collision occurs, False otherwise
Return type: bool
- target_name (
-
remove_object
(name)¶ Remove an object from the collision checking environment.
Parameters: name ( str
) – name of object to remove
-
set_object
(name, filename, T_world_obj=None)¶ Add an object to the collision checking environment.
Parameters: - name (
str
) – name of object to remove - filename (
str
) – filename of triangular mesh (e.g. .STL or .OBJ) - T_world_obj (
autolab_core.RigidTransform
) – transformation from object to world frame
- name (
-
set_transform
(name, T_world_obj)¶ Set the pose of an object in the environment.
Parameters: - name (
str
) – name of object to move - T_world_obj (
autolab_core.RigidTransform
) – transformation from object to world frame
- name (
-
-
class
dexnet.grasping.
GraspCollisionChecker
(gripper, env=None, view=False, win_height=1200, win_width=1200, cam_dist=0.5)¶ Collision checker that automatcially handles grasp objects.
-
add_graspable_object
(graspable, T_obj_world=RigidTransform(rotation=[[ 1. 0. 0.] [ 0. 1. 0.] [ 0. 0. 1.]], translation=[ 0. 0. 0.], from_frame=obj, to_frame=world))¶ Adds the target object to the environment.
Parameters: - graspable (
GraspableObject3D
) – the object to add - T_obj_world (
autolab_core.RigidTransform
) – the transformation from obj to world frame
- graspable (
-
collides_along_approach
(grasp, approach_dist, delta_approach, key=None)¶ Checks whether a grasp collides along its approach direction. Currently assumes that the collision checker has loaded the object.
Parameters: - grasp (
ParallelJawPtGrasp3D
) – grasp to check collisions for - approach_dist (float) – how far back to check along the approach direction
- delta_approach (float) – how finely to discretize poses along the approach direction
- key (str) – key of object to grasp
Returns: whether or not the grasp is in collision
Return type: bool
- grasp (
-
grasp_in_collision
(T_obj_gripper, key=None)¶ Check collision of grasp with target object.
Parameters: - T_obj_gripper (
autolab_core.RigidTransform
) – pose of the gripper w.r.t the object - key (str) – key of object to grasp
Returns: True if the grasp is in collision, False otherwise
Return type: bool
- T_obj_gripper (
-
obj_names
¶ List of object names
-
set_graspable_object
(graspable, T_obj_world=RigidTransform(rotation=[[ 1. 0. 0.] [ 0. 1. 0.] [ 0. 0. 1.]], translation=[ 0. 0. 0.], from_frame=obj, to_frame=world))¶ Adds and sets the target object in the environment.
Parameters: graspable ( GraspableObject3D
) – the object to grasp
-
set_table
(filename, T_table_world)¶ Set the table geometry and position in the environment.
Parameters: - filename (
str
) – name of table mesh file (e.g. .STL or .OBJ) - T_table_world (
autolab_core.RigidTransform
) – pose of table w.r.t. world
- filename (
-
set_target_object
(key)¶ Sets the target graspable object.
-