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) q_goal = q_init [::]
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)
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() q_init[0:3] = [-2, 0, 0.55]