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")
