diff --git a/script/scenarios/demos/talos_flatGround.py b/script/scenarios/demos/talos_flatGround.py index fe79107845370378e0bf6e2bc18526c76d293fe0..687c4726652bff64c2aec9365f6e42bd1f69704c 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 9323f46cecfd7bb58e82dc246e9bd496d4e847c1..92e79f319013bc56a846184f0b2d6e6c83dc2b13 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 51058738129f43b9e670cdaeeff6c148b92b053b..ebdef89613756e60420537b4cf9d7bdfafeda5db 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 c0a95fa3123d5621879c2c8758ed161780a3957b..b03476a986f114f8d5a543bd73ae219e6340d4c4 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 80a19f99d793d10857bc2f35289bce6b69978c5a..7d80c17bd2e33a47428ddd12a588af0a5ca2fe14 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)