Commit 97d59ab5 authored by Pierre Fernbach's avatar Pierre Fernbach
Browse files

updates scripts of IROS18

parent b3129bc4
......@@ -50,7 +50,7 @@ rLegLimbOffset=[0,0,-0.035]#0.035
rLegNormal = [0,0,1]
rLegx = 0.09; rLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_rleg_db.db",rLegId,"forward")
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset)
fullBody.addLimb(rLegId,rLeg,'',rLegOffset,rLegNormal, rLegx, rLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=rLegLimbOffset,kinematicConstraintsMin=0.3)
fullBody.runLimbSampleAnalysis(rLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(rLegId, "./db/hrp2_rleg_db.db")
......@@ -60,7 +60,7 @@ lLegLimbOffset=[0,0,0.035]
lLegNormal = [0,0,1]
lLegx = 0.09; lLegy = 0.05
#fullBody.addLimbDatabase("./db/hrp2_lleg_db.db",lLegId,"forward")
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset)
fullBody.addLimb(lLegId,lLeg,'',lLegOffset,rLegNormal, lLegx, lLegy, 100000, "fixedStep1", 0.01,"_6_DOF",limbOffset=lLegLimbOffset,kinematicConstraintsMin=0.3)
fullBody.runLimbSampleAnalysis(lLegId, "ReferenceConfiguration", True)
#fullBody.saveLimbDatabase(lLegId, "./db/hrp2_lleg_db.db")
fullBody.setReferenceConfig (q_ref)
......@@ -240,3 +240,85 @@ displayOneStepConstraints(r)
#player.interpolate(2,len(configs)-1)
"""
""" # tests front line
s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
s02,success = StateHelper.addNewContact(s0,rLegId,[0.2,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s02.sId)
s04,success = StateHelper.addNewContact(s0,rLegId,[0.4,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s04.sId)
s06,success = StateHelper.addNewContact(s0,rLegId,[0.6,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s06.sId)
s07,success = StateHelper.addNewContact(s0,rLegId,[0.7,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s07.sId)
s075,success = StateHelper.addNewContact(s0,rLegId,[0.75,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s07.sId)
s08,success = StateHelper.addNewContact(s0,rLegId,[0.8,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s08.sId)
s085,success = StateHelper.addNewContact(s0,rLegId,[0.85,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s09.sId)
"""
"""#test backline
s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
s02,success = StateHelper.addNewContact(s0,rLegId,[-0.2,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s02.sId)
s04,success = StateHelper.addNewContact(s0,rLegId,[-0.4,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s04.sId)
s06,success = StateHelper.addNewContact(s0,rLegId,[-0.6,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s06.sId)
s07,success = StateHelper.addNewContact(s0,rLegId,[-0.7,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s07.sId)
s07i,success = StateHelper.removeContact(s0,rLegId)
fullBody.isReachableFromState(s0.sId,s07i.sId)
fullBody.isReachableFromState(s07i.sId,s07.sId)
s075,success = StateHelper.addNewContact(s0,rLegId,[-0.75,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s075.sId)
s075i,success = StateHelper.removeContact(s0,rLegId)
fullBody.isReachableFromState(s0.sId,s075i.sId)
fullBody.isReachableFromState(s075i.sId,s075.sId)
s08,success = StateHelper.addNewContact(s0,rLegId,[-0.8,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s08.sId)
s085,success = StateHelper.addNewContact(s0,rLegId,[-0.85,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s09.sId)
"""
# test right / diag
s0 = State(fullBody,q=q_init,limbsIncontact = [rLegId,lLegId])
s02,success = StateHelper.addNewContact(s0,rLegId,[0.2,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s02.sId)
s04,success = StateHelper.addNewContact(s0,rLegId,[0.4,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s04.sId)
s06,success = StateHelper.addNewContact(s0,rLegId,[0.6,0-0.1,0.01],[0,0,1])
assert(success)
fullBody.isReachableFromState(s0.sId,s06.sId)
s07,success = StateHelper.addNewContact(s0,rLegId,[0.7,0-0.1,0.01],[0,0,1])
assert(success)
......@@ -167,8 +167,12 @@ r(smid.q())
sf2 = State(fullBody,q=q_goal,limbsIncontact=[lLegId,rLegId])
"""
fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,True)
fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,timings=[0.4,0.2,0.4])
fullBody.isDynamicallyReachableFromState(si.sId,smid.sId,timings=[0.8,0.6,0.8])
fullBody.isDynamicallyReachableFromState(smid2.sId,sf.sId,timings=[0.8,0.6,0.8])
pid = fullBody.isDynamicallyReachableFromState(smid.sId,smid2.sId,True)
import disp_bezier
pp.dt = 0.00001
disp_bezier.showPath(r,pp,pid)
......@@ -185,6 +189,8 @@ for i in range(1,4):
r.client.gui.writeNodeFile('s',path+'_S.stl')
"""
"""
com = fullBody.getCenterOfMass()
com[1] = 0
......
......@@ -128,6 +128,30 @@ configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 1, filterStat
r(configs[-1])
tInterpolateConfigs = time.time() - tStart
pid = fullBody.isDynamicallyReachableFromState(17,18,True)
import disp_bezier
pp.dt = 0.0001
disp_bezier.showPath(r,pp,pid)
x = [ 2.47985, -0.25492, 0.962874]
createSphere('s',r)
moveSphere('s',r,x)
displayBezierConstraints(r)
path = "/local/dev_hpp/screenBlender/iros2018/polytopes/hyq/path"
for i in range(1,4):
r.client.gui.writeNodeFile('path_'+str(int(pid[i]))+'_root',path+str(i-1)+'.obj')
r.client.gui.writeNodeFile('s',path+'_S.stl')
noCOQP = 0
for i in range(len(configs)-2):
......@@ -137,7 +161,6 @@ for i in range(len(configs)-2):
f = open("/local/fernbac/bench_iros18/success/log_successSideWall.log","a")
f.write("num states : "+str(len(configs))+"\n")
if noCOQP>0:
......
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