def contactSequenceFromRBPRMConfigs(fb, beginId, endId):
"""
Build a multicontact_api ContactSequence from rbprm states list
:param fb: an instance of rbprm.FullBody
:param beginId: the first state Id
:param endId: the last state Id
:return: a multicontact_api ContactSequence
"""
logger.warning("generate contact sequence from planning : ")
n_states = endId - beginId + 1
# There could be either contact break, creation or repositionning between each adjacent states.
# But there should be only contacts break or creation between each adjacent contactPhases
cs = ContactSequence(0)
prev_phase = None
phaseId = 0
# create initial ContactPhase
cs.append(createPhaseFromRBPRMState(fb, beginId))
for stateId in range(beginId + 1,
endId + 1): # from the second state to the last one
logger.info("current state id = %d", stateId)
previous_phase = cs.contactPhases[-1]
eeName, variationType = getContactVariationBetweenStates(
fb, stateId - 1, stateId)
if eeName is not None:
if variationType == VariationType.REPOSITIONNED:
# in case of repositionning, the centroidal motion will happend in the next intermediate phase,
# and thus the previous_phase will not move :
copyPhaseInitToFinal(previous_phase)
else:
# set the final values of the previous phase to be the current one :
setPhaseFinalValues(fb, stateId, previous_phase)
if variationType == VariationType.BROKEN:
# remove the given contact :
cs.breakContact(eeName)
else:
# get placement of the new contact from the planning :
contact_placement = contactPlacementFromRBPRMState(
fb, stateId, eeName)
if variationType == VariationType.CREATED:
# create a new contact :
cs.createContact(eeName, ContactPatch(contact_placement))
elif variationType == VariationType.REPOSITIONNED:
# move existing contact to new placement :
cs.moveEffectorToPlacement(eeName, contact_placement)
# set the initial values for the current phase from planning :
setPhaseInitialValues(fb, stateId, cs.contactPhases[-1])
# set the same values as the final ones for the intermediate state created :
setPhaseFinalValues(fb, stateId, cs.contactPhases[-2])
# else : no contact changes, ignore this state
setPhaseFinalValues(fb, endId, cs.contactPhases[-1])
return cs
def build_cs_from_sl1m_mip(fb, q_ref, root_end, pb, allfeetpos, use_orientation, use_interpolated_orientation,
q_init = None, first_phase = None):
"""
Build a multicontact_api.ContactSequence from the SL1M outputs when 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 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
num_steps = len(pb["phaseData"]) - 1 # number of contact repositionning
limbs_names = fb.limbs_names
num_effectors = len(limbs_names)
logger.info(" limbs names : %s", limbs_names)
cs = ContactSequence(0)
if first_phase:
cs.append(first_phase)
elif q_init:
# if only the configuration is provided, assume all effectors are in contact
addPhaseFromConfig(fb, cs, q_init, limbs_names)
else:
raise ValueError("build_cs_from_sl1m should have either q_init or first_phase argument defined")
logger.info("Initial phase added, contacts : %s ", cs.contactPhases[0].effectorsInContact())
# loop for all effector placements, and create the required contact phases
previous_eff_placements = allfeetpos[0]
if len(previous_eff_placements) != num_effectors:
raise NotImplementedError("A phase in the output of SL1M do not have all the effectors in contact.")
for pid, eff_placements in enumerate(allfeetpos[1:]):
logger.info("Loop allfeetpos, id = %d", pid)
if len(eff_placements) != num_effectors:
raise NotImplementedError("A phase in the output of SL1M do not have all the effectors in contact.")
switch = False # True if a repostionning have been detected
for k, pos in enumerate(eff_placements):
if norm(pos - previous_eff_placements[k]) > 1e-3:
if switch:
raise NotImplementedError("Several contact changes between two adjacent phases in SL1M output")
switch = True
ee_name = fb.dict_limb_joint[limbs_names[k]]
#pos[2] += EPS_Z # FIXME: apply epsz along the normal
pos = fb.dict_offset[ee_name].actInv(pos)
logger.info("Move effector %s ", ee_name)
logger.info("To position %s ", pos)
placement = SE3.Identity()
placement.translation = pos
# compute orientation of the contact from the surface normal:
phase_data = pb["phaseData"][pid+1] # +1 because the for loop start at id = 1
n = normal_from_ineq(phase_data["S"][phase_data["id_surface"]])
placement.rotation = rotationFromNormal(n)
logger.debug("new contact placement : %s", placement)
# TODO add yaw rotation from guide here !
cs.moveEffectorToPlacement(ee_name, placement)
#if not switch:
# raise RuntimeError("No contact changes between two adjacent phases in SL1M output")
# assign com position to the last two phases :
# swinging phase, same init and final position
"""
cs.contactPhases[-2].c_init = coms[pid * 2]
cs.contactPhases[-2].c_final = coms[pid * 2 + 1]
# phase with all the contacts:
cs.contactPhases[-1].c_init = coms[pid * 2 + 1]
if pid * 2 + 2 < len(coms):
cs.contactPhases[-1].c_final = coms[pid * 2 + 2]
else:
cs.contactPhases[-1].c_final = cs.contactPhases[-1].c_init
"""
previous_eff_placements = eff_placements
# 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][0][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)
"""
p_final = cs.contactPhases[-1]
p_final.c_final = computeCenterOfSupportPolygonFromPhase(p_final, fb.DEFAULT_COM_HEIGHT)
p_final.c_init = p_final.c_final
return cs
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
-1.45,
0.0,
0.0,
0.,
-0.005, # Right Arm
0.,
0.
] + [0]*6
q[:2] = [-0.15, 0.25]
addPhaseFromConfig(fb, cs, q, [fb.rLegId, fb.lLegId])
q[:3] = [0.11, 0.25, 1.18127]
p_up = createPhaseFromConfig(fb, q, [fb.rLegId, fb.lLegId])
#make the first step to the platform
cs.moveEffectorToPlacement(fb.rfoot,p_up.contactPatch(fb.rfoot).placement)
cs.moveEffectorToPlacement(fb.lfoot,p_up.contactPatch(fb.lfoot).placement)
# Make the second step to connect the cs_plateform
cs.moveEffectorToPlacement(fb.rfoot,p0_platform.contactPatch(fb.rfoot).placement)
cs.moveEffectorToPlacement(fb.lfoot,p0_platform.contactPatch(fb.lfoot).placement)
# copy the cs_plateform
for phase in cs_platforms.contactPhases[1:]:
cs.append(phase)
## Add final step :
q[0] = 1.14
p_end = createPhaseFromConfig(fb, q, [fb.rLegId, fb.lLegId])
q[:3] = [1.37, 0.25, 1.02127]