fix script for hrp2 down slope

Conflicts:
	script/dynamic/slalom_hyq_interpKino.py
	script/dynamic/slalom_hyq_pathKino.py

script/dynamic/downSlope_hrp2_pathKino.py

@@ -26,7 +26,7 @@ urdfName = 'hrp2_trunk_flexible'
 urdfNameRom =  ['hrp2_larm_rom','hrp2_rarm_rom','hrp2_lleg_rom','hrp2_rleg_rom']
 urdfSuffix = ""
 srdfSuffix = ""
-vMax = 2;
+vMax = 4;
 aMax = 6;
 extraDof = 6
 
@@ -48,7 +48,7 @@ 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([-2,2,-0.5,0.5,-2,2,0,0,0,0,0,0])
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-4,4,-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
@@ -105,8 +105,29 @@ r(q_init)
 
 #r.solveAndDisplay("rm",1,0.01)
 
+"""
+t = ps.solve()
+
+if isinstance(t, list):
+	ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.	
+
+f= open("/local/dev_hpp/logs/benchHrp2_slope_LP.txt","a")
+f.write("t = "+str(ts))
+f.close()
+"""
+
+
+for i in range(0,50):
+  t = ps.solve()
+  if isinstance(t, list):
+    ts = t[0]* 3600. + t[1] * 60. + t[2] + t[3]/1000.	
+  f= open("/local/dev_hpp/logs/benchHrp2_slope_LP.txt","a")
+  f.write("t = "+str(ts) + "\n")
+  f.write("path_length = "+str(ps.client.problem.pathLength(i)) + "\n")
+  f.close()
+  print "problem "+str(i)+"solved \n"
+  ps.clearRoadmap()
 
-ps.solve()
 
 #ps.client.problem.prepareSolveStepByStep()
 

script/dynamic/slalom_hyq_interpKino.py

+#Importing helper class for RBPRM
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+from hpp.gepetto import Viewer
+
+#calling script darpa_hyq_path to compute root path
+import slalom_hyq_pathKino as tp
+
+from os import environ
+ins_dir = environ['DEVEL_DIR']
+db_dir = ins_dir+"/install/share/hyq-rbprm/database/hyq_"
+
+pathId = tp.ps.numberPaths()-1
+
+packageName = "hyq_description"
+meshPackageName = "hyq_description"
+rootJointType = "freeflyer"
+
+#  Information to retrieve urdf and srdf files.
+urdfName = "hyq"
+urdfSuffix = ""
+srdfSuffix = ""
+
+#  This time we load the full body model of HyQ
+fullBody = FullBody () 
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.setJointBounds ("base_joint_xyz", [-6,6, -2.5, 2.5, 0.0, 1.])
+
+#  Setting a number of sample configurations used
+nbSamples = 20000
+dynamic=True
+
+ps = tp.ProblemSolver(fullBody)
+ps.client.problem.setParameter("aMax",tp.aMax)
+ps.client.problem.setParameter("vMax",tp.vMax)
+r = tp.Viewer (ps,viewerClient=tp.r.client)
+
+rootName = 'base_joint_xyz'
+
+
+def addLimbDb(limbId, heuristicName, loadValues = True, disableEffectorCollision = False):
+	fullBody.addLimbDatabase(str(db_dir+limbId+'.db'), limbId, heuristicName,loadValues, disableEffectorCollision)
+
+rLegId = 'rfleg'
+lLegId = 'lhleg'
+rarmId = 'rhleg'
+larmId = 'lfleg'
+
+addLimbDb(rLegId, "manipulability")
+addLimbDb(lLegId, "manipulability")
+addLimbDb(rarmId, "manipulability")
+addLimbDb(larmId, "manipulability")
+
+q_0 = fullBody.getCurrentConfig(); 
+q_init = fullBody.getCurrentConfig(); q_init[0:7] = tp.ps.configAtParam(0,0.01)[0:7] # use this to get the correct orientation
+q_goal = fullBody.getCurrentConfig(); q_goal[0:7] = tp.ps.configAtParam(pathId,tp.ps.pathLength(pathId))[0:7]
+dir_init = tp.ps.configAtParam(pathId,0.01)[7:10]
+acc_init = tp.ps.configAtParam(pathId,0.01)[10:13]
+dir_goal = tp.ps.configAtParam(pathId,tp.ps.pathLength(pathId))[7:10]
+acc_goal = tp.ps.configAtParam(pathId,tp.ps.pathLength(pathId))[10:13]
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+# copy extraconfig for start and init configurations
+q_init[configSize:configSize+3] = dir_init[::]
+q_init[configSize+3:configSize+6] = acc_init[::]
+q_goal[configSize:configSize+3] = dir_goal[::]
+q_goal[configSize+3:configSize+6] = acc_goal[::]
+
+
+fullBody.setStaticStability(False)
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init)
+q_init = fullBody.generateContacts(q_init,dir_init,acc_init,2)
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,2)
+
+
+# specifying the full body configurations as start and goal state of the problem
+fullBody.setStartState(q_init,[larmId,rLegId,rarmId,lLegId])
+fullBody.setEndState(q_goal,[larmId,rLegId,rarmId,lLegId])
+
+
+r(q_init)
+# computing the contact sequence
+
+configs = fullBody.interpolate(0.04,pathId=pathId,robustnessTreshold = 0, filterStates = True)
+
+
+print "number of configs =", len(configs)
+r(configs[-1])
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+import fullBodyPlayer
+player = fullBodyPlayer.Player(fullBody,pp,tp,configs,draw=True,optim_effector=True,use_velocity=dynamic,pathId = pathId)
+
+#player.displayContactPlan()
+
+r(configs[2])
+player.interpolate(2,40)
+
+#player.play()
+
+
+
+"""
+
+camera = [0.5681925415992737,
+ -6.707448482513428,
+ 2.5206544399261475,
+ 0.8217507600784302,
+ 0.5693002343177795,
+ 0.020600343123078346,
+ 0.01408931240439415]
+r.client.gui.setCameraTransform(0,camera)
+
+"""
+
+
+
+
+"""
+import hpp.corbaserver.rbprm.tools.cwc_trajectory
+import hpp.corbaserver.rbprm.tools.path_to_trajectory
+import hpp.corbaserver.rbprm.tools.cwc_trajectory_helper
+
+reload(hpp.corbaserver.rbprm.tools.cwc_trajectory)
+reload(hpp.corbaserver.rbprm.tools.path_to_trajectory)
+reload(hpp.corbaserver.rbprm.tools.cwc_trajectory_helper)
+reload(fullBodyPlayer)
+
+
+"""
+
+

