def tryCreateContactAround(s, eff_id, pos, normal, num_max_sample= 0,rotation = Quaternion.Identity(),num_try = 1000): sres, succ = StateHelper.addNewContact(s, eff_id, pos, normal, num_max_sample= num_max_sample,rotation = quatToConfig(rotation)) i_try = 0 x_bounds = [-0.1,0.1] y_bounds = [-0.05,0.05] print("try create contact around, first try success : ",succ) print("result = ",sres.q()) while not succ and i_try < num_try: print("try create contact around, try : ",i_try) x = random.uniform(x_bounds[0],x_bounds[1]) y = random.uniform(y_bounds[0],y_bounds[1]) offset = np.matrix([x,y,0]).T offset = rotation.matrix() * offset new_pos = pos[::] for i in range(3): new_pos[i] += offset[i,0] sres, succ = StateHelper.addNewContact(s, eff_id, new_pos, normal, num_max_sample= num_max_sample,rotation = quatToConfig(rotation)) i_try += 1 return sres,succ
def generateContactSequence(): RF, root_init, pb, coms, footpos, allfeetpos, res = runLPScript() # load scene and robot fb, v = initScene(cfg.Robot, cfg.ENV_NAME, False) q_init = fb.referenceConfig[::] + [0] * 6 q_init[0:7] = root_init #q_init[2] += fb.referenceConfig[2] - 0.98 # 0.98 is in the _path script 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 = ContactSequenceHumanoid(0) addPhaseFromConfig(fb, v, 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.contact_phases[-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] += 0.005 # FIXME it shouldn't be required !! # compute desired foot rotation : if USE_ORIENTATION: if pId < len(pb["phaseData"]) - 1: quat0 = Quaternion(pb["phaseData"][pId]["rootOrientation"]) quat1 = Quaternion(pb["phaseData"][pId + 1]["rootOrientation"]) rot = quat0.slerp(0.5, quat1) # check if feets do not cross : if moving == RF: qr = rot ql = Quaternion( prev_contactPhase.LF_patch.placement.rotation) else: ql = rot qr = Quaternion( prev_contactPhase.RF_patch.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 = Quaternion(phase["rootOrientation"] ) # rotation of the root, from the guide else: rot = Quaternion(phase["rootOrientation"] ) # rotation of the root, from the guide else: rot = Quaternion.Identity() placement = SE3() placement.translation = np.matrix(pos).T placement.rotation = rot.matrix() moveEffectorToPlacement(fb, v, cs, movingID, placement, initStateCenterSupportPolygon=True) # final phase : # fixme : assume root is in the middle of the last 2 feet pos ... q_end = fb.referenceConfig[::] + [0] * 6 p_end = (allfeetpos[-1] + allfeetpos[-2]) / 2. for i in range(3): q_end[i] += p_end[i] setFinalState(cs, q=q_end) displaySteppingStones(cs, v.client.gui, v.sceneName, fb) return cs, fb, v
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