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 display_wb(self, t=None):
if t is None:
display_tools.displayWBmotion(self.viewer, self.cs_wb.concatenateQtrajectories(), self.cfg.DT_DISPLAY)
else:
display_tools.displayWBatT(self.viewer, self.cs_wb, t)
def dispWB(t=None):
if t is None:
display_tools.displayWBmotion(viewer, res.q_t, cfg.IK_dt,
cfg.DT_DISPLAY)
else:
display_tools.displayWBatT(viewer, res, t)
from mlp.utils import check_path
print "## Begin validation of the final motion (collision and joint-limits)"
validator = check_path.PathChecker(fullBody, cs_com, res.nq, True)
motion_valid, t_invalid = validator.check_motion(res.q_t)
print "## Check final motion, valid = ", motion_valid
if not motion_valid:
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:
def dispWB(t=None):
if t is None:
display_tools.displayWBmotion(viewer,cs_wb.concatenateQtrajectories(), cfg.DT_DISPLAY)
else:
display_tools.displayWBatT(viewer, cs_wb, t)
if cfg.SAVE_CS_REF:
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)
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