OpenRAVE Documentation

checkvisibility Module

Computes the visibilty extents of a camera and an object.

../../_images/checkvisibility.jpg

Running the Example:

openrave.py --example checkvisibility

Description

Uses the visibiltymodel generator and VisualFeedback - rmanipulation interface.

Command-line

Usage: openrave.py [options]

Computes if an object is visibile inside the robot cameras.

Options:
  -h, --help            show this help message and exit
  --scene=SCENE         Scene file to load
                        (default=data/testwamcamera.env.xml)

  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

Main Python Code

def main(env,options):
    "Main example code."
    env.Load(options.scene)
    # initialiation
    with env:
        robot = env.GetRobots()[0]
        # move a cup in front of another to show power of visibility
        body = env.GetKinBody('mug6')
        if body is not None:
            T = body.GetTransform()
            T[0:3,3] = [-0.14,0.146,0.938]
            body.SetTransform(T)

        vmodels = []
        print 'creating visibility structures, please wait...'
        sensors = []
        for sensor in robot.GetAttachedSensors():
            # if sensor is a camera
            if sensor.GetSensor() is not None and sensor.GetSensor().Supports(Sensor.Type.Camera):
                sensor.GetSensor().Configure(Sensor.ConfigureCommand.PowerOn)
                sensor.GetSensor().Configure(Sensor.ConfigureCommand.RenderDataOn)
                # go through all objects
                for target in env.GetBodies():
                    # load the visibility model
                    vmodel = databases.visibilitymodel.VisibilityModel(robot,target=target,sensorname=sensor.GetName())
                    if not vmodel.load():
                        vmodel.autogenerate()
                    # set internal discretization parameters 
                    vmodel.visualprob.SetParameter(raydensity=0.002,allowableocclusion=0.0)
                    vmodels.append(vmodel)
                sensors.append(sensor.GetSensor())

    print 'try moving objects in and out of the sensor view. green is inside'
    while True:
        print '-----'
        with env:
            handles = []
            for vmodel in vmodels:
                if vmodel.visualprob.ComputeVisibility():
                    # draw points around the object
                    ab = vmodel.target.ComputeAABB()
                    corners = array([[1,1,1],[1,1,-1],[1,-1,1],[1,-1,-1],[-1,1,1],[-1,1,-1],[-1,-1,1],[-1,-1,-1]],float64)
                    handles.append(env.plot3(tile(ab.pos(),(8,1))+corners*tile(ab.extents(),(8,1)),10,[0,1,0]))
                    print '%s is visible in sensor %s'%(vmodel.target.GetName(),vmodel.sensorname)
            oldhandles = handles # replace
        time.sleep(0.2)

Class Definitions

openravepy.examples.checkvisibility.main(env, options)[source]

Main example code.

openravepy.examples.checkvisibility.run(*args, **kwargs)[source]

Command-line execution of the example.

Parameters:args – arguments for script to parse, if not specified will use sys.argv

Questions/Feedback

Having problems with OpenRAVE?