OpenRAVE Documentation

  • 0.8.0
  • 0.8.2
  • Documentation version: latest_stable

pr2turnlever Module

Shows how to set a workspace trajectory for the hand and have a robot plan it.

../../_images/pr2turnlever.jpg

Running the Example:

openrave.py --example pr2turnlever

Description

Shows how to instantiate a planner in python and pass in a workspace trajectory.

Command-line

Usage: openrave.py --example pr2turnlever [options]

Shows how to set a workspace trajectory for the hand and have a robot plan it.

Options:
  -h, --help            show this help message and exit
  --scene=SCENE         scene to load

  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)
    target = env.GetKinBody('lever')
    if target is None:
        target = RaveCreateKinBody(env,'')
        target.InitFromBoxes(array([[0,0.1,0,0.01,0.1,0.01]]),True)
        target.SetName('lever')
        env.Add(target)
        T = eye(4)
        T[0:3,3] = [-0.2,-0.2,1]
        target.SetTransform(T)
        
    robot = env.GetRobots()[0]

    robot.SetActiveManipulator('rightarm')
    ikmodel = databases.inversekinematics.InverseKinematicsModel(robot,iktype=IkParameterizationType.Transform6D)
    if not ikmodel.load():
        ikmodel.autogenerate()

    with env:
        robot.SetActiveDOFs(ikmodel.manip.GetArmIndices())
        basemanip=interfaces.BaseManipulation(robot)
        taskmanip=interfaces.TaskManipulation(robot)
        taskmanip.ReleaseFingers()
    robot.WaitForController(0)

    with env:
        Toffset = eye(4)
        Toffset[0:3,3] = array([0,0.2,0])
        Ttarget0 = target.GetTransform()
        Ttarget1 = dot(Ttarget0,matrixFromAxisAngle([pi/2,0,0]))

        # with target.CreateKinBodyStateSaver():
        #     target.SetTransform(Ttarget1)
        #     raw_input('as')
        Tgrasp0 = dot(matrixFromAxisAngle([pi/2,0,0]),matrixFromAxisAngle([0,pi/2,0]))
        Tgrasp0[0:3,3] = dot(Ttarget0,Toffset)[0:3,3]
        Tgraspoffset = dot(linalg.inv(Ttarget0),Tgrasp0)
        Tgrasp1 = dot(Ttarget1,Tgraspoffset)

        # check if ik solutions exist
        sol0=ikmodel.manip.FindIKSolution(Tgrasp0,IkFilterOptions.CheckEnvCollisions)
        assert(sol0 is not None)
        sol1=ikmodel.manip.FindIKSolution(Tgrasp1,IkFilterOptions.CheckEnvCollisions)
        assert(sol1 is not None)
        traj = RaveCreateTrajectory(env,'')
        spec = IkParameterization.GetConfigurationSpecificationFromType(IkParameterizationType.Transform6D,'linear')
        traj.Init(spec)
        for angle in arange(0,pi/2,0.05):
            Ttarget = dot(Ttarget0,matrixFromAxisAngle([angle,0,0]))
            Tgrasp = dot(Ttarget,Tgraspoffset)
            traj.Insert(traj.GetNumWaypoints(),poseFromMatrix(Tgrasp))

        planningutils.RetimeAffineTrajectory(traj,maxvelocities=ones(7),maxaccelerations=5*ones(7))

    h=misc.DrawAxes(env,Ttarget0)
    basemanip.MoveToHandPosition(matrices=[Tgrasp0])
    robot.WaitForController(0)
    taskmanip.CloseFingers()
    robot.WaitForController(0)

    with env:
        robot.Grab(target)
        print 'planning for turning lever'
        planner = RaveCreatePlanner(env,'workspacetrajectorytracker')
        params = Planner.PlannerParameters()
        params.SetRobotActiveJoints(robot)
        params.SetExtraParameters('<workspacetrajectory>%s</workspacetrajectory>'%traj.serialize(0))
        planner.InitPlan(robot,params)
        
        outputtraj = RaveCreateTrajectory(env,'')
        success=planner.PlanPath(outputtraj)
        assert(success)

    # also create reverse the trajectory and run infinitely
    trajectories = [outputtraj,planningutils.ReverseTrajectory(outputtraj)]
    while True:
        for traj in trajectories:
            robot.GetController().SetPath(traj)
            robot.WaitForController(0)
        if options.testmode:
            break
    

Class Definitions

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

Main example code.

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

Command-line execution of the example.

Parameters:args – arguments for script to parse, if not specified will use sys.argv
openravepy.examples.pr2turnlever.waitrobot(robot)[source]

busy wait for robot completion

Questions/Feedback

Having problems with OpenRAVE?