def create_stairs_cs():
ENV_NAME = "multicontact/bauzil_stairs"
fb, v = display_tools.initScene(Robot, ENV_NAME, False)
cs = ContactSequence(0)
# Create an initial contact phase :
q_ref = fb.referenceConfig[::] + [0] * 6
q_ref[0:2] = [0.07, 1.2]
addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId])
step_height = 0.1
step_width = 0.3
displacement = SE3.Identity()
displacement.translation = array([step_width, 0, step_height])
cs.moveEffectorOf(fb.rfoot, displacement)
cs.moveEffectorOf(fb.lfoot, displacement)
q_end = q_ref[::]
q_end[0] += step_width
q_end[2] += step_height
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, array(com), q=q_end)
return cs
v.addLandmark(sceneName, 1)
#initialize an empty contact sequence
cs = ContactSequence(0)
# start from the reference configuration of the robot :
q_ref = fb.referenceConfig[::] + [0] * 6
#q_ref[0:2] = [ , ] # change the x,y position of the base as needed
# q_ref[2] += # the z position of the base already correspond to a floor at z=0, change it accordingly to the environment
# q_ref[3:7] = # quaternion (x,y,z) of the base
# display the configuration
v(q_ref)
com = array(fb.getCenterOfMass())
# create a first contact phase corresponding to this configuration
limbsInContact = [fb.rLegId, fb.lLegId
] # define the limbs in contact for this first phase
addPhaseFromConfig(fb, cs, q_ref, limbsInContact)
transform = SE3.Identity()
cs.moveEffectorOf(fb.rfoot, transform)
cs.moveEffectorOf(fb.lfoot, transform)
cs.moveEffectorOf(fb.rfoot, transform)
cs.moveEffectorOf(fb.lfoot, transform)
setFinalState(cs, com, q_ref)
assert cs.haveConsistentContacts()
cs.saveAsBinary("talos_flatGround.cs")
cs = ContactSequence(0)
#Create an initial contact phase :
q_ref = fb.referenceConfig[::] + [0] * 6
q_ref[0:2] = [0.07, 1.2]
addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId])
num_steps = 6
step_height = 0.1
step_width = 0.3
displacement = SE3.Identity()
displacement.translation = array([step_width, 0, step_height])
for i in range(num_steps):
cs.moveEffectorOf(fb.rfoot, displacement)
cs.moveEffectorOf(fb.lfoot, displacement)
q_end = q_ref[::]
q_end[0] += step_width * num_steps
q_end[2] += step_height * num_steps
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, array(com), q=q_end)
display_tools.displaySteppingStones(cs, gui, sceneName, fb)
DEMO_NAME = "talos_stairs10"
filename = DEMO_NAME + ".cs"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
fb.setPostureWeights(fb.postureWeights_straff[::] + [0] * 6)
v(q_ref)
addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId, fb.rArmId, fb.lArmId])
num_steps = 1
step_height = 0.22
step_width = 0.207
displacement = SE3.Identity()
displacement.translation = array([step_width, 0, step_height])
gait = [fb.rhand, fb.lhand, fb.rfoot, fb.lfoot]
for i in range(num_steps):
for eeName in gait:
cs.moveEffectorOf(eeName, displacement)
cs.breakContact(fb.rhand)
cs.breakContact(fb.lhand)
q_end = q_ref[::]
q_end[0] += 0.15 + num_steps * step_width
q_end[2] += num_steps * step_height
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
display_tools.displaySteppingStones(cs, gui, sceneName, fb)
DEMO_NAME = "talos_stairsAirbus"
filename = DEMO_NAME + ".cs"
