From dbfd6f338a53521d9066f3ea31e6a92b70b6528a Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Thu, 3 Jun 2021 19:28:11 +0900 Subject: [PATCH 1/4] Use qInit instead of qIn to initialize mc_rtc i.e. use target commands instead of current encoders values --- src/MCControl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/MCControl.cpp b/src/MCControl.cpp index 5366525..337e42b 100644 --- a/src/MCControl.cpp +++ b/src/MCControl.cpp @@ -342,7 +342,7 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id) if(!init) { mc_rtc::log::info("Init controller"); - controller.init(qIn); + controller.init(qInit); init = true; } if(controller.run()) From ceea3ef71b57db159a9317dfdbd233cddcec3e16 Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Fri, 4 Jun 2021 08:48:35 +0900 Subject: [PATCH 2/4] Updated sample Python script to connect qInit to mc_openrtm --- projects/JVRC1/cnoid/sim_mc.py | 1 + 1 file changed, 1 insertion(+) 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")) From 78b16a2dec49a6435443e9b64a50ffb897a0452a Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Fri, 4 Jun 2021 08:48:58 +0900 Subject: [PATCH 3/4] Added security check to qInit --- src/MCControl.cpp | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/MCControl.cpp b/src/MCControl.cpp index 337e42b..22a23f2 100644 --- a/src/MCControl.cpp +++ b/src/MCControl.cpp @@ -342,7 +342,15 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id) if(!init) { mc_rtc::log::info("Init controller"); - controller.init(qInit); + 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()) From 88e70fae3987a0d3ae5167cc782f926a21befc7d Mon Sep 17 00:00:00 2001 From: Rafael Cisneros Date: Fri, 4 Jun 2021 10:03:38 +0900 Subject: [PATCH 4/4] Applied .clang-format-fix.sh --- src/MCControl.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/MCControl.cpp b/src/MCControl.cpp index 22a23f2..7a13dff 100644 --- a/src/MCControl.cpp +++ b/src/MCControl.cpp @@ -348,7 +348,8 @@ RTC::ReturnCode_t MCControl::onExecute(RTC::UniqueId ec_id) } 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"); + 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;