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
q_ref[24] -= 1.3 # lift up arms q_ref[32] -= 1.3 v(q_ref) addPhaseFromConfig(fb, cs, q_ref, [fb.rLegId, fb.lLegId]) pRH = SE3.Identity() pRH = rotatePlacement(pRH, 'y', -0.8115781021773631) pLH = pRH.copy() #pLH.translation = np.matrix([-2.635,-1.235,1.13]).T #pRH.translation = np.matrix([-2.635,-2.28,1.13]).T pLH.translation = array([-2.842, -1.235, 0.91]) pRH.translation = array([-2.842, -2.28, 0.91]) cs.createContact(fb.rhand, ContactPatch(pRH)) cs.createContact(fb.lhand, ContactPatch(pLH)) displacement = SE3.Identity() displacement.translation = array([0.15, 0, 0]) cs.moveEffectorOf(fb.rfoot, displacement) cs.moveEffectorOf(fb.lfoot, displacement) num_steps = 1 step_height = 0.22 step_width = 0.207 gait = [fb.rhand, fb.lhand, fb.rfoot, fb.lfoot] displacement.translation = array([step_width, 0, step_height]) for i in range(num_steps):
print("number of contact phases in the contact sequence : ", cs.size())
# there is now one phase in our 'cs' object
# we can now add new contact phases by changing the contacts of the last phase of the sequence:
cs.breakContact(
fb.rfoot
) # add a new contact phase to the sequence: break the right feet contact
# create a new contact for the right feet at a different position :
placement = cs.contactPhases[0].contactPatch(
fb.rfoot).placement.copy() # take the previous position of the right feet
pos = placement.translation
pos[0] += 0.15 # move of 15cm in the x direction
placement.translation = pos
# add a new contact phase, with a contact created for the right feet at the given placement :
cs.createContact(fb.rfoot, ContactPatch(placement))
#display the contacts in gepetto viewer :
#display_tools.displaySteppingStones(cs, gui, v.sceneName, fb)
print("number of contact phases in the contact sequence : ", cs.size())
# There is now 3 contact phases (corresponding to one step)
#first phase:
print("Right feet contact of phase 0 ")
print(cs.contactPhases[0].isEffectorInContact(fb.rfoot))
print(cs.contactPhases[0].contactPatch(fb.rfoot).placement)
print("Left feet contact of phase 0 ")
print(cs.contactPhases[0].isEffectorInContact(fb.lfoot))
print(cs.contactPhases[0].contactPatch(fb.lfoot).placement)
#second phase:
print("Right feet contact of phase 1 ")