joint1L_bounds_prev=fullBody.getJointBounds('leg_left_1_joint') joint3L_bounds_prev=fullBody.getJointBounds('leg_left_3_joint') joint1R_bounds_prev=fullBody.getJointBounds('leg_right_1_joint') joint3R_bounds_prev=fullBody.getJointBounds('leg_right_3_joint') fullBody.setJointBounds('leg_left_1_joint',[-0.2,0.7]) fullBody.setJointBounds('leg_left_3_joint',[-1.3,0.4]) fullBody.setJointBounds('leg_right_1_joint',[-0.7,0.2]) fullBody.setJointBounds('leg_right_3_joint',[-1.3,0.4]) # add the 6 extraDof for velocity and acceleration (see *_path.py script) fullBody.client.robot.setDimensionExtraConfigSpace(6) vMax = 0.5# linear velocity bound for the root aMax = 0.5# linear acceleration bound for the root fullBody.client.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,-aMax,aMax,-aMax,aMax,0,0]) ps = ProblemSolver( fullBody ) vf = ViewerFactory (ps) ps.setParameter("Kinodynamic/velocityBound",vMax) ps.setParameter("Kinodynamic/accelerationBound",aMax) ps.setRandomSeed(random.SystemRandom().randint(0, 999999)) #load the viewer try : v = vf.createViewer(displayCoM = True) except Exception: print "No viewer started !" class FakeViewer(): sceneName = "" def __init__(self): return def __call__(self,q): return
indexECS = rbprmBuilder.getConfigSize() - rbprmBuilder.client.robot.getDimensionExtraConfigSpace() # Creating an instance of HPP problem solver psGuide = ProblemSolver( rbprmBuilder ) # define parameters used by various methods : psGuide.setParameter("Kinodynamic/velocityBound",vMax) psGuide.setParameter("Kinodynamic/accelerationBound",aMax) psGuide.setParameter("DynamicPlanner/sizeFootX",0.01) psGuide.setParameter("DynamicPlanner/sizeFootY",0.01) psGuide.setParameter("DynamicPlanner/friction",mu) # sample only configuration with null velocity and acceleration : psGuide.setParameter("ConfigurationShooter/sampleExtraDOF",False) # initialize the viewer : from hpp.gepetto import ViewerFactory vf = ViewerFactory (psGuide) # load the module to analyse the environnement and compute the possible contact surfaces from hpp.corbaserver.affordance.affordance import AffordanceTool afftool = AffordanceTool () afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005]) afftool.loadObstacleModel ("hpp_environments", cfg.SCENE, "planning", vf) try : v = vf.createViewer(displayArrows = True) except Exception: print "No viewer started !" class FakeViewer(): def __init__(self): return def __call__(self,q): return
from hpp.environments import Buggy robot = Buggy("buggy") robot.setJointBounds ("base_joint_xy", [-5, 16, -4.5, 4.5]) from hpp.corbaserver import ProblemSolver ps = ProblemSolver (robot) from hpp.gepetto import ViewerFactory gui = ViewerFactory (ps) gui.loadObstacleModel ('hpp_environments', "scene", "scene") q_init = robot.getCurrentConfig () q_goal = q_init [::] q_init[0:2] = [-3.7, -4]; gui (q_init) q_goal [0:2] = [15,2] gui (q_goal) ps.setInitialConfig (q_init) ps.addGoalConfig (q_goal) ps.selectSteeringMethod ("ReedsShepp") ps.selectPathPlanner ("DiffusingPlanner") ps.addPathOptimizer ("RandomShortcut") t = ps.solve () print ("solving time", t)
def play(): gameWidget.update() def end(): gameWidget.end() sys.argv = [] gui = ViewerClient() gui.gui.createWindow("ia") gui.gui.createWindow("player") iaRobot = Buggy("buggy_ia") ips = ProblemSolver(iaRobot) ivf = ViewerFactory(ips) ivf.loadObstacleModel('hpp_environments', "scene", "scene_ia") iv = ivf.createViewer(viewerClient=gui) iaRobot.client.problem.selectProblem("player") playerRobot = Buggy("buggy_player") pps = ProblemSolver(playerRobot) pvf = ViewerFactory(pps) pvf.loadObstacleModel('hpp_environments', "scene", "scene_player") pvf(playerRobot.client.robot.getCurrentConfig()) pv = pvf.createViewer(viewerClient=gui) gui.gui.addSceneToWindow("buggy_player", 1) gui.gui.addSceneToWindow("scene_player", 1) gui.gui.addSceneToWindow("buggy_ia", 0) gui.gui.addSceneToWindow("scene_ia", 0)