SimpleFullbodyInverseKinematicsSolver.h
Go to the documentation of this file.
00001 #ifndef SimpleFullbodyInverseKinematicsSolver_H
00002 #define SimpleFullbodyInverseKinematicsSolver_H
00003 
00004 #include <hrpModel/Body.h>
00005 #include "../ImpedanceController/JointPathEx.h"
00006 #include "../ImpedanceController/RatsMatrix.h"
00007 
00008 // Class for Simple Fullbody Inverse Kinematics
00009 //   Input : target root pos and rot, target COG, target joint angles, target EE coords
00010 //   Output : joint angles
00011 //   Algorithm : Limb IK + move base
00012 class SimpleFullbodyInverseKinematicsSolver
00013 {
00014 private:
00015     // Robot model for IK
00016     hrp::BodyPtr m_robot;
00017     // Org (current) joint angles before IK
00018     hrp::dvector qorg;
00019     // IK fail checking
00020     int ik_error_debug_print_freq;
00021     std::string print_str;
00022     bool has_ik_failed;
00023 public:
00024     // IK parameter for each limb
00025     struct IKparam {
00026         // IK target EE coords
00027         hrp::Vector3 target_p0;
00028         hrp::Matrix33 target_r0;
00029         // EE offset, EE link
00030         hrp::Vector3 localPos;
00031         hrp::Matrix33 localR;
00032         hrp::Link* target_link;
00033         // IK solver and parameter
00034         hrp::JointPathExPtr manip;
00035         double avoid_gain, reference_gain;
00036         // IK fail checking
00037         size_t pos_ik_error_count, rot_ik_error_count;
00038         // Solve ik or not
00039         bool is_ik_enable;
00040         // Name of parent link in limb
00041         std::string parent_name;
00042         // Limb length
00043         double max_limb_length, limb_length_margin;
00044         IKparam ()
00045             : avoid_gain(0.001), reference_gain(0.01),
00046               pos_ik_error_count(0), rot_ik_error_count(0),
00047               limb_length_margin(0.02)
00048         {
00049         };
00050     };
00051     std::map<std::string, IKparam> ikp;
00052     // Used for ref joint angles overwrite before IK
00053     std::vector<int> overwrite_ref_ja_index_vec;
00054     // IK targets and current?
00055     hrp::dvector qrefv;
00056     hrp::Vector3 target_root_p;
00057     hrp::Matrix33 target_root_R;
00058     hrp::Vector3 current_root_p;
00059     hrp::Matrix33 current_root_R;
00060     // IK params
00061     double move_base_gain, ratio_for_vel;
00062     // For IK fail checking
00063     double pos_ik_thre, rot_ik_thre;
00064     // For limb stretch avoidance
00065     double d_root_height, limb_stretch_avoidance_time_const, limb_stretch_avoidance_vlimit[2], m_dt;
00066     bool use_limb_stretch_avoidance;
00067 
00068     SimpleFullbodyInverseKinematicsSolver (hrp::BodyPtr& _robot, const std::string& _print_str, const double _dt)
00069         : m_robot(_robot),
00070           ik_error_debug_print_freq(static_cast<int>(0.2/_dt)), // once per 0.2 [s]
00071           has_ik_failed(false),
00072           overwrite_ref_ja_index_vec(), ikp(),
00073           move_base_gain(0.8), ratio_for_vel(1.0),
00074           pos_ik_thre(0.5*1e-3), // [m]
00075           rot_ik_thre((1e-2)*M_PI/180.0), // [rad]
00076           print_str(_print_str), m_dt(_dt),
00077           use_limb_stretch_avoidance(false), limb_stretch_avoidance_time_const(1.5)
00078     {
00079         qorg.resize(m_robot->numJoints());
00080         qrefv.resize(m_robot->numJoints());
00081         limb_stretch_avoidance_vlimit[0] = -1000 * 1e-3 * _dt; // lower limit
00082         limb_stretch_avoidance_vlimit[1] = 50 * 1e-3 * _dt; // upper limit
00083     };
00084     ~SimpleFullbodyInverseKinematicsSolver () {};
00085 
00086     void initializeInterlockingJoints (std::vector<std::pair<hrp::Link*, hrp::Link*> > & interlocking_joints)
00087     {
00088         for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00089             std::cerr << "[" << print_str << "] Interlocking Joints for [" << it->first << "]" << std::endl;
00090             it->second.manip->setInterlockingJointPairIndices(interlocking_joints, print_str);
00091         }
00092     };
00093     void storeCurrentParameters()
00094     {
00095         current_root_p = m_robot->rootLink()->p;
00096         current_root_R = m_robot->rootLink()->R;
00097         for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00098             qorg[i] = m_robot->joint(i)->q;
00099         }
00100     };
00101     void revertRobotStateToCurrent ()
00102     {
00103         for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00104             if (it->second.is_ik_enable) {
00105                 for ( unsigned int j = 0; j < it->second.manip->numJoints(); j++ ){
00106                     int i = it->second.manip->joint(j)->jointId;
00107                     m_robot->joint(i)->q = qorg[i];
00108                 }
00109             }
00110         }
00111         m_robot->rootLink()->p = current_root_p;
00112         m_robot->rootLink()->R = current_root_R;
00113         m_robot->calcForwardKinematics();
00114     };
00115     void setReferenceJointAngles ()
00116     {
00117         for ( unsigned int i = 0; i < m_robot->numJoints(); i++ ){
00118             qrefv[i] = m_robot->joint(i)->q;
00119         }
00120     };
00121     // Solve fullbody IK using limb IK
00122     void solveFullbodyIK (const hrp::Vector3& _dif_cog, const bool is_transition)
00123     {
00124         hrp::Vector3 dif_cog(ratio_for_vel*_dif_cog);
00125         dif_cog(2) = m_robot->rootLink()->p(2) - target_root_p(2);
00126         m_robot->rootLink()->p = m_robot->rootLink()->p + -1 * move_base_gain * dif_cog;
00127         m_robot->rootLink()->R = target_root_R;
00128         // Avoid limb stretch
00129         {
00130           std::vector<hrp::Vector3> tmp_p;
00131           std::vector<std::string> tmp_name;
00132           for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00133             if (it->first.find("leg") != std::string::npos) {
00134               tmp_p.push_back(it->second.target_p0);
00135               tmp_name.push_back(it->first);
00136             }
00137           }
00138           limbStretchAvoidanceControl(tmp_p, tmp_name);
00139         }
00140         // Overwrite by ref joint angle
00141         for (size_t i = 0; i < overwrite_ref_ja_index_vec.size(); i++) {
00142             m_robot->joint(overwrite_ref_ja_index_vec[i])->q = qrefv[overwrite_ref_ja_index_vec[i]];
00143         }
00144         m_robot->calcForwardKinematics();
00145         for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00146             if (it->second.is_ik_enable) solveLimbIK (it->second, it->first, ratio_for_vel, is_transition);
00147         }
00148     };
00149     // Solve limb IK
00150     bool solveLimbIK (IKparam& param, const std::string& limb_name, const double ratio_for_vel, const bool is_transition)
00151     {
00152         param.manip->calcInverseKinematics2Loop(param.target_p0, param.target_r0, 1.0, param.avoid_gain, param.reference_gain, &qrefv, ratio_for_vel,
00153                                                 param.localPos, param.localR);
00154         checkIKTracking(param, limb_name, is_transition);
00155         return true;
00156     }
00157     // IK fail check
00158     void checkIKTracking (IKparam& param, const std::string& limb_name, const bool is_transition)
00159     {
00160         hrp::Vector3 vel_p, vel_r;
00161         vel_p = param.target_p0 - (param.target_link->p + param.target_link->R * param.localPos);
00162         rats::difference_rotation(vel_r, (param.target_link->R * param.localR), param.target_r0);
00163         if (vel_p.norm() > pos_ik_thre && is_transition) {
00164             if (param.pos_ik_error_count % ik_error_debug_print_freq == 0) {
00165                 std::cerr << "[" << print_str << "] Too large IK error in " << limb_name << " (vel_p) = [" << vel_p(0) << " " << vel_p(1) << " " << vel_p(2) << "][m], count = " << param.pos_ik_error_count << std::endl;
00166             }
00167             param.pos_ik_error_count++;
00168             has_ik_failed = true;
00169         } else {
00170             param.pos_ik_error_count = 0;
00171         }
00172         if (vel_r.norm() > rot_ik_thre && is_transition) {
00173             if (param.rot_ik_error_count % ik_error_debug_print_freq == 0) {
00174                 std::cerr << "[" << print_str << "] Too large IK error in " << limb_name << " (vel_r) = [" << vel_r(0) << " " << vel_r(1) << " " << vel_r(2) << "][rad], count = " << param.rot_ik_error_count << std::endl;
00175             }
00176             param.rot_ik_error_count++;
00177             has_ik_failed = true;
00178         } else {
00179             param.rot_ik_error_count = 0;
00180         }
00181     };
00182     // Reset IK fail params
00183     void resetIKFailParam() {
00184         has_ik_failed = false;
00185         for ( std::map<std::string, IKparam>::iterator it = ikp.begin(); it != ikp.end(); it++ ) {
00186             it->second.pos_ik_error_count = it->second.rot_ik_error_count = 0;
00187         }
00188     }
00189     // Get IKparam
00190     void getIKParam (std::vector<std::string>& ee_vec, _CORBA_Unbounded_Sequence<OpenHRP::AutoBalancerService::IKLimbParameters>& ik_limb_parameters)
00191     {
00192         ik_limb_parameters.length(ee_vec.size());
00193         for (size_t i = 0; i < ee_vec.size(); i++) {
00194             IKparam& param = ikp[ee_vec[i]];
00195             OpenHRP::AutoBalancerService::IKLimbParameters& ilp = ik_limb_parameters[i];
00196             ilp.ik_optional_weight_vector.length(param.manip->numJoints());
00197             std::vector<double> ov;
00198             ov.resize(param.manip->numJoints());
00199             param.manip->getOptionalWeightVector(ov);
00200             for (size_t j = 0; j < param.manip->numJoints(); j++) {
00201                 ilp.ik_optional_weight_vector[j] = ov[j];
00202             }
00203             ilp.sr_gain = param.manip->getSRGain();
00204             ilp.avoid_gain = param.avoid_gain;
00205             ilp.reference_gain = param.reference_gain;
00206             ilp.manipulability_limit = param.manip->getManipulabilityLimit();
00207         }
00208     };
00209     // Set IKparam
00210     void setIKParam (std::vector<std::string>& ee_vec, const _CORBA_Unbounded_Sequence<OpenHRP::AutoBalancerService::IKLimbParameters>& ik_limb_parameters)
00211     {
00212         std::cerr << "[" << print_str << "]  IK limb parameters" << std::endl;
00213         bool is_ik_limb_parameter_valid_length = true;
00214         if (ik_limb_parameters.length() != ee_vec.size()) {
00215             is_ik_limb_parameter_valid_length = false;
00216             std::cerr << "[" << print_str << "]   ik_limb_parameters invalid length! Cannot be set. (input = " << ik_limb_parameters.length() << ", desired = " << ee_vec.size() << ")" << std::endl;
00217         } else {
00218             for (size_t i = 0; i < ee_vec.size(); i++) {
00219                 if (ikp[ee_vec[i]].manip->numJoints() != ik_limb_parameters[i].ik_optional_weight_vector.length())
00220                     is_ik_limb_parameter_valid_length = false;
00221             }
00222             if (is_ik_limb_parameter_valid_length) {
00223                 for (size_t i = 0; i < ee_vec.size(); i++) {
00224                     IKparam& param = ikp[ee_vec[i]];
00225                     const OpenHRP::AutoBalancerService::IKLimbParameters& ilp = ik_limb_parameters[i];
00226                     std::vector<double> ov;
00227                     ov.resize(param.manip->numJoints());
00228                     for (size_t j = 0; j < param.manip->numJoints(); j++) {
00229                         ov[j] = ilp.ik_optional_weight_vector[j];
00230                     }
00231                     param.manip->setOptionalWeightVector(ov);
00232                     param.manip->setSRGain(ilp.sr_gain);
00233                     param.avoid_gain = ilp.avoid_gain;
00234                     param.reference_gain = ilp.reference_gain;
00235                     param.manip->setManipulabilityLimit(ilp.manipulability_limit);
00236                 }
00237             } else {
00238                 std::cerr << "[" << print_str << "]   ik_optional_weight_vector invalid length! Cannot be set. (input = [";
00239                 for (size_t i = 0; i < ee_vec.size(); i++) {
00240                     std::cerr << ik_limb_parameters[i].ik_optional_weight_vector.length() << ", ";
00241                 }
00242                 std::cerr << "], desired = [";
00243                 for (size_t i = 0; i < ee_vec.size(); i++) {
00244                     std::cerr << ikp[ee_vec[i]].manip->numJoints() << ", ";
00245                 }
00246                 std::cerr << "])" << std::endl;
00247             }
00248         }
00249         if (is_ik_limb_parameter_valid_length) {
00250             printIKparam(ee_vec);
00251         }
00252     };
00253     // Avoid limb stretch
00254     void limbStretchAvoidanceControl (const std::vector<hrp::Vector3>& target_p, const std::vector<std::string>& target_name)
00255     {
00256       m_robot->calcForwardKinematics();
00257       double tmp_d_root_height = 0.0, prev_d_root_height = d_root_height;
00258       if (use_limb_stretch_avoidance) {
00259         for (size_t i = 0; i < target_p.size(); i++) {
00260           // Check whether inside limb length limitation
00261           hrp::Link* parent_link = m_robot->link(ikp[target_name[i]].parent_name);
00262           hrp::Vector3 rel_target_p = target_p[i] - parent_link->p; // position from parent to target link (world frame)
00263           double limb_length_limitation = ikp[target_name[i]].max_limb_length - ikp[target_name[i]].limb_length_margin;
00264           double tmp = limb_length_limitation * limb_length_limitation - rel_target_p(0) * rel_target_p(0) - rel_target_p(1) * rel_target_p(1);
00265           if (rel_target_p.norm() > limb_length_limitation && tmp >= 0) {
00266             tmp_d_root_height = std::min(tmp_d_root_height, rel_target_p(2) + std::sqrt(tmp));
00267           }
00268         }
00269         // Change root link height depending on limb length
00270         d_root_height = tmp_d_root_height == 0.0 ? (- 1/limb_stretch_avoidance_time_const * d_root_height * m_dt + d_root_height) : tmp_d_root_height;
00271       } else {
00272         d_root_height = - 1/limb_stretch_avoidance_time_const * d_root_height * m_dt + d_root_height;
00273       }
00274       d_root_height = vlimit(d_root_height, prev_d_root_height + limb_stretch_avoidance_vlimit[0], prev_d_root_height + limb_stretch_avoidance_vlimit[1]);
00275       m_robot->rootLink()->p(2) += d_root_height;
00276     };
00277     double vlimit(const double value, const double llimit_value, const double ulimit_value)
00278     {
00279       if (value > ulimit_value) {
00280         return ulimit_value;
00281       } else if (value < llimit_value) {
00282         return llimit_value;
00283       }
00284       return value;
00285     };
00286     // Set parameter
00287     void printParam () const
00288     {
00289         std::cerr << "[" << print_str << "]   move_base_gain = " << move_base_gain << std::endl;
00290         std::cerr << "[" << print_str << "]   pos_ik_thre = " << pos_ik_thre << "[m], rot_ik_thre = " << rot_ik_thre << "[rad]" << std::endl;
00291     };
00292     // Set IKparam
00293     void printIKparam (std::vector<std::string>& ee_vec)
00294     {
00295       std::cerr << "[" << print_str << "]   ik_optional_weight_vectors = ";
00296       for (size_t i = 0; i < ee_vec.size(); i++) {
00297           IKparam& param = ikp[ee_vec[i]];
00298           std::vector<double> ov;
00299           ov.resize(param.manip->numJoints());
00300           param.manip->getOptionalWeightVector(ov);
00301           std::cerr << "[";
00302           for (size_t j = 0; j < param.manip->numJoints(); j++) {
00303               std::cerr << ov[j] << " ";
00304           }
00305           std::cerr << "]";
00306       }
00307       std::cerr << std::endl;
00308       std::cerr << "[" << print_str << "]   sr_gains = [";
00309       for (size_t i = 0; i < ee_vec.size(); i++) {
00310           std::cerr << ikp[ee_vec[i]].manip->getSRGain() << ", ";
00311       }
00312       std::cerr << "]" << std::endl;
00313       std::cerr << "[" << print_str << "]   avoid_gains = [";
00314       for (size_t i = 0; i < ee_vec.size(); i++) {
00315           std::cerr << ikp[ee_vec[i]].avoid_gain << ", ";
00316       }
00317       std::cerr << "]" << std::endl;
00318       std::cerr << "[" << print_str << "]   reference_gains = [";
00319       for (size_t i = 0; i < ee_vec.size(); i++) {
00320           std::cerr << ikp[ee_vec[i]].reference_gain << ", ";
00321       }
00322       std::cerr << "]" << std::endl;
00323       std::cerr << "[" << print_str << "]   manipulability_limits = [";
00324       for (size_t i = 0; i < ee_vec.size(); i++) {
00325           std::cerr << ikp[ee_vec[i]].manip->getManipulabilityLimit() << ", ";
00326       }
00327       std::cerr << "]" << std::endl;
00328     };
00329     hrp::Vector3 getEndEffectorPos(const std::string& limb_name){
00330       return ikp[limb_name].target_link->p + ikp[limb_name].target_link->R * ikp[limb_name].localPos;
00331     }
00332     hrp::Matrix33 getEndEffectorRot(const std::string& limb_name){
00333       return ikp[limb_name].target_link->R * ikp[limb_name].localR;
00334     }
00335 };
00336 
00337 #endif // SimpleFullbodyInverseKinematicsSolver_H


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