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
(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
cs_com = addEffectorTrajectoryInCS(cs_com, res)
filename = cfg.CONTACT_SEQUENCE_PATH + "/" + cfg.DEMO_NAME + "_COM_eff.cs"
print "Write contact sequence binary file with effector trajectories : ", filename
cs_com.saveAsBinary(filename)
def dispCS(step=0.2):
display_tools.displayContactSequence(viewer, cs, step)