OpenRAVE Documentation

grasping Module

Simulate grasping of objects and computing force closure metrics.

../../_images/grasping.jpg

[source]

Running the Generator

openrave.py --database grasping

_databases_grasping_usage:

Usage

Dynamically generate/load the grasping set for a robot manipulator and target object:

robot.SetActiveManipulator(...)
gmodel = openravepy.databases.grasping.GraspingModel(robot,target)
if not gmodel.load():
    gmodel.autogenerate()

To compute at most 10 valid grasps in the current environment do:

validgrasps,validindices = gmodel.computeValidGrasps(returnnum=10)
validgrasp=validgrasps[0] # choose first grasp
gmodel.showgrasp(validgrasp) # show the grasp
gmodel.moveToPreshape(validgrasp) # move to the preshape of the first grasp
Tgrasp = gmodel.getGlobalGraspTransform(validgrasp,collisionfree=True) # get the grasp transform
basemanip = openravepy.interfaces.BaseManipulation(robot)
basemanip.MoveToHandPosition(matrices=[Tgrasp]) # move the robot to the grasp

It is also possible to return an iterator to the valid grasps via:

for validgrasps,validindices in gmodel.validGraspIterator():
    gmodel.showgrasp(validgrasp) # show the grasp

Description

OpenRAVE can simulate grasps for any type of robotic hand, evaluate the quality of the grasps, and use those grasps in a more complex grasp planning framework. This tutorial is meant to introduce you to the grasper plugin and the scripts provided to manage the testing and simulation. At the end, you should be able to create grasp tables and use them effectively in OpenRAVE.

A grasp is simulated by giving the end-effector an initial pose and initial joint angles (preshape). Then the end effector moves along a direction (usually along the normal of the palm) until it hits the target object. Once hit, the ‘fingers’ of the end-effector slowly close around the object until they cannot close further. The contacts between the end-effector and target object are extracted, and force closure is calculated. The Grasper - grasper is responsible for this simulation, the scripts just pass in the correct parameters to it.

Grasp set creation first tries to uniformly sample the surface of the object to determine where to the approach directions should be. Sampling the actual geometric surface of the object can lead to unwanted results due to possible concavities like the handle of a cup. A simpler approach is to take the bounding box of the object and sample its surface uniformly (see GraspingModel.computeBoxApproachRays).

../../_images/grasping_box_sampling.jpg

Once the surface of the box is sampled, the intersection of the object and a ray originating from each point going inward is taken. The normal of the object’s surface from each of these intersection points is taken to be the approaching direction of the end-effector. The red lines in the above image indicate the rays along which the end-effector will approach the cup.

../../_images/grasping_surface_sampling.jpg

Once the initial pose, preshape, and approach direction are chosen, the grasper planner is called, which queries the contact points of the grasp and analyzes them for force closure.

../../_images/barrett_grasp_strategy.jpg

Render the final configuration of the end-effector closing down on the target object along with the friction cones at each contact point (red transparent cones).

../../_images/grasping_barrett_mug1.jpg ../../_images/grasping_barrett_mug3.jpg

Calling GraspingModel.generate generates tables for a ketchup bottle.

../../_images/grasping_barrett_ketchup1.jpg ../../_images/grasping_barrett_ketchup2.jpg

Features

Here’s a short list of features of the grasper planner and problem interfaces:

  • the grasper planner takes care of initializing the robot transform given the grasp parameters. the parameters structure for the now contains: stand off, target body, roll, direction, center, and several flags for controlling internal behavior (definition in in grasper/plugindefs.h)
  • the grasper problem has two functions: grasp and computedistancemap. grasp just takes care of filling the parameters structure for the planner and returning contact points.
  • All grasp parameters like roll, direction, and center offset now specified in target body space. The user never has to transform them correspondingly anymore (this was causing many headaches before).
  • The grasp coordinate system is defined to be the manipulator’s grasp coordinate system (ie, it isn’t a link). This allows grasps to define a center of approach. Each grasp also requires an approach direction, which can be specified by the manipulationdirection parameter; if none specified, the manipulator’s direction is used.
  • Because the grasper planner reads the gripper links from the manipulator definition, it can now function correctly just by being passed the full robot. Inside the loop, the gripper is separated momentarily to complete the grasping process, the rest of the body is ignored. This allows users to test grasps on a real scene without having to introduce a floating hand into the scene.

Command-line

Usage: openrave.py --database grasping [options]

Grasp set generation example for any robot/body pair.

