fullBody.setStaticStability(True)
fullBody.setCurrentConfig(q_init)
v(q_init)
fullBody.setCurrentConfig(q_goal)
v(q_goal)
v.addLandmark('anymal/base', 0.3)
v(q_init)
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init, fullBody.limbs_names,
[[0., 0., 1.] for _ in range(4)])
fullBody.setEndState(q_goal, fullBody.limbs_names,
[[0., 0., 1.] for _ in range(4)])
print("Generate contact plan ...")
tStart = time.time()
configs = fullBody.interpolate(0.002,
pathId=pId,
robustnessTreshold=robTreshold,
filterStates=True,
testReachability=True,
quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print("Done. ")
print("Contact sequence computed in " + str(tInterpolateConfigs) + " s.")
print("number of configs :", len(configs))
fullBody.resetJointsBounds()
normals)
else:
fullBody.setStartState(
q_init,
[fullBody.lArmId, fullBody.lLegId, fullBody.rArmId, fullBody.rLegId],
normals)
fullBody.setEndState(
q_goal,
[fullBody.lArmId, fullBody.lLegId, fullBody.rArmId, fullBody.rLegId],
normals)
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.002,
pathId=pId,
robustnessTreshold=1,
filterStates=True,
quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : " + str(tInterpolateConfigs) + " s"
print "number of configs :", len(configs)
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
if len(configs) < 2:
cg_success = False
print "Error during contact generation."
else:
cg_success = True
print "Contact generation Done."
