[demo] add scripts for talos on flat ground

script/dynamic/talos/flatGround_pyrene_interp.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.corbaserver.rbprm.rbprmfullbody import FullBody
+from hpp.gepetto import Viewer
+import time
+#from constraint_to_dae import *
+from hpp.corbaserver.rbprm.rbprmstate import State,StateHelper
+#from display_tools import *
+import talos.flatGround_pyrene_pathKino as tp
+import time
+
+tPlanning = tp.tPlanning
+
+
+packageName = "talos_description"
+meshPackageName = "talos_description"
+rootJointType = "freeflyer"
+##
+#  Information to retrieve urdf and srdf files.
+urdfName = "talos"
+urdfSuffix = "_full_v2"
+srdfSuffix = ""
+pId = tp.ps.numberPaths() -1
+fullBody = FullBody ()
+
+fullBody.loadFullBodyModel(urdfName, rootJointType, meshPackageName, packageName, urdfSuffix, srdfSuffix)
+fullBody.setJointBounds ("base_joint_xyz",  [-5,5, -1.5, 1.5, 0.95, 1.05])
+fullBody.client.basic.robot.setDimensionExtraConfigSpace(tp.extraDof)
+fullBody.client.basic.robot.setExtraConfigSpaceBounds([-0,0,-0,0,-0,0,0,0,0,0,0,0])
+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, displayCoM = True)
+
+
+q_ref = [
+        0,0,1.0192720229567027,1,0,0,0, # root_joint
+        0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_left
+        0.0, 0.0, -0.411354, 0.859395, -0.448041, -0.001708, # leg_right
+        0, 0.006761, # torso
+        0.25847, 0.173046, -0.0002, -0.525366, 0, 0, 0.1, # arm_left
+        0, 0, 0, 0, 0, 0, 0, # gripper_left
+        -0.25847, -0.173046, 0.0002, -0.525366, 0, 0, 0.1, # arm_right
+        0, 0, 0, 0, 0, 0, 0, # gripper_right
+        0, 0, # head
+        0,0,0,0,0,0]; r (q_ref)
+
+q_init = q_ref[::]
+
+#fullBody.setReferenceConfig(q_ref)
+"""
+#test correspondance with reduced : 
+q_init[19] = -0.5
+q_init[20] = 0.8
+r(q_init)
+"""
+
+fullBody.setCurrentConfig (q_init)
+qfar=q_ref[::]
+qfar[2] = -5
+
+tStart = time.time()
+nbSamples = 50000
+rLegId = 'rleg'
+rLeg = 'leg_right_1_joint'
+rfoot = 'leg_right_sole_fix_joint'
+rLegOffset = [0,0,0]
+rLegNormal = [0,0,1]
+rLegx = 0.1; rLegy = 0.06
+fullBody.addLimb(rLegId,rLeg,rfoot,rLegOffset,rLegNormal, rLegx, rLegy, nbSamples, "forward", 0.01)
+#fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
+
+lLegId = 'lleg'
+lLeg = 'leg_left_1_joint'
+lfoot = 'leg_left_sole_fix_joint'
+lLegOffset = [0,0,0]
+lLegNormal = [0,0,1]
+lLegx = 0.1; lLegy = 0.06
+fullBody.addLimb(lLegId,lLeg,lfoot,lLegOffset,rLegNormal, lLegx, lLegy, nbSamples, "forward", 0.01)
+#fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
+
+"""
+rarmId = 'rarm'
+rarm = 'arm_right_1_joint'
+rHand = 'gripper_right_inner_single_joint'
+rArmOffset = [0,0,0.1]
+rArmNormal = [0,0,1]
+rArmx = 0.02; rArmy = 0.02
+fullBody.addLimb(rarmId,rarm,rHand,rArmOffset,rArmNormal, rArmx, rArmy, nbSamples, "EFORT", 0.01)
+fullBody.runLimbSampleAnalysis(rarmId, "ReferenceConfiguration", True)
+
+larmId = 'larm'
+larm = 'arm_left_1_joint'
+lHand = 'gripper_left_inner_single_joint'
+lArmOffset = [0,0,-0.1]
+lArmNormal = [0,0,1]
+lArmx = 0.02; lArmy = 0.02
+fullBody.addLimb(larmId,larm,lHand,lArmOffset,lArmNormal, lArmx, lArmy, nbSamples, "EFORT", 0.01)
+fullBody.runLimbSampleAnalysis(larmId, "ReferenceConfiguration", True)
+"""
+
+
+
+tGenerate =  time.time() - tStart
+print "generate databases in : "+str(tGenerate)+" s"
+
+
+q_0 = fullBody.getCurrentConfig(); 
+#~ fullBody.createOctreeBoxes(r.client.gui, 1, rarmId, q_0,)
+
+
+
+
+configSize = fullBody.getConfigSize() -fullBody.client.basic.robot.getDimensionExtraConfigSpace()
+
+q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation
+q_goal = q_init[::]; q_goal[0:7] = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[0:7]
+dir_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS:tp.indexECS+3]
+acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6]
+dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3]
+acc_goal = [0,0,0]
+
+robTreshold = 3
+# 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] = [0,0,0]
+
+q_init[2] = q_ref[2]+0.01
+q_goal[2] = q_ref[2]+0.01
+
+fullBody.setStaticStability(True)
+# Randomly generating a contact configuration at q_init
+fullBody.setCurrentConfig (q_init)
+r(q_init)
+#q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
+r(q_init)
+
+# Randomly generating a contact configuration at q_end
+fullBody.setCurrentConfig (q_goal)
+#q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
+r(q_goal)
+
+# specifying the full body configurations as start and goal state of the problem
+r.addLandmark('talos/base_link',0.3)
+r(q_init)
+
+
+fullBody.setStartState(q_init,[rLegId,lLegId])
+fullBody.setEndState(q_goal,[rLegId,lLegId])
+
+
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (fullBody.client.basic, r)
+
+import fullBodyPlayerHrp2
+
+tStart = time.time()
+configsFull = fullBody.interpolate(0.001,pathId=pId,robustnessTreshold = 2, filterStates = False)
+tInterpolateConfigs = time.time() - tStart
+print "number of configs :", len(configsFull)
+
+
+
+
+from planning.config import *
+from generate_contact_sequence import *
+
+beginState = 0
+endState = len(configsFull)-2
+configs=configsFull[beginState:endState+1]
+cs = generateContactSequence(fullBody,configs,beginState, endState,r,curves_initGuess =curves_initGuess , timings_initGuess =timings_initGuess)
+#player.displayContactPlan()
+
+
+filename = OUTPUT_DIR + "/" + OUTPUT_SEQUENCE_FILE
+cs.saveAsXML(filename, "ContactSequence")
+print "save contact sequence : ",filename
+
+
+
+
+

