rbprmBuilder = Builder () rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2]) rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom']) rbprmBuilder.setNormalFilter('hyq_lhleg_rom', [0,0,1], 0.9) rbprmBuilder.setNormalFilter('hyq_rfleg_rom', [0,0,1], 0.9) rbprmBuilder.setNormalFilter('hyq_lfleg_rom', [0,0,1], 0.9) rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9) rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1]) #~ from hpp.corbaserver.rbprm. import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver( rbprmBuilder ) r = Viewer (ps) q_init = rbprmBuilder.getCurrentConfig (); q_init = [-6,-3,0.8,1,0,0,0]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = [4, 4, 0.8, 1, 0, 0, 0]; r (q_goal) ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.client.problem.selectConFigurationShooter("RbprmShooter") ps.client.problem.selectPathValidation("RbprmPathValidation",0.01) r.loadObstacleModel (packageName, name_of_scene, "planning")
rbprmBuilder = Builder () rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,5, -4, 4, 0.6, 2]) rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom']) rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',]) rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',]) rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1]) #~ from hpp.corbaserver.rbprm. import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver( rbprmBuilder ) r = Viewer (ps) q_init = rbprmBuilder.getCurrentConfig (); q_init [0:3] = [-5, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = q_init [::] q_goal [0:3] = [-2.5, 0, 0.63]; #pass the hole #~ q_goal [0:3] = [5, 0, 0.63]; r (q_goal) #pass the bridge ps.addPathOptimizer("RandomShortcut") ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool
urdfSuffix = "" srdfSuffix = "" rbprmBuilder = Builder () rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4]) rbprmBuilder.setFilter(['hyq_rhleg_rom']) # , 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom']) rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean']) # We also bound the rotations of the torso. rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3]) # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver( rbprmBuilder ) r = Viewer (ps) r.loadObstacleModel (packageName, "darpa", "planning") # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig (); q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = q_init [::] q_goal [0:3] = [3, 0, 0.63]; r (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance import Client c = Client () c.affordance.analyseAll ()
rbprmBuilder.setJointBounds("base_joint_xyz", [-5.5, 5.5, -2.5, 2.5, 0.55, 0.6]) rbprmBuilder.setJointBounds('CHEST_JOINT0', [-0.05, 0.05]) rbprmBuilder.setJointBounds('CHEST_JOINT1', [-0.05, 0.05]) # We also bound the rotations of the torso. (z, y, x) rbprmBuilder.boundSO3([-math.pi, math.pi, -0.1, 0.1, -0.1, 0.1]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds( [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]) indexECS = rbprmBuilder.getConfigSize( ) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace() # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax)) ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax)) ps.client.problem.setParameter("orientedPath", omniORB.any.to_any(0.)) ps.client.problem.setParameter("friction", omniORB.any.to_any(MU)) ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24)) ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14)) r = Viewer(ps, displayArrows=True) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(packageName, "slalom", "planning", r) #r.loadObstacleModel (packageName, "ground", "planning") afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', [ 'Support', ]) # We also bound the rotations of the torso. (z, y, x) rbprmBuilder.boundSO3([-math.pi, math.pi, -0.1, 0.1, -0.1, 0.1]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds( [-vMax, vMax, -vMax, vMax, 0, 0, 0, 0, 0, 0, 0, 0]) # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax)) ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax)) ps.client.problem.setParameter("orientedPath", omniORB.any.to_any(1.)) ps.client.problem.setParameter("friction", mu) ps.client.problem.setTimeOutPathPlanning(500) r = Viewer(ps, displayArrows=True) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(packageName, "slalom", "planning", r) #r.loadObstacleModel (packageName, "ground", "planning") afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5]) r.addLandmark(r.sceneName, 1)
extraDof = 6 rbprmBuilder = Builder () rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 2.5]) rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1]) rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom']) rbprmBuilder.setAffordanceFilter('robot_test_lleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('robot_test_rleg_rom', ['Support']) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0]) #~ from hpp.corbaserver.rbprm. import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver( rbprmBuilder ) ps.client.problem.setParameter("aMax",aMax) ps.client.problem.setParameter("vMax",vMax) ps.client.problem.setParameter("sizeFootX",0.24) ps.client.problem.setParameter("sizeFootY",0.14) r = Viewer (ps) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.loadObstacleModel (packageName, "ground_jump_easy", "planning", r) afftool.visualiseAffordances('Support', r, r.color.brown) q_init = rbprmBuilder.getCurrentConfig (); #q_init[(len(q_init)-3):]=[0,0,1] # set normal for init / goal config q_init [0:3] = [-4, 1, 0.9]; rbprmBuilder.setCurrentConfig q_init[3:7] = [0.7071,0,0,0.7071]
# Information to retrieve urdf and srdf files. urdfName = "hrp2_14" urdfSuffix = "_reduced" srdfSuffix = "" fullBody = FullBody() fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) fullBody.setJointBounds("base_joint_xyz", [-1, 1, -4, -1, 1, 2.2]) from hpp.corbaserver.rbprm.problem_solver import ProblemSolver nbSamples = 50000 ps = ProblemSolver(fullBody) #~ AFTER loading obstacles rLegId = '7rLeg' rLeg = 'RLEG_JOINT0' rLegOffset = [ 0, -0.105, 0, ] rLegNormal = [0, 1, 0] rLegx = 0.09 rLegy = 0.05 fullBody.addLimb(rLegId, rLeg, '', rLegOffset, rLegNormal, rLegx, rLegy, nbSamples, 0.01)
rbprmBuilder.setFilter( ['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_lhleg_rom']) rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', [ 'Support', ]) # We also bound the rotations of the torso. rbprmBuilder.boundSO3([-0.4, 0.4, -3, 3, -3, 3]) # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) # Choosing a path optimizer ps.addPathOptimizer("RandomShortcut") from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.setAffordanceConfig('Lean', [0.5, 0.03, 0.00005]) r = None try: r = Viewer(ps) afftool.loadObstacleModel(packageName, "twister", "planning", r) except: pass
rbprmBuilder.setFilter( ['hyq_lhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_rhleg_rom']) rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean']) rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support', 'Lean']) rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', [ 'Support', ]) # We also bound the rotations of the torso. rbprmBuilder.boundSO3([-0.4, 0.4, -3, 3, -3, 3]) # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) r = Viewer(ps) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig() q_init[0:3] = [-2, 0, 0.63] rbprmBuilder.setCurrentConfig(q_init) r(q_init) q_goal = q_init[::] q_goal[0:3] = [3, 0, 0.63] r(q_goal) ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool
urdfNameRoms = [ 'hrp2_larm_rom', 'hrp2_rarm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom' ] urdfSuffix = "" srdfSuffix = "" scene = sys.argv[len(sys.argv) - 1] tested = Builder() tested.loadModel(urdfNameTested, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) #~ tested.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9) #~ tested.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom']) ps = ProblemSolver(tested) r = Viewer(ps) r.loadObstacleModel(packageName, scene, "planning") tested.setJointBounds("base_joint_xyz", [-10., 10, -10, 10, 0, 20]) ps.client.problem.selectConFigurationShooter("RbprmShooter") q_init = tested.getCurrentConfig() q_init[0:3] = [-10, -0.82, 1.25] tested.setCurrentConfig(q_init) r(q_init) q_goal = q_init[::] q_goal[0:3] = [-9, -0.65, 1.25] r(q_goal) ps.setInitialConfig(q_init) ps.addGoalConfig(q_goal)
rbprmBuilder = Builder () rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4]) rbprmBuilder.setFilter(['hyq_lhleg_rom', 'hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']) rbprmBuilder.setAffordanceFilter('hyq_rhleg_rom', ['Support', 'Lean']) rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',]) rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support', 'Lean']) rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',]) # We also bound the rotations of the torso. rbprmBuilder.boundSO3([-0.4,0.4,-3,3,-3,3]) # Creating an instance of HPP problem solver and the viewer from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver( rbprmBuilder ) r = Viewer (ps) # Setting initial and goal configurations q_init = rbprmBuilder.getCurrentConfig (); q_init [0:3] = [-2, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init) q_goal = q_init [::] q_goal [0:3] = [3, 0, 0.63]; r (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.loadObstacleModel (packageName, "darpa", "planning", r) afftool.loadObstacleModel ("hpp-ompl-benchmark", "cubicles_robot", "robo", r)
rbprmBuilder.setFilter(['hrp2_lleg_rom', 'hrp2_rleg_rom']) rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support']) # We also bound the rotations of the torso. (z, y, x) rbprmBuilder.boundSO3([-0.001, 0.001, -0.001, 0.001, -0.001, 0.001]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds( [-4.5, 4.5, 0, 0, -2, 2, 0, 0, 0, 0, 0, 0]) indexECS = rbprmBuilder.getConfigSize( ) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace() # Creating an instance of HPP problem solver and the viewer ps = ProblemSolver(rbprmBuilder) ps.client.problem.setParameter("aMax", omniORB.any.to_any(aMax)) ps.client.problem.setParameter("aMaxZ", omniORB.any.to_any(10.)) ps.client.problem.setParameter("vMax", omniORB.any.to_any(vMax)) ps.client.problem.setParameter("tryJump", omniORB.any.to_any(1.)) ps.client.problem.setParameter("sizeFootX", omniORB.any.to_any(0.24)) ps.client.problem.setParameter("sizeFootY", omniORB.any.to_any(0.14)) ps.client.problem.setTimeOutPathPlanning(3600) r = Viewer(ps, displayArrows=True) from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool() afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel(packageName, "downSlope", "planning", r) #r.loadObstacleModel (packageName, "ground", "planning")