OpenRAVE Documentation

movehandstraight Module

Tests moving the end effctor of the manipulator in straight paths.

../../_images/movehandstraight.jpg

Running the Example:

openrave.py --example movehandstraight

Description

Shows how to use the MoveHandStraight basemanipulation command. The example picks a random trajectory of the end effector and tests if this trajectory is feasible to achieve in the robot.

Command-line

Usage: openrave.py [options]

Shows how choose IK solutions so that move hand straight can move without
discontinuities.

Options:
  -h, --help            show this help message and exit
  --scene=SCENE         Scene file to load
                        (default=data/puma_tabletop.env.xml)
  --manipname=MANIPNAME
                        Choose the manipulator to perform movement for

  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)
    robot = env.GetRobots()[0]
    if options.manipname is not None:
        robot.SetActiveManipulator(options.manipname)
    with env:
        ikmodel = databases.inversekinematics.InverseKinematicsModel(robot=robot,iktype=IkParameterization.Type.Transform6D)
        if not ikmodel.load():
            ikmodel.autogenerate()
        basemanip = interfaces.BaseManipulation(robot)
        taskmanip = interfaces.TaskManipulation(robot)
        robot.SetJointValues([-0.97],ikmodel.manip.GetGripperIndices())
        Tstart = array([[ -1,  0,  0,   2.00000000e-01], [  0,0,   1, 6.30000000e-01], [  0,   1  , 0,   5.50000000e-02], [  0,0,0,1]])
        sol = ikmodel.manip.FindIKSolution(Tstart,IkFilterOptions.CheckEnvCollisions)
        robot.SetDOFValues(sol,ikmodel.manip.GetArmIndices())
    #basemanip.MoveToHandPosition([Tstart],maxiter=1000,maxtries=1,seedik=4)
    #robot.WaitForController(0)

    if len(ikmodel.manip.GetGripperIndices()) > 0:
        taskmanip.CloseFingers()
        robot.WaitForController(0)
        with env:
            target = env.GetKinBody('cylinder_green_3')
            robot.Grab(target)

    updir = array((0,0,1))
    success = basemanip.MoveHandStraight(direction=updir,stepsize=0.01,minsteps=1,maxsteps=40)
    robot.WaitForController(0)
    success = basemanip.MoveHandStraight(direction=-updir,stepsize=0.01,minsteps=1,maxsteps=40)
    robot.WaitForController(0)

    # test verification with offset (should succeed)
    T = ikmodel.manip.GetTransform()
    T[1,3] += 0.1
    success = basemanip.MoveHandStraight(direction=updir,starteematrix=T,stepsize=0.01,minsteps=1,maxsteps=20)
    robot.WaitForController(0)

    print 'checking for existance of trajectories with random queries of moving in a straight line'
    armlength = 0
    armjoints = [j for j in robot.GetDependencyOrderedJoints() if j.GetJointIndex() in ikmodel.manip.GetArmIndices()]
    eetrans = ikmodel.manip.GetTransform()[0:3,3]
    for j in armjoints[::-1]:
        armlength += sqrt(sum((eetrans-j.GetAnchor())**2))
        eetrans = j.GetAnchor()
    stepsize=0.01
    failedattempt = 0
    while True:
        with env:
            #Tee = dot(ikmodel.manip.GetTransform(),matrixFromAxisAngle(random.rand(3)-0.5,0.2*random.rand()))
            Tee = matrixFromAxisAngle(random.rand(3)-0.5,pi*random.rand())
            direction = random.rand(3)-0.5
            direction /= linalg.norm(direction)
            x = random.rand(3)-0.5
            length = 0.6*random.rand()*armlength
            Tee[0:3,3] = eetrans + x/linalg.norm(x)*(armlength-length)
            maxsteps=int(length/stepsize)+1
            minsteps = maxsteps/2
            h = env.drawlinelist(array([Tee[0:3,3],Tee[0:3,3]+direction*maxsteps*stepsize]),1)
        try:
            success = basemanip.MoveHandStraight(direction=direction,starteematrix=Tee,stepsize=stepsize,minsteps=minsteps,maxsteps=maxsteps)
            params = (direction,Tee)
            print '%d failed attemps before found'%failedattempt,repr(params)
            failedattempt = 0
            h = env.drawlinelist(array([Tee[0:3,3],Tee[0:3,3]+direction*maxsteps*stepsize]),4,[0,0,1])
            robot.WaitForController(0)
            
        except planning_error,e:
            failedattempt += 1

Class Definitions

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

Main example code.

openravepy.examples.movehandstraight.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?