add test script for 'robot_test'

script/tests/robot_test_path.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+white=[1.0,1.0,1.0,1.0]
+green=[0.23,0.75,0.2,0.5]
+yellow=[0.85,0.75,0.15,1]
+pink=[1,0.6,1,1]
+orange=[1,0.42,0,1]
+brown=[0.85,0.75,0.15,0.5]
+blue = [0.0, 0.0, 0.8, 1.0]
+grey = [0.7,0.7,0.7,1.0]
+red = [0.8,0.0,0.0,1.0]
+
+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 = ""
+
+rbprmBuilder = Builder ()
+rbprmBuilder.loadModel(urdfName, urdfNameRom, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [-8,6, -2, 2, 0.4, 1.3])
+rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-1.0,1.0])
+rbprmBuilder.setFilter(['robot_test_lleg_rom', 'robot_test_rleg_rom'])
+rbprmBuilder.setNormalFilter('robot_test_lleg_rom', [0,0,1], 0.5)
+rbprmBuilder.setNormalFilter('robot_test_rleg_rom', [0,0,1], 0.5)
+
+
+#~ from hpp.corbaserver.rbprm. import ProblemSolver
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+
+ps = ProblemSolver( rbprmBuilder )
+
+r = Viewer (ps)
+
+r.loadObstacleModel (packageName, "groundcrouch", "planning")
+
+
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init [0:3] = [-6, 0, 0.9]; rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+
+q_goal = q_init [::]
+#q_goal [0:3] = [-2, 0, 0.9]; r (q_goal) # premiere passerelle
+q_goal [0:3] = [4, 0, 0.9]; r (q_goal) # pont
+
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.client.problem.selectSteeringMethod("SteeringDynamic")
+ps.selectPathPlanner("RRTdynamic")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+
+r(q_init)
+
+r.solveAndDisplay("rm",1,0.02)
+
+
+#t = ps.solve ()
+
+#r.displayRoadmap("rm",0.005)
+
+r.displayPathMap("rmPath",0,0.02)
+
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+
+pp(0)
+
+
+pp.displayPath(1,blue)
+r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
+
+
+pp (1)
+
+#r.client.gui.removeFromGroup("rm",r.sceneName)
+r.client.gui.removeFromGroup("rmPath",r.sceneName)
+r.client.gui.removeFromGroup("path_1_root",r.sceneName)
+#~ pp.toFile(1, "/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+

