q_goal[2] = q_ref[2]
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()
v.addLandmark('anymal/base_0', 0.3)
v(q_init)
#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)
# specify the full body configurations as start and goal state of the problem
normals = [[0., 0., 1.] for _ in range(4)]
if q_goal[
1] < 0: # goal on the right side of the circle, start motion with right leg first
fullBody.setStartState(
q_init,
[fullBody.rArmId, fullBody.rLegId, fullBody.lArmId, fullBody.lLegId],
normals)
fullBody.setEndState(
q_goal,
[fullBody.rArmId, fullBody.rLegId, fullBody.lArmId, fullBody.lLegId],
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,
robTreshold = 5
# copy extraconfig for start and init configurations
q_init[configSize:configSize + 3] = vel_init[::]
q_init[configSize + 3:configSize + 6] = acc_init[::]
q_goal[configSize:configSize + 3] = vel_goal[::]
q_goal[configSize + 3:configSize + 6] = [0, 0, 0]
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
normals = [[0., 0., 1.] for _ in range(4)]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig(q_init)
v(q_init)
fullBody.setStartState(q_init, Robot.limbs_names, normals)
fullBody.setEndState(q_goal, Robot.limbs_names, normals)
fullBody.setCurrentConfig(q_goal)
v(q_goal)
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.001,
pathId=pId,
robustnessTreshold=robTreshold,
filterStates=True,
testReachability=True,
quasiStatic=True)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "Contact plan generated in : " + str(tInterpolateConfigs) + " s"
q_init[2] = q_ref[2]
q_goal[2] = q_ref[2]
fullBody.setStaticStability(True)
fullBody.setCurrentConfig(q_init)
v(q_init)
fullBody.setCurrentConfig(q_goal)
v(q_goal)
v.addLandmark('anymal_reachability/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)
fullBody.setEndState(q_goal, fullBody.limbs_names)
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))
#raw_input("Press Enter to display the contact sequence ...")
#displayContactSequence(v,configs)
v.addLandmark('anymal/base_0', 0.3)
v(q_init)
#fullBody.setReferenceConfig(fullBody.referenceConfig_legsApart[::]+[0]*6)
# specify the full body configurations as start and goal state of the problem
normals = [[0., 0., 1.] for _ in range(4)]
if q_goal[
1] < 0: # goal on the right side of the circle, start motion with right leg first
fullBody.setStartState(
q_init,
[fullBody.rArmId, fullBody.rLegId, fullBody.lArmId, fullBody.lLegId],
normals)
fullBody.setEndState(
q_goal,
[fullBody.rArmId, fullBody.rLegId, fullBody.lArmId, fullBody.lLegId],
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,