filterStates=True)
tInterpolateConfigs = time.time() - tStart
print("number of configs : ", len(configsFull))
print("generated in " + str(tInterpolateConfigs) + " s")
r(configsFull[len(configsFull) - 1])
tPlanning = tp.tPlanning
import parse_bench
parse_bench.parseBenchmark(tp.t)
player = fullBodyPlayerHrp2.Player(fullBody,
pp,
tp,
configsFull,
draw=False,
use_window=1,
optim_effector=True,
use_velocity=False,
pathId=pId)
# remove the last config (= user defined q_goal, not consitent with the previous state)
#r(configs[0])
player.displayContactPlan()
exist = fullBody.isReachableFromState(9, 10)
displayTwoStepConstraints(r, exist)
#player.interpolate(2,len(configs)-1)
0.194963,
-1.44666,
-0.370341,
-0.170618,
1.43348,
0.0299991,
0,
8.5612e-05,
2.99991,
0,
0.0085612,
]
q[-6:] = [0] * 6
s0 = State(fullBody, q=q, limbsIncontact=[rLegId, lLegId, rarmId, larmId])
s1 = fullBody.createState(q, [rLegId, lLegId, larmId])
pid = fullBody.isReachableFromState(s0, s1)
pid
r(q)
displayOneStepConstraints(r, False)
#~ r(configs[15])
#~ player.interpolate(10,100)
#player.play()
"""
camera = [0.5681925415992737,
-6.707448482513428,
2.5206544399261475,
0.8217507600784302,
r(q_init)
#q_init = fullBody.generateContacts(q_init,dir_init,acc_init,robTreshold)
r(q_init)
# Randomly generating a contact configuration at q_end
fullBody.setCurrentConfig(q_goal)
#q_goal = fullBody.generateContacts(q_goal, dir_goal,acc_goal,robTreshold)
r(q_goal)
# specifying the full body configurations as start and goal state of the problem
r.addLandmark('hrp2_14/BODY', 0.3)
r(q_init)
fullBody.setStartState(q_init, [rLegId, lLegId])
fullBody.setEndState(q_goal, [rLegId, lLegId])
"""
init_sid = fullBody.addState(q_init,[rLegId,lLegId])
int_sid = fullBody.addState(q_init,[rLegId])
fullBody.isReachableFromState(init_sid,int_sid)
displayOneStepConstraints(r)
"""
#p = fullBody.computeContactPointsAtState(init_sid)
#p = fullBody.computeContactPointsAtState(int_sid)
"""
q = q_init[::]
q[0] += 0.3
