From 97912eec45a41f8172d6b6df5b51b11bc1217203 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 10 Oct 2014 14:54:47 +0900 Subject: [PATCH 1/2] (AutoBalancer, ImpedanceController) : Enable onDeactivated function for ImpedanceController and AutoBalancer --- rtc/AutoBalancer/AutoBalancer.cpp | 9 ++++----- rtc/AutoBalancer/AutoBalancer.h | 2 +- rtc/ImpedanceController/ImpedanceController.cpp | 9 ++++----- rtc/ImpedanceController/ImpedanceController.h | 2 +- 4 files changed, 10 insertions(+), 12 deletions(-) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 37da614c476..4182ca441f5 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -268,12 +268,11 @@ RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id) return RTC::RTC_OK; } -/* - RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id) - { +RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << "AutoBalancer::onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; - } -*/ +} #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 ) //#define DEBUGP2 ((loop%200==0)) diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index 1905090270e..2176606f224 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -64,7 +64,7 @@ class AutoBalancer // The deactivated action (Active state exit action) // former rtc_active_exit() - // virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); // The execution action that is invoked periodically // former rtc_active_do() diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp index a884a5dc39b..f20d1322bd7 100644 --- a/rtc/ImpedanceController/ImpedanceController.cpp +++ b/rtc/ImpedanceController/ImpedanceController.cpp @@ -206,12 +206,11 @@ RTC::ReturnCode_t ImpedanceController::onActivated(RTC::UniqueId ec_id) return RTC::RTC_OK; } -/* - RTC::ReturnCode_t ImpedanceController::onDeactivated(RTC::UniqueId ec_id) - { +RTC::ReturnCode_t ImpedanceController::onDeactivated(RTC::UniqueId ec_id) +{ + std::cout << "ImpedanceController::onDeactivated(" << ec_id << ")" << std::endl; return RTC::RTC_OK; - } -*/ +} #define DEBUGP ((m_debugLevel==1 && loop%200==0) || m_debugLevel > 1 ) RTC::ReturnCode_t ImpedanceController::onExecute(RTC::UniqueId ec_id) diff --git a/rtc/ImpedanceController/ImpedanceController.h b/rtc/ImpedanceController/ImpedanceController.h index b4a8f2e2213..1d7660dd68d 100644 --- a/rtc/ImpedanceController/ImpedanceController.h +++ b/rtc/ImpedanceController/ImpedanceController.h @@ -63,7 +63,7 @@ class ImpedanceController // The deactivated action (Active state exit action) // former rtc_active_exit() - // virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); + virtual RTC::ReturnCode_t onDeactivated(RTC::UniqueId ec_id); // The execution action that is invoked periodically // former rtc_active_do() From 02769b57df94d62f0228a78d30ed4ad293cd7c74 Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Fri, 10 Oct 2014 15:05:17 +0900 Subject: [PATCH 2/2] (ImpedanceController, AutoBalancer, Stabilizer) : Move to idling mode if stop() and start() are called. This is discussed in https://github.com/fkanehiro/hrpsys-base/issues/215 --- rtc/AutoBalancer/AutoBalancer.cpp | 6 ++++++ rtc/ImpedanceController/ImpedanceController.cpp | 5 +++++ rtc/Stabilizer/Stabilizer.cpp | 7 +++++++ 3 files changed, 18 insertions(+) diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 4182ca441f5..caf6606b2d7 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -271,6 +271,12 @@ RTC::ReturnCode_t AutoBalancer::onActivated(RTC::UniqueId ec_id) RTC::ReturnCode_t AutoBalancer::onDeactivated(RTC::UniqueId ec_id) { std::cout << "AutoBalancer::onDeactivated(" << ec_id << ")" << std::endl; + Guard guard(m_mutex); + if (control_mode == MODE_ABC) { + stopABCparam(); + control_mode = MODE_IDLE; + transition_count = 1; // sync in one controller loop + } return RTC::RTC_OK; } diff --git a/rtc/ImpedanceController/ImpedanceController.cpp b/rtc/ImpedanceController/ImpedanceController.cpp index f20d1322bd7..7a76dfabe1c 100644 --- a/rtc/ImpedanceController/ImpedanceController.cpp +++ b/rtc/ImpedanceController/ImpedanceController.cpp @@ -209,6 +209,11 @@ RTC::ReturnCode_t ImpedanceController::onActivated(RTC::UniqueId ec_id) RTC::ReturnCode_t ImpedanceController::onDeactivated(RTC::UniqueId ec_id) { std::cout << "ImpedanceController::onDeactivated(" << ec_id << ")" << std::endl; + Guard guard(m_mutex); + for ( std::map::iterator it = m_impedance_param.begin(); it != m_impedance_param.end(); it++ ) { + deleteImpedanceController(it->first); + m_impedance_param[it->first].transition_count = 1; + } return RTC::RTC_OK; } diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index 0b45cb25482..b6e65e36add 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -301,6 +301,11 @@ RTC::ReturnCode_t Stabilizer::onActivated(RTC::UniqueId ec_id) RTC::ReturnCode_t Stabilizer::onDeactivated(RTC::UniqueId ec_id) { std::cout << m_profile.instance_name<< ": onDeactivated(" << ec_id << ")" << std::endl; + if ( (control_mode == MODE_ST || control_mode == MODE_AIR) ) { + sync_2_idle (); + control_mode = MODE_IDLE; + transition_count = 1; // sync in one controller loop + } return RTC::RTC_OK; } @@ -1275,6 +1280,7 @@ RTC::ReturnCode_t Stabilizer::onRateChanged(RTC::UniqueId ec_id) void Stabilizer::sync_2_st () { + std::cerr << "Sync IDLE => ST" << std::endl; pangx_ref = pangy_ref = pangx = pangy = 0; rdx = rdy = rx = ry = 0; d_rpy[0] = d_rpy[1] = 0; @@ -1292,6 +1298,7 @@ void Stabilizer::sync_2_st () void Stabilizer::sync_2_idle () { + std::cerr << "Sync ST => IDLE" << std::endl; transition_count = MAX_TRANSITION_COUNT; for (int i = 0; i < m_robot->numJoints(); i++ ) { transition_joint_q[i] = m_robot->joint(i)->q;