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)