[script] update python script for talos_platform_random

script/scenarios/memmo/talos_platform_random.py

@@ -26,6 +26,7 @@ fullBody.client.robot.setExtraConfigSpaceBounds([-tp.vMax,tp.vMax,-tp.vMax,tp.vM
 ps = tp.ProblemSolver( fullBody )
 ps.setParameter("Kinodynamic/velocityBound",tp.vMax)
 ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
+ps.setParameter("FullBody/frictionCoefficient",tp.mu)
 #load the viewer
 try :
     v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
@@ -46,8 +47,8 @@ except Exception:
 q_ref = fullBody.referenceConfig_legsApart[::]+[0]*6
 q_init = q_ref[::]
 fullBody.setReferenceConfig(q_ref)
-
 fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
+fullBody.usePosturalTaskContactCreation(True)
 
 fullBody.setCurrentConfig (q_init)
 
@@ -56,9 +57,9 @@ tStart = time.time()
 # generate databases : 
 
 nbSamples = 100000
-fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
+fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints,kinematicConstraintsMin = 0.85)
 #fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
-fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
+fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
 #fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
 
 
@@ -83,19 +84,6 @@ q_init[configSize+3:configSize+6] = acc_init[::]
 q_goal[configSize:configSize+3] = vel_goal[::]
 q_goal[configSize+3:configSize+6] = [0,0,0]
 
-'''
-state_id = fullBody.generateStateInContact(q_init, vel_init,robustnessThreshold=5)
-assert fullBody.rLegId in fullBody.getAllLimbsInContact(state_id), "Right leg not in contact in initial state"
-assert fullBody.lLegId in fullBody.getAllLimbsInContact(state_id), "Left leg not in contact in initial state"
-q_init = fullBody.getConfigAtState(state_id)
-v(q_init)
-
-state_id = fullBody.generateStateInContact(q_goal, vel_goal,robustnessThreshold=5)
-assert fullBody.rLegId in fullBody.getAllLimbsInContact(state_id), "Right leg not in contact in final state"
-assert fullBody.lLegId in fullBody.getAllLimbsInContact(state_id), "Left leg not in contact in final state"
-q_goal = fullBody.getConfigAtState(state_id)
-v(q_goal)
-'''
 
 
 fullBody.setStaticStability(True)
@@ -106,7 +94,7 @@ q_init[2]=q_ref[2]
 q_goal[2]=q_ref[2]
 
 
-# define init gait according the direction of motion, try to move first the leg on the outside of the turn : 
+# define init gait according to the direction of motion, try to move first the leg on the outside of the turn : 
 if q_goal[0] > q_init[0] : #go toward x positif
   if q_goal[1] > q_init[1]: # turn left
     gait = [fullBody.rLegId,fullBody.lLegId]
@@ -118,11 +106,14 @@ else : # go toward x negatif
   else : # turn left
     gait = [fullBody.rLegId,fullBody.lLegId]
 
+v(q_init)
 fullBody.setStartState(q_init,gait)
+v(q_goal)
 fullBody.setEndState(q_goal,gait)
-v(q_init)
+
 print "Generate contact plan ..."
 tStart = time.time()
+v(q_init)
 configs = fullBody.interpolate(0.005,pathId=pId,robustnessTreshold = 1, filterStates = True,testReachability=True,quasiStatic=True)
 tInterpolateConfigs = time.time() - tStart
 print "Done."

script/scenarios/memmo/talos_platform_random_path.py

@@ -12,7 +12,7 @@ vGoal = 0.01
 vMax = 0.5# linear velocity bound for the root
 aMax = 0.5# linear acceleration bound for the root
 extraDof = 6
-mu=0.5# coefficient of friction
+mu=0.3# coefficient of friction
 # Creating an instance of the helper class, and loading the robot
 # Creating an instance of the helper class, and loading the robot
 rbprmBuilder = Robot ()
@@ -40,7 +40,7 @@ ps.setParameter("Kinodynamic/velocityBound",vMax)
 ps.setParameter("Kinodynamic/accelerationBound",aMax)
 ps.setParameter("DynamicPlanner/sizeFootX",0.2)
 ps.setParameter("DynamicPlanner/sizeFootY",0.12)
-ps.setParameter("DynamicPlanner/friction",0.5)
+ps.setParameter("DynamicPlanner/friction",mu)
 # sample only configuration with null velocity and acceleration :
 ps.setParameter("ConfigurationShooter/sampleExtraDOF",False)
 
@@ -52,7 +52,7 @@ vf = ViewerFactory (ps)
 from hpp.corbaserver.affordance.affordance import AffordanceTool
 afftool = AffordanceTool ()
 afftool.setAffordanceConfig('Support', [0.5, 0.03, 0.00005])
-afftool.loadObstacleModel ("hpp_environments", "multicontact/plateforme_not_flat", "planning", vf, reduceSizes=[0.16,0,0])
+afftool.loadObstacleModel ("hpp_environments", "multicontact/plateforme_not_flat", "planning", vf, reduceSizes=[0.1,0,0])
 try :
     v = vf.createViewer(displayArrows = True)
 except Exception:
@@ -86,8 +86,8 @@ end_plateform_id = random.randint(init_plateform_id+1,4)
 #  end_plateform_id+=1
 
 # set x position from the plateform choosen : 
-x_init = 0.16 + 0.92*init_plateform_id
-x_goal = 0.16 + 0.92*end_plateform_id
+x_init = 0.16 + 0.925*init_plateform_id
+x_goal = 0.16 + 0.925*end_plateform_id
 
 # uniformly sample y position
 y_init = random.uniform(Y_BOUNDS[0],Y_BOUNDS[1])