exampleinversereachability.py

from openravepy import *
from openravepy.databases import inversekinematics, inversereachability, grasping
import numpy
import time

manipname = 'leftarm'
numgrasps = 3
numrobotposresults = 2
transparency = 0.7
maxvelmult = 1.0

if __name__ == '__main__':
    env = Environment()
    env.Load('robots/pr2-beta-sim.robot.xml')
    env.SetViewer('qtcoin')
    time.sleep(1)
    robot = env.GetRobot('pr2')
    robot.SetActiveManipulator(manipname)
    manip = robot.GetActiveManipulator()
    
    ikmodel = inversekinematics.InverseKinematicsModel(robot=robot,iktype=IkParameterization.Type.Transform6D)
    if not ikmodel.load():
        print 'Generating ikmodel...'
        ikmodel.autogenerate()
    irmodel = inversereachability.InverseReachabilityModel(robot)
    if not irmodel.load():
        print 'Generating inversereachabilitymodel...'
        irmodel.autogenerate()
    Tbase = manip.GetBase().GetTransform()

    with env:
        newrobots = []
        for ind in range(numgrasps):
            newrobot = RaveCreateRobot(env,robot.GetXMLId())
            newrobot.Clone(robot,0)
            for link in newrobot.GetLinks():
                for geom in link.GetGeometries():
                    geom.SetTransparency(transparency)
            newrobots.append(newrobot)
            env.Add(newrobot,True)

    while True:
        Tgrasps = []
        for i in range(numgrasps):
            Tgrasp = manip.GetBase().GetTransform()
            Tgrasp[0:3,3] += numpy.random.rand(3)/3
            #axis = numpy.random.rand(3)
            #axis /= numpy.linalg.norm(axis)
            #Tgrasp[0:3,0:3] = rotationMatrixFromAxisAngle(axis, numpy.pi/6)
            Tgrasps.append((Tgrasp,0))
        with env:
            hs = []
            for Tgrasp,i in Tgrasps:
                hs.append(misc.DrawAxes(env, Tgrasp))
            ikparam = IkParameterization()
            #from IPython.terminal import embed; ipshell=embed.InteractiveShellEmbed(config=embed.load_default_config())(local_ns=locals())
            try:
                for j, baseinfo in enumerate(irmodel.sampleBaseDistributionIterator(Tgrasps,logllthresh=2.0,weight=0.2,Nprematuresamples=0,zaxis=None)):
                    robotpose = baseinfo[0]
                    graspindex = baseinfo[1]
                    jointvalues, jointindices = baseinfo[2]
                    print j
                    h = misc.DrawAxes(env, robotpose)
                    for Tgrasp, newrobot in zip(Tgrasps,newrobots):
                        print Tgrasp
                        newmanip = newrobot.GetManipulator(manipname)
                        newrobot.SetTransform(matrixFromPose(robotpose))
                        env.UpdatePublishedBodies()
                        Ttarget = Tgrasp[0]
                        ikparam.SetTransform6D(Ttarget)
                        sol = newmanip.FindIKSolution(ikparam, 0)
                        if sol is None:
                            print "failed to solve ik for Tgrasp %s using robot %s"%(Tgrasp,newrobot.GetName())
                            break
                        newrobot.SetDOFValues(sol, newmanip.GetArmIndices())
                        env.UpdatePublishedBodies()
                    time.sleep(3)
                    if j > numrobotposresults:
                        print 'go to the next base position'
                        break
            except Exception, e:
                print "failure: ", e
            time.sleep(5)