解析的にIKを解くSciurus17用のサンプルを追加 (#15)

* S17のサンプルを追加。IKの計算は正しくない

* 右腕のIKを修正

* S17 IKサンプルの骨組みを作成

* S17 picking IKの骨組みを作成

* S17の右腕ピッキングIKを実装

* コメントの文言を修正

* コメントの文言を修正

* S17ピッキングIKを修正

* cpplint v1.6.0からinclude_subdirのエラーを検出するようになったため除外する

* q_listにはサーボIDではなくリンクIDをセットするため、修正

* 目標角度がNaNになる場合の処理とテストを追加

* S17 IKサンプルの完成

* S17 IKサンプルの説明をREADMEに追記

* sciurus17_3groups.yamlを削除

* IKの条件分岐を修正。手先姿勢をFKから求める

* ファイル名からinverse_kinematicsからrt_manipulators_ikに変更

* インクルードガードを修正

Author: Shota Aoki

.tools/cpplint.bash

@@ -9,7 +9,7 @@ SAMPLES02_DIR=$SCRIPT_DIR/../samples/samples02
 SAMPLES03_DIR=$SCRIPT_DIR/../samples/samples03
 
 function cpplint_check () {
-    cpplint --filter=-build/c++11,-runtime/reference --linelength=100 --extensions=hpp,cpp $1/include/* $1/src/* $1/test/*
+    cpplint --filter=-build/c++11,-runtime/reference,-build/include_subdir --linelength=100 --extensions=hpp,cpp $1/include/* $1/src/* $1/test/*
 }
 
 echo "cpplintを実行し、コードフォーマットをチェックします"

samples/samples03/CMakeLists.txt

@@ -15,12 +15,13 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
 # サンプルのビルド
 set(list_samples
     x7_3dof_inverse_kinematics
+    s17_3dof_inverse_kinematics
 )
 foreach(sample IN LISTS list_samples)
     message("${sample}")
     add_executable(${sample}
         src/${sample}.cpp
-        src/inverse_kinematics.cpp
+        src/rt_manipulators_ik.cpp
     )
     target_include_directories(${sample} PUBLIC
         include

samples/samples03/README.md

@@ -33,13 +33,16 @@ $ ./build_samples.bash
 
 次のコマンドを実行します。
 CRANE-X7では、手先が0.3m前方の5点に向かって移動します。
+Sciurus17では、右手先が0.4m前方の4点に向かって移動します。
 
 ***安全のためロボットの周りに物や人を近づけないでください。***
 
 ```sh
 # CRANE-X7の場合
 $ cd bin/
 $ ./x7_3dof_inverse_kinematics
+# Sciurus17の場合
+$ ./s17_3dof_inverse_kinematics
 ```
 
 実行結果（CRANE-X7の場合）
@@ -75,10 +78,10 @@ IKに成功しました
 ### 解説
 
 解析的に逆運動学を解く関数を使用する場合、
-`samples03/include/inverse_kinematics.hpp`をincludeします。
+`samples03/include/rt_manipulators_ik.hpp`をincludeします。
 
 ```cpp
-#include "inverse_kinematics.hpp"
+#include "rt_manipulators_ik.hpp"
 ```
 
 CRANE-X7向けの逆運動学計算関数として、
@@ -126,3 +129,49 @@ for (const auto & [target_id, q_value] : q_list) {
   hardware.set_position(links[target_id].dxl_id, q_value);
 }
 ```
+
+Sciurus17向けの逆運動学計算関数には、
+`samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)`と
+`samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)`
+を用意しています。
+引数にはリンク構成と、目標位置(`Eigen::Vector3d`)、
+関節角度の格納先(`std::map<unsigned int, double>` または `kinematics_utils::q_list_t`)を入力します。
+
+`samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list)`
+は、右腕の第6リンクの原点を目標位置(`target_p`)として受け取り、
+右腕の第1、2、4関節の目標角度を出力する関数です。
+
+`samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list)`
+は、右ハンドの先端を目標位置(`target_p`)として受け取り、
+右腕の第1、2、4関節と、第5、6、7関節の目標角度を出力する関数です。
+手先が地面(-Z)方向を向くように逆運動学を解きます。
+
+```cpp
+Eigen::Vector3d target_p;
+kinematics_utils::q_list_t q_list;
+
+// 右手目標位置(X方向に0.2m、Y方向に-0.3m、Z方向に0.2m)
+target_p << 0.2, -0.3, 0.2;
+
+// 逆運動学を解き、関節角度を取得する
+samples03::s17_3dof_right_arm_inverse_kinematics(links, target_p, q_list);
+// 関節角度をサーボモータへ設定する
+for (const auto & [target_id, q_value] : q_list) {
+  hardware.set_position(links[target_id].dxl_id, q_value);
+}
+```
+
+```cpp
+Eigen::Vector3d target_p;
+kinematics_utils::q_list_t q_list;
+// ピッキング姿勢の逆運動学
+// 手先の目標位置(X方向に0.2m、Y方向に-0.3m、Z方向に0.0m)
+target_p << 0.2, -0.3, 0.0;
+
+// 逆運動学を解き、関節角度を取得する
+samples03::s17_3dof_right_arm_picking_inverse_kinematics(links, target_p, q_list);
+// 関節角度をサーボモータへ設定する
+for (const auto & [target_id, q_value] : q_list) {
+  hardware.set_position(links[target_id].dxl_id, q_value);
+}
+```

samples/samples03/include/inverse_kinematics.hpp renamed to samples/samples03/include/rt_manipulators_ik.hpp

@@ -12,8 +12,8 @@
 // See the License for the specific language governing permissions and
 // limitations under the License.
 
-#ifndef SAMPLES_SAMPLES03_INCLUDE_INVERSE_KINEMATICS_HPP_
-#define SAMPLES_SAMPLES03_INCLUDE_INVERSE_KINEMATICS_HPP_
+#ifndef SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_IK_HPP_
+#define SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_IK_HPP_
 
 #include "rt_manipulators_cpp/kinematics_utils.hpp"
 
@@ -25,7 +25,13 @@ bool x7_3dof_inverse_kinematics(const kinematics_utils::links_t & links,
 bool x7_3dof_picking_inverse_kinematics(const kinematics_utils::links_t & links,
                                         const Eigen::Vector3d & target_p,
                                         kinematics_utils::q_list_t & q_list);
+bool s17_3dof_right_arm_inverse_kinematics(const kinematics_utils::links_t & links,
+                                           const Eigen::Vector3d & target_p,
+                                           kinematics_utils::q_list_t & q_list);
+bool s17_3dof_right_arm_picking_inverse_kinematics(const kinematics_utils::links_t & links,
+                                                   const Eigen::Vector3d & target_p,
+                                                   kinematics_utils::q_list_t & q_list);
 
 }  // namespace samples03
 
-#endif  // SAMPLES_SAMPLES03_INCLUDE_INVERSE_KINEMATICS_HPP_
+#endif  // SAMPLES_SAMPLES03_INCLUDE_RT_MANIPULATORS_IK_HPP_