From 5d2a9e8b35abe9146fa2aa797fdff2fed5b40ef3 Mon Sep 17 00:00:00 2001
From: Pierre Fernbach <pierre.fernbach@laas.fr>
Date: Wed, 24 Jul 2019 15:23:01 +0200
Subject: [PATCH] [script] update python script for talos_platform_random
---
.../scenarios/memmo/talos_platform_random.py | 27 +++++++------------
.../memmo/talos_platform_random_path.py | 10 +++----
2 files changed, 14 insertions(+), 23 deletions(-)
diff --git a/script/scenarios/memmo/talos_platform_random.py b/script/scenarios/memmo/talos_platform_random.py
index 632849d9..20642523 100644
--- a/script/scenarios/memmo/talos_platform_random.py
+++ b/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."
diff --git a/script/scenarios/memmo/talos_platform_random_path.py b/script/scenarios/memmo/talos_platform_random_path.py
index ec67c819..24018452 100644
--- a/script/scenarios/memmo/talos_platform_random_path.py
+++ b/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])
--
GitLab