Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Fix timestep check #31

Merged
merged 2 commits into from
Feb 10, 2023
Merged

Fix timestep check #31

Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cf67bdd
Timestep check should be done only once during init
rohanpsingh Feb 9, 2023
3f7df46
Rename log entry: cmdTau to tauCmd
rohanpsingh Feb 9, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
46 changes: 24 additions & 22 deletions src/MCControl.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -631,30 +631,32 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id)

if(controller.running)
{
// confirm mc-rtc timestep is same as IOB timestep
if(!m_is_simulation)
{
if(!open_iob())
{
failed_iob("open_iob", "(mc_openrtm constructor)");
mc_rtc::log::error_and_throw<std::runtime_error>("[mc_openrtm] Cannot verify timesteps of IOB and mc-rtc.");
}
double iob_ts = get_signal_period() / 1e9;
if(fabs(iob_ts - controller.controller().timeStep) > 1e-6)
{
mc_rtc::log::error_and_throw<std::runtime_error>(
"[mc_openrtm] Missmatch between IOB timestep ({}ms) and mc_rtc ({}ms).", iob_ts,
controller.controller().timeStep);
}
if(!close_iob())
{
failed_iob("close_iob", "(mc_openrtm constructor)");
mc_rtc::log::error_and_throw<std::runtime_error>("[mc_openrtm] Could not close IOB.");
}
}
if(!init)
{
mc_rtc::log::info("Init controller");
// confirm mc-rtc timestep is same as IOB timestep
if(!m_is_simulation)
{
if(!open_iob())
{
failed_iob("open_iob", "(mc_openrtm constructor)");
mc_rtc::log::error_and_throw<std::runtime_error>("[mc_openrtm] Cannot verify timesteps of IOB and mc-rtc.");
}
double iob_ts = get_signal_period() / 1e9;
if(fabs(iob_ts - controller.controller().timeStep) > 1e-6)
{
mc_rtc::log::error_and_throw<std::runtime_error>(
"[mc_openrtm] Missmatch between IOB timestep ({}s) and mc_rtc ({}s).", iob_ts,
controller.controller().timeStep);
}
if(!close_iob())
{
failed_iob("close_iob", "(mc_openrtm constructor)");
mc_rtc::log::error_and_throw<std::runtime_error>("[mc_openrtm] Could not close IOB.");
}
mc_rtc::log::info("[mc_openrtm] IOB timestep = {}s and mc_rtc {}s.", iob_ts,
controller.controller().timeStep);
}
if(qInit.size() == qIn.size())
{
controller.init(qInit);
Expand All @@ -666,7 +668,7 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id)
controller.init(qIn);
}
controller.controller().logger().addLogEntry("perf_LoopDt", [&]() { return loop_dt.count(); });
controller.controller().logger().addLogEntry("cmdTau", [&]() { return cmdTauIn; });
controller.controller().logger().addLogEntry("tauCmd", [&]() { return cmdTauIn; });
init = true;
}
if(controller.run())
Expand Down