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)
