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]