urdfSuffix = ""
srdfSuffix = ""
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-3,6, -2, 3, 0.3, 4])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
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([-1,1,-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.9, 0, 0.63]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [2, 0, 1.57]; r (q_goal)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
ps.setInitialConfig (q_init)
packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [0, 20, -1, 1, 0, 2.2])
#~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [
'Support',
])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', [
'Support',
])
rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support'])
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.5)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.9)
#~ rbprmBuilder.setNormalFilter('hyq_rhleg_rom', [0,0,1], 0.9)
rbprmBuilder.boundSO3([-0., 0, -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] = [0, 0, 0.58]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_init[0:3] = [0.8, 0, 0.58]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [0, 2.2, -1, 1, 0.7, 2.5])
#~ rbprmBuilder.setJointBounds ("base_joint_xyz", [-20,20.2, -10, 10, 0.7, 2.5])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setFilter(['hrp2_lleg_rom', 'hrp2_rleg_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
rbprmBuilder.boundSO3([-0.1, 0.1, -3, 3, -1.0, 1.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
q0 = rbprmBuilder.getCurrentConfig()
q_init = rbprmBuilder.getCurrentConfig()
r(q_init)
q_goal = q_init[::]
q_goal[0:3] = [0.19, 0.05, 0.9]
r(q_goal)
packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [-5, 5, -1.5, 1.5, 0.5, 0.8])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
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])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
[-0.2, 0.2, -0.2, 0.2, 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", aMax)
ps.client.problem.setParameter("vMax", vMax)
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
afftool.loadObstacleModel(packageName, "ground", "planning", r)
#r.loadObstacleModel (packageName, "ground", "planning")
rbprmBuilder.setFilter(['hrp2_lleg_rom', 'hrp2_rleg_rom'])
#rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean'])
#rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [
'Support',
])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
vMax = 0.2
aMax = 0.1
extraDof = 6
rbprmBuilder.setJointBounds("base_joint_xyz", [-3, 4.5, -2, 2.5, 0.55, 0.65])
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))
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2.2, -1, 1, 0.7, 2.5])
#~ rbprmBuilder.setJointBounds ("base_joint_xyz", [-20,20.2, -10, 10, 0.7, 2.5])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-1.0,1.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q0 = rbprmBuilder.getCurrentConfig ();
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [0.19, 0.05, 0.9]; r (q_goal)
rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
]
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName,
packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [0, 2, -1.4, 1.01, 0, 1.01])
rbprmBuilder.setFilter(['hrp2_larm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom'])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support', 'Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
rbprmBuilder.boundSO3([-0.7, 0.7, 0, 0, -0.0, 0.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver(rbprmBuilder)
r = Viewer(ps)
q0 = rbprmBuilder.getCurrentConfig()
q_init = rbprmBuilder.getCurrentConfig()
r(q_init)
q_goal = q_init[::]
q_init = rbprmBuilder.getCurrentConfig()
q_init[0:3] = [0.36, -0.37772165525546453, 0.968450617921899]
packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [0.8, 5.6, -0.5, 0.5, 0.4, 1.2])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
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.2, 0.2, -0.3, 0.3, -0.3, 0.3])
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", aMax)
ps.client.problem.setParameter("vMax", vMax)
r = Viewer(ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
afftool = AffordanceTool()
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['hrp2_lleg_rom', 'hrp2_rleg_rom'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [
'Support',
])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
vMax = 0.2
aMax = 0.1
extraDof = 6
#rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2.5,0.5 ,3, 0.6, 0.6])
rbprmBuilder.setJointBounds("base_joint_xyz", [-1, 2.5, 0.5, 3, 0.6, 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.0, 0.0, -0.0, 0.0])
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(1.))
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))
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk_flexible'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-1,2, -1.5, 1, 0.5, 0.9])
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom','hrp2_larm_rom'])
#~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
#~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.6)
rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
rbprmBuilder.boundSO3([-1.5,1.5,0,3,-0.0,0.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q0 = rbprmBuilder.getCurrentConfig ();
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_goal = q_init [::]
q_init = rbprmBuilder.getCurrentConfig ();
q_init[0:3] = [0.15, -0.45, 0.8]; r(q_init)
rbprmBuilder.setFilter([])
#rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean'])
#rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
vMax = 0.2
aMax = 0.1
extraDof = 6
MU = 0.5
rbprmBuilder.setJointBounds("base_joint_xyz",
[-1.2, 1.5, -0.1, 0.1, 0.55, 0.85])
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([-0, 0, -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("aMaxZ", omniORB.any.to_any(10.))
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))
meshPackageName = "hpp-rbprm-corba"
urdfName = "hrp2_trunk_flexible"
urdfNameRoms = ["hrp2_larm_rom", "hrp2_rarm_rom", "hrp2_lleg_rom", "hrp2_rleg_rom"]
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [-2, 5, -1, 1, 0.55, 4])
rbprmBuilder.setFilter(["hrp2_lleg_rom", "hrp2_rleg_rom"])
rbprmBuilder.setNormalFilter("hrp2_larm_rom", [0, 0, 1], 0.4)
rbprmBuilder.setNormalFilter("hrp2_rarm_rom", [0, 0, 1], 0.4)
rbprmBuilder.setNormalFilter("hrp2_lleg_rom", [0, 0, 1], 0.4)
rbprmBuilder.setNormalFilter("hrp2_rleg_rom", [0, 0, 1], 0.4)
rbprmBuilder.boundSO3([-0.1, 0.1, -0.1, 0.1, -1.0, 0.0])
# ~ 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] = [-2, 0, 0.55]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
q_goal = q_init[::]
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1.4, 1.01, 0, 1.01])
rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
#~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support','Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support','Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', ['Support'])
rbprmBuilder.boundSO3([-0.7,0.7,0,0,-0.0,0.0])
#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
ps = ProblemSolver( rbprmBuilder )
r = Viewer (ps)
q0 = rbprmBuilder.getCurrentConfig ();
q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
q_goal = q_init [::]
q_init = rbprmBuilder.getCurrentConfig ();
#~ q_init[0:3] = [0.36, -0.32, 1.01]; r(q_init)
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk_flexible'
urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.55, 4])
rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
rbprmBuilder.setNormalFilter('hrp2_larm_rom', [0,0,1], 0.4)
rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [0,0,1], 0.4)
rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.4)
rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.4)
rbprmBuilder.boundSO3([-0.1,0.1,-0.1,0.1,-1.0,0.0])
#~ 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] = [-2, 0, 0.55]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
q_goal = q_init [::]
q_goal [0:3] = [3, 0, 0.55]; r (q_goal)
srdfSuffix = ""
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['hyq_rhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom','hyq_lhleg_rom'])
# ... and only if a contact surface includes the gravity
rbprmBuilder.setAffordanceFilter('hyq_lhleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_rfleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('hyq_lfleg_rom', ['Support',])
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)
# Choosing a path optimizer
ps.addPathOptimizer("RandomShortcut")
#aMax = omniORB.any.to_any(0.3);
extraDof = 6
mu=omniORB.any.to_any(MU)
# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-5,5, -1.5, 1.5, 0.95, 1.05])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
rbprmBuilder.setFilter(['talos_lleg_rom','talos_rleg_rom'])
rbprmBuilder.setAffordanceFilter('talos_lleg_rom', ['Support',])
rbprmBuilder.setAffordanceFilter('talos_rleg_rom', ['Support'])
# We also bound the rotations of the torso. (z, y, x)
rbprmBuilder.boundSO3([-0.1,0.1,-0.65,0.65,-0.2,0.2])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-1,1,-1,1,-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",aMax)
ps.client.problem.setParameter("vMax",vMax)
ps.client.problem.setParameter("sizeFootX",omniORB.any.to_any(0.2))
ps.client.problem.setParameter("sizeFootY",omniORB.any.to_any(0.12))
ps.client.problem.setParameter("friction",mu)
r = Viewer (ps)
from hpp.corbaserver.affordance.affordance import AffordanceTool
urdfSuffix = ""
srdfSuffix = ""
# name_of_scene = "groundcrouch"
name_of_scene = "basic"
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)
rbprmBuilder.setJointBounds("base_joint_xyz",
[-1.7, 2.75, 0.95, 1.05, 0.1, 1.8])
rbprmBuilder.setJointBounds('CHEST_JOINT0', [0, 0])
rbprmBuilder.setJointBounds('CHEST_JOINT1', [-0.35, 0.1])
rbprmBuilder.setJointBounds('HEAD_JOINT0', [0, 0])
rbprmBuilder.setJointBounds('HEAD_JOINT1', [0, 0])
# The following lines set constraint on the valid configurations:
# a configuration is valid only if all limbs can create a contact ...
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))
