コード例 #1
0
def test_graspreachability():
    import mobilemanipulation,graspplanning
    env = Environment()
    env.SetViewer('qtcoin')
    env.Reset()
    env.Load('data/lab1.env.xml')
    robot = env.GetRobots()[0]
    planning = graspplanning.GraspPlanning(robot)
    gmodel=planning.graspables[0][0]
    dests=planning.graspables[0][1]
    self = mobilemanipulation.GraspReachability(robot=robot,gmodel=gmodel)
    starttime = time.time()
    densityfn,samplerfn,bounds,validgrasps = self.computeGraspDistribution(logllthresh=2.4)
    print('time to build distribution: %fs'%(time.time()-starttime))
    h = self.irmodel.showBaseDistribution(densityfn,bounds,self.target.GetTransform()[2,3],thresh=1.0)
コード例 #2
0
def test_hrp2():
    python convexdecomposition.py --volumeSplitThresholdPercent=5 --mergeThresholdPercent=10 --padding=0.005
    rosrun openrave_database kinematicreachability_ros.py --manipname=leftarm --xyzdelta=0.04 --launchservice='8*localhost' 
    python kinematicreachability.py --manipname=rightarm --xyzdelta=0.02
    python kinematicreachability.py --manipname=leftarm --xyzdelta=0.02
    python kinematicreachability.py --manipname=rightarm_chest --xyzdelta=0.02
    python inversereachability.py --manipname=rightarm --heightthresh=0.02 --quatthresh=0.2
    python inversereachability.py --manipname=leftarm --heightthresh=0.02 --quatthresh=0.2
    python inversereachability.py --manipname=rightarm_chest --heightthresh=0.02 --quatthresh=0.2
    python inversereachability.py --manipname=leftarm_chest --heightthresh=0.02 --quatthresh=0.2
    python inversereachability.py --manipname=leftarm_chest --heightthresh=0.02 --quatthresh=0.2 --id=0 --jointvalues='0'
    python inversereachability.py --manipname=leftarm_chest --heightthresh=0.02 --quatthresh=0.2 --id=43 --jointvalues='0.43'
    python grasping.py --robot=robots/hrp2jsk.robot.xml --manipname=rightarm --target=scenes/cereal_frootloops.kinbody.xml --standoff=0 --boxdelta=0.01 --normalanglerange=1 --avoidlink=RWristCam
    python grasping.py --robot=robots/hrp2jsk.robot.xml --manipname=leftarm --target=scenes/cereal_frootloops.kinbody.xml --standoff=0 --boxdelta=0.01 --normalanglerange=1 --graspingnoise=0.01 --noviewer
    rosrun openrave_database grasping_ros.py --robot=robots/hrp2jsk.robot.xml --manipname=leftarm_chest --target=scenes/cereal_frootloops.kinbody.xml --standoff=0 --boxdelta=0.01 --normalanglerange=1 --graspingnoise=0.01 --launchservice='8*localhost'
    rosrun openrave_database grasping_ros.py --robot=robots/hrp2jsk.robot.xml --manipname=leftarm_chest2 --target=scenes/jskcup0.kinbody.xml --standoff=0 --boxdelta=0.01 --normalanglerange=1 --graspingnoise=0.01 --launchservice='8*localhost'

    import inversereachability
    env = Environment()
    robot = env.ReadRobotXMLFile('robots/hrp2jsk.robot.xml')
    env.AddRobot(robot)
    robot.SetActiveManipulator('leftarm')
    self = inversereachability.InverseReachabilityModel(robot=robot)
    heightthresh=0.02
    quatthresh=0.1
    self.generate(heightthresh=heightthresh,quatthresh=quatthresh)

    hand = env.ReadRobotXMLFile('robots/hrp2rhandjsk.robot.xml')
    env.AddRobot(hand)
    hand.SetTransform(Tgrasp)

    # test head movement
    import inversekinematics
    env = Environment()
    robot = env.ReadRobotXMLFile('robots/hrp2jsk08.robot.xml')
    env.AddRobot(robot)
    robot.SetActiveManipulator('head')
    manip = robot.GetActiveManipulator()
    ikmodel = inversekinematics.InverseKinematicsModel(robot,IkParameterization.Type.Direction3D)
    if not ikmodel.load():
        ikmodel.generate()
    
    import inversereachability,mobilemanipulation,graspplanning,visibilitymodel
    env = Environment()
    env.SetViewer('qtcoin')
    env.Reset()
    env.Load('scenes/r602kitchen1.env.xml')
    robot = env.GetRobots()[0]
    origjointvalues = robot.GetJointValues()
    # define all the manipulators to use
    manips = [robot.GetManipulators('rightarm_chest')[0], robot.GetManipulators('rightarm_chest2')[0]]#,robot.GetManipulators('leftarm_chest')[0], robot.GetManipulators('leftarm_chest2')[0]]
    irmodels = []
    with robot:
        for manip in manips:
            robot.SetActiveManipulator(manip)
            dofindices = inversereachability.InverseReachabilityModel.getdofindices(manip)
            for id,value in [('0',[0]),('43',[0.43]),('n43',[-0.43])]:
                robot.SetJointValues(value,dofindices)
                irmodel = inversereachability.InverseReachabilityModel(robot=robot,id=id)
                if irmodel.load():
                    irmodels.append(irmodel)
                else:
                    print('failed to load irmodel',manip.GetName(),id)
    irgmodels = []
    targets = []
    for manip in manips:
        robot.SetActiveManipulator(manip)
        planning = graspplanning.GraspPlanning(robot,nodestinations=True)
        for gmodel,dests in planning.graspables:
            if True:#gmodel.target.GetName() == 'cereal0' or gmodel.target.GetName() == 'cereal1':
                for irmodel in irmodels:
                    if irmodel.manip == gmodel.manip:
                        irgmodels.append([irmodel,gmodel])
                        if not gmodel.target in targets:
                            targets.append(gmodel.target)
    grmodel = mobilemanipulation.GraspReachability(robot=robot,irgmodels=irgmodels)
    self = mobilemanipulation.MobileManipulationPlanning(robot,grmodel=grmodel)

    usevisibilitycamera = 'wristcam'
    gmodel = self.graspObjectMobileSearch(usevisibilitycamera=usevisibilitycamera)

    table = env.GetKinBody('table')
    if table is not None:
        graspables = None
        Trolls = [matrixFromAxisAngle(array((0,0,1)),roll) for roll in arange(0,2*pi,pi/4)]
        alldests = graspplanning.GraspPlanning.setRandomDestinations(targets,table,transdelta=0.05,Trolls=Trolls,randomize=False)
        targetdests=zip(targets,alldests)
        self.graspAndPlaceObjectMobileSearch(targetdests=targetdests)

    
    #h = gr.showBaseDistribution(thresh=1.0,logllthresh=logllthresh)
    #grmodel.testSampling(weight=1.5,logllthresh=0.5,randomgrasps=True,randomplacement=False,updateenv=False)
    
    validgrasps,validindices = gr.gmodel.computeValidGrasps(checkik=False,backupdist=0.01)
    grmodel.gmodel.showgrasp(validgrasps[0],collisionfree=True)

    densityfn,samplerfn,bounds,validgrasps = gr.computeGraspDistribution(logllthresh=logllthresh)
    goals,numfailures=gr.sampleGoals(lambda goals: samplerfn(goals,weight=1.0),updateenv=True)
    grasp,pose,q = goals[0]
    robot.SetTransform(pose)
    robot.SetJointValues(q)
    basemanip.CloseFingers()

    grasp = gr.gmodel.grasps[283]
    Tgrasp = gr.gmodel.getGlobalGraspTransform(grasp,collisionfree=True)
    equivalenceclass,logll = gr.irmodel.getEquivalenceClass(Tgrasp)
    densityfn,samplerfn,bounds = gr.irmodel.computeBaseDistribution(Tgrasp,logllthresh=logllthresh)
    h = gr.irmodel.showBaseDistribution(densityfn,bounds,zoffset=gr.target.GetTransform()[2,3],thresh=1.0)

    env = Environment()
    robot = env.ReadRobotXMLFile('robots/hrp2jsk08.robot.xml')
    env.AddRobot(robot)
    body = env.ReadKinBodyXMLFile('scenes/cereal_frootloops.kinbody.xml')
    env.AddKinBody(body)
    T = eye(4)
    T[0:3,3] = [0.466,-0.157,0.544]
    body.SetTransform(T)
    robot.SetActiveManipulator('rightarm_chest')
    robot.Grab(body)
    robot.SetJointValues([-1.4,1.35239005,1.036349],[5,7,8])
    robot.CheckSelfCollision()