Python Builder.setFilter Exemples

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

q0 = rbprmBuilder.getCurrentConfig()

Exemple #2

Fichier : downSlope_hyq_pathKino.py Projet : nim65s/hpp-rbprm-corba

    '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)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
    [-vMax, vMax, -1, 1, -2, 2, 0, 0, 0, 0, 0, 0])

# Creating an instance of HPP problem solver and the viewer

Exemple #3

Fichier : test_basic2.py Projet : stonneau/hpp_tests

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 ();
q_init = [-6,-3,0.8,1,0,0,0]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)

Exemple #4

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)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
    [-1, 1, -1, 1, -0.5, 0.5, 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)

Exemple #5

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

ps = ProblemSolver(rbprmBuilder)

Exemple #6

Fichier : darpa_hrp2_path.py Projet : nim65s/hpp-rbprm-corba

meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk_lowBody_flexible'
urdfNameRoms = [
    'hrp2_larm_rom', 'hrp2_rarm_rom', 'hrp2_lleg_rom', 'hrp2_rleg_rom'
]
urdfSuffix = ""
srdfSuffix = ""

# Creating an instance of the helper class, and loading the robot
rbprmBuilder = Builder()
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])

Exemple #7

rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hyq_trunk_large'
urdfNameRom = [
    'hyq_lhleg_rom', 'hyq_lfleg_rom', 'hyq_rfleg_rom', 'hyq_rhleg_rom'
]
urdfSuffix = ""
srdfSuffix = ""

rbprmBuilder = Builder()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName,
                       packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [-2, 5, -1, 1, 0.3, 4])

rbprmBuilder.setFilter(['hyq_rhleg_rom'])
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)

# 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[::]

Exemple #8

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

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

Exemple #9

Fichier : test4.py Projet : humanoid-path-planner/hpp-affordance

urdfSuffix = ""
srdfSuffix = ""

rbprmBuilder = Builder()
rbprmBuilder.loadModel(
    urdfName,
    urdfNameRom,
    rootJointType,
    meshPackageName,
    packageName,
    urdfSuffix,
    srdfSuffix,
)
rbprmBuilder.setJointBounds("base_joint_xyz", [-2, 5, -1, 1, 0.3, 4])

rbprmBuilder.setFilter(
    ["hyq_lhleg_rom", "hyq_lfleg_rom", "hyq_rfleg_rom", "hyq_rhleg_rom"])
rbprmBuilder.setAffordanceFilter("hyq_rhleg_rom", ["Support", "Lean"])
rbprmBuilder.setAffordanceFilter(
    "hyq_rfleg_rom",
    [
        "Support",
    ],
)
rbprmBuilder.setAffordanceFilter("hyq_lhleg_rom", ["Support", "Lean"])
rbprmBuilder.setAffordanceFilter(
    "hyq_lfleg_rom",
    [
        "Support",
    ],
)
# We also bound the rotations of the torso.

Exemple #10

Fichier : standing_hrp2_path.py Projet : 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)

Exemple #11

Fichier : robot_jumpEasy_path.py Projet : nim65s/hpp-rbprm-corba

rootJointType = 'freeflyer'
packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'robot_test_trunk'
urdfNameRom = ['robot_test_lleg_rom','robot_test_rleg_rom']
urdfSuffix = ""
srdfSuffix = ""
vMax = 1;
aMax = 5;
extraDof = 6
rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-6,6, -3, 3, 0, 2.5])
rbprmBuilder.boundSO3([-0.1,0.1,-1,1,-1,1])
rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
rbprmBuilder.setAffordanceFilter('robot_test_lleg_rom', ['Support'])
rbprmBuilder.setAffordanceFilter('robot_test_rleg_rom', ['Support'])
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])


#~ from hpp.corbaserver.rbprm. import ProblemSolver
from hpp.corbaserver.rbprm.problem_solver import ProblemSolver

ps = ProblemSolver( rbprmBuilder )
ps.client.problem.setParameter("aMax",aMax)
ps.client.problem.setParameter("vMax",vMax)
ps.client.problem.setParameter("sizeFootX",0.24)
ps.client.problem.setParameter("sizeFootY",0.14)
r = Viewer (ps)

Exemple #12

packageName = 'hpp-rbprm-corba'
meshPackageName = 'hpp-rbprm-corba'
urdfName = 'hrp2_trunk_flexible'
urdfNameRoms = [
    'hrp2_larm_rom_visual', 'hrp2_rarm_rom_visual', 'hrp2_lleg_rom_visual',
    'hrp2_rleg_rom_visual'
]
urdfSuffix = ""
srdfSuffix = ""

rbprmBuilder = Builder()

rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName,
                       packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [0, 3, -2, 0, 0.3, 1])
rbprmBuilder.setFilter(['hrp2_lleg_rom_visual', 'hrp2_rleg_rom_visual'])
rbprmBuilder.setAffordanceFilter('hrp2_rarm_rom_visual', ['Lean'])
rbprmBuilder.setAffordanceFilter('hrp2_lleg_rom_visual', [
    'Support',
])
rbprmBuilder.setAffordanceFilter('hrp2_rleg_rom_visual', ['Support'])
rbprmBuilder.boundSO3([-0., 0, -1, 1, -1, 1])
vMax = 1
aMax = 10
extraDof = 6
rbprmBuilder.client.basic.robot.setDimensionExtraConfigSpace(extraDof)
rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds(
    [-vMax, vMax, -vMax, vMax, 0, 0, 0, 0, 0, 0, 0, 0])
indexECS = rbprmBuilder.getConfigSize(
) - rbprmBuilder.client.basic.robot.getDimensionExtraConfigSpace()

Exemple #13

Fichier : darpa_hrp2_path.py Projet : pFernbach/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", [-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()

Exemple #14

Fichier : stair_bauzil_hyq_path.py Projet : pFernbach/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 = "hyq_trunk"
urdfNameRom = ["hyq_lhleg_rom", "hyq_lhleg_rom", "hyq_rfleg_rom", "hyq_rhleg_rom"]
urdfSuffix = ""
srdfSuffix = ""

rbprmBuilder = Builder()

rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds("base_joint_xyz", [-1, 4, -1, 1, 0, 2])
rbprmBuilder.setFilter(["hyq_lhleg_rom", "hyq_rfleg_rom"])

# ~ 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] = [-1, 0, 0.4]
rbprmBuilder.setCurrentConfig(q_init)
r(q_init)
# ~ q_init [0:3] = [0, -0.63, 0.6]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
# ~ q_init [3:7] = [ 0.98877108,  0.        ,  0.14943813,  0.        ]

Exemple #15

Fichier : test3.py Projet : 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 = 'hyq_trunk_large'
urdfNameRom = ['hyq_lhleg_rom','hyq_lfleg_rom','hyq_rfleg_rom','hyq_rhleg_rom']
urdfSuffix = ""
srdfSuffix = ""

rbprmBuilder = Builder ()
rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
rbprmBuilder.setJointBounds ("base_joint_xyz", [-2,5, -1, 1, 0.3, 4])

rbprmBuilder.setFilter(['hyq_rhleg_rom']) 
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)

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