Commit 0313f992 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

[script] update talos_flatGround

parent 62ad7edf
...@@ -25,16 +25,17 @@ v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True) ...@@ -25,16 +25,17 @@ v = tp.Viewer (ps,viewerClient=tp.v.client, displayCoM = True)
q_ref = fullBody.referenceConfig[::]+[0]*6 q_ref = fullBody.referenceConfig[::]+[0]*6
q_init = q_ref[::] q_init = q_ref[::]
fullBody.setReferenceConfig(q_ref) fullBody.setReferenceConfig(q_ref)
fullBody.setPostureWeights(fullBody.postureWeights[::]+[0]*6)
fullBody.setCurrentConfig (q_init) fullBody.setCurrentConfig (q_init)
print "Generate limb DB ..." print "Generate limb DB ..."
tStart = time.time() tStart = time.time()
# generate databases : # generate databases :
nbSamples = 50000 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) 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.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) fullBody.runLimbSampleAnalysis(fullBody.lLegId, "ReferenceConfiguration", True)
...@@ -45,18 +46,18 @@ print "Databases generated in : "+str(tGenerate)+" s" ...@@ -45,18 +46,18 @@ print "Databases generated in : "+str(tGenerate)+" s"
#define initial and final configurations : #define initial and final configurations :
configSize = fullBody.getConfigSize() -fullBody.client.robot.getDimensionExtraConfigSpace() 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] 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] vel_init = tp.ps.configAtParam(pId,0)[tp.indexECS:tp.indexECS+3]
acc_init = tp.ps.configAtParam(pId,0.01)[tp.indexECS+3:tp.indexECS+6] acc_init = tp.ps.configAtParam(pId,0)[tp.indexECS+3:tp.indexECS+6]
dir_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId)-0.01)[tp.indexECS:tp.indexECS+3] vel_goal = tp.ps.configAtParam(pId,tp.ps.pathLength(pId))[tp.indexECS:tp.indexECS+3]
acc_goal = [0,0,0] acc_goal = [0,0,0]
robTreshold = 3 robTreshold = 3
# copy extraconfig for start and init configurations # 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_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] q_goal[configSize+3:configSize+6] = [0,0,0]
...@@ -81,7 +82,7 @@ fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId]) ...@@ -81,7 +82,7 @@ fullBody.setEndState(q_goal,[fullBody.rLegId,fullBody.lLegId])
print "Generate contact plan ..." print "Generate contact plan ..."
tStart = time.time() 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 tInterpolateConfigs = time.time() - tStart
print "Done." print "Done."
print "Contact plan generated in : "+str(tInterpolateConfigs)+" s" print "Contact plan generated in : "+str(tInterpolateConfigs)+" s"
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment