From e1ac4e4b6010120f3b02c70493485b7b0076a54b Mon Sep 17 00:00:00 2001
From: pFernbach <pierre.fernbach@gmail.com>
Date: Thu, 20 Jun 2019 15:46:44 +0200
Subject: [PATCH] [scripts] update talos scripts in demo folder
---
script/scenarios/demos/talos_flatGround.py | 4 ++--
script/scenarios/demos/talos_navBauzil.py | 15 ++++++++-------
script/scenarios/demos/talos_navBauzil_hard.py | 18 +++++++++---------
.../demos/talos_navBauzil_hard_path.py | 3 ++-
script/scenarios/demos/talos_navBauzil_path.py | 1 +
5 files changed, 22 insertions(+), 19 deletions(-)
diff --git a/script/scenarios/demos/talos_flatGround.py b/script/scenarios/demos/talos_flatGround.py
index fe791078..687c4726 100644
--- a/script/scenarios/demos/talos_flatGround.py
+++ b/script/scenarios/demos/talos_flatGround.py
@@ -33,7 +33,7 @@ print "Generate limb DB ..."
tStart = time.time()
# generate databases :
-nbSamples = 100000
+nbSamples = 50000
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.75)
#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.75)
@@ -83,7 +83,7 @@ fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
print "Generate contact plan ..."
tStart = time.time()
-configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True)
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
diff --git a/script/scenarios/demos/talos_navBauzil.py b/script/scenarios/demos/talos_navBauzil.py
index 9323f46c..92e79f31 100644
--- a/script/scenarios/demos/talos_navBauzil.py
+++ b/script/scenarios/demos/talos_navBauzil.py
@@ -6,7 +6,7 @@ print "Plan guide trajectory ..."
import talos_navBauzil_path as tp
print "Done."
import time
-
+Robot.urdfSuffix += "_safeFeet"
pId = tp.ps.numberPaths() -1
@@ -34,17 +34,18 @@ q_ref = fullBody.referenceConfig[::]+[0,0,0,0,0,0]
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
-
+fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
+fullBody.usePosturalTaskContactCreation(True)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
nbSamples = 10000
-fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01)
-fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
+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.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_rLeg_walk.db")
-fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01)
-fullBody.runLimbSampleAnalysis(fullBody.lLegId, "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.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/talos_lLeg_walk.db")
tGenerate = time.time() - tStart
@@ -89,7 +90,7 @@ fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
print "Generate contact plan ..."
tStart = time.time()
-configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=False)
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
diff --git a/script/scenarios/demos/talos_navBauzil_hard.py b/script/scenarios/demos/talos_navBauzil_hard.py
index 51058738..ebdef896 100644
--- a/script/scenarios/demos/talos_navBauzil_hard.py
+++ b/script/scenarios/demos/talos_navBauzil_hard.py
@@ -6,7 +6,7 @@ print "Plan guide trajectory ..."
import talos_navBauzil_hard_path as tp
print "Done."
import time
-
+Robot.urdfSuffix += "_safeFeet"
pId = tp.ps.numberPaths() -1
fullBody = Robot ()
@@ -30,20 +30,20 @@ ps.setParameter("Kinodynamic/accelerationBound",tp.aMax)
v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
# load a reference configuration
-q_ref = fullBody.referenceConfig_legsApart[::] + [0]*6
+q_ref = fullBody.referenceConfig[::] + [0]*6
q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref)
fullBody.setCurrentConfig (q_init)
-
+fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
+fullBody.usePosturalTaskContactCreation(True)
print "Generate limb DB ..."
tStart = time.time()
# generate databases :
-nbSamples = 20000
-fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep06", 0.01)
-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)
-fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
+nbSamples = 10000
+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.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep06", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints,kinematicConstraintsMin = 0.85)
+
#In this scenario, the arms are not used to create contact, but they may move to avoid collision.
fullBody.addNonContactingLimb(fullBody.lArmId,fullBody.larm,fullBody.lhand,5000)
@@ -93,7 +93,7 @@ fullBody.setEndState(q_goal,[fullBody.lLegId,fullBody.rLegId])
print "Generate contact plan ..."
tStart = time.time()
-configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True)
+configs = fullBody.interpolate(0.01,pathId=pId,robustnessTreshold = 2, filterStates = True,testReachability=True,quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
diff --git a/script/scenarios/demos/talos_navBauzil_hard_path.py b/script/scenarios/demos/talos_navBauzil_hard_path.py
index c0a95fa3..b03476a9 100644
--- a/script/scenarios/demos/talos_navBauzil_hard_path.py
+++ b/script/scenarios/demos/talos_navBauzil_hard_path.py
@@ -4,7 +4,7 @@ from hpp.corbaserver import Client
from hpp.corbaserver import ProblemSolver
import time
-
+Robot.urdfName += "_large"
vMax = 0.3 # linear velocity bound for the root
aMax = 0.1 # linear acceleration bound for the root
extraDof = 6
@@ -61,6 +61,7 @@ v.addLandmark(v.sceneName,1)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
+q_init[8] = 0.006761 # torso 2 position in reference config
q_init [0:3] = [-0.7,2,0.98]
v (q_init)
ps.setInitialConfig (q_init)
diff --git a/script/scenarios/demos/talos_navBauzil_path.py b/script/scenarios/demos/talos_navBauzil_path.py
index 80a19f99..7d80c17b 100644
--- a/script/scenarios/demos/talos_navBauzil_path.py
+++ b/script/scenarios/demos/talos_navBauzil_path.py
@@ -61,6 +61,7 @@ v.addLandmark(v.sceneName,1)
# Setting initial configuration
q_init = rbprmBuilder.getCurrentConfig ();
+q_init[8] = 0.006761 # torso 2 position in reference config
q_init [0:3] = [-0.9,1.5,0.98]
q_init[-6:-3] = [0.07,0,0]
v (q_init)
--
GitLab