OpenRAVE Documentation

Source code for openravepy.examples.tutorial_inversereachability

#!/usr/bin/env python
# -*- coding: utf-8 -*-
# Copyright (C) 2010-2011 Huan Liu (liuhuanjim013@gmail.com)
# 
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#     http://www.apache.org/licenses/LICENSE-2.0
# 
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
"""Generate and use the inverse-reachability database in OpenRAVE for the PR2 robot.

.. examplepre-block:: tutorial_inversereachability
   :image-width: 400

Prerequisite
------------

IK solvers
~~~~~~~~~~

Generate the IK solver for leftarm_torso. It takes several minutes:

.. code-block:: bash

  openrave.py --database inversekinematics --robot=robots/pr2-beta-static.zae --manipname=leftarm_torso

[optional] Download inverse-reachability database
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Get the inverse-reachability and reachability databases for OpenRAVE r1974 from::
 
  http://people.csail.mit.edu/liuhuan/pr2/openrave/openrave_database/robot.1fd7b38c8ca370ea2f8d4ab79bbb074b.tgz

and decompress the file under:

.. code-block:: bash

  ~/.openrave/

You can also `Generate database`_.

Generate database
-----------------

**Generate the databases with single core**

.. code-block:: bash

  openrave.py --database inversereachability --robot=robots/pr2-beta-static.zae --manipname=leftarm_torso --ignorefreespace 
  
This process will generate both reachability and inverse-rechability databases. It will take more than 10 hours (Intel Core2 Duo E8400 @ 3GHz with 4G of memory).

**Generate the databases with multiple cores**

If you have the openrave_planning ROS stack installed, you can try a parallelized version of the reachability database generation:

First:

.. code-block:: bash

  rosmake openrave_data
  
Then:

.. code-block:: bash

  roscore
  
Finally, in a separate terminal:

.. code-block:: bash

  rosrun openrave_database kinematicreachability_ros.py --robot=robots/pr2-beta-static.zae --manipname=leftarm_torso --ignorefreespace --launchservice='8*localhost'
  
To add another computer with 8 cores add:

.. code-block:: bash
  
  --launchservice='8*newhost'

Once the reachability database is generated, generate the inversereachability database (which will take much less time): 

.. code-block:: bash

  openrave.py --database inversereachability --robot=robots/pr2-beta-static.zae --manipname=leftarm_torso --ignorefreespace 


There are a few other parameters that can be set:

.. code-block:: python

   heightthresh = .05 # The max radius of the arm to perform the computation (default=0.05)
   quatthresh = .15 # The max radius of the arm to perform the computation (default=0.15)
   show_maxnumber = 10 # Number of robots to show simultaneously (default=20)
   id = None # Special id differentiating inversereachability models
   jointvalues = None # String of joint values that connect the robot base link to the manipulator base link
   show_transparency = .8 # Transparency of the robots to show (default=.8)

You can also `[optional] Download inverse-reachability database`_.

Visualize database
------------------

Visualize inverse-reachability database
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

This tutorial visualizes the database for one grasp, there will be another tutorial that will show how to visualize the base distribution for all possible grasps for an object. 

.. Figure:: ../../images/examples/tutorial_inversereachability_ir_grasps.png
   :width: 640
    
   The blue cloud indicates possible base positions for grasping the object.


Visualize reachability database
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Command::

   openrave.py --database kinematicreachability --robot=robots/pr2-beta-static.zae --show

.. figure:: ../../images/examples/tutorial_inversereachability_front.png
   :width: 640
    
   The color indicates reachability.

.. figure:: ../../images/examples/tutorial_inversereachability_back.png
   :width: 640 px
    
   The color indicates reachability.

Use database: Move the robot where it can perform a specific grasp
------------------------------------------------------------------

.. figure:: ../../images/examples/tutorial_inversereachability_goal.png
   :width: 640 px

   Robot in a place where it can grasp the cup.
   
Set up environment
~~~~~~~~~~~~~~~~~~

.. code-block:: python

    # set up planning environment
    env = Environment()
    env.SetViewer('qtcoin')
    
Set up goal
~~~~~~~~~~~

.. code-block:: python

    # set up goal grasp transform
    # goal grasp transform specified in global frame, this equals manip.GetTransform() in the goal state    
    O_T_grasp = array([[ -9.88017917e-01,  -1.54339954e-01 ,  0.00000000e+00 ,  1.06494129e+00],
                       [  1.54339954e-01,  -9.88017917e-01 ,  0.00000000e+00 ,  5.51449812e-05],
                       [  0.00000000e+00 ,  0.00000000e+00 ,  1.00000000e+00 ,  9.55221763e-01],
                       [  0.00000000e+00 ,  0.00000000e+00,   0.00000000e+00  , 1.00000000e+00]]) 

.. figure:: ../../images/examples/tutorial_inversereachability_goal_grasp.png
   :width: 640 px
   
   Goal grasp
   
Set up robot
~~~~~~~~~~~~

.. code-block:: python

        # initialize robot pose
        v = self.robot.GetActiveDOFValues()
        v[self.robot.GetJoint('l_shoulder_pan_joint').GetDOFIndex()]= 3.14/2
        v[self.robot.GetJoint('r_shoulder_pan_joint').GetDOFIndex()] = -3.14/2
        v[self.robot.GetJoint('l_gripper_l_finger_joint').GetDOFIndex()] = .54
        self.robot.SetActiveDOFValues(v)

.. figure:: ../../images/examples/tutorial_inversereachability_before.png
   :width: 640 px

   Robot starting state
   
Load database
~~~~~~~~~~~~~

.. code-block:: python

        # load inverserechability database
        self.irmodel = inversereachability.InverseReachabilityModel(robot=self.robot)
        starttime = time.time()
        print 'loading irmodel'
        if not self.irmodel.load():
            print 'do you want to generate irmodel for your robot? it might take several hours'
            print 'or you can go to http://people.csail.mit.edu/liuhuan/pr2/openrave/openrave_database/ to get the database for PR2'
            input = raw_input('[Y/n]')
            if input == 'y' or input == 'Y' or input == '\\n' or input == '':
                class IrmodelOption:
                self.irmodel.autogenerate()
                self.irmodel.load()
            else:
                raise ValueError('')
                
        print 'time to load inverse-reachability model: %fs'%(time.time()-starttime)

Get robot base distribution
~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. code-block:: python

        densityfn,samplerfn,bounds = self.irmodel.computeBaseDistribution(Tgrasp)

- Input for computeBaseDistribution()::

   Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame
           equals manip.GetTransform() in the goal state

- Output for computeBaseDistribution()::

   densityfn: gaussian kernel density function taking poses of openrave quaternion type, returns probabilities
   samplerfn: gaussian kernel sampler function taking number of sample and weight, returns robot base poses and joint states
   bounds: 2x3 array, bounds of samples, [[min rotation, min x, min y],[max rotation, max x, max y]]

Find valid poses from the distribution
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Code fragment from sanbox/mobilemanipulation.py

.. code-block:: python

        # initialize sampling parameters
        goals = []
        numfailures = 0
        starttime = time.time()
        timeout = inf
        with self.robot:
            while len(goals) < N:
                if time.time()-starttime > timeout:
                    break
                poses,jointstate = samplerfn(N-len(goals))
                for pose in poses:
                    self.robot.SetTransform(pose)
                    self.robot.SetDOFValues(*jointstate)
                    # validate that base is not in collision
                    if not self.manip.CheckIndependentCollision(CollisionReport()):
                        q = self.manip.FindIKSolution(Tgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions)
                        if q is not None:
                            values = self.robot.GetDOFValues()
                            values[self.manip.GetArmIndices()] = q
                            goals.append((Tgrasp,pose,values))
                        elif self.manip.FindIKSolution(Tgrasp,0) is None:
                            numfailures += 1

Move robot to valid poses
~~~~~~~~~~~~~~~~~~~~~~~~~

.. code-block:: python

        print 'showing %d results'%N
        for ind,goal in enumerate(goals):
            raw_input('press ENTER to show goal %d'%ind)
            Tgrasp,pose,values = goal
            self.robot.SetTransform(pose)
            self.robot.SetDOFValues(values)

.. figure:: ../../images/examples/tutorial_inversereachability_after.png
   :width: 640 px
   
   One sample from the goal distribution.
   
[optional] Overlay results
~~~~~~~~~~~~~~~~~~~~~~~~~~

Code fragment from `databases.inversereachability`

.. code-block:: python

        transparency = .8
        with self.env:
            self.env.Remove(self.robot)
            newrobots = []
            for goal in goals:
                Tgrasp,T,values = goal
                newrobot = RaveCreateRobot(self.env,self.robot.GetXMLId())
                newrobot.Clone(self.robot,0)
                newrobot.SetName(self.robot.GetName())
                for link in newrobot.GetLinks():
                    for geom in link.GetGeometries():
                        geom.SetTransparency(transparency)
                self.env.Add(newrobot,True)
                with self.env:
                    newrobot.SetTransform(T)
                    newrobot.SetDOFValues(values)
                newrobots.append(newrobot)


.. figure:: ../../images/examples/tutorial_inversereachability.jpg
   :width: 640 px
   
   Top view of ten samples.

.. figure:: ../../images/examples/tutorial_inversereachability_10_solutions_side.png
   :width: 640 px
   
   Side view of ten samples.
   
Related classes
---------------

- `databases.inversereachability` - Code that generates the inverse-reachability database
- `databases.kinematicreachability` - Code that generates the reachability database

.. examplepost-block:: tutorial_inversereachability
   
"""
from __future__ import with_statement # for python 2.5
__author__ = 'Huan Liu'

