JointLimitTable.cpp
Go to the documentation of this file.
00001 #include "JointLimitTable.h"
00002 #include <iostream>
00003 #include <iomanip>
00004 #include <limits.h>
00005 #include <float.h>
00006 
00007 double hrp::JointLimitTable::getInterpolatedLimitAngle (const double target_joint_angle, const bool is_llimit_angle) const
00008 {
00009     double target_angle = target_joint_angle * 180.0 / M_PI; // [rad]=>[deg]
00010     int int_target_angle = static_cast<int>(std::floor(target_angle));
00011     int target_range[2] = {int_target_angle, 1+int_target_angle};
00012     double self_joint_range[2];
00013     for (size_t i = 0; i < 2; i++) {
00014         size_t idx = std::min(std::max(target_llimit_angle, target_range[i]), target_ulimit_angle) - target_llimit_angle;
00015         self_joint_range[i] = (is_llimit_angle ? llimit_table(idx) : ulimit_table(idx));
00016     }
00017     double tmp_ratio = target_angle - int_target_angle;
00018     return (self_joint_range[0] * (1-tmp_ratio) + self_joint_range[1] * tmp_ratio) * M_PI / 180.0; // [deg]=>[rad]
00019 };
00020 
00021 void hrp::readJointLimitTableFromProperties (std::map<std::string, hrp::JointLimitTable>& joint_limit_tables,
00022                                              hrp::BodyPtr m_robot,
00023                                              const std::string& prop_string,
00024                                              const std::string& instance_name)
00025 {
00026     if (prop_string != "") {
00027         coil::vstring limit_tables = coil::split(prop_string, ":");
00028         size_t limit_table_size = 6; // self_joint_name:target_joint_name:target_min_angle:target_max_angle:min:max
00029         size_t num_limit_table = limit_tables.size() / limit_table_size;
00030         std::cerr << "[" << instance_name << "] Load joint limit table [" << num_limit_table << "]" << std::endl;
00031         for (size_t i = 0; i < num_limit_table; i++) {
00032             size_t start_idx = i*limit_table_size;
00033             int target_llimit_angle, target_ulimit_angle;
00034             coil::stringTo(target_llimit_angle, limit_tables[start_idx+2].c_str());
00035             coil::stringTo(target_ulimit_angle, limit_tables[start_idx+3].c_str());
00036             coil::vstring llimit_str_v = coil::split(limit_tables[start_idx+4], ",");
00037             coil::vstring ulimit_str_v = coil::split(limit_tables[start_idx+5], ",");
00038             hrp::dvector llimit_table(llimit_str_v.size()), ulimit_table(ulimit_str_v.size());
00039             int target_jointId = -1;
00040             for (size_t j = 0; j < m_robot->numJoints(); j++) {
00041                 if ( m_robot->joint(j)->name == limit_tables[start_idx+1]) target_jointId = m_robot->joint(j)->jointId;
00042             }
00043             if ( llimit_str_v.size() != ulimit_str_v.size() || target_jointId == -1 ) {
00044                 std::cerr << "[" << instance_name << "] " << limit_tables[start_idx+0] << ":" << limit_tables[start_idx+1] << " fail" << std::endl;
00045             } else {
00046                 std::cerr << "[" << instance_name << "] " << limit_tables[start_idx+0] << ":" << limit_tables[start_idx+1] << "(" << target_jointId << ")" << std::endl;
00047                 std::cerr << "[" << instance_name << "]   target_llimit_angle " << limit_tables[start_idx+2] << "[deg], target_ulimit_angle " << limit_tables[start_idx+3] << "[deg]" << std::endl;
00048                 std::cerr << "[" << instance_name << "]   llimit_table[deg] " << limit_tables[start_idx+4] << std::endl;
00049                 std::cerr << "[" << instance_name << "]   ulimit_table[deg] " << limit_tables[start_idx+5] << std::endl;
00050                 for (int j = 0; j < llimit_table.size(); j++) {
00051                     coil::stringTo(llimit_table[j], llimit_str_v[j].c_str());
00052                     coil::stringTo(ulimit_table[j], ulimit_str_v[j].c_str());
00053                 }
00054                 joint_limit_tables.insert(std::pair<std::string, hrp::JointLimitTable>(limit_tables[start_idx],
00055                                                                                        hrp::JointLimitTable(target_jointId, target_llimit_angle, target_ulimit_angle, llimit_table, ulimit_table)));
00056             }
00057         }
00058     } else {
00059         std::cerr << "[" << instance_name << "] Do not load joint limit table" << std::endl;
00060     }
00061 };


hrpsys
Author(s): AIST, Fumio Kanehiro
autogenerated on Wed Sep 6 2017 02:35:55