rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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", [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', '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)
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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", [0, 2, -1, 1, 0, 2.2]) #~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']) rbprmBuilder.setAffordanceFilter('3Rarm', ['Support']) rbprmBuilder.setAffordanceFilter('4Larm', ['Support']) rbprmBuilder.setAffordanceFilter('0rLeg', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('1lLeg', ['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
urdfName = 'hrp2_trunk_flexible' urdfNameRom = [ 'hrp2_larm_rom', 'hrp2_rarm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = 4.5 aMax = 6. extraDof = 6 # 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", [-1.25,2, -0.5, 5.5, 0.6, 1.8]) 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)
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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.3, 1, -0.5, 0.5, 0, 0.9]) rbprmBuilder.setFilter( ['hrp2_larm_rom', 'hrp2_rarm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom']) #~ rbprmBuilder.setNormalFilter('hrp2_larm_rom', [-1,0,0], 0.5) #~ rbprmBuilder.setNormalFilter('hrp2_rarm_rom', [-1,0,0], 0.5) #~ rbprmBuilder.setNormalFilter('hrp2_lleg_rom', [0,0,1], 0.5) #~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.5) rbprmBuilder.boundSO3([-0.4, 0.4, -3, 3, -3, 3]) #~ from hpp.corbaserver.rbprm. import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder) r = Viewer(ps)
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) # 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']) #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", [0,1,0.6 ,1.1, 0.45, 1.5]) 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.))
from hpp.gepetto import Viewer rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' meshPackageName = 'hpp-rbprm-corba' # URDF file describing the trunk of the robot HyQ urdfName = 'hyq_trunk_large' # URDF files describing the reachable workspace of each limb of HyQ urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom'] 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", [-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)
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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("root_joint", [0, 2, -1, 1, 0, 2.2]) #~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']) rbprmBuilder.setAffordanceFilter('3Rarm', ['Support']) rbprmBuilder.setAffordanceFilter('0rLeg', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('1lLeg', ['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.problem_solver import ProblemSolver ps = ProblemSolver(rbprmBuilder)
#~ urdfName = 'hyq_trunk_large' urdfName = 'hyq_trunk' # URDF files describing the reachable workspace of each limb of HyQ urdfNameRom = [ 'hyq_lhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_rhleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = 2 aMax = 10 extraDof = 6 # 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", [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)
meshPackageName = 'hpp-rbprm-corba' urdfName = 'hrp2_trunk_flexible' urdfNameRoms = [ 'hrp2_larm_rom', 'hrp2_rarm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom' ] urdfSuffix = "" srdfSuffix = "" cl = Client() cl.problem.selectProblem("robot0") rbprmBuilder2 = Robot("root_robot0") rbprmBuilder = Builder() rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) rbprmBuilder.setJointBounds("base_joint_xyz", [0, 10, -4, 4, 0, 10.2]) rbprmBuilder.setFilter(['hrp2_lleg_rom', 'hrp2_rleg_rom']) rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support', 'Lean']) rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support', 'Lean']) #~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support','Lean']) rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hrp2_lleg_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
from hpp.corbaserver.rbprm.rbprmbuilder import Builder from hpp.gepetto import Viewer rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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.3,1, -0.5, 0.5, 0, 0.9]) rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_lleg_rom','hrp2_rleg_rom']) 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.4,0.4,-3,3,-3,3]) #~ 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)
#~ urdfName = 'hyq_trunk_large' urdfName = 'hyq_trunk' # URDF files describing the reachable workspace of each limb of HyQ urdfNameRom = [ 'hyq_lhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_rhleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = 2 aMax = 10 extraDof = 6 # 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, 2.5, -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)
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) t = ps.solve () res = {} x_start = 0 y_start = 0 x_max = 2
from hpp.corbaserver.rbprm.rbprmbuilder import Builder from hpp.gepetto import Viewer rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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)
meshPackageName = 'hpp-rbprm-corba' # URDF file describing the trunk of the robot HyQ urdfName = 'hyq_trunk' # URDF files describing the reachable workspace of each limb of HyQ urdfNameRom = [ 'hyq_lhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_rhleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = 4 aMax = 10 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( ['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.client.basic.robot.setDimensionExtraConfigSpace(extraDof) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds( [-vMax, vMax, -vMax, vMax, 0, 0, 0, 0, 0, 0, 0, 0])
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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, 3, -1, 1, 0, 6]) #~ rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_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','Lean']) rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support']) #~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean']) #~ rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean']) rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('hrp2_rleg_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.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) # 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']) 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))
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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", [0, 1.55, -0.9, -0.55, 0.50, 1.3]) #rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2, -1, 1, 0, 2.2]) rbprmBuilder.setJointBounds('CHEST_JOINT0', [0, 0]) rbprmBuilder.setJointBounds('CHEST_JOINT1', [0, 0.45]) rbprmBuilder.setJointBounds('HEAD_JOINT0', [0, 0]) rbprmBuilder.setJointBounds('HEAD_JOINT1', [0, 0]) rbprmBuilder.setFilter(['hrp2_rleg_rom']) rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support']) rbprmBuilder.boundSO3([-0., 0, -1, 1, -1, 1]) vMax = 0.3 aMax = 0.3
rootJointType = 'freeflyer' packageName = 'talos-rbprm' meshPackageName = 'talos-rbprm' urdfName = 'talos_trunk' urdfNameRom = ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom'] urdfSuffix = "" srdfSuffix = "" vMax = omniORB.any.to_any(0.3); aMax = omniORB.any.to_any(0.1); #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
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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", [-3, 3, -2, 2, 0, 1]) #~ rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_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','Lean']) #~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support']) #~ rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean']) rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean']) rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('hrp2_rleg_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)
packageName, urdfSuffix, srdfSuffix) # 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_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))
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) t = ps.solve () res = {} x_start = -1.5 y_start = 0 x_max = 2.84
from hpp.gepetto import Viewer rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' meshPackageName = 'hpp-rbprm-corba' # URDF file describing the trunk of the robot HyQ urdfName = 'hyq_trunk_large' # URDF files describing the reachable workspace of each limb of HyQ urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom'] 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)
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' meshPackageName = 'hpp-rbprm-corba' urdfName = 'hyq_trunk' urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom'] 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 ();
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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, 0, -1, 0, 0, 0.7]) #~ rbprmBuilder.setFilter(['hrp2_larm_rom','hrp2_rarm_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','Lean']) rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support']) #~ rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Lean']) #~ rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Lean']) rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom', [ 'Support', ]) rbprmBuilder.setAffordanceFilter('hrp2_rleg_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)
# URDF file describing the trunk of the robot HyQ urdfName = 'hyq_trunk_large' # URDF files describing the reachable workspace of each limb of HyQ urdfNameRom = [ 'hyq_lhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_rhleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = 2 aMax = 5 extraDof = 6 # 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, 2.5, -6, 6, 0.6, 0.65]) # 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. (z, y, x) #rbprmBuilder.boundSO3([-3,3,-0.1,0.1,-0.1,0.1]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
packageName = 'hpp-rbprm-corba' meshPackageName = 'hpp-rbprm-corba' urdfName = 'spiderman_trunk' urdfNameRoms = [ 'SpidermanLFootSphere', 'SpidermanRFootSphere', 'SpidermanLHandSphere', 'SpidermanRHandSphere' ] urdfSuffix = "" srdfSuffix = "" ecsSize = 4 base_joint_xyz_limits = [-10, 10, -10, 15, 0, 10] rbprmBuilder = Builder() # RBPRM rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) rbprmBuilder.setJointBounds("base_joint_xyz", base_joint_xyz_limits) rbprmBuilder.boundSO3([-0.2, 0.2, -3.14, 3.14, -0.3, 0.3]) rbprmBuilder.setFilter(urdfNameRoms) affordanceType = ['Support'] rbprmBuilder.setAffordanceFilter('SpidermanLFootSphere', affordanceType) rbprmBuilder.setAffordanceFilter('SpidermanRFootSphere', affordanceType) rbprmBuilder.setAffordanceFilter('SpidermanLHandSphere', affordanceType) rbprmBuilder.setAffordanceFilter('SpidermanRHandSphere', affordanceType) rbprmBuilder.setContactSize(0.03, 0.08) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(ecsSize) rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds( [0, 0, 0, 0, 0, 0, -3.14, 3.14]) #~ from hpp.corbaserver.rbprm. import ProblemSolver from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
urdfNameRom = [ 'hrp2_larm_rom', 'hrp2_rarm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = omniORB.any.to_any(0.3) aMax = omniORB.any.to_any(0.2) #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", [-1.25,2, -0.5, 5.5, 0.6, 1.8]) rbprmBuilder.setJointBounds("base_joint_xyz", [-1, 2, -0.5, 0.5, 0.5, 0.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']) 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.1, 0.1, -0.65, 0.65, -0.2, 0.2]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder from hpp.gepetto import Viewer rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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", [0,2, -1, 1, 0, 2.2]) #~ rbprmBuilder.setFilter(['hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']) #~ 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 ();
rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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, 3, -0.5, 0.5, -0.4, 0.6]) rbprmBuilder.setFilter(['hrp2_lleg_rom', 'hrp2_rleg_rom']) rbprmBuilder.setAffordanceFilter('hrp2_larm_rom', ['Support', 'Lean']) rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom', ['Support', 'Lean']) rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom', ['Support']) rbprmBuilder.setAffordanceFilter('hrp2_lleg_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)
rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix) # The following lines set constraint on the valid configurations: # a configuration is valid only if all limbs can create a contact ... 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.))
# URDF file describing the trunk of the robot HyQ urdfName = 'hyq_trunk_large' # URDF files describing the reachable workspace of each limb of HyQ urdfNameRom = [ 'hyq_lhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_rhleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = 1 aMax = 5 extraDof = 6 # 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", [-4, 4, -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)
urdfName = 'hrp2_trunk_flexible' urdfNameRom = [ 'hrp2_larm_rom', 'hrp2_rarm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = 4 aMax = 6 extraDof = 6 # 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", [-1.25,2, -0.5, 5.5, 0.6, 1.8]) rbprmBuilder.setJointBounds("base_joint_xyz", [-2, 4, 0, 2, 0.2, 1.4]) 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.1, 0.1, -0.65, 0.65, -0.2, 0.2]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
urdfName = 'hyq_trunk_large' # URDF files describing the reachable workspace of each limb of HyQ urdfNameRom = [ 'hyq_lhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_rhleg_rom' ] urdfSuffix = "" srdfSuffix = "" vMax = 4 aMax = 5 extraDof = 6 # 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", [-1.25,2, -0.5, 5.5, 0.6, 1.8]) rbprmBuilder.setJointBounds("base_joint_xyz", [-1.25, 5, 0, 2, 0.45, 1.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. (z, y, x) rbprmBuilder.boundSO3([-0.1, 0.1, -0.65, 0.65, -0.2, 0.2]) rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
from hpp.corbaserver.rbprm.rbprmbuilder import Builder from hpp.gepetto import Viewer rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' 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", [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)