script/tests/truck_hrp2_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+import time 
+
+import truck_hrp2_path as tp
+
+
+
+packageName = "hrp2_14_description"
+meshPackageName = "hrp2_14_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "hrp2_14"
+urdfSuffix = "_reduced"
+srdfSuffix = ""
+
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz", [0,2.2, -1, 1, 0.7, 2.5])
+
+
+ps = tp.ProblemSolver( fullBody )
+r = tp.Viewer (ps)
+r.client.gui.removeFromGroup("planning",r.sceneName)
+r.client.gui.removeFromGroup("hrp2_trunk_flexible",r.sceneName)
+
+#~ AFTER loading obstacles
+rLegId = '0rLeg'
+rLeg = 'RLEG_JOINT0'
+rLegOffset = [0,-0.105,0,]
+rLegNormal = [0,1,0]
+rLegx = 0.09; rLegy = 0.05
+fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 10000, "manipulability", 0.03)
+
+lLegId = '1lLeg'
+lLeg = 'LLEG_JOINT0'
+lLegOffset = [0,-0.105,0]
+lLegNormal = [0,1,0]
+lLegx = 0.09; lLegy = 0.05
+fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 10000, "manipulability", 0.03)
+
+rarmId = '3Rarm'
+rarm = 'RARM_JOINT0'
+rHand = 'RARM_JOINT5'
+rArmOffset = [-0.05,-0.050,-0.050]
+rArmNormal = [1,0,0]
+rArmx = 0.024; rArmy = 0.024
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, 20000, "forward", 0.05)
+
+larmId = '4Larm'
+larm = 'LARM_JOINT0'
+lHand = 'LARM_JOINT5'
+lArmOffset = [-0.05,-0.050,-0.050]
+lArmNormal = [1,0,0]
+lArmx = 0.024; lArmy = 0.024
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, 20000, "forward", 0.05)
+
+q_0 = fullBody.getCurrentConfig(); 
+
+confsize = len(tp.q_init)
+q_init = fullBody.getCurrentConfig(); q_init[0:confsize] = tp.q_init[0:confsize]
+q_goal = fullBody.getCurrentConfig(); q_goal[0:confsize] = tp.q_goal[0:confsize]
+
+
+fullBody.setCurrentConfig (q_init)
+#~ q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.generateContacts(q_init, [0,0,-1]); r (q_init)
+
+fullBody.setCurrentConfig (q_goal)
+#~ r(q_goal)
+q_goal = fullBody.generateContacts(q_goal, [0,0,1])
+#~ r(q_goal)
+
+#~ gui = r.client.gui
+
+
+
+
+fullBody.setStartState(q_init,[rLegId,lLegId,rarmId,larmId])
+#~ fullBody.setStartState(q_init,[rLegId,lLegId])
+#~ fullBody.setStartState(q_init,[rLegId,lLegId,rarmId])
+#~ fullBody.setStartState(q_init,[rLegId,lLegId,larmId])
+#~ fullBody.setEndState(q_goal,[rLegId,lLegId,rarmId,larmId])
+#~ fullBody.setEndState(q_goal,[rLegId,lLegIdlarmId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])
+#~ 
+#~ r(q_init)
+configs = fullBody.interpolate(0.1)
+r.loadObstacleModel ('hpp-rbprm-corba', "truck", "contact")
+#~ fullBody.exportAll(r, configs, 'truck_hrp2_not_robust');
+
+
+i = 0;
+while(i < (len(configs)-1)):
+  fullBody.draw(configs[i],r); i=i+1; i-1
+  time.sleep(0.3)
+
+
+
+
+#~ 
+#~ f1 = open("hrp2_standing_29_10_15","w+")
+#~ f1.write(str(configs))
+#~ f1.close()

script/tests/truck_hrp2_path.py

+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_lleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+
+rbprmBuilder = Builder ()
+
+rbprmBuilder.loadModel(urdfName, urdfNameRoms, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+rbprmBuilder.setJointBounds ("base_joint_xyz", [0,2.2, -1, 1, 0.7, 2.5])
+#~ rbprmBuilder.setJointBounds ("base_joint_xyz", [-20,20.2, -10, 10, 0.7, 2.5])
+#~ rbprmBuilder.setFilter(['hrp2_lleg_rom','hrp2_rleg_rom'])
+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.6)
+#~ rbprmBuilder.setNormalFilter('hrp2_rleg_rom', [0,0,1], 0.6)
+rbprmBuilder.boundSO3([-0.1,0.1,-3,3,-1.0,1.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 [::]
+q_goal [0:3] = [0.19, 0.05, 0.9]; r (q_goal)
+
+
+rbprmBuilder.client.basic.robot.setJointConfig('base_joint_SO3',[0.7316888688738209, 0, 0.6816387600233341, 0]); q_init = rbprmBuilder.getCurrentConfig (); r (q_init)
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init[0:9] = [0.27, -0.05, 1.2, 1.0, 0.0, 0.0, 0.0, 0.0, 1.0472]
+
+rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+
+q_goal[0:3] = [2,0,1.7]; 
+q_goal[0:3]=[1.6,-0.1,1.5];
+#~ q_init [0:6] = [0.0, -2.2, 2.0, 0.7316888688738209, 0.0, 0.6816387600233341]; 
+#~ rbprmBuilder.setCurrentConfig (q_init); r (q_init)
+
+
+#~ ps.addPathOptimizer("GradientBased")
+ps.addPathOptimizer("RandomShortcut")
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+r.loadObstacleModel (packageName, "truck", "planning")
+t = ps.solve ()
+t
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+#~ pp.fromFile("/home/stonneau/dev/hpp/src/hpp-rbprm-corba/script/paths/stair.path")
+#~ 
+#~ pp (2)
+r.displayRoadmap("rm",0.02)
+r.displayPathMap("rmPath",0,0.025)
+pp (0)
+