def finish_solve(self):
if self.cfg.DISPLAY_FEET_TRAJ:
if self.cfg.IK_store_effector:
displayEffectorTrajectories(self.cs_wb, self.viewer, self.fullBody)
else:
displayEffectorTrajectories(self.cs_ref, self.viewer, self.fullBody)
if self.cfg.CHECK_FINAL_MOTION and self.cs_wb and self.cs_wb.size() > 0:
print("## Begin validation of the final motion (collision and joint-limits)")
validator = check_path.PathChecker(self.fullBody, self.cfg.CHECK_DT, True)
motion_valid, t_invalid = validator.check_motion(self.cs_wb.concatenateQtrajectories())
print("## Check final motion, valid = ", motion_valid)
if not motion_valid:
print("## First invalid time : ", t_invalid)
if self.cfg.WRITE_STATUS:
with open(self.cfg.STATUS_FILENAME, "a") as f:
f.write("motion_valid: " + str(motion_valid) + "\n")
elif self.cs_wb and self.cs_wb.size() > 0:
self.motion_valid = True
else:
self.motion_valid = False
if self.cfg.DISPLAY_WB_MOTION:
input("Press Enter to display the whole body motion ...")
display_tools.displayWBmotion(self.viewer, self.cs_wb.concatenateQtrajectories(), self.cfg.DT_DISPLAY)
if self.cfg.PLOT:
plot.plotALLFromWB(self.cs_ref_iters, self.cs_wb_iters, self.cfg)
if self.cfg.ITER_DYNAMIC_FILTER > 0:
plot.compareCentroidal(self.cs, self.cs_com_iters, self.cfg)
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
print "## First invalid time : ", t_invalid
if cfg.WRITE_STATUS:
f = open(cfg.STATUS_FILENAME, "a")
f.write("motion_valid: " + str(motion_valid) + "\n")
f.close()
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
print("Write contact sequence binary file with centroidal and end effector trajectories: ", filename)
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)
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)