script/dynamic/slalom_hyq_pathKino.py

+## Importing helper class for setting up a reachability planning problem
+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+
+# Importing Gepetto viewer helper class
+from hpp.gepetto import Viewer
+import time
+
+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 = ""
+vMax = 1;
+aMax = 2;
+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", [-6,6, -2.5, 2.5, 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)
+rbprmBuilder.client.basic.robot.setExtraConfigSpaceBounds([-vMax,vMax,-vMax,vMax,0,0,0,0,0,0,0,0])
+
+# 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",aMax)
+ps.client.problem.setParameter("vMax",vMax)
+r = Viewer (ps)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (packageName, "slalom", "planning", r)
+#r.loadObstacleModel (packageName, "ground", "planning")
+afftool.visualiseAffordances('Support', r, [0.25, 0.5, 0.5])
+r.addLandmark(r.sceneName,1)
+
+# Setting initial and goal configurations
+q_init = rbprmBuilder.getCurrentConfig ();
+q_init[3:7] = [1,0,0,0]
+q_init [0:3] = [-5, 1.2, 0.63]; r (q_init)
+
+
+rbprmBuilder.setCurrentConfig (q_init)
+q_goal = q_init [::]
+
+q_goal[3:7] = [1,0,0,0]
+q_goal [0:3] = [5, 1, 0.63]; r(q_goal)
+
+r (q_goal)
+
+
+# Choosing a path optimizer
+ps.setInitialConfig (q_init)
+ps.addGoalConfig (q_goal)
+# Choosing RBPRM shooter and path validation methods.
+ps.client.problem.selectConFigurationShooter("RbprmShooter")
+ps.client.problem.selectPathValidation("RbprmPathValidation",0.05)
+# Choosing kinodynamic methods : 
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.selectPathPlanner("DynamicPlanner")
+
+#solve the problem :
+r(q_init)
+
+#ps.client.problem.prepareSolveStepByStep()
+
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+
+#r.solveAndDisplay("rm",1,0.01)
+
+
+t = ps.solve ()
+
+
+
+"""
+camera = [0.6293167471885681,
+ -9.560577392578125,
+ 10.504343032836914,
+ 0.9323806762695312,
+ 0.36073973774909973,
+ 0.008668755181133747,
+ 0.02139890193939209]
+r.client.gui.setCameraTransform(0,camera)
+"""
+
+"""
+r.client.gui.removeFromGroup("rm",r.sceneName)
+r.client.gui.removeFromGroup("rmstart",r.sceneName)
+r.client.gui.removeFromGroup("rmgoal",r.sceneName)
+for i in range(0,ps.numberNodes()):
+  r.client.gui.removeFromGroup("vecRM"+str(i),r.sceneName)
+
+"""
+
+
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(0)
+r.client.gui.setVisibility("path_0_root","ALWAYS_ON_TOP")
+#display path
+pp.speed=0.3
+#pp (0)
+
+#display path with post-optimisation
+
+
+ps.client.problem.extractPath(0,0,11.18)
+r.client.gui.removeFromGroup("path_0_root",r.sceneName)
+pp.displayVelocityPath(1)
+r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
+#pp (1)
+
+
+
+"""
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+"""
+
+
+"""
+for i in range(1,10):
+    rbprmBuilder.client.basic.problem.optimizePath(i)
+    r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+    pp.displayVelocityPath(i+1)
+    #time.sleep(2)
+"""
+
+"""
+i=0
+
+ps.clearRoadmap()
+ps.solve()
+r.client.gui.removeFromGroup("path_"+str(i)+"_root",r.sceneName)
+i = i+1
+pp.displayVelocityPath(i)
+
+pp(i)
+
+
+"""
+
+"""
+r.client.gui.addCurve("c1",qlist,r.color.red)
+r.client.gui.setVisibility("c1","ALWAYS_ON_TOP")
+r.client.gui.addToGroup("c1",r.sceneName)
+
+
+r.client.gui.addCurve("c2",qlist2,r.color.blue)
+r.client.gui.setVisibility("c2","ALWAYS_ON_TOP")
+r.client.gui.addToGroup("c2",r.sceneName)
+
+
+
+"""
+
+
+"""
+nodes = ["hyq_trunk_large/base_link","Vec_Acceleration","Vec_Velocity"]
+r.client.gui.setCaptureTransform("yaml/hyq_slalom_path.yaml",nodes)
+r.client.gui.captureTransformOnRefresh(True)
+pp(1)
+r.client.gui.captureTransformOnRefresh(False)
+
+r.client.gui.writeNodeFile('path_1_root','meshs/slalom_path.obj')
+
+"""
+
+