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('talos/base_link', 0.3)
v(q_init)
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init, [fullBody.lLegId, fullBody.rLegId])
fullBody.setEndState(q_goal, [fullBody.lLegId, fullBody.rLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,
pathId=pId,
robustnessTreshold=2,
filterStates=True,
testReachability=False)
tInterpolateConfigs = time.time() - tStart
print "Done."
print "number of configs :", len(configs)
raw_input("Press Enter to display the contact sequence ...")
displayContactSequence(v, configs)
# specify the full body configurations as start and goal state of the problem
if q_goal[
1] < 0: # goal on the right side of the circle, start motion with right leg first
fullBody.setStartState(q_init, [fullBody.rLegId, fullBody.lLegId])
fullBody.setEndState(q_goal, [fullBody.rLegId, fullBody.lLegId])
else:
fullBody.setStartState(q_init, [fullBody.lLegId, fullBody.rLegId])
fullBody.setEndState(q_goal, [fullBody.lLegId, fullBody.rLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.005,
pathId=pId,
robustnessTreshold=3,
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."
if q_goal[1] > q_init[1]: # turn right
gait = [fullBody.lLegId, fullBody.rLegId]
else: # turn left
gait = [fullBody.rLegId, fullBody.lLegId]
v(q_init)
fullBody.setStartState(q_init, gait)
v(q_goal)
fullBody.setEndState(q_goal, gait)
print "Generate contact plan ..."
tStart = time.time()
v(q_init)
configs = fullBody.interpolate(0.005,
pathId=pId,
robustnessTreshold=1,
filterStates=True,
testReachability=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."
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('talos/base_link', 0.3)
v(q_init)
# specify the full body configurations as start and goal state of the problem
fullBody.setStartState(q_init, [fullBody.rLegId, fullBody.lLegId])
fullBody.setEndState(q_goal, [fullBody.rLegId, fullBody.lLegId])
print "Generate contact plan ..."
tStart = time.time()
configs = fullBody.interpolate(0.01,
pathId=pId,
robustnessTreshold=2,
filterStates=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)