diff --git a/projects/JVRC1/cnoid/sim_mc.py b/projects/JVRC1/cnoid/sim_mc.py index cc8f638..4cc328a 100644 --- a/projects/JVRC1/cnoid/sim_mc.py +++ b/projects/JVRC1/cnoid/sim_mc.py @@ -126,6 +126,7 @@ def startMCControl(): mc.setProperty("is_enabled", "1") def connectMCControl(): + connectPorts(sh.port("qOut"), mc.port("qInit")) connectPorts(rh.port("q"), mc.port("qIn")) connectPorts(rh.port("dq"), mc.port("alphaIn")) connectPorts(rh.port("tauOut"), mc.port("taucIn")) diff --git a/src/MCControl.cpp b/src/MCControl.cpp index 5366525..7a13dff 100644 --- a/src/MCControl.cpp +++ b/src/MCControl.cpp @@ -342,7 +342,16 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id) if(!init) { mc_rtc::log::info("Init controller"); - controller.init(qIn); + if(qInit.size() == qIn.size()) + { + controller.init(qInit); + } + else + { + mc_rtc::log::warning("The size of qInit does not mach the size of qIn, please update the corresponding " + "Python script... using qIn instead to initialize the controller"); + controller.init(qIn); + } init = true; } if(controller.run())