重力補償サンプルの追加 (#23)

* X7の重力補償関数とサンプル追加 * gravity_compensationのテストを追加 * target_idを指定するように実装変更 * S17のリンク情報で重力補償をテストした * S17にも対応したため、garvity_compensationに関数名を変更 * S17の重力補償サンプルを追加 * 質量位置と慣性テンソル読み込みのコメントアウトを解除 * 質量位置を読み取ったため、テストを修正 * 電流トルク比を更新 * サンプルの文言修正 * 重心位置、慣性テンソルの読み込みを実装したため、samples02のREADMEを修正 * READMEの更新 * コメントを更新 * コメントの更新 * CIにsamples02, 03のビルドを追加
rt-net · Apr 8, 2022 · 415cb6c · 415cb6c
1 parent 707e8d7
commit 415cb6c
Show file tree

Hide file tree

Showing 15 changed files with 1,052 additions and 108 deletions.
diff --git a/.github/workflows/build_test.yaml b/.github/workflows/build_test.yaml
@@ -51,6 +51,8 @@ jobs:
       - name: Build Samples
         run: |
           ./samples/samples01/build_samples.bash
+          ./samples/samples02/build_samples.bash
+          ./samples/samples03/build_samples.bash
       - name: Test library
         run: ./rt_manipulators_lib/run_test_library.bash
       - name: Test Samples

diff --git a/rt_manipulators_lib/src/kinematics_utils.cpp b/rt_manipulators_lib/src/kinematics_utils.cpp
@@ -112,53 +112,53 @@ std::vector<manipulators_link::Link> parse_link_config_file(const std::string &
     } catch (...) {
     }
 
-    // TODO(ShotaAk) 自リンクに対する重心位置の読み込みを実装する
-    // try {
-    //   link.c << std::stod(str_vec[COL_CENTER_OF_MASS_X]) * MM_TO_METERS,
-    //             std::stod(str_vec[COL_CENTER_OF_MASS_Y]) * MM_TO_METERS,
-    //             std::stod(str_vec[COL_CENTER_OF_MASS_Z]) * MM_TO_METERS;
-    // } catch (...) {
-    // }
-
-    // TODO(ShotaAk) 自リンクに対する慣性テンソルの読み込みを実装する
-    // try {
-    //   link.I << std::stod(str_vec[COL_INERTIA_XX]) * MM2_TO_METERS2,
-    //             std::stod(str_vec[COL_INERTIA_XY]) * MM2_TO_METERS2,
-    //             std::stod(str_vec[COL_INERTIA_XZ]) * MM2_TO_METERS2,
-    //             std::stod(str_vec[COL_INERTIA_XY]) * MM2_TO_METERS2,
-    //             std::stod(str_vec[COL_INERTIA_YY]) * MM2_TO_METERS2,
-    //             std::stod(str_vec[COL_INERTIA_YZ]) * MM2_TO_METERS2,
-    //             std::stod(str_vec[COL_INERTIA_XZ]) * MM2_TO_METERS2,
-    //             std::stod(str_vec[COL_INERTIA_YZ]) * MM2_TO_METERS2,
-    //             std::stod(str_vec[COL_INERTIA_ZZ]) * MM2_TO_METERS2;
-    // } catch (...) {
-    // }
+    // 質量位置
+    try {
+      link.c << std::stod(str_vec[COL_CENTER_OF_MASS_X]) * MM_TO_METERS,
+                std::stod(str_vec[COL_CENTER_OF_MASS_Y]) * MM_TO_METERS,
+                std::stod(str_vec[COL_CENTER_OF_MASS_Z]) * MM_TO_METERS;
+    } catch (...) {
+    }
+
+    // 慣性テンソル
+    try {
+      link.I << std::stod(str_vec[COL_INERTIA_XX]) * MM2_TO_METERS2,
+                std::stod(str_vec[COL_INERTIA_XY]) * MM2_TO_METERS2,
+                std::stod(str_vec[COL_INERTIA_XZ]) * MM2_TO_METERS2,
+                std::stod(str_vec[COL_INERTIA_XY]) * MM2_TO_METERS2,
+                std::stod(str_vec[COL_INERTIA_YY]) * MM2_TO_METERS2,
+                std::stod(str_vec[COL_INERTIA_YZ]) * MM2_TO_METERS2,
+                std::stod(str_vec[COL_INERTIA_XZ]) * MM2_TO_METERS2,
+                std::stod(str_vec[COL_INERTIA_YZ]) * MM2_TO_METERS2,
+                std::stod(str_vec[COL_INERTIA_ZZ]) * MM2_TO_METERS2;
+    } catch (...) {
+    }
 
     // 親リンクに対する関節軸ベクトル
     // q=0のとき、ローカル座標系の姿勢をワールド座標系の姿勢に一致させるため、
     // 関節軸ベクトルに合わせて重心位置と慣性テンソルを座標変換させる
     std::string axis = str_vec[COL_AXIS_OF_ROTATION];
-    // auto rot = rotation_from_euler(0, 0, 0);
+    auto rot = rotation_from_euler_ZYX(0, 0, 0);
     if (axis == "X+") {
-      // rot = rotation_from_euler(0, M_PI_2, 0);
+      rot = rotation_from_euler_ZYX(0, M_PI_2, 0);
       link.a << 1, 0, 0;
     } else if (axis == "X-") {
-      // rot = rotation_from_euler(0, -M_PI_2, 0);
+      rot = rotation_from_euler_ZYX(0, -M_PI_2, 0);
       link.a << -1, 0, 0;
     } else if (axis == "Y+") {
-      // rot = rotation_from_euler(-M_PI_2, 0, 0);
+      rot = rotation_from_euler_ZYX(0, 0, -M_PI_2);
       link.a << 0, 1, 0;
     } else if (axis == "Y-") {
-      // rot = rotation_from_euler(M_PI_2, 0, 0);
+      rot = rotation_from_euler_ZYX(0, 0, M_PI_2);
       link.a << 0, -1, 0;
     } else if (axis == "Z+") {
       link.a << 0, 0, 1;
     } else if (axis == "Z-") {
-      // rot = rotation_from_euler(M_PI, 0, 0);
+      rot = rotation_from_euler_ZYX(0, 0, M_PI);
       link.a << 0, 0, -1;
     }
-    // link.c = rot * link.c;
-    // link.I = rot * link.I * rot.transpose();
+    link.c = rot * link.c;
+    link.I = rot * link.I * rot.transpose();
 
     try {
       link.dxl_id = std::stoi(str_vec[COL_DXL_ID]);

diff --git a/rt_manipulators_lib/test/test_kinematics_utils.cpp b/rt_manipulators_lib/test/test_kinematics_utils.cpp
@@ -103,79 +103,79 @@ TEST_F(KinematicsUtilsFixture, load_link_m) {
   EXPECT_DOUBLE_EQ(links[10].m, 0.8);
 }
 
-// TEST_F(KinematicsUtilsFixture, load_link_c) {
-//   double x = 0.001;  // meters
-//   double y = 0.002;  // meters
-//   double z = 0.003;  // meters
-
-//   EXPECT_DOUBLE_EQ(links[1].c[0], x) << "回転軸方向:-";
-//   EXPECT_DOUBLE_EQ(links[1].c[1], y) << "回転軸方向:-";
-//   EXPECT_DOUBLE_EQ(links[1].c[2], z) << "回転軸方向:-";
-
-//   EXPECT_DOUBLE_EQ(links[2].c[0], x) << "回転軸方向:Z+";
-//   EXPECT_DOUBLE_EQ(links[2].c[1], y) << "回転軸方向:Z+";
-//   EXPECT_DOUBLE_EQ(links[2].c[2], z) << "回転軸方向:Z+";
-
-//   // Z-は座標系をX軸回りに180 deg回転して表現する
-//   EXPECT_DOUBLE_EQ(links[3].c[0], x) << "回転軸方向:Z-";
-//   EXPECT_DOUBLE_EQ(links[3].c[1], -y) << "回転軸方向:Z-";
-//   EXPECT_DOUBLE_EQ(links[3].c[2], -z) << "回転軸方向:Z-";
-
-//   EXPECT_DOUBLE_EQ(links[4].c[0], x) << "回転軸方向:Y+";
-//   EXPECT_DOUBLE_EQ(links[4].c[1], z) << "回転軸方向:Y+";
-//   EXPECT_DOUBLE_EQ(links[4].c[2], -y) << "回転軸方向:Y+";
-
-//   EXPECT_DOUBLE_EQ(links[5].c[0], x) << "回転軸方向:Y-";
-//   EXPECT_DOUBLE_EQ(links[5].c[1], -z) << "回転軸方向:Y-";
-//   EXPECT_DOUBLE_EQ(links[5].c[2], y) << "回転軸方向:Y-";
-
-//   EXPECT_DOUBLE_EQ(links[6].c[0], z) << "回転軸方向:X+";
-//   EXPECT_DOUBLE_EQ(links[6].c[1], y) << "回転軸方向:X+";
-//   EXPECT_DOUBLE_EQ(links[6].c[2], -x) << "回転軸方向:X+";
-
-//   EXPECT_DOUBLE_EQ(links[7].c[0], -z) << "回転軸方向:X-";
-//   EXPECT_DOUBLE_EQ(links[7].c[1], y) << "回転軸方向:X-";
-//   EXPECT_DOUBLE_EQ(links[7].c[2], x) << "回転軸方向:X-";
-// }
-
-// TEST_F(KinematicsUtilsFixture, load_link_I) {
-//   Eigen::Matrix3d expected;
-//   expected << 1, 2, 4,
-//               2, 3, 5,
-//               4, 5, 6;
-//   expect_matrix_approximation(links[1].I, expected, "回転軸方向:-");
-
-//   expected << 1, 2, 4,
-//               2, 3, 5,
-//               4, 5, 6;
-//   expect_matrix_approximation(links[2].I, expected, "回転軸方向:Z+");
-
-//   // Z-は座標系をX軸回りに180 deg回転して表現する
-//   expected << 1, -2, -4,
-//               -2, 3, 5,
-//               -4, 5, 6;
-//   expect_matrix_approximation(links[3].I, expected, "回転軸方向:Z-");
-
-//   expected << 1, 4, -2,
-//               4, 6, -5,
-//               -2, -5, 3;
-//   expect_matrix_approximation(links[4].I, expected, "回転軸方向:Y+");
-
-//   expected << 1, -4, 2,
-//               -4, 6, -5,
-//               2, -5, 3;
-//   expect_matrix_approximation(links[5].I, expected, "回転軸方向:Y-");
-
-//   expected << 6, 5, -4,
-//               5, 3, -2,
-//               -4, -2, 1;
-//   expect_matrix_approximation(links[6].I, expected, "回転軸方向:X+");
-
-//   expected << 6, -5, -4,
-//               -5, 3, 2,
-//               -4, 2, 1;
-//   expect_matrix_approximation(links[7].I, expected, "回転軸方向:X-");
-// }
+TEST_F(KinematicsUtilsFixture, load_link_c) {
+  double x = 0.001;  // meters
+  double y = 0.002;  // meters
+  double z = 0.003;  // meters
+
+  EXPECT_DOUBLE_EQ(links[1].c[0], x) << "回転軸方向:-";
+  EXPECT_DOUBLE_EQ(links[1].c[1], y) << "回転軸方向:-";
+  EXPECT_DOUBLE_EQ(links[1].c[2], z) << "回転軸方向:-";
+
+  EXPECT_DOUBLE_EQ(links[2].c[0], x) << "回転軸方向:Z+";
+  EXPECT_DOUBLE_EQ(links[2].c[1], y) << "回転軸方向:Z+";
+  EXPECT_DOUBLE_EQ(links[2].c[2], z) << "回転軸方向:Z+";
+
+  // Z-は座標系をX軸回りに180 deg回転して表現する
+  EXPECT_DOUBLE_EQ(links[3].c[0], x) << "回転軸方向:Z-";
+  EXPECT_DOUBLE_EQ(links[3].c[1], -y) << "回転軸方向:Z-";
+  EXPECT_DOUBLE_EQ(links[3].c[2], -z) << "回転軸方向:Z-";
+
+  EXPECT_DOUBLE_EQ(links[4].c[0], x) << "回転軸方向:Y+";
+  EXPECT_DOUBLE_EQ(links[4].c[1], z) << "回転軸方向:Y+";
+  EXPECT_DOUBLE_EQ(links[4].c[2], -y) << "回転軸方向:Y+";
+
+  EXPECT_DOUBLE_EQ(links[5].c[0], x) << "回転軸方向:Y-";
+  EXPECT_DOUBLE_EQ(links[5].c[1], -z) << "回転軸方向:Y-";
+  EXPECT_DOUBLE_EQ(links[5].c[2], y) << "回転軸方向:Y-";
+
+  EXPECT_DOUBLE_EQ(links[6].c[0], z) << "回転軸方向:X+";
+  EXPECT_DOUBLE_EQ(links[6].c[1], y) << "回転軸方向:X+";
+  EXPECT_DOUBLE_EQ(links[6].c[2], -x) << "回転軸方向:X+";
+
+  EXPECT_DOUBLE_EQ(links[7].c[0], -z) << "回転軸方向:X-";
+  EXPECT_DOUBLE_EQ(links[7].c[1], y) << "回転軸方向:X-";
+  EXPECT_DOUBLE_EQ(links[7].c[2], x) << "回転軸方向:X-";
+}
+
+TEST_F(KinematicsUtilsFixture, load_link_I) {
+  Eigen::Matrix3d expected;
+  expected << 1, 2, 4,
+              2, 3, 5,
+              4, 5, 6;
+  expect_matrix_approximation(links[1].I, expected, "回転軸方向:-");
+
+  expected << 1, 2, 4,
+              2, 3, 5,
+              4, 5, 6;
+  expect_matrix_approximation(links[2].I, expected, "回転軸方向:Z+");
+
+  // Z-は座標系をX軸回りに180 deg回転して表現する
+  expected << 1, -2, -4,
+              -2, 3, 5,
+              -4, 5, 6;
+  expect_matrix_approximation(links[3].I, expected, "回転軸方向:Z-");
+
+  expected << 1, 4, -2,
+              4, 6, -5,
+              -2, -5, 3;
+  expect_matrix_approximation(links[4].I, expected, "回転軸方向:Y+");
+
+  expected << 1, -4, 2,
+              -4, 6, -5,
+              2, -5, 3;
+  expect_matrix_approximation(links[5].I, expected, "回転軸方向:Y-");
+
+  expected << 6, 5, -4,
+              5, 3, -2,
+              -4, -2, 1;
+  expect_matrix_approximation(links[6].I, expected, "回転軸方向:X+");
+
+  expected << 6, -5, -4,
+              -5, 3, 2,
+              -4, 2, 1;
+  expect_matrix_approximation(links[7].I, expected, "回転軸方向:X-");
+}
 
 TEST_F(KinematicsUtilsFixture, load_link_a) {
   Eigen::Vector3d expected;

diff --git a/samples/README.md b/samples/README.md
@@ -28,7 +28,7 @@ $ ./build_samples.bash
 
 サンプルプログラムの使い方については、[samples02/README.md](./samples02/README.md)を参照してください。
 
-## サンプル集03 目標軌道に沿ってリンクを動かす
+## サンプル集03 解析的な逆運動学、トルク制御
 
 次のコマンドを実行して、サンプル集をビルドします。
 

diff --git a/samples/samples02/README.md b/samples/samples02/README.md
@@ -127,8 +127,6 @@ std::vector<manipulators_link::Link> links;
 リンク構成を表現したCSVファイルを`kinematics_utils::parse_link_config_file(file_path)`で読み込むことで、
 リンク構成を取得できます。
 
-**※重心位置、慣性テンソルの読み込みは未実装です**
-
 ```cpp
 // CSVファイルを解析してリンク構成を取得する
 std::vector<manipulators_link::Link> links = kinematics_utils::parse_link_config_file("../config/crane-x7_links.csv");

diff --git a/samples/samples03/CMakeLists.txt b/samples/samples03/CMakeLists.txt
@@ -16,12 +16,15 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
 set(list_samples
     x7_3dof_inverse_kinematics
     s17_3dof_inverse_kinematics
+    x7_gravity_compensation
+    s17_gravity_compensation
 )
 foreach(sample IN LISTS list_samples)
     message("${sample}")
     add_executable(${sample}
         src/${sample}.cpp
         src/rt_manipulators_ik.cpp
+        src/rt_manipulators_dynamics.cpp
     )
     target_include_directories(${sample} PUBLIC
         include

diff --git a/samples/samples03/README.md b/samples/samples03/README.md
@@ -1,11 +1,13 @@
-# サンプル集03 目標軌道に沿ってリンクを動かす
+# サンプル集03 解析的な逆運動学、トルク制御
 
-- [サンプル集03 目標軌道に沿ってリンクを動かす](#サンプル集03-目標軌道に沿ってリンクを動かす)
+- [サンプル集03 解析的な逆運動学、トルク制御](#サンプル集03-解析的な逆運動学トルク制御)
   - [サンプルのビルド](#サンプルのビルド)
   - [ロボットのサーボモータの動かし方について](#ロボットのサーボモータの動かし方について)
   - [運動学計算で使用するライブラリについて](#運動学計算で使用するライブラリについて)
   - [解析的に逆運動学を解いて手先を任意の位置・姿勢に移動させる](#解析的に逆運動学を解いて手先を任意の位置姿勢に移動させる)
     - [解説](#解説)
+  - [重力補償トルクをサーボモータに入力する](#重力補償トルクをサーボモータに入力する)
+    - [解説](#解説-1)
 
 ## サンプルのビルド
 
@@ -48,7 +50,7 @@ $ ./s17_3dof_inverse_kinematics
 実行結果（CRANE-X7の場合）
 
 ```sh
-./x7_3dof_inverse_kinematics
+$ ./x7_3dof_inverse_kinematics
 手先目標位置をもとに解析的に逆運動学を解き、CRANE-X7を動かすサンプルです.
 リンク情報ファイル:../config/crane-x7_links.csvを読み込みます
 ...
@@ -175,3 +177,69 @@ for (const auto & [target_id, q_value] : q_list) {
   hardware.set_position(links[target_id].dxl_id, q_value);
 }
 ```
+
+## 重力補償トルクをサーボモータに入力する
+
+次のコマンドを実行します。
+サンプルを終了する場合はキーボードのEscキーを入力してください。
+
+***安全のためいつでもブレーキモードボタンを押せるように準備してください***
+
+```sh
+# CRANE-X7の場合
+$ cd bin/
+$ ./x7_gravity_compensation
+# Sciurus17の場合
+$ ./s17_gravity_compensation
+```
+
+実行結果（CRANE-X7の場合）
+
+```sh
+$ ./x7_gravity_compensation
+現在姿勢をもとに重力補償トルクを計算し、CRANE-X7のサーボモータに入力するサンプルです
+リンク情報ファイル:../config/crane-x7_links.csvを読み込みます
+...
+
+5秒後に重力補償トルクをサーボモータへ入力します
+終了する場合はEscキーを押してください
+...
+```
+
+### 解説
+
+重力補償トルクを計算する関数を使用する場合、
+`samples03/include/rt_manipulators_dynamics.hpp`をincludeします。
+
+```cpp
+#include "rt_manipulators_dynamics.hpp"
+```
+
+重力補償トルク計算関数として、
+`samples03_dynamics::gravity_compensation(links, target_id, torque_to_current, q_list)`
+があります。
+引数にはリンク構成と、手先リンク番号、
+電流トルク比(`std::map<unsigned int, double>` または `samples03_dynamics::torque_to_current_t`)、
+関節電流の格納先(`std::map<unsigned int, double>` または `kinematics_utils::q_list_t`)を入力します。
+
+```cpp
+kinematics_utils::link_id_t target_id = 8;
+samples03_dynamics::torque_to_current_t torque_to_current = {
+  {2, 1.0 / 2.20},
+  {3, 1.0 / 3.60},
+  {4, 1.0 / 2.20},
+  {5, 1.0 / 2.20},
+  {6, 1.0 / 2.20},
+  {7, 1.0 / 2.20},
+  {8, 1.0 / 2.20}
+};
+
+kinematics_utils::q_list_t q_list;
+
+// 重力補償トルクを計算する
+samples03_dynamics::gravity_compensation(links, target_id, torque_to_current, q_list);
+// 関節電流をサーボモータへ設定する
+for (const auto & [target_id, q_value] : q_list) {
+  hardware.set_current(links[target_id].dxl_id, q_value);
+}
+```