script/dynamic/talos/flatGround_pyrene_pathKino.py

+from hpp.corbaserver.rbprm.rbprmbuilder import Builder
+from hpp.gepetto import Viewer
+from hpp.corbaserver import Client
+from hpp.corbaserver.robot import Robot as Parent
+from hpp.corbaserver.rbprm.problem_solver import ProblemSolver
+import omniORB.any
+from planning.configs.straight_walk_config import *
+import time
+
+class Robot (Parent):
+	rootJointType = 'freeflyer'
+	packageName = 'talos-rbprm'
+	meshPackageName = 'talos-rbprm'
+	# URDF file describing the trunk of the robot HyQ
+	urdfName = 'talos_trunk'
+	urdfSuffix = ""
+	srdfSuffix = ""
+	def __init__ (self, robotName, load = True):
+		Parent.__init__ (self, robotName, self.rootJointType, load)
+		self.tf_root = "base_footprint"
+		self.client.basic = Client ()
+		self.load = load
+		
+
+rootJointType = 'freeflyer'
+packageName = 'talos-rbprm'
+meshPackageName = 'talos-rbprm'
+urdfName = 'talos_trunk'
+urdfNameRom =  ['talos_larm_rom','talos_rarm_rom','talos_lleg_rom','talos_rleg_rom']
+urdfSuffix = ""
+srdfSuffix = ""
+vMax = omniORB.any.to_any(0.2);
+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))
+ps.client.problem.setParameter("friction",mu)
+r = Viewer (ps)
+
+from hpp.corbaserver.affordance.affordance import AffordanceTool
+afftool = AffordanceTool ()
+afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
+afftool.loadObstacleModel (ENV_PACKAGE_NAME, ENV_NAME, ENV_PREFIX, r)
+#r.loadObstacleModel (packageName, "ground", "planning")
+#afftool.visualiseAffordances('Support', r, r.color.lightBrown)
+#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] = [0, 0, 0.98]; r (q_init)
+
+"""
+# test correspondance with fullBody : 
+q_init[7] = -0.5
+q_init[8] = 0.7
+r(q_init)
+"""
+
+
+#q_init[3:7] = [0.7071,0,0,0.7071]
+#q_init [0:3] = [1, 1, 0.65]
+
+rbprmBuilder.setCurrentConfig (q_init)
+q_goal = q_init [::]
+
+
+q_goal[3:7] = [1,0,0,0]
+q_goal[0] = 1.5
+
+r (q_goal)
+#~ q_goal [0:3] = [-1.5, 0, 0.63]; 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("RbprmDynamicPathValidation",0.05)
+# Choosing kinodynamic methods :
+ps.selectSteeringMethod("RBPRMKinodynamic")
+ps.selectDistance("KinodynamicDistance")
+ps.selectPathPlanner("DynamicPlanner")
+ps.selectPathProjector('Progressive',0.05)
+#solve the problem :
+r(q_init)
+
+
+#r.solveAndDisplay("rm",1,0.01)
+tStart=time.time()
+
+t = ps.solve ()
+
+tPlanning = time.time() -tStart
+
+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")
+
+
+"""
+if isinstance(t, list):
+	t = t[0]* 3600000 + t[1] * 60000 + t[2] * 1000 + t[3]
+f = open('log.txt', 'a')
+f.write("path computation " + str(t) + "\n")
+f.close()
+"""
+
+"""
+for i in range(0,9):
+  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.client.problem.prepareSolveStepByStep()
+
+#ps.client.problem.finishSolveStepByStep()
+"""
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+"""
+
+
+
+
+"""
+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)
+
+"""
+
+
+"""
+# for seed 1486657707
+ps.client.problem.extractPath(0,0,2.15) 
+
+# Playing the computed path
+from hpp.gepetto import PathPlayer
+pp = PathPlayer (rbprmBuilder.client.basic, r)
+pp.dt=0.03
+pp.displayVelocityPath(1)
+r.client.gui.setVisibility("path_1_root","ALWAYS_ON_TOP")
+#display path
+pp.speed=0.3
+#pp (0)
+"""
+#display path with post-optimisation
+
+
+
+
+
+q_far = q_init[::]
+q_far[2] = -3
+r(q_far)
+
+
+
+
+
+ # Manually add waypoints to roadmap:
+"""
+ps.client.problem.prepareSolveStepByStep()
+pbCl = rbprmBuilder.client.basic.problem
+q1= [0.6, 1, 0.5, 1, 0, 0, 0, 0.0, 0, 0.0, 0.0, 3, 0.0, -1.5, 0.0, 0.0, 0.0]
+
+pbCl.addConfigToRoadmap (q1)
+
+
+pbCl.directPath(q1,q_goal,True)
+
+pbCl.directPath(q_init,q1,False)
+r.client.gui.removeFromGroup("path_"+str(ps.numberPaths()-2)+"_root",r.sceneName)
+pp.displayVelocityPath(ps.numberPaths()-1)
+
+pbCl.addEdgeToRoadmap (q_init, q1, 1, False)
+pbCl.addEdgeToRoadmap (q1, q_goal, 0, False)
+
+"""
+