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)
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"
print("Write contact sequence binary file : ", filename)
cs.saveAsBinary(filename)
def build_cs_from_sl1m_l1(fb, q_ref, root_end, pb, RF, allfeetpos, use_orientation, use_interpolated_orientation,
q_init = None, first_phase = None):
"""
Build a multicontact_api.ContactSequence from the SL1M outputs, when not using the MIP formulation
:param fb: an instance of rbprm.Fullbody
:param q_ref: the reference wholebody configuration of the robot
:param root_end: the final base position
:param pb: the SL1M problem dictionary, containing all the contact surfaces and data
:param RF: the Id of the right feet in the SL1M formulation
:param allfeetpos: the list of all foot position for each phase, computed by SL1M
:param use_orientation: if True, change the contact yaw rotation to match the orientation of the base in the guide
:param use_interpolated_orientation: if True, the feet yaw orientation will 'anticipate' the base orientation
of the next phase
:param q_init: the initial wholebody configuration (either this or first_phase should be provided)
:param first_phase: the first multicontact_api.ContactPhase object (either this or q_init should be provided)
:return: the multicontact_api.ContactSequence, with all ContactPhase created at the correct placement
"""
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequence(0)
if q_init:
addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId])
elif first_phase:
cs.append(first_phase)
else:
raise ValueError("build_cs_from_sl1m should have either q_init or first_phase argument defined")
# loop over all phases of pb and add them to the cs :
for pId in range(2, len(pb["phaseData"])): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contactPhases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += EPS_Z # FIXME it shouldn't be required !!
# compute desired foot rotation :
if use_orientation:
rot = compute_orientation_for_feet_placement(fb, pb, pId, moving, RF, prev_contactPhase, use_interpolated_orientation)
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.array(pos).T
placement.rotation = rot.matrix()
cs.moveEffectorToPlacement(movingID, placement)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = q_ref.tolist() + [0] * 6
#p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
#for i in range(3):
# q_end[i] += p_end[i]
q_end[0:7] = root_end
feet_height_end = allfeetpos[-1][2]
logger.info("feet height final = %s", feet_height_end)
q_end[2] = feet_height_end + q_ref[2]
q_end[2] += EPS_Z
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
return cs
def generateContactSequence():
#RF,root_init,pb, coms, footpos, allfeetpos, res = runLPScript()
RF, root_init, root_end, pb, coms, footpos, allfeetpos, res = runLPFromGuideScript(
)
multicontact_api.switchToNumpyArray()
# load scene and robot
fb, v = initScene(cfg.Robot, cfg.ENV_NAME, True)
q_init = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
q_init[0:7] = root_init
feet_height_init = allfeetpos[0][2]
print("feet height initial = ", feet_height_init)
q_init[2] = feet_height_init + cfg.IK_REFERENCE_CONFIG[2]
q_init[2] += EPS_Z
#q_init[2] = fb.referenceConfig[2] # 0.98 is in the _path script
if v:
v(q_init)
# init contact sequence with first phase : q_ref move at the right root pose and with both feet in contact
# FIXME : allow to customize that first phase
cs = ContactSequence(0)
addPhaseFromConfig(fb, cs, q_init, [fb.rLegId, fb.lLegId])
# loop over all phases of pb and add them to the cs :
for pId in range(
2, len(pb["phaseData"])
): # start at 2 because the first two ones are already done in the q_init
prev_contactPhase = cs.contactPhases[-1]
#n = normal(pb["phaseData"][pId])
phase = pb["phaseData"][pId]
moving = phase["moving"]
movingID = fb.lfoot
if moving == RF:
movingID = fb.rfoot
pos = allfeetpos[pId] # array, desired position for the feet movingID
pos[2] += EPS_Z # FIXME it shouldn't be required !!
# compute desired foot rotation :
if cfg.SL1M_USE_ORIENTATION:
quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if pId < len(pb["phaseData"]) - 1:
quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"])
else:
quat1 = Quaternion(pb["phaseData"][pId]["rootOrientation"])
if cfg.SL1M_USE_INTERPOLATED_ORIENTATION:
rot = quat0.slerp(0.5, quat1)
# check if feets do not cross :
if moving == RF:
qr = rot
ql = Quaternion(
prev_contactPhase.contactPatch(
fb.lfoot).placement.rotation)
else:
ql = rot
qr = Quaternion(
prev_contactPhase.contactPatch(
fb.rfoot).placement.rotation)
rpy = matrixToRpy((qr * (ql.inverse())).matrix(
)) # rotation from the left foot pose to the right one
if rpy[2, 0] > 0: # yaw positive, feet are crossing
rot = quat0 # rotation of the root, from the guide
else:
rot = quat0 # rotation of the root, from the guide
else:
rot = Quaternion.Identity()
placement = SE3()
placement.translation = np.array(pos).T
placement.rotation = rot.matrix()
cs.moveEffectorToPlacement(movingID, placement)
# final phase :
# fixme : assume root is in the middle of the last 2 feet pos ...
q_end = cfg.IK_REFERENCE_CONFIG.tolist() + [0] * 6
#p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2.
#for i in range(3):
# q_end[i] += p_end[i]
q_end[0:7] = root_end
feet_height_end = allfeetpos[-1][2]
print("feet height final = ", feet_height_end)
q_end[2] = feet_height_end + cfg.IK_REFERENCE_CONFIG[2]
q_end[2] += EPS_Z
fb.setCurrentConfig(q_end)
com = fb.getCenterOfMass()
setFinalState(cs, com, q=q_end)
if cfg.DISPLAY_CS_STONES:
displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
return cs, fb, v