Options:
  -h, --help            show this help message and exit
  --plannername=PLANNERNAME
                        The grasper planner to use for this model
                        (default=none)
  --target=TARGET       The filename of the target body whose grasp set to be
                        generated (default=data/mug1.kinbody.xml)
  --boxdelta=BOXDELTA   Step size of of box surface sampling
  --spheredelta=SPHEREDELTA
                        Delta angle between directions on the sphere
  --normalanglerange=NORMALANGLERANGE
                        The range of angles around the surface normal to
                        approach from (default=0.0)
  --directiondelta=DIRECTIONDELTA
                        The average distance of approach directions for each
                        surface point in radians (default=0.4)
  --translationstepmult=TRANSLATIONSTEPMULT
                        The multiplier for translational step sizes vs
                        rotational step sizes
  --finestep=FINESTEP   The second stage step size for the joints
  --standoff=STANDOFFS  Add a standoff distance
  --roll=ROLLS          Add a roll angle
  --preshape=PRESHAPES  Add a preshape for the manipulator gripper joints
  --manipulatordirection=MANIPULATORDIRECTIONS
                        Add a direction for the gripper to face at when
                        approaching (in the manipulator coordinate system)
  --avoidlink=AVOIDLINKS
                        Add a link name to avoid at all costs (like sensor
                        links)
  --friction=FRICTION   Friction between robot and target object (default=0.3)
  --graspingnoise=GRASPINGNOISE
                        Random undeterministic noise to add to the target
                        object, represents the max possible displacement of
                        any point on the object. Noise is added after global
                        direction and start have been determined (default=0)
  --graspindex=GRASPINDEX
                        If set, then will only show this grasp index

  OpenRAVE Environment Options:
    --loadplugin=_LOADPLUGINS
                        List all plugins and the interfaces they provide.
    --collision=_COLLISION
                        Default collision checker to use
    --physics=_PHYSICS  physics engine to use (default=none)
    --viewer=_VIEWER    viewer to use (default=qtcoin)
    --server=_SERVER    server to use (default=None).
    --serverport=_SERVERPORT
                        port to load server on (default=4765).
    --module=_MODULES   module to load, can specify multiple modules. Two
                        arguments are required: "name" "args".
    -l _LEVEL, --level=_LEVEL, --log_level=_LEVEL
                        Debug level, one of
                        (fatal,error,warn,info,debug,verbose,verifyplans)
    --testmode          if set, will run the program in a finite amount of
                        time and spend computation time validating results.
                        Used for testing

  OpenRAVE Database Generator General Options:
    --show              Graphically shows the built model
    --getfilename       If set, will return the final database filename where
                        all data is stored
    --gethas            If set, will exit with 0 if datafile is generated and
                        up to date, otherwise will return a 1. This will
                        require loading the model and checking versions, so
                        might be a little slow.
    --robot=ROBOT       OpenRAVE robot to load
                        (default=robots/barrettsegway.robot.xml)
    --numthreads=NUMTHREADS
                        number of threads to compute the database with
                        (default=1)
    --manipname=MANIPNAME
                        The name of the manipulator on the robot to use

Class Definitions

class openravepy.databases.grasping.GraspingModel(robot, target, maxvelmult=None)[source]

Bases: openravepy.databases.DatabaseGenerator

Holds all functions/data related to a grasp between a robot hand and a target

static CreateOptionParser()[source]
GetLocalGraspTransform(grasp, collisionfree=False)[source]
class GripperVisibility(manip)[source]

When ‘entered’ will hide all the non-gripper links in order to facilitate visiblity of the gripper

static GraspingModel.InitializeFromParser(Model=None, parser=None, defaultviewer=True, args=[], *margs, **kwargs)[source]
GraspingModel.autogenerateparams(options=None)[source]
GraspingModel.clone(envother)[source]
GraspingModel.computeBoxApproachRays(delta=0.02, normalanglerange=0, directiondelta=0.40000000000000002)[source]
GraspingModel.computePlaneApproachRays(center, sidex, sidey, delta=0.02, normalanglerange=0, directiondelta=0.40000000000000002)[source]
GraspingModel.computeSphereApproachRays(delta=0.10000000000000001, normalanglerange=0, directiondelta=0.40000000000000002)[source]
GraspingModel.computeValidGrasps(startindex=0, checkcollision=True, checkik=True, checkgrasper=True, backupdist=0.0, returnnum=inf)[source]

Returns the set of grasps that satisfy conditions like collision-free and reachable.

Parameters:
  • returnnum – If set, will also return once that many number of grasps are found.
  • backupdist – If > 0, then will move the hand along negative approach direction and check for validity.
  • checkgrasper – If True, will execute the grasp and check if gripper only contacts target.
  • startindex – The index to start searching for grasps
  • checkik – If True will check that the grasp is reachable by the arm.
  • checkcollision – If true will return only collision-free grasps. If checkik is also True, will return grasps that have collision-free arm solutions.
GraspingModel.drawContacts(contacts, conelength=0.029999999999999999, transparency=0.5)[source]
GraspingModel.generate(*args, **kwargs)[source]

Generates all the worker items, processes them, and stores the results. For an argument list, take a look at generatepcg()

GraspingModel.generatepcg(preshapes=None, standoffs=None, rolls=None, approachrays=None, graspingnoise=None, forceclosure=True, forceclosurethreshold=1.0000000000000001e-09, checkgraspfn=None, manipulatordirections=None, translationstepmult=None, finestep=None, friction=None, avoidlinks=None, plannername=None, boxdelta=None, spheredelta=None, normalanglerange=None)[source]

