updates scripts of IROS18

script/dynamic/flatGround_hrp2_interpSTATIC_testTransition.py

@@ -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)
+
+
+

script/dynamic/plateform_hrp2_interp_testTransition.py

@@ -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

script/dynamic/sideWall_hyq_interpKino_intersection.py

@@ -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: