si = State(fullBody,q=q_init,limbsIncontact=[lLegId,rLegId])
n = [0.0, -0.42261828000211843, 0.9063077785212101]
p = [0.775, 0.22, -0.019]
moveSphere('s',r,p)
smid,success = StateHelper.addNewContact(si,lLegId,p,n,100)
assert(success)
smid2,success = StateHelper.addNewContact(sf,lLegId,p,n,100)
assert(success)
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])
import disp_bezier
pp.dt = 0.00001
disp_bezier.showPath(r,pp,pid)
x = [0.776624, 0.219798, 0.846351]
moveSphere('s',r,x)
displayBezierConstraints(r)
player.interpolate(0, len(configs) - 1)
#### whole body :
from hpp.corbaserver.rbprm.tools.path_to_trajectory import *
from disp_bezier import *
beginState = 0
endState = len(configs) - 2
time_per_paths = []
total_paths_ids = []
merged_paths_ids = []
for id_state in range(beginState, endState):
pid = fullBody.isDynamicallyReachableFromState(id_state, id_state + 1,
True)
assert (len(pid) > 0
and "isDynamicallyReachable failed for state " + str(id_state))
showPath(r, pp, pid)
pId1 = int(pid[0])
pId2 = int(pid[1])
pId3 = int(pid[2])
paths_ids = fullBody.effectorRRTFromPosBetweenState(
id_state, id_state + 1, pId1, pId2, pId3, 1)
time_per_paths += [ps.pathLength(pId1)]
time_per_paths += [ps.pathLength(pId2)]
time_per_paths += [ps.pathLength(pId3)]
total_paths_ids += paths_ids[:-1]
merged_paths_ids += [paths_ids[-1]]
robTreshold = 3 # copy extraconfig for start and init configurations q_init[configSize:configSize+3] = dir_init[::] q_init[configSize+3:configSize+6] = acc_init[::] q_goal[configSize:configSize+3] = dir_goal[::] q_goal[configSize+3:configSize+6] = [0,0,0] # FIXME : test q_init[2] = q_ref[2]+0.01 q_goal[2] = q_ref[2]+0.01 fullBody.setStaticStability(True) # Randomly generating a contact configuration at q_init fullBody.setCurrentConfig (q_init) r(q_init) #q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold) r(q_init) fullBody.isDynamicallyReachableFromState(s.sId,s2.sId,timings=[0.4,0.6,0.4])
from hpp.gepetto import PathPlayer
pp = PathPlayer(fullBody.client.basic, r)
pp.dt = 0.001
configs = fullBody.interpolate(0.05,
pathId=0,
robustnessTreshold=0,
filterStates=True)
print("number of configs :", len(configs))
r(configs[-1])
noCOQP = 0
for i in range(len(configs) - 1):
pid = fullBody.isDynamicallyReachableFromState(i, i + 1)
if len(pid) == 0:
noCOQP += 1
f = open("/local/fernbac/bench_iros18/success/log_successStairsNoRamp.log",
"a")
if noCOQP > 0:
f.write("fail, with " + str(noCOQP) + " infeasibles transitions\n")
else:
f.write("all transition feasibles\n")
f.close()
"""
from fullBodyPlayerHrp2 import Player
player = Player(fullBody,pp,tp,configs,draw=False,optim_effector=False,use_velocity=False,pathId = 0)
assert (success)
smid2, success = StateHelper.addNewContact(sf, lLegId, p, n)
assert (success)
r(smid.q())
sf2 = State(fullBody, q=q_goal, limbsIncontact=[lLegId, rLegId])
"""
com = fullBody.getCenterOfMass()
com[1] = 0
"""
import disp_bezier
pp.dt = 0.0001
pids = []
curves = []
timings = []
pid = fullBody.isDynamicallyReachableFromState(si.sId, smid.sId, True)
#assert (len(pid)>0)
disp_bezier.showPath(r, pp, pid)
curves += [fullBody.getPathAsBezier(int(pid[0]))]
timings += [[
ps.pathLength(int(pid[1])),
ps.pathLength(int(pid[2])),
ps.pathLength(int(pid[3]))
]]
pids += pid
pid = fullBody.isDynamicallyReachableFromState(smid.sId, smid2.sId, True)
#assert (len(pid)>0)
disp_bezier.showPath(r, pp, pid)
curves += [fullBody.getPathAsBezier(int(pid[0]))]
timings += [[
ps.pathLength(int(pid[1])),
pp = PathPlayer (fullBody.client.basic, r)
pp.dt = 0.001
r(q_init)
# computing the contact sequence
tStart = time.time()
#~ configs = fullBody.interpolate(0.08,pathId=1,robustnessTreshold = 2, filterStates = True)
configs = fullBody.interpolate(0.001,pathId=0,robustnessTreshold = 1, filterStates = True)
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')