Generates a grasp set by searching space and evaluating contact points.

All grasp parameters have to be in the bodies’s coordinate system (ie: approachrays). @param checkgraspfn: If set, then will be used to validate the grasp. If its evaluation returns false, then grasp will not be added to set. Called by checkgraspfn(contacts,finalconfig,grasp,info)

GraspingModel.getGlobalApproachDir(grasp)[source]

returns the global approach direction

GraspingModel.getGlobalGraspTransform(grasp, collisionfree=False)[source]

returns the final grasp transform before fingers start closing. If collisionfree is set to True, then will return a grasp that is guaranteed to be not in collision with the target object when at its preshape. This is achieved by by moving the hand back along igraspdir.

GraspingModel.getPreshape(grasp)[source]

returns the preshape joint values

GraspingModel.getfilename(read=False)[source]
GraspingModel.getversion()[source]
GraspingModel.has()[source]
GraspingModel.init(friction, avoidlinks, plannername=None)[source]
GraspingModel.load()[source]
GraspingModel.moveToPreshape(grasp, execute=True, outputtraj=None, outputtrajobj=None)[source]

uses a planner to safely move the hand to the preshape and returns the trajectory

GraspingModel.preprocess()[source]
GraspingModel.runGrasp(grasp, graspingnoise=None, translate=True, forceclosure=False, translationstepmult=None, finestep=None)[source]
GraspingModel.runGraspFromTrans(grasp)[source]

squeeze the fingers to test whether the completed grasp only collides with the target, throws an exception if it fails. Otherwise returns the Grasp parameters. Uses the grasp transformation directly.

GraspingModel.save()[source]
GraspingModel.setPreshape(grasp)[source]

sets the preshape on the robot, assumes environment is locked

GraspingModel.show(delay=0.10000000000000001, options=None, forceclosure=True, showcontacts=True)[source]
GraspingModel.showgrasp(grasp, collisionfree=False, useik=False, delay=None)[source]
GraspingModel.testGrasp(graspingnoise=None, Ngraspingtries=20, forceclosurethreshold=1.0000000000000001e-09, **kwargs)[source]
GraspingModel.validGraspIterator(startindex=0, checkcollision=True, checkik=True, checkgrasper=True, backupdist=0.0, randomgrasps=False, returnfinal=False)[source]

Returns an iterator for valid grasps that satisfy certain conditions.

Parameters:returnfinal – if True will return the contacts and finalconfig of the simulation grasp

See computeValidGrasps() for description of parameters.

openravepy.databases.grasping.RaveCreateCollisionChecker((Environment)env, (str)name) → CollisionChecker :

OPENRAVE_API CollisionCheckerBasePtr RaveCreateCollisionChecker(EnvironmentBasePtr penv, const std::string & name)

openravepy.databases.grasping.RaveCreateModule((Environment)env, (str)name) → Module :

OPENRAVE_API ModuleBasePtr RaveCreateModule(EnvironmentBasePtr penv, const std::string & name)

openravepy.databases.grasping.RaveCreateTrajectory((Environment)env, (str)name) → Trajectory :

OPENRAVE_API TrajectoryBasePtr RaveCreateTrajectory(EnvironmentBasePtr penv, const std::string & name = “” )

Return an empty trajectory instance.
openravepy.databases.grasping.RaveDestroy() → None :

OPENRAVE_API void RaveDestroy()

Destroys the entire OpenRAVE state and all loaded environments.

This functions should be always called before program shutdown in order to assure all resources are relased appropriately.

openravepy.databases.grasping.RaveFindDatabaseFile((str)arg1, (bool)arg2) → str :

OPENRAVE_API std::string RaveFindDatabaseFile(const std::string & filename, bool bRead = true )

Searches for a filename in the database and returns a full path/URL to it.

Parameters
filename -
the relative filename in the database
bRead -
if true will only return a file if it exists. If false, will return the filename of the first valid database directory.
Return
a non-empty string if a file could be found.
openravepy.databases.grasping.quatRotateDirection((object)arg1, (object)sourcedir, targetdir) → object :

RaveVector < T > quatRotateDirection(const RaveVector < T > & sourcedir, const RaveVector < T > & targetdir)

Return the minimal quaternion that orients sourcedir to targetdir.

Parameters
sourcedir -
direction of the original vector, 3 values
targetdir -
new direction, 3 values
openravepy.databases.grasping.rotationMatrixFromQuat((object)quat) → object :

RaveTransformMatrix < T > matrixFromQuat(const RaveVector < T > & quat)

Converts a quaternion to a 3x3 matrix.

Parameters
quat -
quaternion, (s,vx,vy,vz)
openravepy.databases.grasping.run(args, *margs, **kwargs)[source]

Command-line execution of the example. args specifies a list of the arguments to the script.

Questions/Feedback

Having problems with OpenRAVE?