Add joint limit table and currently use el

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<int>(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<std::string, hrp::JointLimitTable>& 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<std::string, hrp::JointLimitTable>(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;
+ }
+};

rtc/ImpedanceController/JointPathEx.h

@@ -3,6 +3,8 @@
 #include <hrpModel/Body.h>
 #include <hrpModel/Link.h>
 #include <hrpModel/JointPath.h>
+#include <cmath>
+#include <coil/stringutil.h>
 
 // hrplib/hrpUtil/MatrixSolvers.h
 namespace hrp {
@@ -35,6 +37,36 @@ namespace hrp {
 
  typedef boost::shared_ptr<JointPathEx> 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<std::string, hrp::JointLimitTable>& joint_mm_tables,
+ hrp::BodyPtr m_robot,
+ const std::string& prop_string,
+ const std::string& instance_name);
 };
 
 #include <iomanip>

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})

rtc/SoftErrorLimiter/SoftErrorLimiter.cpp

@@ -133,6 +133,9 @@ RTC::ReturnCode_t SoftErrorLimiter::onInitialize()
  soft_limit_error_beep_freq = static_cast<int>(1.0/(4.0*dt)); // soft limit error => 4 times / 1[s]
  position_limit_error_beep_freq = static_cast<int>(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<std::string, hrp::JointLimitTable>::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;

rtc/SoftErrorLimiter/SoftErrorLimiter.h

@@ -17,6 +17,7 @@
 #include <rtm/DataOutPort.h>
 #include <rtm/idl/BasicDataTypeSkel.h>
 #include "HRPDataTypes.hh"
+#include "../ImpedanceController/JointPathEx.h"
 
 // Service implementation headers
 // <rtc-template block="service_impl_h">
@@ -141,6 +142,7 @@ class SoftErrorLimiter
 
  private:
  boost::shared_ptr<robot> m_robot;
+ std::map<std::string, hrp::JointLimitTable> joint_limit_tables;
  unsigned int m_debugLevel;
  int dummy, position_limit_error_beep_freq, soft_limit_error_beep_freq;
 };