diff --git a/script/scenarios/demos/talos_flatGround.py b/script/scenarios/demos/talos_flatGround.py index 9561a96fefed2333c28d9af802f725f6d2692248..404a8dc2d905bad9223e24723d5ea86214a70b1a 100644 --- a/script/scenarios/demos/talos_flatGround.py +++ b/script/scenarios/demos/talos_flatGround.py @@ -25,16 +25,17 @@ v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True) q_ref = fullBody.referenceConfig[::]+[0]*6 q_init = q_ref[::] fullBody.setReferenceConfig(q_ref) +fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6) fullBody.setCurrentConfig (q_init) print "Generate limb DB ..." tStart = time.time() # generate databases : -nbSamples = 50000 -fullBody.addLimb(fullBody.rLegId,fullBody.rleg,fullBody.rfoot,fullBody.rLegOffset,fullBody.rLegNormal, fullBody.rLegx, fullBody.rLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.rLegKinematicConstraints) +nbSamples = 100000 +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.75) fullBody.runLimbSampleAnalysis(fullBody.rLegId, "ReferenceConfiguration", True) -fullBody.addLimb(fullBody.lLegId,fullBody.lleg,fullBody.lfoot,fullBody.lLegOffset,fullBody.rLegNormal, fullBody.lLegx, fullBody.lLegy, nbSamples, "fixedStep08", 0.01,kinematicConstraintsPath=fullBody.lLegKinematicConstraints) +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.75) fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True) @@ -45,18 +46,18 @@ print "Databases generated in : "+str(tGenerate)+" s" #define initial and final configurations : configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() -q_init[0:7] = tp.ps.configAtParam(pId,0.01)[0:7] # use this to get the correct orientation +q_init[0:7] = tp.ps.configAtParam(pId,0.001)[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] +vel_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3] +acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6] +vel_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[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:configSize+3] = vel_init[::] q_init[configSize+3:configSize+6] = acc_init[::] -q_goal[configSize:configSize+3] = dir_goal[::] +q_goal[configSize:configSize+3] = vel_goal[::] q_goal[configSize+3:configSize+6] = [0,0,0] @@ -81,7 +82,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"