import time
import openravepy
if not __openravepy_build_doc__:
    from openravepy import *
    from numpy import *
else:
    from numpy import inf

[docs]class InverseReachabilityDemo: def __init__(self,robot): self.robot = robot self.env = self.robot.GetEnv() self.robot.SetActiveManipulator('leftarm_torso') self.manip = self.robot.GetActiveManipulator() # initialize robot pose v = self.robot.GetActiveDOFValues() v[self.robot.GetJoint('l_shoulder_pan_joint').GetDOFIndex()]= 3.14/2 v[self.robot.GetJoint('r_shoulder_pan_joint').GetDOFIndex()] = -3.14/2 v[self.robot.GetJoint('l_gripper_l_finger_joint').GetDOFIndex()] = .54 self.robot.SetActiveDOFValues(v) # load inverserechability database self.irmodel = databases.inversereachability.InverseReachabilityModel(robot=self.robot) starttime = time.time() print 'loading irmodel' if not self.irmodel.load(): print 'do you want to generate irmodel for your robot? it might take several hours' print 'or you can go to http://people.csail.mit.edu/liuhuan/pr2/openrave/openrave_database/ to get the database for PR2' input = raw_input('[Y/n]') if input == 'y' or input == 'Y' or input == '\n' or input == '': self.irmodel.autogenerate() self.irmodel.load() else: raise ValueError('') print 'time to load inverse-reachability model: %fs'%(time.time()-starttime) # make sure the robot and manipulator match the database assert self.irmodel.robot == self.robot and self.irmodel.manip == self.robot.GetActiveManipulator()
[docs] def showPossibleBasePoses(self,Tgrasp, gripper_angle=.548,N=1): """visualizes possible base poses for a grasp specified by Tgrasp and gripper_angle :param Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame. equals manip.GetTransform() in the goal state :param gripper_angle: float, the gripper angle :param N: int, the number of sample poses we want to get """ # setting the gripper angle v = self.robot.GetActiveDOFValues() v[self.robot.GetJoint('l_gripper_l_finger_joint').GetDOFIndex()] = gripper_angle # l gripper self.robot.SetActiveDOFValues(v) print 'showing the goal grasp' self.showGrasp(Tgrasp) # find the robot base distribution for the grasp specified by Tgrasp # Input for computeBaseDistribution(): # Tgrasp: 4x4 numpy.array, row major matrix, the grasp transform in global frame # equals manip.GetTransform() in the goal state # Output for computeBaseDistribution(): # densityfn: gaussian kernel density function taking poses of openrave quaternion type, returns probabilities # samplerfn: gaussian kernel sampler function taking number of sample and weight, returns robot base poses and joint states # bounds: 2x3 array, bounds of samples, [[min rotation, min x, min y],[max rotation, max x, max y]] densityfn,samplerfn,bounds = self.irmodel.computeBaseDistribution(Tgrasp,logllthresh=1.8) if densityfn == None: print 'the specified grasp is not reachable!' return # Code fragment from `examples.mobilemanipulation` # initialize sampling parameters goals = [] numfailures = 0 starttime = time.time() timeout = inf with self.robot: while len(goals) < N: if time.time()-starttime > timeout: break poses,jointstate = samplerfn(N-len(goals)) for pose in poses: self.robot.SetTransform(pose) self.robot.SetDOFValues(*jointstate) # validate that base is not in collision if not self.manip.CheckIndependentCollision(CollisionReport()): q = self.manip.FindIKSolution(Tgrasp,filteroptions=IkFilterOptions.CheckEnvCollisions) if q is not None: values = self.robot.GetDOFValues() values[self.manip.GetArmIndices()] = q goals.append((Tgrasp,pose,values)) elif self.manip.FindIKSolution(Tgrasp,0) is None: numfailures += 1 print 'showing %d results'%N for ind,goal in enumerate(goals): raw_input('press ENTER to show goal %d'%ind) Tgrasp,pose,values = goal self.robot.SetTransform(pose) self.robot.SetDOFValues(values) raw_input('press ENTER to show all results simultaneously') # Code fragment from `databases.inversereachability` transparency = .8 with self.env: # save the environment state self.env.Remove(self.robot) newrobots = [] for goal in goals: Tgrasp,T,values = goal newrobot = self.env.ReadRobotXMLFile(self.robot.GetXMLFilename()) newrobot.SetName(self.robot.GetName()) for link in newrobot.GetLinks(): for geom in link.GetGeometries(): geom.SetTransparency(transparency) self.env.Add(newrobot,True) newrobot.SetTransform(T) newrobot.SetDOFValues(values) newrobots.append(newrobot) print 'overlaying all results, wait for a few seconds for the viewer to update' time.sleep(10) pause()
[docs] def showGrasp(self,Tgrasp,angle=.548): """visualizes a grasp transform :param Tgrasp: a 4x4 row-major matrix in numpy.array format in global frame :param angle: between 0 and .548 """ probot = self.robot pmanip = probot.GetActiveManipulator() v = probot.GetActiveDOFValues() v[self.robot.GetJoint('l_gripper_l_finger_joint').GetDOFIndex()] = angle probot.SetActiveDOFValues(v) with databases.grasping.GraspingModel.GripperVisibility(pmanip): # show only the gripper O_T_R = probot.GetTransform() # robot transform R in global frame O O_T_G = pmanip.GetTransform() # grasping frame G in global frame O G_T_O = linalg.inv(O_T_G) # global frame O in grasping frame G G_T_R = dot(G_T_O, O_T_R) # robot frame R in grasping frame G O_T_G_goal = Tgrasp # final grasping frame G_goal in global frame O O_T_R_goal = dot(O_T_G_goal,G_T_R) # final robot transform R_goal in global frame O probot.SetTransform(O_T_R_goal) pause() probot.SetTransform(O_T_R)
[docs]def pause(): raw_input('press ENTER to continue...')
[docs]def main(env,options): "Main example code." robot = env.ReadRobotXMLFile(options.robot) env.Add(robot) if options.manipname is not None: robot.SetActiveManipulator(options.manipname) else: robot.SetActiveManipulator('leftarm') target = env.ReadKinBodyXMLFile(options.target) env.Add(target) # initialize target pose, for visualization and collision checking purpose only O_T_Target = mat([[1,0,0,1],[0,1,0,0],[0,0,1,.9],[0,0,0,1]]) target.SetTransform(array(O_T_Target)) # set up goal grasp transform # goal grasp transform specified in global frame, this equals manip.GetTransform() in the goal state O_T_grasp = array([[ -9.88017917e-01, -1.54339954e-01 , 0.00000000e+00 , 1.06494129e+00], [ 1.54339954e-01, -9.88017917e-01 , 0.00000000e+00 , 5.51449812e-05], [ 0.00000000e+00 , 0.00000000e+00 , 1.00000000e+00 , 9.55221763e-01], [ 0.00000000e+00 , 0.00000000e+00, 0.00000000e+00 , 1.00000000e+00]]) gripper_angle = .1 # use inversereachability dabase to find the possible robot base poses for the grasp gr = InverseReachabilityDemo(robot) gr.showPossibleBasePoses(O_T_grasp,gripper_angle,10)
from optparse import OptionParser from openravepy.misc import OpenRAVEGlobalArguments @openravepy.with_destroy
[docs]def run(args=None): """Command-line execution of the example. :param args: arguments for script to parse, if not specified will use sys.argv """ parser = OptionParser(description='Move base where the robot can perform target grasp using inversereachability database.') OpenRAVEGlobalArguments.addOptions(parser) parser.add_option('--robot',action="store",type='string',dest='robot',default='robots/pr2-beta-static.zae', help='Robot filename to use (default=%default)') parser.add_option('--manipname',action="store",type='string',dest='manipname',default=None, help='name of manipulator to use (default=%default)') parser.add_option('--target',action="store",type='string',dest='target',default='data/mug2.kinbody.xml', help='filename of the target to use (default=%default)') (options, leftargs) = parser.parse_args(args=args) # use default options OpenRAVEGlobalArguments.parseAndCreateThreadedUser(options,main,defaultviewer=True)
if __name__=='__main__': run()

Questions/Feedback

Having problems with OpenRAVE?