From cea451f604c9ec15d45a2220048168217d1f739f Mon Sep 17 00:00:00 2001 From: Shunichi Nozawa Date: Thu, 26 Mar 2015 08:49:23 +0900 Subject: [PATCH] Add ports for offsets of limbCOP --- python/hrpsys_config.py | 3 +++ rtc/AutoBalancer/AutoBalancer.cpp | 22 ++++++++++++++++++++++ rtc/AutoBalancer/AutoBalancer.h | 2 ++ rtc/Stabilizer/Stabilizer.cpp | 15 +++++++++++++++ rtc/Stabilizer/Stabilizer.h | 2 ++ 5 files changed, 44 insertions(+) diff --git a/python/hrpsys_config.py b/python/hrpsys_config.py index a8537d688f7..b12056a54e4 100755 --- a/python/hrpsys_config.py +++ b/python/hrpsys_config.py @@ -367,6 +367,9 @@ def connectComps(self): if self.abc: connectPorts(self.sh.port(sen+"Out"), self.abc.port("ref_" + sen)) + if self.abc and self.st: + connectPorts(self.abc.port("limbCOPOffset_"+sen), + self.st.port("limbCOPOffset_"+sen)) # actual force sensors if self.rmfo: diff --git a/rtc/AutoBalancer/AutoBalancer.cpp b/rtc/AutoBalancer/AutoBalancer.cpp index 11c4ca087f2..4eb580945e1 100644 --- a/rtc/AutoBalancer/AutoBalancer.cpp +++ b/rtc/AutoBalancer/AutoBalancer.cpp @@ -231,6 +231,8 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() int nforce = npforce + nvforce; m_ref_force.resize(nforce); m_ref_forceIn.resize(nforce); + m_limbCOPOffset.resize(nforce); + m_limbCOPOffsetOut.resize(nforce); for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i)->name); } @@ -248,6 +250,15 @@ RTC::ReturnCode_t AutoBalancer::onInitialize() std::cerr << "[" << m_profile.instance_name << "] name = " << std::string("ref_"+sensor_names[i]) << std::endl; ref_forces.push_back(hrp::Vector3(0,0,0)); } + // set limb cop offset port + std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << nforce << ")" << std::endl; + for (unsigned int i=0; i(nm.c_str(), m_limbCOPOffset[i]); + registerOutPort(nm.c_str(), *m_limbCOPOffsetOut[i]); + m_limbCOPOffset[i].data.x = m_limbCOPOffset[i].data.y = m_limbCOPOffset[i].data.z = 0.0; + std::cerr << "[" << m_profile.instance_name << "] name = " << nm << std::endl; + } sbp_offset = hrp::Vector3(0,0,0); sbp_cog_offset = hrp::Vector3(0,0,0); //use_force = MODE_NO_FORCE; @@ -481,6 +492,11 @@ RTC::ReturnCode_t AutoBalancer::onExecute(RTC::UniqueId ec_id) m_controlSwingSupportTime.tm = m_qRef.tm; m_controlSwingSupportTimeOut.write(); + for (unsigned int i=0; iwrite(); + } + return RTC::RTC_OK; } @@ -513,6 +529,12 @@ void AutoBalancer::getTargetParameters() for (size_t i = 0; i < 2; i++) for (size_t j = 0; j < 3; j++) default_zmp_offsets[i](j) = default_zmp_offsets_output[i*3+j]; + m_limbCOPOffset[contact_states_index_map["rleg"]].data.x = default_zmp_offsets[0](0); + m_limbCOPOffset[contact_states_index_map["rleg"]].data.y = default_zmp_offsets[0](1); + m_limbCOPOffset[contact_states_index_map["rleg"]].data.z = default_zmp_offsets[0](2); + m_limbCOPOffset[contact_states_index_map["lleg"]].data.x = default_zmp_offsets[1](0); + m_limbCOPOffset[contact_states_index_map["lleg"]].data.y = default_zmp_offsets[1](1); + m_limbCOPOffset[contact_states_index_map["lleg"]].data.z = default_zmp_offsets[1](2); if (DEBUGP) { std::cerr << "[" << m_profile.instance_name << "] default_zmp_offsets (interpolated)" << std::endl; std::cerr << "[" << m_profile.instance_name << "] rleg = " << default_zmp_offsets[0].format(Eigen::IOFormat(Eigen::StreamPrecision, 0, ", ", ", ", "", "", " [", "]")) << "[m]" << std::endl; diff --git a/rtc/AutoBalancer/AutoBalancer.h b/rtc/AutoBalancer/AutoBalancer.h index 61ffa84e034..817b11bb90a 100644 --- a/rtc/AutoBalancer/AutoBalancer.h +++ b/rtc/AutoBalancer/AutoBalancer.h @@ -143,6 +143,8 @@ class AutoBalancer OutPort m_contactStatesOut; TimedDoubleSeq m_controlSwingSupportTime; OutPort m_controlSwingSupportTimeOut; + std::vector m_limbCOPOffset; + std::vector *> m_limbCOPOffsetOut; // for debug OutPort m_cogOut; diff --git a/rtc/Stabilizer/Stabilizer.cpp b/rtc/Stabilizer/Stabilizer.cpp index 6342e8dec5c..93e16a937b7 100644 --- a/rtc/Stabilizer/Stabilizer.cpp +++ b/rtc/Stabilizer/Stabilizer.cpp @@ -164,6 +164,8 @@ RTC::ReturnCode_t Stabilizer::onInitialize() int npforce = m_robot->numSensors(hrp::Sensor::FORCE); m_wrenches.resize(npforce); m_wrenchesIn.resize(npforce); + m_limbCOPOffset.resize(npforce); + m_limbCOPOffsetIn.resize(npforce); for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i); m_wrenchesIn[i] = new RTC::InPort(std::string(s->name+"Ref").c_str(), m_wrenches[i]); @@ -172,6 +174,14 @@ RTC::ReturnCode_t Stabilizer::onInitialize() std::cerr << "[" << m_profile.instance_name << "] force sensor" << std::endl; std::cerr << "[" << m_profile.instance_name << "] name = " << s->name << std::endl; } + std::cerr << "[" << m_profile.instance_name << "] limbCOPOffset ports (" << npforce << ")" << std::endl; + for (unsigned int i=0; isensor(hrp::Sensor::FORCE, i); + std::string nm("limbCOPOffset_"+s->name); + m_limbCOPOffsetIn[i] = new RTC::InPort(nm.c_str(), m_limbCOPOffset[i]); + registerInPort(nm.c_str(), *m_limbCOPOffsetIn[i]); + std::cerr << "[" << m_profile.instance_name << "] name = " << nm << std::endl; + } // setting from conf file // rleg,TARGET_LINK,BASE_LINK,x,y,z,rx,ry,rz,rth #<=pos + rot (axis+angle) @@ -387,6 +397,11 @@ RTC::ReturnCode_t Stabilizer::onExecute(RTC::UniqueId ec_id) m_wrenchesIn[i]->read(); } } + for (size_t i = 0; i < m_limbCOPOffsetIn.size(); ++i) { + if ( m_limbCOPOffsetIn[i]->isNew() ) { + m_limbCOPOffsetIn[i]->read(); + } + } if (is_legged_robot) { getCurrentParameters(); diff --git a/rtc/Stabilizer/Stabilizer.h b/rtc/Stabilizer/Stabilizer.h index ffa24dfa12b..ef012f6604e 100644 --- a/rtc/Stabilizer/Stabilizer.h +++ b/rtc/Stabilizer/Stabilizer.h @@ -163,6 +163,7 @@ class Stabilizer RTC::TimedOrientation3D m_baseRpy; RTC::TimedBooleanSeq m_contactStates; RTC::TimedDoubleSeq m_controlSwingSupportTime; + std::vector m_limbCOPOffset; RTC::TimedBooleanSeq m_actContactStates; // for debug ouput RTC::TimedPoint3D m_originRefZmp, m_originRefCog, m_originRefCogVel, m_originNewZmp; @@ -186,6 +187,7 @@ class Stabilizer RTC::InPort m_baseRpyIn; RTC::InPort m_contactStatesIn; RTC::InPort m_controlSwingSupportTimeIn; + std::vector *> m_limbCOPOffsetIn; std::vector m_wrenches; std::vector *> m_wrenchesIn;