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 = "" 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)
#~ print sys.args rootJointType = 'freeflyer' packageName = 'hpp-rbprm-corba' meshPackageName = 'hpp-rbprm-corba' urdfNameTested = 'hrp2_trunk_flexible' urdfNameRoms = ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom'] urdfSuffix = "" 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)
# but hpp-rbprm-server 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)