逆運動学ライブラリとX7のサンプルを追加 (#10)

* FKのテストをlink1まで追加

* FKのテストを実装

* cherry-pick find_routeを実装

* cherry-pick find_routeに例外処理を追加

* cherry-pick 型エイリアスを作成

* get_q_listを実装

* calc_errorを実装

* calc_basic_jacobianを実装(テストは未実施)

* cherry-pick set_q_listを実装

* calc_errorをcalc_error_Rにリネーム

* calc_error_pを実装

* calc_errorを実装

* Inverse Kinematics LMを実装。テスト方法について検討中

* CRANE-X7のリンクパラメータを用いたテストを追加

* 失敗するテストをコメントアウト(後ほど修正する)

* linkクラスにdxl_idを追加

* linkクラスにset_q_within_limitを追加

* set_q_listにwithin_limitオプションを追加

* IK演算時に関節位置制限を設ける。IKのテストに失敗しているため要調査

* rotation_to_omegaを実装

* calc_error_Rの実装を修正。

* IKを修正。後面のIKに失敗する、上面の特異姿勢IKに失敗する問題が残っている

* 関節可動範囲を取得できるようにhardwareを更新

* x7_inverse_kinematicsサンプルを追加

* set_q_listのwithin_limitオプションのテストを追加

* サンプルとREADMEの更新

* READMEのlinkクラスを更新

* READMEの目次を更新

* fix typo

* Update rt_manipulators_lib/src/kinematics_utils.cpp

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

* Update rt_manipulators_lib/src/kinematics_utils.cpp

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

* rotation_to_omegaのコメントを修正

* rotation_to_omegaをrotation_to_axis_angle_representationに変更

* Update rt_manipulators_lib/src/kinematics_utils.cpp

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

* Update rt_manipulators_lib/src/kinematics_utils.cpp

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

* inverse_kinematics_LMのパラメータを定数にした

* IKのリファクタリング

* IKのomegaを0.01に変更

* IKについてREADMEの文言を更新

Co-authored-by: kenjiinukai <36922159+kenjiinukai@users.noreply.github.com>

Author: Shota Aoki

rt_manipulators_lib/include/hardware.hpp

@@ -60,6 +60,8 @@ class Hardware {
   bool get_temperature(const uint8_t id, int8_t& temperature);
   bool get_temperature(const std::string& joint_name, int8_t& temperature);
   bool get_temperatures(const std::string& group_name, std::vector<int8_t>& temperatures);
+  bool get_max_position_limit(const uint8_t & id, double & max_position_limit);
+  bool get_min_position_limit(const uint8_t & id, double & min_position_limit);
   bool set_position(const uint8_t id, const double position);
   bool set_position(const std::string& joint_name, const double position);
   bool set_positions(const std::string& group_name, std::vector<double>& positions);

rt_manipulators_lib/include/hardware_joints.hpp

@@ -64,6 +64,8 @@ class Joints{
   bool get_temperature(const dxl_id_t & id, temperature_t & temperature);
   bool get_temperature(const joint_name_t & joint_name, temperature_t & temperature);
   bool get_temperatures(const group_name_t & group_name, std::vector<temperature_t>& temperatures);
+  bool get_max_position_limit(const dxl_id_t & id, position_t & max_position_limit);
+  bool get_min_position_limit(const dxl_id_t & id, position_t & min_position_limit);
   bool set_position(const dxl_id_t & id, const position_t & position);
   bool set_position(const joint_name_t & joint_name, const position_t & position);
   bool set_positions(const group_name_t & group_name, const std::vector<position_t> & positions);

rt_manipulators_lib/include/kinematics.hpp

@@ -15,14 +15,19 @@
 #ifndef RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_HPP_
 #define RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_HPP_
 
+#include <eigen3/Eigen/Dense>
 #include <vector>
 
+#include "kinematics_utils.hpp"
 #include "link.hpp"
 
 namespace kinematics {
 
 void forward_kinematics(std::vector<manipulators_link::Link> & links, const int & start_id);
-
+bool inverse_kinematics_LM(
+  const kinematics_utils::links_t & links, const kinematics_utils::link_id_t & target_id,
+  const Eigen::Vector3d & target_p, const Eigen::Matrix3d & target_R,
+  kinematics_utils::q_list_t & result_q_list);
 }  // namespace kinematics
 
 #endif  // RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_HPP_

rt_manipulators_lib/include/kinematics_utils.hpp

@@ -16,20 +16,35 @@
 #define RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_UTILS_HPP_
 
 #include <eigen3/Eigen/Dense>
+#include <map>
 #include <string>
 #include <vector>
 
 #include "link.hpp"
 
 namespace kinematics_utils {
 
+using links_t = std::vector<manipulators_link::Link>;
+using link_id_t = unsigned int;
+using q_list_t = std::map<link_id_t, double>;
+
 std::vector<manipulators_link::Link> parse_link_config_file(const std::string & file_path);
 void print_links(const std::vector<manipulators_link::Link> & links, const int & start_id);
 Eigen::Matrix3d skew_symmetric_matrix_for_cross_product(const Eigen::Vector3d & v);
 Eigen::Matrix3d rodrigues(const Eigen::Vector3d & a, const double theta);
 Eigen::Vector3d rotation_to_euler_ZYX(const Eigen::Matrix3d & mat);
 Eigen::Matrix3d rotation_from_euler_ZYX(
   const double & z, const double & y, const double & x);
+Eigen::Vector3d rotation_to_axis_angle_representation(const Eigen::Matrix3d & mat);
+std::vector<link_id_t> find_route(const links_t & links, const link_id_t & target_id);
+q_list_t get_q_list(const links_t & links, const std::vector<link_id_t> & id_list);
+bool set_q_list(links_t & links, const q_list_t & q_list, const bool & within_limit = false);
+Eigen::Vector3d calc_error_R(const Eigen::Matrix3d & target, const Eigen::Matrix3d & current);
+Eigen::Vector3d calc_error_p(const Eigen::Vector3d & target, const Eigen::Vector3d & current);
+Eigen::VectorXd calc_error(
+  const Eigen::Vector3d & target_p, const Eigen::Matrix3d & target_R,
+  const manipulators_link::Link & current_link);
+Eigen::MatrixXd calc_basic_jacobian(const links_t & links, const link_id_t & target_id);
 
 }  // namespace kinematics_utils
 

rt_manipulators_lib/include/link.hpp

@@ -16,6 +16,8 @@
 #define RT_MANIPULATORS_LIB_INCLUDE_LINK_HPP_
 
 #include <eigen3/Eigen/Dense>
+
+#include <algorithm>
 #include <string>
 
 namespace manipulators_link {
@@ -25,7 +27,8 @@ namespace manipulators_link {
 class Link{
  public:
   Link():
-    name("ダミーリンク"), sibling(0), child(0), parent(0), q(0), dq(0), ddq(0), m(0) {
+    name("ダミーリンク"), sibling(0), child(0), parent(0), q(0), dq(0), ddq(0), m(0), dxl_id(0),
+    min_q(0), max_q(0) {
     // 0ベクトル、単位行列で初期化
     p.setZero();
     R.setIdentity();
@@ -52,6 +55,10 @@ class Link{
   double m;  // 質量
   Eigen::Vector3d c;  // 自リンクに対する重心位置
   Eigen::Matrix3d I;  // 自リンクに対する慣性テンソル
+  int dxl_id;  // 対応するDynamixelのID
+  double min_q;  // 関節位置の下限値
+  double max_q;  // 関節位置の上限値
+  void set_q_within_limit(const double & desired_q) { q = std::clamp(desired_q, min_q, max_q); }
 };
 
 }  // namespace manipulators_link

rt_manipulators_lib/src/hardware.cpp

@@ -448,6 +448,14 @@ bool Hardware::get_temperatures(const std::string& group_name, std::vector<int8_
   return joints_.get_temperatures(group_name, temperatures);
 }
 
+bool Hardware::get_max_position_limit(const uint8_t & id, double & max_position_limit) {
+  return joints_.get_max_position_limit(id, max_position_limit);
+}
+
+bool Hardware::get_min_position_limit(const uint8_t & id, double & min_position_limit) {
+  return joints_.get_min_position_limit(id, min_position_limit);
+}
+
 bool Hardware::set_position(const uint8_t id, const double position) {
   return joints_.set_position(id, position);
 }

rt_manipulators_lib/src/hardware_joints.cpp

@@ -211,6 +211,24 @@ bool Joints::get_temperatures(
   return true;
 }
 
+bool Joints::get_max_position_limit(const dxl_id_t & id, position_t & max_position_limit) {
+  if (!has_joint(id)) {
+    std::cerr << "ID:" << std::to_string(id) << "のジョイントは存在しません." << std::endl;
+    return false;
+  }
+  max_position_limit = joint(id)->max_position_limit();
+  return true;
+}
+
+bool Joints::get_min_position_limit(const dxl_id_t & id, position_t & min_position_limit) {
+  if (!has_joint(id)) {
+    std::cerr << "ID:" << std::to_string(id) << "のジョイントは存在しません." << std::endl;
+    return false;
+  }
+  min_position_limit = joint(id)->min_position_limit();
+  return true;
+}
+
 bool Joints::set_position(const dxl_id_t & id, const position_t & position) {
   if (!has_joint(id)) {
     std::cerr << "ID:" << std::to_string(id) << "のジョイントは存在しません." << std::endl;

rt_manipulators_lib/src/kinematics.cpp

@@ -44,4 +44,73 @@ void forward_kinematics(std::vector<manipulators_link::Link> & links, const int
   forward_kinematics(links, links[start_id].child);
 }
 
+bool inverse_kinematics_LM(
+  const kinematics_utils::links_t & links, const kinematics_utils::link_id_t & target_id,
+  const Eigen::Vector3d & target_p, const Eigen::Matrix3d & target_R,
+  kinematics_utils::q_list_t & result_q_list) {
+  // 参考: https://www.jstage.jst.go.jp/article/jrsj/29/3/29_3_269/_pdf/-char/ja
+  auto calc_links = links;
+  auto route = kinematics_utils::find_route(links, target_id);
+  auto q_list = kinematics_utils::get_q_list(links, route);
+
+  const double we_pos = 1 / 0.1;  // 位置成分の重み  (代表長さの逆数)
+  const double we_ang = 1 / (2*M_PI);  // 姿勢成分の重み
+  const int num_of_iterations = 100;  // qを更新するための反復回数
+  const double omega = 0.01;  // 0.1 ~ 0.001 * 代表リンク長の2乗
+  const double initial_q_value = 0;  // 初期値を0にすると特異姿勢になるため、適当な角度を設定する
+  const double error_tolerance = 1.0E-6;  // 誤差の許容量。誤差がこれより小さければ反復計算を終える
+
+  // We : 拘束条件に対する重み行列
+  auto We_vec = Eigen::VectorXd(6);
+  We_vec << we_pos, we_pos, we_pos, we_ang, we_ang, we_ang;
+  Eigen::MatrixXd We = We_vec.asDiagonal();
+
+  // qを0にリセットする
+  for (auto q_i = q_list.begin(); q_i != q_list.end(); ++q_i) {
+    q_i->second = initial_q_value;
+  }
+
+  // qをセットしてリンク情報を更新する
+  bool set_q_within_limit = true;
+  kinematics_utils::set_q_list(calc_links, q_list, set_q_within_limit);
+  forward_kinematics(calc_links, 1);
+
+  for (int n=0; n < num_of_iterations; n++) {
+    // 基礎ヤコビ行列を計算
+    auto J = kinematics_utils::calc_basic_jacobian(calc_links, target_id);
+
+      // 位置・姿勢の誤差を計算
+    auto error = kinematics_utils::calc_error(target_p, target_R, calc_links[target_id]);
+
+    // 誤差が小さければ終了
+    if (error.norm() < error_tolerance) {
+      result_q_list = q_list;
+      return true;
+    }
+
+    // 減衰因子の計算
+    auto E = 0.5 * error.transpose() * We * error;
+    auto Wn_ = omega * Eigen::MatrixXd::Identity(q_list.size(), q_list.size());
+    auto Wn = E * Eigen::MatrixXd::Identity(q_list.size(), q_list.size()) + Wn_;
+
+    // LM法の更新則
+    auto H = J.transpose() * We * J + Wn;
+    auto dq = H.inverse() * J.transpose() * We * error;  // H-1 * g
+
+    // dqをqに加算
+    int dq_i = 0;
+    for (auto q_i = q_list.begin(); q_i != q_list.end(); ++q_i) {
+      q_i->second += dq(dq_i);
+      dq_i++;
+    }
+
+    // qをセットしてリンク情報を更新する
+    kinematics_utils::set_q_list(calc_links, q_list, set_q_within_limit);
+    forward_kinematics(calc_links, 1);
+  }
+
+  result_q_list = q_list;
+  return false;
+}
+
 }  // namespace kinematics

rt_manipulators_lib/src/kinematics_utils.cpp

@@ -12,8 +12,10 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
+#include <algorithm>
 #include <fstream>
 #include <iostream>
+#include <limits>
 #include <string>
 
 #include "kinematics_utils.hpp"
@@ -57,6 +59,7 @@ std::vector<manipulators_link::Link> parse_link_config_file(const std::string &
   const int COL_INERTIA_YZ = 22;
   const int COL_INERTIA_ZZ = 23;
   const int COL_AXIS_OF_ROTATION = 29;
+  const int COL_DXL_ID = 32;
   const double MM_TO_METERS = 1e-3;
   const double MM2_TO_METERS2 = 1e-9;
   const double G_TO_KG = 1e-3;
@@ -157,6 +160,11 @@ std::vector<manipulators_link::Link> parse_link_config_file(const std::string &
     // link.c = rot * link.c;
     // link.I = rot * link.I * rot.transpose();
 
+    try {
+      link.dxl_id = std::stoi(str_vec[COL_DXL_ID]);
+    } catch (...) {
+    }
+
     links.push_back(link);
   }
 
@@ -228,4 +236,117 @@ Eigen::Matrix3d rotation_from_euler_ZYX(
   return q.matrix();
 }
 
+Eigen::Vector3d rotation_to_axis_angle_representation(const Eigen::Matrix3d & mat) {
+  // 姿勢行列から等価角軸ベクトルへの変換
+  // 参考：https://www.jstage.jst.go.jp/article/jrsj/29/3/29_3_269/_pdf/-char/ja
+  auto l = Eigen::Vector3d(
+    mat(2, 1) - mat(1, 2),
+    mat(0, 2) - mat(2, 0),
+    mat(1, 0) - mat(0, 1));
+  auto l_norm = l.norm();
+
+  if (l_norm != 0) {
+    return (std::atan2(l_norm, mat.trace() - 1) / l_norm) * l;
+  } else if (mat(0, 0) > 0 && mat(1, 1) > 0 && mat(2, 2) > 0) {
+    // 行列の対角成分がすべて1のとき、ゼロベクトルを返す
+    return Eigen::Vector3d::Zero();
+  }
+  // 行列の対角成分が(1, -1, -1), (-1, 1, -1), (-1, -1, 1)のときの処理
+  return M_PI_2 * Eigen::Vector3d(mat(0, 0) + 1, mat(1, 1) + 1, mat(2, 2) + 1);
+}
+
+std::vector<link_id_t> find_route(const links_t & links, const link_id_t & target_id) {
+  // 目標リンク(target_id)までの経路を抽出する
+  // 返り値のベクトルの末尾がtarget_idになる
+  std::vector<link_id_t> id_list;
+
+  if (target_id <= 1 || target_id >= links.size()) {
+    std::cerr << __func__ << ": 目標リンクIDには1より大きく、"
+              << "linksのサイズより小さい数値をセットしてください" << std::endl;
+    return id_list;
+  }
+
+  id_list.push_back(target_id);
+
+  auto parent_id = links[target_id].parent;
+  // ベースリンク(ID = 1)は含めない
+  while (parent_id != 1) {
+    id_list.push_back(parent_id);
+    parent_id = links[parent_id].parent;
+  }
+
+  // 末尾をtarget_idにするため、ベクトルの並びを逆順にする
+  std::reverse(id_list.begin(), id_list.end());
+  return id_list;
+}
+
+q_list_t get_q_list(const links_t & links, const std::vector<link_id_t> & id_list) {
+  // リンク構成から指定されたIDの関節位置qを抽出する
+  q_list_t q_list;
+  for (auto target_id : id_list) {
+    if (target_id < links.size()) {
+      q_list[target_id] = links[target_id].q;
+    } else {
+      std::cerr << __func__ << ": 無効なIDです:" << target_id << std::endl;
+    }
+  }
+  return q_list;
+}
+
+bool set_q_list(links_t & links, const q_list_t & q_list, const bool & within_limit) {
+  // リンク構成の指定されたIDに関節位置qを書き込む
+  bool result = true;
+  for (const auto & [target_id, q_value] : q_list) {
+    if (target_id < links.size()) {
+      if (within_limit) {
+        links[target_id].set_q_within_limit(q_value);
+      } else {
+        links[target_id].q = q_value;
+      }
+    } else {
+      std::cerr << __func__ << ": 無効なIDです:" << target_id << std::endl;
+      result = false;
+    }
+  }
+  return result;
+}
+
+Eigen::Vector3d calc_error_R(const Eigen::Matrix3d & target, const Eigen::Matrix3d & current) {
+  // 回転行列の差を求める
+  return rotation_to_axis_angle_representation(target * current.transpose());
+}
+
+Eigen::Vector3d calc_error_p(const Eigen::Vector3d & target, const Eigen::Vector3d & current) {
+  // 位置の差を求める
+  return target - current;
+}
+
+Eigen::VectorXd calc_error(
+  const Eigen::Vector3d & target_p, const Eigen::Matrix3d & target_R,
+  const manipulators_link::Link & current_link) {
+  // 目標位置・姿勢と、リンクの現在位置・姿勢との差を求める
+  auto error_p = calc_error_p(target_p, current_link.p);
+  auto error_r = calc_error_R(target_R, current_link.R);
+
+  Eigen::VectorXd error(6);
+  error << error_p, error_r;
+  return error;
+}
+
+Eigen::MatrixXd calc_basic_jacobian(const links_t & links, const link_id_t & target_id) {
+  // 基礎ヤコビ行列を求める(各軸は回転関節)
+  auto route = find_route(links, target_id);
+  auto target_p = links[target_id].p;
+  Eigen::MatrixXd J(6, route.size());
+  J.setZero();
+
+  for (int i=0; i < route.size(); i++) {
+    auto link_id = route[i];
+    auto a = links[link_id].R * links[link_id].a;
+    J.col(i) << a.cross(target_p - links[link_id].p), a;
+  }
+
+  return J;
+}
+
 }  // namespace kinematics_utils

rt_manipulators_lib/test/CMakeLists.txt

@@ -17,6 +17,7 @@ include(GoogleTest)
 set(list_tests
     test_joint
     test_kinematics_utils
+    test_kinematics
 )
 foreach(test_executable IN LISTS list_tests)
     message("${test_executable}")