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
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)
# q_ref[3:7] = # quaternion (x,y,z) of the base
# display the configuration
v(q_ref)
# 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)
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)
