Esempi in Python per Builder.setJointBounds

Esempio n. 1

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)

Esempio n. 2

File: stair_bauzil_hrp2_path3.py Progetto: nim65s/hpp-rbprm-corba

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

Esempio n. 3

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)

Esempio n. 4

File: standing_hrp2_path.py Progetto: anna-seppala/hpp-rbprm-corba

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)

Esempio n. 5

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

Esempio n. 6

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)

Esempio n. 7

File: stair_bauzil_hrp2_path.py Progetto: nim65s/hpp-rbprm-corba

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)

Esempio n. 8

#~ 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)

Esempio n. 9

File: scale_hrp2_path.py Progetto: nim65s/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 = ""

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

Esempio n. 10

File: standing_hrp2_path.py Progetto: stonneau/hpp-rbprm-corba

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)

Esempio n. 11

File: flatStep_hyq_pathKino.py Progetto: nim65s/hpp-rbprm-corba

#~ 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)

Esempio n. 12

File: computecol_free.py Progetto: stonneau/hpp-rbprm-corba

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

Esempio n. 13

File: car_hrp2_path.py Progetto: anna-seppala/hpp-rbprm-corba

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)

Esempio n. 14

File: hyq_jumpEasy_path.py Progetto: nim65s/hpp-rbprm-corba

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

Esempio n. 15

File: scale_hrp2_path_2.py Progetto: teguhSL/hpp-rbprm-corba

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)

Esempio n. 16

File: slalom_bauzil_hrp2_pathKino.py Progetto: nim65s/hpp-rbprm-corba

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

Esempio n. 17

File: stair_bauzil_hrp2_path.py Progetto: nim65s/hpp-rbprm-corba

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

Esempio n. 18

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

Esempio n. 19

File: grasp_hrp2_path.py Progetto: teguhSL/hpp-rbprm-corba

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)

Esempio n. 20

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

Esempio n. 21

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

Esempio n. 22

File: mount_hyq_path.py Progetto: nim65s/hpp-rbprm-corba

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)

Esempio n. 23

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 ();

Esempio n. 24

File: chair_hrp2_path.py Progetto: nim65s/hpp-rbprm-corba

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)

Esempio n. 25

File: detour_hyq_path.py Progetto: stonneau/hpp-rbprm-corba-1

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

Esempio n. 26

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

Esempio n. 27

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)

Esempio n. 28

File: stair_bauzil_hrp2_path.py Progetto: airobert/hpp-rbprm-corba

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 ();

Esempio n. 29

File: down_hrp2_path.py Progetto: teguhSL/hpp-rbprm-corba

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)

Esempio n. 30

File: darpa_hrp2_path.py Progetto: nim65s/hpp-rbprm-corba

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

Esempio n. 31

File: darpa_hyq_pathKino.py Progetto: teguhSL/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 = ""
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)

Esempio n. 32

File: downSlope_easy_hrp2_pathKino.py Progetto: nim65s/hpp-rbprm-corba

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)

Esempio n. 33

File: downSlope_hyq_pathKino.py Progetto: nim65s/hpp-rbprm-corba

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)

Esempio n. 34

File: polaris_hrp2_path_no_step.py Progetto: stonneau/hpp-rbprm-corba

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)