運動学ライブラリの追加 (#8)

* 空のkinematicsライブラリを実装

* link.hpp 追加

* リンク情報リファレンスを解析する

* eigen3のinclude方法を修正

* parse_link_config_file関数を実装

* forward_kinematicsを実装

* X7 forward kinematicsサンプル実装

* S17_forwad_kinematicsサンプル追加

* READMEの追加と、サンプルの微修正

* サンプルに目次を追加

* mother表記をparent表記に変更

* 歪対称行列のコメント修正

* googletest環境を追加

* Add test steps to github workflow

* googletestのインストール方法を修正

* テスト失敗時に詳細を表示する

* jointクラスの初期化テストを追加

* testディレクトリにもcpplintを適用する

* linkはunistdの予約語だったため、manipulators_linkに変更する

* kinematics_utilsのテストを追加

* Lintエラーを修正

* CIでEigen3をインストールする

* kinematics_utilsにrotation_from_eulerを追加

* コンフリクト修正

* 回転軸方向に合わせて重心座標を回転させる

* 回転軸方向に合わせて慣性テンソルを回転させる

* Link.a のテストを追加

* 重心位置と慣性テンソルの読み込みをコメントアウト。実際に使用する際に再実装する

* READMEの更新

* 重心位置、慣性テンソルの読み込みは未実装であることをREADMEに追記

* skew_symmetric_matrixのテストを実装

* rodriguesのテストを追加

* rotation_to_euler_ZYXのテストを追加

* Update samples/samples02/README.md

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

* skew_symmetric_matrix_for_cross_productに改名

* rotation_to_euler_XYZに機能変更

* rotation_from_euler_XYZに改名

* rotation_from_euler_ZYXに修正

* rotation_to_euler_ZYXに修正

* サンプルの姿勢の表示順番を変更

* rotation_to_euler_ZYXのテストを更新

* Update samples/samples02/README.md

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_from_euler_ZYXのロールピッチヨー表記をzyx表記に変更

* Update samples/samples02/src/x7_forward_kinematics.cpp

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

* Update samples/samples02/src/s17_forward_kinematics.cpp

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

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

Author: Shota Aoki

.github/workflows/build_test.yaml

@@ -36,7 +36,7 @@ jobs:
           cmake ..
           sudo make install
       - name: Install dependencies
-        run: sudo apt install libyaml-cpp-dev
+        run: sudo apt install libyaml-cpp-dev libeigen3-dev
       - uses: actions/checkout@v2
       - name: Build and insall library
         run: ./rt_manipulators_lib/build_install_library.bash

.tools/cpplint.bash

@@ -5,6 +5,7 @@ set -e
 SCRIPT_DIR=$(dirname $0)
 LIB_DIR=$SCRIPT_DIR/../rt_manipulators_lib
 SAMPLES01_DIR=$SCRIPT_DIR/../samples/samples01
+SAMPLES02_DIR=$SCRIPT_DIR/../samples/samples02
 
 function cpplint_check () {
     cpplint --filter=-build/c++11,-runtime/reference --linelength=100 --extensions=hpp,cpp $1/include/* $1/src/* $1/test/*
@@ -14,4 +15,5 @@ echo "cpplintを実行し、コードフォーマットをチェックします"
 
 cpplint_check $LIB_DIR
 cpplint_check $SAMPLES01_DIR
+cpplint_check $SAMPLES02_DIR
 

README.md

@@ -14,6 +14,7 @@
 - g++ (>= 7.5.0)
 - [DYNAMIXEL SDK](https://github.com/ROBOTIS-GIT/DynamixelSDK) (linux64向けにビルド&インストール)
 - [yaml-cpp](https://github.com/jbeder/yaml-cpp)
+- Eigen (>= 3.3.4)
 - Linux OS
     - Ubuntu 18.04
     - Ubuntu 20.04
@@ -42,7 +43,7 @@ $ sudo make install
 次のコマンドを実行します。
 
 ```sh
-$ sudo apt install libyaml-cpp-dev 
+$ sudo apt install libyaml-cpp-dev libeigen3-dev
 ```
 
 ### RTマニピュレータC++ライブラリのビルド&インストール

rt_manipulators_lib/include/kinematics.hpp

@@ -0,0 +1,28 @@
+// Copyright 2021 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_HPP_
+
+#include <vector>
+
+#include "link.hpp"
+
+namespace kinematics {
+
+void forward_kinematics(std::vector<manipulators_link::Link> & links, const int & start_id);
+
+}  // namespace kinematics
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_HPP_

rt_manipulators_lib/include/kinematics_utils.hpp

@@ -0,0 +1,36 @@
+// Copyright 2021 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_UTILS_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_UTILS_HPP_
+
+#include <eigen3/Eigen/Dense>
+#include <string>
+#include <vector>
+
+#include "link.hpp"
+
+namespace kinematics_utils {
+
+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);
+
+}  // namespace kinematics_utils
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_KINEMATICS_UTILS_HPP_

rt_manipulators_lib/include/link.hpp

@@ -0,0 +1,59 @@
+// Copyright 2021 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#ifndef RT_MANIPULATORS_LIB_INCLUDE_LINK_HPP_
+#define RT_MANIPULATORS_LIB_INCLUDE_LINK_HPP_
+
+#include <eigen3/Eigen/Dense>
+#include <string>
+
+namespace manipulators_link {
+
+// Linkクラスの参考情報：
+// 梶田秀司 編著, ヒューマノイドロボット 改訂２版, オーム社, 2020
+class Link{
+ public:
+  Link():
+    name("ダミーリンク"), sibling(0), child(0), parent(0), q(0), dq(0), ddq(0), m(0) {
+    // 0ベクトル、単位行列で初期化
+    p.setZero();
+    R.setIdentity();
+    v.setZero();
+    w.setZero();
+    a.setZero();
+    b.setZero();
+    c.setZero();
+    I.setIdentity();
+  }
+  std::string name;  // リンク名
+  int sibling;  // 姉妹兄弟リンクID
+  int child;  // 子リンクID
+  int parent;  // 親リンクID
+  Eigen::Vector3d p;  // ワールド座標系での位置
+  Eigen::Matrix3d R;  // ワールド座標系での姿勢
+  Eigen::Vector3d v;  // ワールド座標系での速度
+  Eigen::Vector3d w;  // ワールド座標系での角速度ベクトル
+  double q;  // 関節位置
+  double dq;  // 関節速度
+  double ddq;  // 関節加速度
+  Eigen::Vector3d a;  // 親リンクに対する関節軸ベクトル
+  Eigen::Vector3d b;  // 親リンクに対する相対位置
+  double m;  // 質量
+  Eigen::Vector3d c;  // 自リンクに対する重心位置
+  Eigen::Matrix3d I;  // 自リンクに対する慣性テンソル
+};
+
+}  // namespace manipulators_link
+
+#endif  // RT_MANIPULATORS_LIB_INCLUDE_LINK_HPP_

rt_manipulators_lib/src/CMakeLists.txt

@@ -9,20 +9,27 @@ add_library(${library_name}
     joint.cpp
     hardware_joints.cpp
     hardware_communicator.cpp
+    kinematics.cpp
+    kinematics_utils.cpp
 )
 set_target_properties(${library_name} PROPERTIES VERSION 1.0.0 SOVERSION 1)
 
-target_include_directories(${library_name}
-    PUBLIC ${PROJECT_SOURCE_DIR}/include
+target_include_directories(${library_name} PUBLIC
+    ${PROJECT_SOURCE_DIR}/include
 )
+message("${CMAKE_THREAD_LIBS_INIT}")
 target_link_libraries(${library_name} PUBLIC
     ${YAML_CPP_LIBRARIES}
     ${CMAKE_THREAD_LIBS_INIT}
     dxl_x64_cpp
 )
 
 install(TARGETS ${library_name}
-    LIBRARY DESTINATION lib)
+    EXPORT ${library_name}-export
+    LIBRARY DESTINATION lib
+    INCLUDES DESTINATION include
+    PUBLIC_HEADER DESTINATION include/${library_name}
+)
 
 install(DIRECTORY ${PROJECT_SOURCE_DIR}/include/
     DESTINATION include/${CMAKE_PROJECT_NAME})

rt_manipulators_lib/src/kinematics.cpp

@@ -0,0 +1,47 @@
+// Copyright 2021 RT Corporation
+//
+// Licensed under the Apache License, Version 2.0 (the "License");
+// you may not use this file except in compliance with the License.
+// You may obtain a copy of the License at
+//
+//     http://www.apache.org/licenses/LICENSE-2.0
+//
+// Unless required by applicable law or agreed to in writing, software
+// distributed under the License is distributed on an "AS IS" BASIS,
+// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+// See the License for the specific language governing permissions and
+// limitations under the License.
+
+#include <iostream>
+
+#include "kinematics.hpp"
+#include "kinematics_utils.hpp"
+
+namespace kinematics {
+
+namespace utils = kinematics_utils;
+
+void forward_kinematics(std::vector<manipulators_link::Link> & links, const int & start_id) {
+  // 指定されたリンクIDからchild、siblingに向かって逐次的に順運動学を解き、
+  // リンクの位置・姿勢を更新する
+  if (start_id == 0) {
+    return;
+  }
+
+  if (start_id >= links.size()) {
+    std::cerr << "引数start_idには引数linksの要素数より小さい数字を入力して下さい" << std::endl;
+    return;
+  }
+
+  if (start_id != 1) {
+      auto parent_id = links[start_id].parent;
+      links[start_id].p = links[parent_id].R * links[start_id].b + links[parent_id].p;
+      links[start_id].R = links[parent_id].R *
+        utils::rodrigues(links[start_id].a, links[start_id].q);
+  }
+
+  forward_kinematics(links, links[start_id].sibling);
+  forward_kinematics(links, links[start_id].child);
+}
+
+}  // namespace kinematics