def run_centroidal(self, iter_dynamic_filter = 0):
print("------------------------------")
print("### MLP : centroidal ###")
generate_centroidal, CentroidalInputs, CentroidalOutputs = self.cfg.get_centroidal_method()
if not CentroidalInputs.checkAndFillRequirements(self.cs, self.cfg, self.fullBody):
raise RuntimeError(
"The current contact sequence cannot be given as input to the centroidal method selected.")
if iter_dynamic_filter > 0:
if self.cs_wb:
self.cs_initGuess = self.cs_wb
self.cs_com = generate_centroidal(self.cfg, self.cs, self.cs_initGuess,
self.fullBody, self.viewer,
iter_dynamic_filter == 0)
CentroidalOutputs.assertRequirements(self.cs_com)
self.cs_com_iters += [self.cs_com]
if self.cfg.WRITE_STATUS and iter_dynamic_filter == 0:
with open(self.cfg.STATUS_FILENAME, "a") as f:
f.write("centroidal_success: True\n")
if self.cfg.DISPLAY_COM_TRAJ:
colors = [self.viewer.color.blue, self.viewer.color.green]
display_tools.displayCOMTrajectory(self.cs_com, self.gui, self.viewer.sceneName, self.cfg.SOLVER_DT, colors)
if self.cfg.PLOT_CENTROIDAL:
plot.plotCOMTraj(self.cs_com, self.cfg.SOLVER_DT, " - iter " + str(iter_dynamic_filter))
plot.plotAMTraj(self.cs_com, self.cfg.SOLVER_DT, " - iter " + str(iter_dynamic_filter))
plot.plt.show(block=False)
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_centroidal_init_guess(self):
print("------------------------------")
print("### MLP : centroidal, initial Guess ###")
generate_centroidal_initguess, CentroidalInitGuessInputs, CentroidalInitGuessOutputs = self.cfg.get_centroidal_initguess_method()
if not CentroidalInitGuessInputs.checkAndFillRequirements(self.cs, self.cfg, self.fullBody):
raise RuntimeError(
"The current contact sequence cannot be given as input to the centroidalInitGuess method selected.")
self.cs_initGuess = generate_centroidal_initguess(self.cfg, self.cs,
fullBody=self.fullBody,
viewer=self.viewer)
CentroidalInitGuessOutputs.assertRequirements(self.cs_initGuess)
if self.cfg.DISPLAY_INIT_GUESS_TRAJ and self.cs_initGuess:
colors = [self.viewer.color.red, self.viewer.color.yellow]
display_tools.displayCOMTrajectory(self.cs_initGuess, self.gui, self.viewer.sceneName, self.cfg.SOLVER_DT, colors, "_init")
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 ###"
from mlp.centroidal import generateCentroidalTrajectory
cs_com = generateCentroidalTrajectory(cs, cs_initGuess, fullBody, viewer)
if cfg.WRITE_STATUS:
f = open(cfg.STATUS_FILENAME, "a")
f.write("centroidal_success: True\n")
f.close()
if cfg.SAVE_CS_COM:
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + cfg.DEMO_NAME + "_COM.cs"
print "Write contact sequence binary file with centroidal trajectory : ", filename
cs_com.saveAsBinary(filename)
if cfg.DISPLAY_CS:
if display_tools.DisplayContactSequenceRequirements.checkAndFillRequirements(cs, cfg, fullBody):
input("Press Enter to display the contact sequence ...")
display_tools.displayContactSequence(viewer, cs)
print("------------------------------")
print("### MLP : centroidal, initial Guess ###")
import mlp.centroidal.initGuess as centroidalInitGuess
if not centroidalInitGuess.Inputs.checkAndFillRequirements(cs,cfg, fullBody):
raise RuntimeError("The current contact sequence cannot be given as input to the centroidalInitGuess method selected.")
cs_initGuess = centroidalInitGuess.generateCentroidalTrajectory(cs, fullBody=fullBody, viewer=viewer)
centroidalInitGuess.Outputs.assertRequirements(cs_initGuess)
if cfg.DISPLAY_INIT_GUESS_TRAJ and cs_initGuess:
colors = [viewer.color.red, viewer.color.yellow]
display_tools.displayCOMTrajectory(cs_initGuess, gui, viewer.sceneName, cfg.SOLVER_DT, colors, "_init")
cs_ref = None
cs_wb = None
cs_wb_iters = []
cs_com_iters = []
cs_ref_iters = []
for iterDynFilter in range(cfg.ITER_DYNAMIC_FILTER + 1):
if iterDynFilter > 0:
print("\n########################################")
print("#### Iter " + str(iterDynFilter) + " of the dynamic filter. #### ")
print("######################################## \n")
cfg.IK_trackAM = False
cfg.w_am = cfg.w_am_track
cfg.kp_am = cfg.kp_am_track
if cs_wb is not None: