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 ")