Skip to content

Commit 28bfc76

Browse files
author
Shota Aoki
authored
解析的にIKを解くCRANE-X7用のサンプルを追加 (#14)
* X7のIKを解析的に解く関数を追加。テスト実装済み。サンプルは未完成 * X7のサンプルを作成 * ピッキング姿勢のIKを解く関数を追加 * サンプルを更新 * samples03をcpplintでチェックする * CIでsamples03のテストを実行する * samples03のREADME更新 * samplesディレクトリのREADMEを更新
1 parent 2514751 commit 28bfc76

17 files changed

+876
-1
lines changed

‎.github/workflows/build_test.yaml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -53,3 +53,5 @@ jobs:
5353
./samples/samples01/build_samples.bash
5454
- name: Test library
5555
run: ./rt_manipulators_lib/run_test_library.bash
56+
- name: Test Samples
57+
run: ./samples/samples03/run_test.bash

‎.tools/cpplint.bash‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -6,6 +6,7 @@ SCRIPT_DIR=$(dirname $0)
66
LIB_DIR=$SCRIPT_DIR/../rt_manipulators_lib
77
SAMPLES01_DIR=$SCRIPT_DIR/../samples/samples01
88
SAMPLES02_DIR=$SCRIPT_DIR/../samples/samples02
9+
SAMPLES03_DIR=$SCRIPT_DIR/../samples/samples03
910

1011
function cpplint_check () {
1112
cpplint --filter=-build/c++11,-runtime/reference --linelength=100 --extensions=hpp,cpp $1/include/* $1/src/* $1/test/*
@@ -16,4 +17,5 @@ echo "cpplintを実行し、コードフォーマットをチェックします"
1617
cpplint_check $LIB_DIR
1718
cpplint_check $SAMPLES01_DIR
1819
cpplint_check $SAMPLES02_DIR
20+
cpplint_check $SAMPLES03_DIR
1921

‎samples/README.md‎

Lines changed: 10 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -30,4 +30,13 @@ $ ./build_samples.bash
3030

3131
## サンプル集03 目標軌道に沿ってリンクを動かす
3232

33-
未実装です。
33+
次のコマンドを実行して、サンプル集をビルドします。
34+
35+
```sh
36+
$ cd samples03
37+
$ ./build_samples.bash
38+
```
39+
40+
ビルドに成功すると`samples03/bin/`ディレクトリに実行ファイルが生成されます。
41+
42+
サンプルプログラムの使い方については、[samples03/README.md](./samples03/README.md)を参照してください。

‎samples/samples03/.clang-format‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,2 @@
1+
BasedOnStyle: Google
2+
ColumnLimit: 100

‎samples/samples03/CMakeLists.txt‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
cmake_minimum_required(VERSION 3.10)
2+
3+
# set the project name and version
4+
project(samples03 VERSION 1.0)
5+
6+
find_package(Eigen3 3.3 REQUIRED NO_MODULE)
7+
8+
# specify the C++ standard
9+
set(CMAKE_CXX_STANDARD 17)
10+
set(CMAKE_CXX_STANDARD_REQUIRED True)
11+
12+
# 生成された実行ファイルをソースのbinディレクトリに出力する
13+
set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
14+
15+
# サンプルのビルド
16+
set(list_samples
17+
x7_3dof_inverse_kinematics
18+
)
19+
foreach(sample IN LISTS list_samples)
20+
message("${sample}")
21+
add_executable(${sample}
22+
src/${sample}.cpp
23+
src/inverse_kinematics.cpp
24+
)
25+
target_include_directories(${sample} PUBLIC
26+
include
27+
)
28+
target_link_libraries(${sample} PRIVATE rt_manipulators_cpp Eigen3::Eigen)
29+
endforeach()

‎samples/samples03/README.md‎

Lines changed: 128 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,128 @@
1+
# サンプル集03 目標軌道に沿ってリンクを動かす
2+
3+
- [サンプル集03 目標軌道に沿ってリンクを動かす](#サンプル集03-目標軌道に沿ってリンクを動かす)
4+
- [サンプルのビルド](#サンプルのビルド)
5+
- [ロボットのサーボモータの動かし方について](#ロボットのサーボモータの動かし方について)
6+
- [運動学計算で使用するライブラリについて](#運動学計算で使用するライブラリについて)
7+
- [解析的に逆運動学を解いて手先を任意の位置・姿勢に移動させる](#解析的に逆運動学を解いて手先を任意の位置姿勢に移動させる)
8+
- [解説](#解説)
9+
10+
## サンプルのビルド
11+
12+
次のコマンドを実行して、サンプル集をビルドします。
13+
14+
```sh
15+
$ ./build_samples.bash
16+
```
17+
18+
ビルドに成功すると`samples03/bin/`ディレクトリに実行ファイルが生成されます。
19+
20+
## ロボットのサーボモータの動かし方について
21+
22+
あらかじめ、["samples01のREADME"](../samples01/README.md)をよく読み、
23+
ロボットのサーボモータの動かし方を理解してください。
24+
25+
## 運動学計算で使用するライブラリについて
26+
27+
運動学計算で使用するライブラリ
28+
(`kinematics.hpp`、`kinematics_utils.hpp`、`link.hpp`)
29+
の使い方は["samples02のREADME"](../samples02/../README.md)
30+
を参照してください。
31+
32+
## 解析的に逆運動学を解いて手先を任意の位置・姿勢に移動させる
33+
34+
次のコマンドを実行します。
35+
CRANE-X7では、手先が0.3m前方の5点に向かって移動します。
36+
37+
***安全のためロボットの周りに物や人を近づけないでください。***
38+
39+
```sh
40+
# CRANE-X7の場合
41+
$ cd bin/
42+
$ ./x7_3dof_inverse_kinematics
43+
```
44+
45+
実行結果（CRANE-X7の場合）
46+
47+
```sh
48+
./x7_3dof_inverse_kinematics
49+
手先目標位置をもとに解析的に逆運動学を解き、CRANE-X7を動かすサンプルです.
50+
リンク情報ファイル:../config/crane-x7_links.csvを読み込みます
51+
...
52+
53+
5秒後にX7が垂直姿勢へ移行するため、X7の周りに物や人を近づけないで下さい.
54+
----------------------
55+
目標位置:
56+
0.2
57+
0
58+
0.2
59+
IKに成功しました
60+
左奥へ移動
61+
...
62+
63+
ピッキング姿勢のIKを解きます
64+
左奥へ移動
65+
----------------------
66+
目標位置:
67+
0.3
68+
0.1
69+
0.05
70+
IKに成功しました
71+
左手前へ移動
72+
...
73+
```
74+
75+
### 解説
76+
77+
解析的に逆運動学を解く関数を使用する場合、
78+
`samples03/include/inverse_kinematics.hpp`をincludeします。
79+
80+
```cpp
81+
#include "inverse_kinematics.hpp"
82+
```
83+
84+
CRANE-X7向けの逆運動学計算関数として、
85+
`samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)`と
86+
`samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)`
87+
があります。
88+
引数にはリンク構成と、目標位置(`Eigen::Vector3d`)、
89+
関節角度の格納先(`std::map<unsigned int, double>` または `kinematics_utils::q_list_t`)を入力します。
90+
91+
`samples03::x7_3dof_inverse_kinematics(links, target_p, q_list)`
92+
は、第6リンクの原点を目標位置(`target_p`)として受け取り、
93+
第1、2、4関節の目標角度を出力する関数です。
94+
95+
`samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list)`
96+
は、ハンドの先端を目標位置(`target_p`)として受け取り、
97+
第1、2、4関節と、第6、7関節の目標角度を出力する関数です。
98+
手先が地面(-Z)方向を向くように逆運動学を解きます。
99+
100+
```cpp
101+
Eigen::Vector3d target_p;
102+
kinematics_utils::q_list_t q_list;
103+
104+
// 目標位置(X方向に0.2m、Y方向に0.0m、Z方向に0.3m)
105+
target_p << 0.2, 0.0, 0.3;
106+
107+
// 逆運動学を解き、関節角度を取得する
108+
samples03::x7_3dof_inverse_kinematics(links, target_p, q_list);
109+
// 関節角度をサーボモータへ設定する
110+
for (const auto & [target_id, q_value] : q_list) {
111+
hardware.set_position(links[target_id].dxl_id, q_value);
112+
}
113+
```
114+
115+
```cpp
116+
Eigen::Vector3d target_p;
117+
kinematics_utils::q_list_t q_list;
118+
// ピッキング姿勢の逆運動学
119+
// 手先の目標位置(X方向に0.2m、Y方向に0.0m、Z方向に0.0m)
120+
target_p << 0.2, 0.0, 0.0;
121+
122+
// 逆運動学を解き、関節角度を取得する
123+
samples03::x7_3dof_picking_inverse_kinematics(links, target_p, q_list);
124+
// 関節角度をサーボモータへ設定する
125+
for (const auto & [target_id, q_value] : q_list) {
126+
hardware.set_position(links[target_id].dxl_id, q_value);
127+
}
128+
```

‎samples/samples03/build_samples.bash‎

Lines changed: 18 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,18 @@
1+
#!/usr/bin/env bash
2+
3+
set -e
4+
5+
BUILD_DIR=build
6+
7+
echo "サンプルプログラムをビルドします"
8+
cd $(dirname $0)
9+
10+
if [ ! -d $BUILD_DIR ]; then
11+
mkdir $BUILD_DIR
12+
fi
13+
14+
cd $BUILD_DIR
15+
cmake ..
16+
make
17+
18+
echo "サンプルプログラムをビルドしました"

‎samples/samples03/config/crane-x7.yaml‎

Lines changed: 31 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
joint_groups:
2+
arm:
3+
joints:
4+
- joint1
5+
- joint2
6+
- joint3
7+
- joint4
8+
- joint5
9+
- joint6
10+
- joint7
11+
sync_read:
12+
- position
13+
sync_write:
14+
- position
15+
hand:
16+
joints:
17+
- joint_hand
18+
sync_read:
19+
- position
20+
sync_write:
21+
- position
22+
23+
24+
joint1: { id : 2, operating_mode: 3 }
25+
joint2: { id : 3, operating_mode: 3 }
26+
joint3: { id : 4, operating_mode: 3 }
27+
joint4: { id : 5, operating_mode: 3 }
28+
joint5: { id : 6, operating_mode: 3 }
29+
joint6: { id : 7, operating_mode: 3 }
30+
joint7: { id : 8, operating_mode: 3 }
31+
joint_hand: { id : 9, operating_mode: 3 }

‎samples/samples03/config/crane-x7_links.csv‎

Lines changed: 13 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,13 @@
1+
リンク名称,ID,,,,ワールド座標系ホームポジション位置,,,ワールド座標系リンク間相対距離,,,ワールド座標系ホームポジション姿勢,,,質量[g],ローカル座標系質量位置,,,ローカル座標系慣性テンソル(重心),,,,,,ハードウェア可動範囲,,角度制限設定済可動範囲,,ホームポジション角度,回転軸方向,サーボモータ種類,トルク定数,サーボモータID,サーボモータ可動範囲,,通信速度,Return Delay Time,Operation Mode,Pゲイン,Iゲイン,Dゲイン
2+
,自分リンク,妹リンク,子リンク,親リンク,x[mm],y[mm],z[mm],x[mm],y[mm],z[mm],X[deg],Y[deg],Z[deg],m[g],x[mm],y[mm],z[mm],Ixx[gmm^2],Ixy[gmm^2],Iyy[gmm^2],Ixz[gmm^2],Iyz[gmm^2],Izz[gmm^2],θ1[deg],θ2[deg],θ1[deg],θ2[deg],サーボ指令値[deg],±XYZ,型番,[Nm/A],ID,θ1[-],θ2[-],bps,-,,-,-,-
3+
CRANE-X7_Base,1,-,2,-,0,0,0,0,0,0,0,0,0,388,-6.734,-0.001,17.921,324812.981,1.195,482993.163,-47122.898,19.475,585569.734,-,-,-,-,-,-,-,,-,-,-,-,-,,-,-,-
4+
CRANE-X7_Link1,2,-,3,1,0,0,41,0,0,41,0,0,0,253,0.243,-0.105,46.95,178706.106,-73.234,176299.332,888.676,623.013,89586.08,-160,160,-157,157,180,Z+,XM430-W350,1.783,2,262,3834,3Mbps,0,3,1000,300,200
5+
CRANE-X7_Link2,3,-,4,2,0,0,105,0,0,64,90,0,0,136,0.031,34.172,0.019,168092.477,20.51,96197.568,-400.417,62.417,134475.028,-90,90,-90,90,180,Y-,XM540-W270,2.409,3,1024,3072,3Mbps,0,3,2200,500,500
6+
CRANE-X7_Link3,4,-,5,3,0,0,170,0,0,65,0,0,0,321,-13.722,-0.081,95.202,1819759.341,753.977,1883280.126,-37299.429,4147.003,158644.809,-160,160,-157,157,180,Z+,XM430-W350,1.783,4,262,3834,3Mbps,0,3,3000,800,1000
7+
CRANE-X7_Link4,5,-,6,4,0,0,355,0,0,185,90,0,0,222,-9.388,80.39,0.064,488905.787,-3904.567,114431.658,82.949,2823.368,502575.787,-160,0,-160,0,180,Y-,XM430-W350,1.783,5,228,2048,3Mbps,0,3,1600,400,400
8+
CRANE-X7_Link5,6,-,7,5,0,0,476,0,0,121,0,0,0,207,-0.084,0.462,63.718,290227.705,100.249,257036.585,-476.631,-16861.834,87512.332,-160,160,-157,157,180,Z+,XM430-W350,1.783,6,262,3834,3Mbps,0,3,800,200,200
9+
CRANE-X7_Link6,7,-,8,6,0,0,605,0,0,129,90,0,0,140,6.1,-3.549,0.826,40139.362,-2908.265,61345.454,639.429,-489.787,73485.846,-90,90,-90,90,180,Y-,XM430-W350,1.783,7,1024,3072,3Mbps,0,3,800,100,200
10+
CRANE-X7_Link7,8,-,9,7,0,0,624,0,0,19,0,0,0,121,0.108,-1.034,20.848,32035.813,-94.503,41433.837,261.225,388.481,42899.559,-170,170,-167,167,180,Z+,XM430-W350,1.783,8,148,3948,3Mbps,0,3,800,100,200
11+
CRANE-X7_HandA,9,10,-,8,-12,0,648,-12,0,24,90,0,0,15.8,-2.647,26.904,3.541,10056.667,-197.178,4954.751,-98.854,536.803,6239.775,0,90,-5,90,180,Y-,XM430-W350,1.783,9,1991,3072,3Mbps,0,3,-,-,-
12+
CRANE-X7_HandB,10,-,-,8,12,0,648,12,0,24,90,0,0,13.9,3.008,30.851,2.845,7483.7,361.284,4088.581,63.385,228.226,4372.991,0,90,-5,90,180,Y+,-,,-,-,-,-,-,,-,-,-
13+
右手系、正面はX方向,,,,,,,,,, ,,,,,,,,,,,,,,,,, c,,,,,,,,,,,,,

‎samples/samples03/config/sciurus17.yaml‎

Lines changed: 75 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,75 @@
1+
2+
joint_groups:
3+
right_arm:
4+
joints:
5+
- right_arm_joint1
6+
- right_arm_joint2
7+
- right_arm_joint3
8+
- right_arm_joint4
9+
- right_arm_joint5
10+
- right_arm_joint6
11+
- right_arm_joint7
12+
sync_read:
13+
- position
14+
sync_write:
15+
- position
16+
17+
right_hand:
18+
joints:
19+
- right_arm_joint_hand
20+
sync_read:
21+
- position
22+
sync_write:
23+
- position
24+
25+
left_arm:
26+
joints:
27+
- left_arm_joint1
28+
- left_arm_joint2
29+
- left_arm_joint3
30+
- left_arm_joint4
31+
- left_arm_joint5
32+
- left_arm_joint6
33+
- left_arm_joint7
34+
sync_read:
35+
- position
36+
sync_write:
37+
- position
38+
39+
left_hand:
40+
joints:
41+
- left_arm_joint_hand
42+
sync_read:
43+
- position
44+
sync_write:
45+
- position
46+
47+
torso:
48+
joints:
49+
- waist_joint
50+
- neck_yaw_joint
51+
- neck_pitch_joint
52+
sync_read:
53+
- position
54+
sync_write:
55+
- position
56+
57+
right_arm_joint1: { id : 2, operating_mode: 3 }
58+
right_arm_joint2: { id : 3, operating_mode: 3 }
59+
right_arm_joint3: { id : 4, operating_mode: 3 }
60+
right_arm_joint4: { id : 5, operating_mode: 3 }
61+
right_arm_joint5: { id : 6, operating_mode: 3 }
62+
right_arm_joint6: { id : 7, operating_mode: 3 }
63+
right_arm_joint7: { id : 8, operating_mode: 3 }
64+
right_arm_joint_hand: { id : 9, operating_mode: 3 }
65+
left_arm_joint1: { id : 10, operating_mode: 3 }
66+
left_arm_joint2: { id : 11, operating_mode: 3 }
67+
left_arm_joint3: { id : 12, operating_mode: 3 }
68+
left_arm_joint4: { id : 13, operating_mode: 3 }
69+
left_arm_joint5: { id : 14, operating_mode: 3 }
70+
left_arm_joint6: { id : 15, operating_mode: 3 }
71+
left_arm_joint7: { id : 16, operating_mode: 3 }
72+
left_arm_joint_hand: { id : 17, operating_mode: 3 }
73+
waist_joint: { id : 18, operating_mode: 3 }
74+
neck_yaw_joint: { id : 19, operating_mode: 3 }
75+
neck_pitch_joint: { id : 20, operating_mode: 3 }

0 commit comments

Comments
 (0)