diff --git a/rtc/ImpedanceController/JointPathEx.cpp b/rtc/ImpedanceController/JointPathEx.cpp index f6b09047fcf..e2ecf6b33c5 100644 --- a/rtc/ImpedanceController/JointPathEx.cpp +++ b/rtc/ImpedanceController/JointPathEx.cpp @@ -426,3 +426,59 @@ bool JointPathEx::calcInverseKinematics2(const Vector3& end_p, const Matrix33& e return converged; } + +double hrp::JointLimitTable::getInterpolatedLimitAngle (const double target_joint_angle, const bool is_llimit_angle) const +{ + double target_angle = target_joint_angle * 180.0 / M_PI; // [rad]=>[deg] + int int_target_angle = static_cast(std::floor(target_angle)); + int target_range[2] = {int_target_angle, 1+int_target_angle}; + double self_joint_range[2]; + for (size_t i = 0; i < 2; i++) { + size_t idx = std::min(std::max(target_llimit_angle, target_range[i]), target_ulimit_angle) - target_llimit_angle; + self_joint_range[i] = (is_llimit_angle ? llimit_table(idx) : ulimit_table(idx)); + } + double tmp_ratio = target_angle - int_target_angle; + return (self_joint_range[0] * (1-tmp_ratio) + self_joint_range[1] * tmp_ratio) * M_PI / 180.0; // [deg]=>[rad] +}; + +void hrp::readJointLimitTableFromProperties (std::map& joint_limit_tables, + hrp::BodyPtr m_robot, + const std::string& prop_string, + const std::string& instance_name) +{ + if (prop_string != "") { + coil::vstring limit_tables = coil::split(prop_string, ":"); + size_t limit_table_size = 6; // self_joint_name:target_joint_name:target_min_angle:target_max_angle:min:max + size_t num_limit_table = limit_tables.size() / limit_table_size; + std::cerr << "[" << instance_name << "] Load joint limit table [" << num_limit_table << "]" << std::endl; + for (size_t i = 0; i < num_limit_table; i++) { + size_t start_idx = i*limit_table_size; + int target_llimit_angle, target_ulimit_angle; + coil::stringTo(target_llimit_angle, limit_tables[start_idx+2].c_str()); + coil::stringTo(target_ulimit_angle, limit_tables[start_idx+3].c_str()); + coil::vstring llimit_str_v = coil::split(limit_tables[start_idx+4], ","); + coil::vstring ulimit_str_v = coil::split(limit_tables[start_idx+5], ","); + hrp::dvector llimit_table(llimit_str_v.size()), ulimit_table(ulimit_str_v.size()); + int target_jointId = -1; + for (size_t j = 0; j < m_robot->numJoints(); j++) { + if ( m_robot->joint(j)->name == limit_tables[start_idx+1]) target_jointId = m_robot->joint(j)->jointId; + } + if ( llimit_str_v.size() != ulimit_str_v.size() || target_jointId == -1 ) { + std::cerr << "[" << instance_name << "] " << limit_tables[start_idx+0] << ":" << limit_tables[start_idx+1] << " fail" << std::endl; + } else { + std::cerr << "[" << instance_name << "] " << limit_tables[start_idx+0] << ":" << limit_tables[start_idx+1] << "(" << target_jointId << ")" << std::endl; + std::cerr << "[" << instance_name << "] target_llimit_angle " << limit_tables[start_idx+2] << "[deg], target_ulimit_angle " << limit_tables[start_idx+3] << "[deg]" << std::endl; + std::cerr << "[" << instance_name << "] llimit_table[deg] " << limit_tables[start_idx+4] << std::endl; + std::cerr << "[" << instance_name << "] ulimit_table[deg] " << limit_tables[start_idx+5] << std::endl; + for (int j = 0; j < llimit_table.size(); j++) { + coil::stringTo(llimit_table[j], llimit_str_v[j].c_str()); + coil::stringTo(ulimit_table[j], ulimit_str_v[j].c_str()); + } + joint_limit_tables.insert(std::pair(limit_tables[start_idx], + hrp::JointLimitTable(target_jointId, target_llimit_angle, target_ulimit_angle, llimit_table, ulimit_table))); + } + } + } else { + std::cerr << "[" << instance_name << "] Do not load joint limit table" << std::endl; + } +}; diff --git a/rtc/ImpedanceController/JointPathEx.h b/rtc/ImpedanceController/JointPathEx.h index 75abb347367..b8ba470484a 100644 --- a/rtc/ImpedanceController/JointPathEx.h +++ b/rtc/ImpedanceController/JointPathEx.h @@ -3,6 +3,8 @@ #include #include #include +#include +#include // hrplib/hrpUtil/MatrixSolvers.h namespace hrp { @@ -35,6 +37,36 @@ namespace hrp { typedef boost::shared_ptr JointPathExPtr; + // JointLimitTable for one joint + // self_joint : a joint to obtain llimit and ulimit from this class. + // target_joint : self_joint's limit is difference for target_joint's joint angle. + class JointLimitTable { + private: + int target_jointId; // jointId for target_joint + int target_llimit_angle, target_ulimit_angle; // llimit and ulimit angle [deg] for target_joint + hrp::dvector llimit_table, ulimit_table; // Tables for self_joint's llimit and ulimit + double getInterpolatedLimitAngle (const double target_joint_angle, const bool is_llimit_angle) const; + public: + JointLimitTable (const int _target_jointId, + const int _target_llimit_angle, const int _target_ulimit_angle, + const hrp::dvector& _llimit_table, const hrp::dvector& _ulimit_table) + : target_jointId(_target_jointId), target_llimit_angle(_target_llimit_angle), target_ulimit_angle(_target_ulimit_angle), llimit_table(_llimit_table), ulimit_table(_ulimit_table) {}; + ~JointLimitTable() {}; + int getTargetJointId () const { return target_jointId; }; + double getLlimit (const double target_joint_angle) const // [rad] + { + return getInterpolatedLimitAngle(target_joint_angle, true); // [rad] + }; + double getUlimit (const double target_joint_angle) const // [rad] + { + return getInterpolatedLimitAngle(target_joint_angle, false); // [rad] + }; + }; + + void readJointLimitTableFromProperties (std::map& joint_mm_tables, + hrp::BodyPtr m_robot, + const std::string& prop_string, + const std::string& instance_name); }; #include diff --git a/rtc/SoftErrorLimiter/CMakeLists.txt b/rtc/SoftErrorLimiter/CMakeLists.txt index 5e31d43b97d..5d9311fb612 100644 --- a/rtc/SoftErrorLimiter/CMakeLists.txt +++ b/rtc/SoftErrorLimiter/CMakeLists.txt @@ -1,4 +1,4 @@ -set(comp_sources SoftErrorLimiter.cpp SoftErrorLimiterService_impl.cpp robot.cpp beep.cpp) +set(comp_sources SoftErrorLimiter.cpp SoftErrorLimiterService_impl.cpp robot.cpp beep.cpp ../ImpedanceController/JointPathEx.cpp) set(libs hrpModel-3.1 hrpUtil-3.1 hrpsysBaseStub) add_library(SoftErrorLimiter SHARED ${comp_sources}) target_link_libraries(SoftErrorLimiter ${libs}) diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp b/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp index de28710ae8c..4bba88320d5 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp +++ b/rtc/SoftErrorLimiter/SoftErrorLimiter.cpp @@ -133,6 +133,9 @@ RTC::ReturnCode_t SoftErrorLimiter::onInitialize() soft_limit_error_beep_freq = static_cast(1.0/(4.0*dt)); // soft limit error => 4 times / 1[s] position_limit_error_beep_freq = static_cast(1.0/(2.0*dt)); // position limit error => 2 times / 1[s] + // load joint limit table + hrp::readJointLimitTableFromProperties (joint_limit_tables, m_robot, prop["joint_limit_table"], std::string(m_profile.instance_name)); + return RTC::RTC_OK; } @@ -244,19 +247,24 @@ RTC::ReturnCode_t SoftErrorLimiter::onExecute(RTC::UniqueId ec_id) So use m_robot->joint(i) for llimit/ulimit */ - bool servo_limit_state = - ((m_robot->joint(i)->llimit > m_qRef.data[i]) || - (m_robot->joint(i)->ulimit < m_qRef.data[i])); + double llimit = m_robot->joint(i)->llimit; + double ulimit = m_robot->joint(i)->ulimit; + if (joint_limit_tables.find(m_robot->joint(i)->name) != joint_limit_tables.end()) { + std::map::iterator it = joint_limit_tables.find(m_robot->joint(i)->name); + llimit = it->second.getLlimit(m_qRef.data[it->second.getTargetJointId()]); + ulimit = it->second.getUlimit(m_qRef.data[it->second.getTargetJointId()]); + } + bool servo_limit_state = ((llimit > m_qRef.data[i]) || (ulimit < m_qRef.data[i])); if ( servo_state == 1 && servo_limit_state ) { std::cerr << "position limit over " << m_robot->joint(i)->name << "(" << i << "), qRef=" << m_qRef.data[i] - << ", llimit =" << m_robot->joint(i)->llimit - << ", ulimit =" << m_robot->joint(i)->ulimit + << ", llimit =" << llimit + << ", ulimit =" << ulimit << ", servo_state = " << ( servo_state ? "ON" : "OFF") << ", prev_angle = " << prev_angle[i] << std::endl; // fix joint angle - if ( m_robot->joint(i)->llimit > m_qRef.data[i] && prev_angle[i] > m_qRef.data[i] ) // ref < llimit and prev < ref -> OK + if ( llimit > m_qRef.data[i] && prev_angle[i] > m_qRef.data[i] ) // ref < llimit and prev < ref -> OK m_qRef.data[i] = prev_angle[i]; - if ( m_robot->joint(i)->ulimit < m_qRef.data[i] && prev_angle[i] < m_qRef.data[i] ) // ulimit < ref and ref < prev -> OK + if ( ulimit < m_qRef.data[i] && prev_angle[i] < m_qRef.data[i] ) // ulimit < ref and ref < prev -> OK m_qRef.data[i] = prev_angle[i]; m_servoState.data[i][0] |= (0x200 << OpenHRP::RobotHardwareService::SERVO_ALARM_SHIFT); position_limit_error = true; diff --git a/rtc/SoftErrorLimiter/SoftErrorLimiter.h b/rtc/SoftErrorLimiter/SoftErrorLimiter.h index 858f461e5f1..7bd9deda1cf 100644 --- a/rtc/SoftErrorLimiter/SoftErrorLimiter.h +++ b/rtc/SoftErrorLimiter/SoftErrorLimiter.h @@ -17,6 +17,7 @@ #include #include #include "HRPDataTypes.hh" +#include "../ImpedanceController/JointPathEx.h" // Service implementation headers // @@ -141,6 +142,7 @@ class SoftErrorLimiter private: boost::shared_ptr m_robot; + std::map joint_limit_tables; unsigned int m_debugLevel; int dummy, position_limit_error_beep_freq, soft_limit_error_beep_freq; };