Skip to content

Commit 49df47f

Browse files
author
Shota Aoki
authored
Sciurus17の逆運動学サンプルを追加 (#11)
* sciurus17のIKのテストを実装 * s17のikサンプルを追加(正常に動作しない姿勢あり) * IKで取得するq_listが可動範囲内に収まるように修正 * サンプルを整えた * READMEにS17の情報を追記 * x7のIKサンプルに、IK成功失敗の出力を追加
1 parent fe52522 commit 49df47f

File tree

7 files changed

+402
-5
lines changed

7 files changed

+402
-5
lines changed

‎rt_manipulators_lib/src/kinematics.cpp‎

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -84,7 +84,7 @@ bool inverse_kinematics_LM(
8484

8585
// 誤差が小さければ終了
8686
if (error.norm() < error_tolerance) {
87-
result_q_list = q_list;
87+
result_q_list = kinematics_utils::get_q_list(calc_links, route);
8888
return true;
8989
}
9090

@@ -109,7 +109,7 @@ bool inverse_kinematics_LM(
109109
forward_kinematics(calc_links, 1);
110110
}
111111

112-
result_q_list = q_list;
112+
result_q_list = kinematics_utils::get_q_list(calc_links, route);
113113
return false;
114114
}
115115

‎rt_manipulators_lib/test/config/sciurus17_links.csv‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
リンク名称,ID,,,,ワールド座標系ホームポジション位置,,,ワールド座標系リンク間相対距離,,,ワールド座標系ホームポジション姿勢,,,質量[g],ローカル座標系質量位置,,,ローカル座標系慣性テンソル(重心),,,,,,ハードウェア可動範囲,,保護設定可動範囲,,ホームポジション角度,回転軸方向,サーボモータ種類,トルク定数,サーボモータID,サーボモータ可動範囲,,通信速度,Return Delay Time,Operation Mode
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+
Sciurus17_Base,1,-,2,-,0,0,0,0,0,0,0,0,0,1722.937,-24.012,0.139,59.566,6398092.663,-6808.969,7611664.357,-331509.04,2026.354,6331936.461,-,-,,,-,-,-,-,-,-,-,-,-,-
4+
Sciurus17_Body,2,24,3,1,0,0,131.5,0,0,131.5,0,0,0,1482.929,57.11,0.372,238.229,15601031.58,-15890.903,13969734.5,-3805596.494,-20695.221,8327993.097,-110,110,-107,107,180,Z+,XM540-W270,2.409,18,831,3265,3Mbps,0,3
5+
Sciurus17_Neck1,3,5,4,2,80.83,0,471,80.83,0,339.5,0,0,0,168,-0.022,0.029,40.86,63143.268,-0.078,66522.419,-126.072,187.375,32744.236,-165,165,-162,162,180,Z+,XM430-W350,1.783,19,205,3891,3Mbps,0,3
6+
Sciurus17_Neck2,4,23,-,3,80.83,0,525,0,0,54,90,0,0,312.172,19.979,78.687,0.151,964350.46,-56112.026,1027677.585,-731.103,1684.659,1065891.198,-90,90,-90,60,180,Y-,XM430-W350,1.783,20,1024,2731,3Mbps,0,3
7+
Sciurus17_R_Link1,5,14,6,2,80.83,-96.5,420,80.83,-96.5,288.5,90,0,0,258,-0.656,-0.264,46.367,176217.476,-5.425,179207.793,586.979,-166.297,87950.071,-160,160,-157,157,180,Y-,XM540-W270,2.409,2,262,3834,3Mbps,0,3
8+
Sciurus17_R_Link2,6,-,7,5,80.83,-160.6,420,0,-64.1,0,0,90,0,134,0.033,-33.96,-0.185,172046.031,-119.176,98834.156,-535.109,580.743,136915.218,-90,90,-90,90,180,X-,XM540-W270,2.409,3,1024,3072,3Mbps,0,3
9+
Sciurus17_R_Link3,7,-,8,6,80.83,-225.6,420,0,-65,0,90,0,0,443,-13.791,0.245,113.391,2336253.636,-1190.372,2414711.907,-75998.279,-8024.912,236305.543,-160,160,-157,157,180,Y-,XM430-W350,1.783,4,262,3834,3Mbps,0,3
10+
Sciurus17_R_Link4,8,-,9,7,80.83,-410.6,420,0,-185,0,0,0,0,241.66,-8.881,-75.749,0.038,603280.74,-6670.94,143154.3,250.846,-2309.247,598152.333,0,156.2,0,156.2,180,Z+,XM540-W150,1.659,5,2048,3825,3Mbps,0,3
11+
Sciurus17_R_Link5,9,25,10,8,80.83,-531.6,420,0,-121,0,90,0,0,224,13.531,-0.304,70.369,294726.731,-4809.196,290568.087,-21387.985,13192.269,125378.656,-160,160,-157,157,180,Y-,XM430-W350,1.783,6,262,3834,3Mbps,0,3
12+
Sciurus17_R_Link6,10,-,11,9,80.83,-660.6,420,0,-129,0,0,0,0,140,-6.156,3.583,0.755,39499.661,-2921.88,60684.342,-612.042,357.397,73112.015,-120,60,-120,60,180,Z+,XM430-W350,1.783,7,683,2731,3Mbps,0,3
13+
Sciurus17_R_Link7,11,-,12,10,80.83,-703.6,420,0,-43,0,90,0,0,121,-0.118,1.006,20.65,32600.928,-69.84,41969.484,-254.224,-409.16,43040.018,-170,170,-167,167,180,Y-,XM430-W350,1.783,8,148,3948,3Mbps,0,3
14+
Sciurus17_R_HandA,12,13,-,11,68.83,-727.6,420,-12,-24,0,0,0,0,15.8,2.649,-26.849,3.757,10192.75,-197.895,5023.728,98.609,-554.407,6322.202,0,30,-5,90,180,Z+,XM430-W350,1.783,9,1991,3072,3Mbps,0,3
15+
Sciurus17_R_HandB,13,-,-,11,80.83,-751.6,420,12,-24,0,0,0,0,13.9,-2.707,-30.901,2.845,7483.7,361.283,4088.581,-63.385,-228.226,4372.991,0,30,-5,90,180,Z+,-,,-,-,-,-,-,-
16+
Sciurus17_L_Link1,14,-,15,2,80.83,96.5,420,80.83,96.5,288.5,-90,0,0,258,-0.537,0.27,46.27,170007.992,13.313,175079.881,454.301,159.916,85282.27,-160,160,-157,157,180,Y+,XM540-W270,2.409,10,262,3834,3Mbps,0,3
17+
Sciurus17_L_Link2,15,-,16,14,80.83,160.6,420,0,64.1,0,0,90,0,134,-0.07,33.946,-0.199,172421.741,-98.964,99076.413,-266.684,-418.743,137151.205,-90,90,-90,90,180,X-,XM540-W270,2.409,11,1024,3072,3Mbps,0,3
18+
Sciurus17_L_Link3,16,-,17,15,80.83,225.6,420,0,65,0,-90,0,0,443,-13.943,-0.241,113.533,2343035.312,1178.732,2422253.94,-72414.462,8074.759,237122.927,-160,160,-157,157,180,Y+,XM430-W350,1.783,12,262,3834,3Mbps,0,3
19+
Sciurus17_L_Link4,17,-,18,16,80.83,410.6,420,0,185,0,0,0,0,241.66,-8.904,75.732,0.033,603838.474,6315.625,143107.114,116.002,2112.212,598666.56,-156.2,0,-156.2,0,180,Z+,XM540-W150,1.659,13,271,2048,3Mbps,0,3
20+
Sciurus17_L_Link5,18,26,19,17,80.83,531.6,420,0,121,0,-90,0,0,224,13.732,0.348,71.158,286973.35,4321.461,282370.572,-19101.45,-13052.23,125680.836,-160,160,-157,157,180,Y+,XM430-W350,1.783,14,262,3834,3Mbps,0,3
21+
Sciurus17_L_Link6,19,-,20,18,80.83,660.6,420,0,129,0,0,0,0,140,-6.173,-3.627,0.744,39902.954,3014.503,61099.851,-613.647,-361.668,73723.447,-60,120,-60,120,180,Z+,XM430-W350,1.783,15,1365,3413,3Mbps,0,3
22+
Sciurus17_L_Link7,20,-,21,19,80.83,703.6,420,0,43,0,90,0,0,121,-0.118,1.074,-20.65,32593.608,-72.866,41969.484,254.224,436.76,43032.698,-170,170,-167,167,180,Y+,XM430-W350,1.783,16,148,3948,3Mbps,0,3
23+
Sciurus17_L_HandA,21,22,-,20,68.83,727.6,420,-12,24,0,0,0,0,15.8,2.649,26.849,3.757,10192.695,197.91,5023.727,98.601,554.38,6322.142,-30,0,-90,5,180,Z+,XM430-W350,1.783,17,1024,2105,3Mbps,0,3
24+
Sciurus17_L_HandB,22,-,-,20,80.83,751.6,420,12,24,0,0,0,0,13.9,-3.008,30.851,2.845,7483.736,-361.268,4088.583,-63.393,228.245,4373.032,-30,0,-90,5,180,Z+,-,,-,-,-,-,-,-
25+
cameraHead_link,23,-,-,3,162.615,20,616,81.785,20,91,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-
26+
cameraBreast_link,24,-,-,1,114.576,0,349.159,114.576,0,217.659,0,60,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-
27+
Sciurus17_R_Link5_ARmarker,25,-,-,8,127.53,-531.6,513,46.7,0,93,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-
28+
Sciurus17_L_Link5_ARmarker,26,-,-,17,127.53,531.6,513,46.7,0,93,0,0,0,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-,-
29+
右手系、正面はX方向,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,,
30+
,,,,,,,,,,,,,,756.36,,,,,,,,,,,,,,,,,,,,,,,

‎rt_manipulators_lib/test/test_kinematics.cpp‎

Lines changed: 156 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -75,6 +75,19 @@ class X7KinematicsFixture: public ::testing::Test {
7575
std::vector<manipulators_link::Link> links;
7676
};
7777

78+
79+
class S17KinematicsFixture: public ::testing::Test {
80+
protected:
81+
virtual void SetUp() {
82+
links = kinematics_utils::parse_link_config_file("../config/sciurus17_links.csv");
83+
}
84+
85+
virtual void TearDown() {
86+
}
87+
88+
std::vector<manipulators_link::Link> links;
89+
};
90+
7891
TEST_F(KinematicsFixture, forward_kinematics) {
7992
// 手先リンクの位置・姿勢を検査する
8093
kinematics::forward_kinematics(links, 1);
@@ -212,3 +225,146 @@ TEST_F(X7KinematicsFixture, inverse_kinematics_LM) {
212225
links, 8, target_p, target_R, q_list)) << "上面：解けなかった位置:" << std::endl << target_p;
213226
}
214227
}
228+
229+
TEST_F(S17KinematicsFixture, inverse_kinematics_LM) {
230+
kinematics::forward_kinematics(links, 1);
231+
// 関節の可動範囲を制限
232+
// 腰
233+
links[2].min_q = -150 * M_PI / 180.0;
234+
links[2].max_q = 150 * M_PI / 180.0;
235+
// 首
236+
links[3].min_q = -90 * M_PI / 180.0;
237+
links[3].max_q = 90 * M_PI / 180.0;
238+
links[4].min_q = -90 * M_PI / 180.0;
239+
links[4].max_q = 90 * M_PI / 180.0;
240+
// 右腕
241+
links[5].min_q = -150 * M_PI / 180.0;
242+
links[5].max_q = 150 * M_PI / 180.0;
243+
links[6].min_q = -90 * M_PI / 180.0;
244+
links[6].max_q = 90 * M_PI / 180.0;
245+
links[7].min_q = -150 * M_PI / 180.0;
246+
links[7].max_q = 150 * M_PI / 180.0;
247+
links[8].min_q = -150 * M_PI / 180.0;
248+
links[8].max_q = 0 * M_PI / 180.0;
249+
links[9].min_q = -150 * M_PI / 180.0;
250+
links[9].max_q = 150 * M_PI / 180.0;
251+
links[10].min_q = -60 * M_PI / 180.0;
252+
links[10].max_q = 120 * M_PI / 180.0;
253+
links[11].min_q = -160 * M_PI / 180.0;
254+
links[11].max_q = 160 * M_PI / 180.0;
255+
// 左腕
256+
links[14].min_q = -150 * M_PI / 180.0;
257+
links[14].max_q = 150 * M_PI / 180.0;
258+
links[15].min_q = -90 * M_PI / 180.0;
259+
links[15].max_q = 90 * M_PI / 180.0;
260+
links[16].min_q = -150 * M_PI / 180.0;
261+
links[16].max_q = 150 * M_PI / 180.0;
262+
links[17].min_q = 0 * M_PI / 180.0;
263+
links[17].max_q = 150 * M_PI / 180.0;
264+
links[18].min_q = -150 * M_PI / 180.0;
265+
links[18].max_q = 150 * M_PI / 180.0;
266+
links[19].min_q = -120 * M_PI / 180.0;
267+
links[19].max_q = 60 * M_PI / 180.0;
268+
links[20].min_q = -160 * M_PI / 180.0;
269+
links[20].max_q = 160 * M_PI / 180.0;
270+
271+
Eigen::Vector3d target_p;
272+
Eigen::Matrix3d target_R;
273+
kinematics_utils::q_list_t q_list;
274+
int right_target_link_id = 11;
275+
int left_target_link_id = 20;
276+
277+
// 右腕と左腕、5方向で0.4m(x or y) * 0.4m(z)の対角線を描く
278+
const int STEPS = 20;
279+
// 正面方向(手先は正面を向ける)
280+
for (int s = 0; s < STEPS; s++) {
281+
// 右腕
282+
// 左上から右下
283+
target_p << 0.3,
284+
0.0 - 0.4 * s / static_cast<double>(STEPS),
285+
0.4 - 0.4 * s / static_cast<double>(STEPS);
286+
target_R = kinematics_utils::rotation_from_euler_ZYX(M_PI_2, 0, 0);
287+
EXPECT_TRUE(kinematics::inverse_kinematics_LM(
288+
links, right_target_link_id, target_p, target_R, q_list))
289+
<< "右腕: 正面：解けなかった位置:" << std::endl << target_p;
290+
291+
// 左腕
292+
// 右上から左下
293+
target_p << 0.3,
294+
0.0 + 0.4 * s / static_cast<double>(STEPS),
295+
0.4 - 0.4 * s / static_cast<double>(STEPS);
296+
target_R = kinematics_utils::rotation_from_euler_ZYX(-M_PI_2, 0, 0);
297+
EXPECT_TRUE(kinematics::inverse_kinematics_LM(
298+
links, left_target_link_id, target_p, target_R, q_list))
299+
<< "左腕: 正面：解けなかった位置:" << std::endl << target_p;
300+
}
301+
302+
// 左面方向(左手先は左面を向ける)
303+
target_R = kinematics_utils::rotation_from_euler_ZYX(0, 0, 0);
304+
for (int s = 0; s < STEPS; s++) {
305+
// 左上から右下
306+
target_p << 0.2 - 0.4 * s / static_cast<double>(STEPS),
307+
0.4,
308+
0.4 - 0.4 * s / static_cast<double>(STEPS);
309+
EXPECT_TRUE(kinematics::inverse_kinematics_LM(
310+
links, left_target_link_id, target_p, target_R, q_list))
311+
<< "左腕: 左面：解けなかった位置:" << std::endl << target_p;
312+
}
313+
314+
// 右面方向(右手先は右面を向ける)
315+
target_R = kinematics_utils::rotation_from_euler_ZYX(0, 0, 0);
316+
for (int s = 0; s < STEPS; s++) {
317+
// 左上から右下
318+
target_p << 0.2 - 0.4 * s / static_cast<double>(STEPS),
319+
-0.4,
320+
0.4 - 0.4 * s / static_cast<double>(STEPS);
321+
EXPECT_TRUE(kinematics::inverse_kinematics_LM(
322+
links, right_target_link_id, target_p, target_R, q_list))
323+
<< "右腕: 右面：解けなかった位置:" << std::endl << target_p;
324+
}
325+
326+
// 後面方向(手先は後面を向ける)
327+
for (int s = 0; s < STEPS; s++) {
328+
// 右腕
329+
// 左上から右下
330+
target_p << -0.30,
331+
0.05 - 0.4 * s / static_cast<double>(STEPS),
332+
0.4 - 0.4 * s / static_cast<double>(STEPS);
333+
target_R = kinematics_utils::rotation_from_euler_ZYX(-M_PI_2, 0, 0);
334+
EXPECT_TRUE(kinematics::inverse_kinematics_LM(
335+
links, right_target_link_id, target_p, target_R, q_list))
336+
<< "右腕: 後面：解けなかった位置:" << std::endl << target_p;
337+
338+
// 左腕
339+
// 右上から左下
340+
target_p << -0.30,
341+
-0.05 + 0.4 * s / static_cast<double>(STEPS),
342+
0.4 - 0.4 * s / static_cast<double>(STEPS);
343+
target_R = kinematics_utils::rotation_from_euler_ZYX(M_PI_2, 0, 0);
344+
EXPECT_TRUE(kinematics::inverse_kinematics_LM(
345+
links, left_target_link_id, target_p, target_R, q_list))
346+
<< "左腕: 後面：解けなかった位置:" << std::endl << target_p;
347+
}
348+
349+
// 上面方向(手先は上面を向ける)
350+
target_R = kinematics_utils::rotation_from_euler_ZYX(0, 0, 0);
351+
for (int s = 0; s < STEPS; s++) {
352+
// 右腕
353+
target_p << 0.4 - 0.4 * s / static_cast<double>(STEPS),
354+
0.0 - 0.4 * s / static_cast<double>(STEPS),
355+
0.5;
356+
target_R = kinematics_utils::rotation_from_euler_ZYX(0, 0, -M_PI_2);
357+
EXPECT_TRUE(kinematics::inverse_kinematics_LM(
358+
links, right_target_link_id, target_p, target_R, q_list))
359+
<< "右腕: 上面：解けなかった位置:" << std::endl << target_p;
360+
361+
// 左腕
362+
target_p << 0.4 - 0.4 * s / static_cast<double>(STEPS),
363+
0.0 + 0.4 * s / static_cast<double>(STEPS),
364+
0.5;
365+
target_R = kinematics_utils::rotation_from_euler_ZYX(0, 0, M_PI_2);
366+
EXPECT_TRUE(kinematics::inverse_kinematics_LM(
367+
links, left_target_link_id, target_p, target_R, q_list))
368+
<< "左腕: 上面：解けなかった位置:" << std::endl << target_p;
369+
}
370+
}

‎samples/samples02/CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,7 @@ set(list_samples
1717
x7_forward_kinematics
1818
s17_forward_kinematics
1919
x7_inverse_kinematics
20+
s17_inverse_kinematics
2021
)
2122
foreach(sample IN LISTS list_samples)
2223
message("${sample}")

‎samples/samples02/README.md‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -169,7 +169,8 @@ kinematics_utils::print_links(links, 1);
169169
## 逆運動学を解いて手先を任意の位置・姿勢に移動させる
170170

171171
次のコマンドを実行します。
172-
ロボットの手先が0.2m前方の5点に向かって移動します。
172+
CRANE-X7では、手先が0.2m前方の5点に向かって移動します。
173+
Sciurus17では、左右の手先が0.4m前方の5点に向かって移動します。
173174

174175
***安全のためロボットの周りに物や人を近づけないでください。***
175176

@@ -178,7 +179,7 @@ kinematics_utils::print_links(links, 1);
178179
$ cd bin/
179180
$ ./x7_inverse_kinematics
180181
# Sciurus17の場合
181-
# 未実装
182+
$ ./s17_inverse_kinematics
182183
```
183184

184185
実行結果（CRANE-X7の場合）

0 commit comments

Comments
 (0)