def generate_contact_sequence_sl1m(cfg): #RF,root_init,pb, coms, footpos, allfeetpos, res = runLPScript(cfg) RF, root_init, root_end, pb, coms, footpos, allfeetpos, res = runLPFromGuideScript(cfg) 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 if cfg.SL1M_USE_MIP: # if MIP is used, allfeetpos contains a list of all position and not just the new position feet_height_init = allfeetpos[0][0][2] else: feet_height_init = allfeetpos[0][2] logger.info("feet height initial = %s", 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) cs = build_cs_from_sl1m(cfg.SL1M_USE_MIP, fb, cfg.IK_REFERENCE_CONFIG, root_end, pb, RF, allfeetpos, cfg.SL1M_USE_ORIENTATION, cfg.SL1M_USE_INTERPOLATED_ORIENTATION, q_init=q_init) if cfg.DISPLAY_CS_STONES: displaySteppingStones(cs, v.client.gui, v.sceneName, fb) return cs, fb, v
def addPhaseFromConfig(fb, v, cs, q, limbsInContact): phase = ContactPhaseHumanoid() phase.reference_configurations.append(np.matrix((q)).T) if v: v(q) fb.setCurrentConfig(q) state = np.matrix(np.zeros(9)).T state[0:3] = np.matrix(fb.getCenterOfMass()).T phase.init_state = state.copy() phase.final_state = state.copy() # set Identity to each contact placement (otherwise it's uninitialized) phase.RF_patch.placement = SE3.Identity() phase.LF_patch.placement = SE3.Identity() phase.RH_patch.placement = SE3.Identity() phase.LH_patch.placement = SE3.Identity() for limb in limbsInContact: eeName = fb.dict_limb_joint[limb] q_j = fb.getJointPosition(eeName) patch = contactPatchForEffector(phase, eeName, fb) placement = SE3FromConfig(q_j) patch.placement = placement.act(fb.dict_offset[eeName]) patch.active = True cs.contact_phases.append(phase) if v: display_tools.displaySteppingStones(cs, v.client.gui, v.sceneName, fb)
def display_stones_lock(self): """ Wait for the lock to access to gepetto-gui and then display the stepping stones for the current contact_sequence """ logger.info("Waiting lock to display stepping stones ...") self.viewer_lock.acquire() logger.info("Display stepping stones ...") displaySteppingStones(self.cs, self.gui, "world", self.cfg.Robot) # FIXME: change world if changed in pinocchio ... logger.info("Display done.") self.viewer_lock.release()
def loadMotionFromFiles(gui, path, npzFilename, csFilename): # load cs from file : cs = ContactSequenceHumanoid(0) cs.loadFromBinary(path + csFilename) displaySteppingStones(cs, gui, sceneName, Robot) colors = [[0., 0., 1., 1.], [0., 1., 0., 1.]] displayCOMTrajectory(cs, gui, sceneName, colors) #extract data from npz archive : res = wb_res.loadFromNPZ(path + npzFilename) displayFeetTrajFromResult(gui, sceneName, res, Robot) plot.plotALLFromWB(cs, res) return res, cs
def run_contact_generation(self): print("### MLP : contact sequence ###") generate_contact_sequence, ContactGenerationOutputs = self.cfg.get_contact_generation_method() self.cs, self.fullBody, self.viewer = generate_contact_sequence(self.cfg) ContactGenerationOutputs.assertRequirements(self.cs) self.gui = self.viewer.client.gui if self.cfg.WRITE_STATUS: with open(self.cfg.STATUS_FILENAME, "a") as f: f = open(self.cfg.STATUS_FILENAME, "a") f.write("gen_cs_success: True\n") if self.cfg.DISPLAY_CS_STONES: display_tools.displaySteppingStones(self.cs, self.gui, self.viewer.sceneName, self.cfg.Robot) if self.cfg.DISPLAY_CS: if display_tools.DisplayContactSequenceRequirements.checkAndFillRequirements(self.cs, self.cfg, self.fullBody): input("Press Enter to display the contact sequence ...") display_tools.displayContactSequence(self.viewer, self.cs)
from mlp.utils.cs_tools import addPhaseFromConfig import multicontact_api from multicontact_api import ContactSequence import mlp.viewer.display_tools as display_tools from pinocchio import SE3 from numpy import array from mlp.utils.cs_tools import walk from solo_rbprm.solo import Robot # change robot here multicontact_api.switchToNumpyArray() ENV_NAME = "multicontact/ground" fb, v = display_tools.initScene(Robot, ENV_NAME, False) gui = v.client.gui sceneName = v.sceneName cs = ContactSequence(0) limbs = fb.limbs_names #Create an initial contact phase : q_ref = fb.referenceConfig[::] + [0] * 6 addPhaseFromConfig(fb, cs, q_ref, limbs) walk(fb, cs, 0.5, 0.1, limbs, first_half_step=False) display_tools.displaySteppingStones(cs, gui, sceneName, fb) filename = "solo_flatGround.cs" print("Write contact sequence binary file : ", filename) cs.saveAsBinary(filename)
cs, fullBody, viewer = generateContactSequence() if cfg.WRITE_STATUS: f = open(cfg.STATUS_FILENAME, "a") f.write("gen_cs_success: True\n") f.close() if cfg.DISPLAY_CS: raw_input("Press Enter to display the contact sequence ...") display_tools.displayContactSequence(viewer, cs) if cfg.SAVE_CS: filename = cfg.CONTACT_SEQUENCE_PATH + "/" + cfg.DEMO_NAME + ".cs" print "Write contact sequence binary file : ", filename cs.saveAsBinary(filename) if cfg.DISPLAY_CS_STONES: display_tools.displaySteppingStones(cs, viewer.client.gui, viewer.sceneName, cfg.Robot) print "------------------------------" print "### MLP : centroidal, initial Guess ###" from mlp.centroidal import generateCentroidalInitGuess cs_initGuess = generateCentroidalInitGuess(cs, fullBody=fullBody, viewer=viewer) if cfg.DISPLAY_INIT_GUESS_TRAJ and cs_initGuess: colors = [viewer.color.red, viewer.color.yellow] display_tools.displayCOMTrajectory(cs_initGuess, viewer.client.gui, viewer.sceneName, colors, "_init") print "------------------------------" print "### MLP : centroidal ###"
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 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