def export_all(self):
cfg = self.cfg
if cfg.SAVE_CS:
if not os.path.exists(cfg.CONTACT_SEQUENCE_PATH):
os.makedirs(cfg.CONTACT_SEQUENCE_PATH)
filename = cfg.CS_FILENAME
print("Write contact sequence binary file : ", filename)
self.cs.saveAsBinary(filename)
if cfg.SAVE_CS_COM:
if not os.path.exists(cfg.CONTACT_SEQUENCE_PATH):
os.makedirs(cfg.CONTACT_SEQUENCE_PATH)
filename = cfg.COM_FILENAME
print("Write contact sequence binary file with centroidal trajectory : ", filename)
self.cs_com.saveAsBinary(filename)
if cfg.SAVE_CS_REF:
Requirements.requireZMPtrajectories(self.cs_ref, self.cfg)
if not os.path.exists(cfg.CONTACT_SEQUENCE_PATH):
os.makedirs(cfg.CONTACT_SEQUENCE_PATH)
filename = cfg.REF_FILENAME
print("Write contact sequence binary file with centroidal and end effector trajectories: ", filename)
self.cs_ref.saveAsBinary(filename)
if cfg.SAVE_CS_WB:
if not os.path.exists(cfg.CONTACT_SEQUENCE_PATH):
os.makedirs(cfg.CONTACT_SEQUENCE_PATH)
filename = cfg.WB_FILENAME
print("Write contact sequence binary file with wholebody trajectories: ", filename)
self.cs_wb.saveAsBinary(filename)
if cfg.EXPORT_OPENHRP and self.motion_valid:
openHRP.export(self.cfg, self.cs_com, self.cs_wb) # FIXME
if cfg.EXPORT_GAZEBO and self.motion_valid:
gazebo.export(self.cfg, self.cs_wb.concatenateQtrajectories())
if cfg.EXPORT_NPZ and self.motion_valid:
npz.export(self.cfg, self.cs_ref, self.cs_wb, self.cfg)
if cfg.EXPORT_BLENDER:
blender.export(self.cfg, self.cs_wb.concatenateQtrajectories(), self.viewer)
blender.exportSteppingStones(self.cfg, self.viewer)
if cfg.EXPORT_SOT:
sotTalosBalance.export(self.cfg, self.cs_wb) # FIXME
else:
motion_valid = True
if cfg.DISPLAY_WB_MOTION:
raw_input("Press Enter to display the whole body motion ...")
display_tools.displayWBmotion(viewer, res.q_t, cfg.IK_dt, cfg.DT_DISPLAY)
if cfg.PLOT:
from mlp.utils import plot
plot.plotKneeTorque(res.t_t, res.phases_intervals, res.tau_t,
(res.nq - res.nu), cfg.Robot.kneeIds)
plot.plotALLFromWB(cs_com, res, cfg.DISPLAY_PLOT, cfg.SAVE_PLOT,
cfg.OUTPUT_DIR + "/plot/" + cfg.DEMO_NAME)
if cfg.EXPORT_OPENHRP and motion_valid:
from mlp.export import openHRP
openHRP.export(cs_com, res)
if cfg.EXPORT_GAZEBO and motion_valid:
from mlp.export import gazebo
gazebo.export(res.q_t)
if cfg.EXPORT_NPZ and motion_valid:
res.exportNPZ(cfg.EXPORT_PATH + "/npz", cfg.DEMO_NAME + ".npz")
if cfg.EXPORT_BLENDER:
from mlp.export import blender
blender.export(res.q_t, viewer, cfg.IK_dt)
blender.exportSteppingStones(viewer)
if cfg.EXPORT_SOT:
from mlp.export import sotTalosBalance
sotTalosBalance.export(res)
if cfg.EXPORT_EFF_IN_CS:
from mlp.utils.util import addEffectorTrajectoryInCS
print("Write contact sequence binary file with wholebody trajectories: ", filename)
cs_wb.saveAsBinary(filename)
if cfg.DISPLAY_WB_MOTION:
input("Press Enter to display the whole body motion ...")
display_tools.displayWBmotion(viewer, cs_wb.concatenateQtrajectories(), cfg.DT_DISPLAY)
if cfg.PLOT:
from mlp.utils import plot
plot.plotALLFromWB(cs_ref_iters, cs_wb_iters, cfg)
if cfg.ITER_DYNAMIC_FILTER > 0:
plot.compareCentroidal(cs, cs_com_iters, cfg)
if cfg.EXPORT_OPENHRP and motion_valid:
from mlp.export import openHRP
openHRP.export(cs_com, cs_wb) # FIXME
if cfg.EXPORT_GAZEBO and motion_valid:
from mlp.export import gazebo
gazebo.export(cs_wb.concatenateQtrajectories())
if cfg.EXPORT_NPZ and motion_valid:
from mlp.export import npz
npz.export(cs_ref, cs_wb, cfg)
if cfg.EXPORT_BLENDER:
from mlp.export import blender
blender.export(cs_wb.concatenateQtrajectories(), viewer)
blender.exportSteppingStones(viewer)
if cfg.EXPORT_SOT:
from mlp.export import sotTalosBalance
sotTalosBalance.export(cs_wb) # TODO