Skip to content

Commit 415cb6c

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

File tree

15 files changed

+1052
-108
lines changed

15 files changed

+1052
-108
lines changed

‎.github/workflows/build_test.yaml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,8 @@ jobs:
5151
- name: Build Samples
5252
run: |
5353
./samples/samples01/build_samples.bash
54+
./samples/samples02/build_samples.bash
55+
./samples/samples03/build_samples.bash
5456
- name: Test library
5557
run: ./rt_manipulators_lib/run_test_library.bash
5658
- name: Test Samples

‎rt_manipulators_lib/src/kinematics_utils.cpp‎

Lines changed: 29 additions & 29 deletions
Original file line numberDiff line numberDiff line change
@@ -112,53 +112,53 @@ std::vector<manipulators_link::Link> parse_link_config_file(const std::string &
112112
} catch (...) {
113113
}
114114

115-
// TODO(ShotaAk) 自リンクに対する重心位置の読み込みを実装する
116-
// try {
117-
// link.c << std::stod(str_vec[COL_CENTER_OF_MASS_X]) * MM_TO_METERS,
118-
// std::stod(str_vec[COL_CENTER_OF_MASS_Y]) * MM_TO_METERS,
119-
// std::stod(str_vec[COL_CENTER_OF_MASS_Z]) * MM_TO_METERS;
120-
// } catch (...) {
121-
// }
122-
123-
// TODO(ShotaAk) 自リンクに対する慣性テンソルの読み込みを実装する
124-
// try {
125-
// link.I << std::stod(str_vec[COL_INERTIA_XX]) * MM2_TO_METERS2,
126-
// std::stod(str_vec[COL_INERTIA_XY]) * MM2_TO_METERS2,
127-
// std::stod(str_vec[COL_INERTIA_XZ]) * MM2_TO_METERS2,
128-
// std::stod(str_vec[COL_INERTIA_XY]) * MM2_TO_METERS2,
129-
// std::stod(str_vec[COL_INERTIA_YY]) * MM2_TO_METERS2,
130-
// std::stod(str_vec[COL_INERTIA_YZ]) * MM2_TO_METERS2,
131-
// std::stod(str_vec[COL_INERTIA_XZ]) * MM2_TO_METERS2,
132-
// std::stod(str_vec[COL_INERTIA_YZ]) * MM2_TO_METERS2,
133-
// std::stod(str_vec[COL_INERTIA_ZZ]) * MM2_TO_METERS2;
134-
// } catch (...) {
135-
// }
115+
// 質量位置
116+
try {
117+
link.c << std::stod(str_vec[COL_CENTER_OF_MASS_X]) * MM_TO_METERS,
118+
std::stod(str_vec[COL_CENTER_OF_MASS_Y]) * MM_TO_METERS,
119+
std::stod(str_vec[COL_CENTER_OF_MASS_Z]) * MM_TO_METERS;
120+
} catch (...) {
121+
}
122+
123+
// 慣性テンソル
124+
try {
125+
link.I << std::stod(str_vec[COL_INERTIA_XX]) * MM2_TO_METERS2,
126+
std::stod(str_vec[COL_INERTIA_XY]) * MM2_TO_METERS2,
127+
std::stod(str_vec[COL_INERTIA_XZ]) * MM2_TO_METERS2,
128+
std::stod(str_vec[COL_INERTIA_XY]) * MM2_TO_METERS2,
129+
std::stod(str_vec[COL_INERTIA_YY]) * MM2_TO_METERS2,
130+
std::stod(str_vec[COL_INERTIA_YZ]) * MM2_TO_METERS2,
131+
std::stod(str_vec[COL_INERTIA_XZ]) * MM2_TO_METERS2,
132+
std::stod(str_vec[COL_INERTIA_YZ]) * MM2_TO_METERS2,
133+
std::stod(str_vec[COL_INERTIA_ZZ]) * MM2_TO_METERS2;
134+
} catch (...) {
135+
}
136136

137137
// 親リンクに対する関節軸ベクトル
138138
// q=0のとき、ローカル座標系の姿勢をワールド座標系の姿勢に一致させるため、
139139
// 関節軸ベクトルに合わせて重心位置と慣性テンソルを座標変換させる
140140
std::string axis = str_vec[COL_AXIS_OF_ROTATION];
141-
// auto rot = rotation_from_euler(0, 0, 0);
141+
auto rot = rotation_from_euler_ZYX(0, 0, 0);
142142
if (axis == "X+") {
143-
// rot = rotation_from_euler(0, M_PI_2, 0);
143+
rot = rotation_from_euler_ZYX(0, M_PI_2, 0);
144144
link.a << 1, 0, 0;
145145
} else if (axis == "X-") {
146-
// rot = rotation_from_euler(0, -M_PI_2, 0);
146+
rot = rotation_from_euler_ZYX(0, -M_PI_2, 0);
147147
link.a << -1, 0, 0;
148148
} else if (axis == "Y+") {
149-
// rot = rotation_from_euler(-M_PI_2, 0, 0);
149+
rot = rotation_from_euler_ZYX(0, 0, -M_PI_2);
150150
link.a << 0, 1, 0;
151151
} else if (axis == "Y-") {
152-
// rot = rotation_from_euler(M_PI_2, 0, 0);
152+
rot = rotation_from_euler_ZYX(0, 0, M_PI_2);
153153
link.a << 0, -1, 0;
154154
} else if (axis == "Z+") {
155155
link.a << 0, 0, 1;
156156
} else if (axis == "Z-") {
157-
// rot = rotation_from_euler(M_PI, 0, 0);
157+
rot = rotation_from_euler_ZYX(0, 0, M_PI);
158158
link.a << 0, 0, -1;
159159
}
160-
// link.c = rot * link.c;
161-
// link.I = rot * link.I * rot.transpose();
160+
link.c = rot * link.c;
161+
link.I = rot * link.I * rot.transpose();
162162

163163
try {
164164
link.dxl_id = std::stoi(str_vec[COL_DXL_ID]);

‎rt_manipulators_lib/test/test_kinematics_utils.cpp‎

Lines changed: 73 additions & 73 deletions
Original file line numberDiff line numberDiff line change
@@ -103,79 +103,79 @@ TEST_F(KinematicsUtilsFixture, load_link_m) {
103103
EXPECT_DOUBLE_EQ(links[10].m, 0.8);
104104
}
105105

106-
// TEST_F(KinematicsUtilsFixture, load_link_c) {
107-
// double x = 0.001; // meters
108-
// double y = 0.002; // meters
109-
// double z = 0.003; // meters
110-
111-
// EXPECT_DOUBLE_EQ(links[1].c[0], x) << "回転軸方向:-";
112-
// EXPECT_DOUBLE_EQ(links[1].c[1], y) << "回転軸方向:-";
113-
// EXPECT_DOUBLE_EQ(links[1].c[2], z) << "回転軸方向:-";
114-
115-
// EXPECT_DOUBLE_EQ(links[2].c[0], x) << "回転軸方向:Z+";
116-
// EXPECT_DOUBLE_EQ(links[2].c[1], y) << "回転軸方向:Z+";
117-
// EXPECT_DOUBLE_EQ(links[2].c[2], z) << "回転軸方向:Z+";
118-
119-
// // Z-は座標系をX軸回りに180 deg回転して表現する
120-
// EXPECT_DOUBLE_EQ(links[3].c[0], x) << "回転軸方向:Z-";
121-
// EXPECT_DOUBLE_EQ(links[3].c[1], -y) << "回転軸方向:Z-";
122-
// EXPECT_DOUBLE_EQ(links[3].c[2], -z) << "回転軸方向:Z-";
123-
124-
// EXPECT_DOUBLE_EQ(links[4].c[0], x) << "回転軸方向:Y+";
125-
// EXPECT_DOUBLE_EQ(links[4].c[1], z) << "回転軸方向:Y+";
126-
// EXPECT_DOUBLE_EQ(links[4].c[2], -y) << "回転軸方向:Y+";
127-
128-
// EXPECT_DOUBLE_EQ(links[5].c[0], x) << "回転軸方向:Y-";
129-
// EXPECT_DOUBLE_EQ(links[5].c[1], -z) << "回転軸方向:Y-";
130-
// EXPECT_DOUBLE_EQ(links[5].c[2], y) << "回転軸方向:Y-";
131-
132-
// EXPECT_DOUBLE_EQ(links[6].c[0], z) << "回転軸方向:X+";
133-
// EXPECT_DOUBLE_EQ(links[6].c[1], y) << "回転軸方向:X+";
134-
// EXPECT_DOUBLE_EQ(links[6].c[2], -x) << "回転軸方向:X+";
135-
136-
// EXPECT_DOUBLE_EQ(links[7].c[0], -z) << "回転軸方向:X-";
137-
// EXPECT_DOUBLE_EQ(links[7].c[1], y) << "回転軸方向:X-";
138-
// EXPECT_DOUBLE_EQ(links[7].c[2], x) << "回転軸方向:X-";
139-
// }
140-
141-
// TEST_F(KinematicsUtilsFixture, load_link_I) {
142-
// Eigen::Matrix3d expected;
143-
// expected << 1, 2, 4,
144-
// 2, 3, 5,
145-
// 4, 5, 6;
146-
// expect_matrix_approximation(links[1].I, expected, "回転軸方向:-");
147-
148-
// expected << 1, 2, 4,
149-
// 2, 3, 5,
150-
// 4, 5, 6;
151-
// expect_matrix_approximation(links[2].I, expected, "回転軸方向:Z+");
152-
153-
// // Z-は座標系をX軸回りに180 deg回転して表現する
154-
// expected << 1, -2, -4,
155-
// -2, 3, 5,
156-
// -4, 5, 6;
157-
// expect_matrix_approximation(links[3].I, expected, "回転軸方向:Z-");
158-
159-
// expected << 1, 4, -2,
160-
// 4, 6, -5,
161-
// -2, -5, 3;
162-
// expect_matrix_approximation(links[4].I, expected, "回転軸方向:Y+");
163-
164-
// expected << 1, -4, 2,
165-
// -4, 6, -5,
166-
// 2, -5, 3;
167-
// expect_matrix_approximation(links[5].I, expected, "回転軸方向:Y-");
168-
169-
// expected << 6, 5, -4,
170-
// 5, 3, -2,
171-
// -4, -2, 1;
172-
// expect_matrix_approximation(links[6].I, expected, "回転軸方向:X+");
173-
174-
// expected << 6, -5, -4,
175-
// -5, 3, 2,
176-
// -4, 2, 1;
177-
// expect_matrix_approximation(links[7].I, expected, "回転軸方向:X-");
178-
// }
106+
TEST_F(KinematicsUtilsFixture, load_link_c) {
107+
double x = 0.001; // meters
108+
double y = 0.002; // meters
109+
double z = 0.003; // meters
110+
111+
EXPECT_DOUBLE_EQ(links[1].c[0], x) << "回転軸方向:-";
112+
EXPECT_DOUBLE_EQ(links[1].c[1], y) << "回転軸方向:-";
113+
EXPECT_DOUBLE_EQ(links[1].c[2], z) << "回転軸方向:-";
114+
115+
EXPECT_DOUBLE_EQ(links[2].c[0], x) << "回転軸方向:Z+";
116+
EXPECT_DOUBLE_EQ(links[2].c[1], y) << "回転軸方向:Z+";
117+
EXPECT_DOUBLE_EQ(links[2].c[2], z) << "回転軸方向:Z+";
118+
119+
// Z-は座標系をX軸回りに180 deg回転して表現する
120+
EXPECT_DOUBLE_EQ(links[3].c[0], x) << "回転軸方向:Z-";
121+
EXPECT_DOUBLE_EQ(links[3].c[1], -y) << "回転軸方向:Z-";
122+
EXPECT_DOUBLE_EQ(links[3].c[2], -z) << "回転軸方向:Z-";
123+
124+
EXPECT_DOUBLE_EQ(links[4].c[0], x) << "回転軸方向:Y+";
125+
EXPECT_DOUBLE_EQ(links[4].c[1], z) << "回転軸方向:Y+";
126+
EXPECT_DOUBLE_EQ(links[4].c[2], -y) << "回転軸方向:Y+";
127+
128+
EXPECT_DOUBLE_EQ(links[5].c[0], x) << "回転軸方向:Y-";
129+
EXPECT_DOUBLE_EQ(links[5].c[1], -z) << "回転軸方向:Y-";
130+
EXPECT_DOUBLE_EQ(links[5].c[2], y) << "回転軸方向:Y-";
131+
132+
EXPECT_DOUBLE_EQ(links[6].c[0], z) << "回転軸方向:X+";
133+
EXPECT_DOUBLE_EQ(links[6].c[1], y) << "回転軸方向:X+";
134+
EXPECT_DOUBLE_EQ(links[6].c[2], -x) << "回転軸方向:X+";
135+
136+
EXPECT_DOUBLE_EQ(links[7].c[0], -z) << "回転軸方向:X-";
137+
EXPECT_DOUBLE_EQ(links[7].c[1], y) << "回転軸方向:X-";
138+
EXPECT_DOUBLE_EQ(links[7].c[2], x) << "回転軸方向:X-";
139+
}
140+
141+
TEST_F(KinematicsUtilsFixture, load_link_I) {
142+
Eigen::Matrix3d expected;
143+
expected << 1, 2, 4,
144+
2, 3, 5,
145+
4, 5, 6;
146+
expect_matrix_approximation(links[1].I, expected, "回転軸方向:-");
147+
148+
expected << 1, 2, 4,
149+
2, 3, 5,
150+
4, 5, 6;
151+
expect_matrix_approximation(links[2].I, expected, "回転軸方向:Z+");
152+
153+
// Z-は座標系をX軸回りに180 deg回転して表現する
154+
expected << 1, -2, -4,
155+
-2, 3, 5,
156+
-4, 5, 6;
157+
expect_matrix_approximation(links[3].I, expected, "回転軸方向:Z-");
158+
159+
expected << 1, 4, -2,
160+
4, 6, -5,
161+
-2, -5, 3;
162+
expect_matrix_approximation(links[4].I, expected, "回転軸方向:Y+");
163+
164+
expected << 1, -4, 2,
165+
-4, 6, -5,
166+
2, -5, 3;
167+
expect_matrix_approximation(links[5].I, expected, "回転軸方向:Y-");
168+
169+
expected << 6, 5, -4,
170+
5, 3, -2,
171+
-4, -2, 1;
172+
expect_matrix_approximation(links[6].I, expected, "回転軸方向:X+");
173+
174+
expected << 6, -5, -4,
175+
-5, 3, 2,
176+
-4, 2, 1;
177+
expect_matrix_approximation(links[7].I, expected, "回転軸方向:X-");
178+
}
179179

180180
TEST_F(KinematicsUtilsFixture, load_link_a) {
181181
Eigen::Vector3d expected;

‎samples/README.md‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ $ ./build_samples.bash
2828

2929
サンプルプログラムの使い方については、[samples02/README.md](./samples02/README.md)を参照してください。
3030

31-
## サンプル集03 目標軌道に沿ってリンクを動かす
31+
## サンプル集03 解析的な逆運動学、トルク制御
3232

3333
次のコマンドを実行して、サンプル集をビルドします。
3434

‎samples/samples02/README.md‎

Lines changed: 0 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -127,8 +127,6 @@ std::vector<manipulators_link::Link> links;
127127
リンク構成を表現したCSVファイルを`kinematics_utils::parse_link_config_file(file_path)`で読み込むことで、
128128
リンク構成を取得できます。
129129

130-
**※重心位置、慣性テンソルの読み込みは未実装です**
131-
132130
```cpp
133131
// CSVファイルを解析してリンク構成を取得する
134132
std::vector<manipulators_link::Link> links = kinematics_utils::parse_link_config_file("../config/crane-x7_links.csv");

‎samples/samples03/CMakeLists.txt‎

Lines changed: 3 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -16,12 +16,15 @@ set(CMAKE_RUNTIME_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/bin)
1616
set(list_samples
1717
x7_3dof_inverse_kinematics
1818
s17_3dof_inverse_kinematics
19+
x7_gravity_compensation
20+
s17_gravity_compensation
1921
)
2022
foreach(sample IN LISTS list_samples)
2123
message("${sample}")
2224
add_executable(${sample}
2325
src/${sample}.cpp
2426
src/rt_manipulators_ik.cpp
27+
src/rt_manipulators_dynamics.cpp
2528
)
2629
target_include_directories(${sample} PUBLIC
2730
include

‎samples/samples03/README.md‎

Lines changed: 71 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,13 @@
1-
# サンプル集03 目標軌道に沿ってリンクを動かす
1+
# サンプル集03 解析的な逆運動学、トルク制御
22

3-
- [サンプル集03 目標軌道に沿ってリンクを動かす](#サンプル集03-目標軌道に沿ってリンクを動かす)
3+
- [サンプル集03 解析的な逆運動学、トルク制御](#サンプル集03-解析的な逆運動学トルク制御)
44
- [サンプルのビルド](#サンプルのビルド)
55
- [ロボットのサーボモータの動かし方について](#ロボットのサーボモータの動かし方について)
66
- [運動学計算で使用するライブラリについて](#運動学計算で使用するライブラリについて)
77
- [解析的に逆運動学を解いて手先を任意の位置・姿勢に移動させる](#解析的に逆運動学を解いて手先を任意の位置姿勢に移動させる)
88
- [解説](#解説)
9+
- [重力補償トルクをサーボモータに入力する](#重力補償トルクをサーボモータに入力する)
10+
- [解説](#解説-1)
911

1012
## サンプルのビルド
1113

@@ -48,7 +50,7 @@ $ ./s17_3dof_inverse_kinematics
4850
実行結果（CRANE-X7の場合）
4951

5052
```sh
51-
./x7_3dof_inverse_kinematics
53+
$ ./x7_3dof_inverse_kinematics
5254
手先目標位置をもとに解析的に逆運動学を解き、CRANE-X7を動かすサンプルです.
5355
リンク情報ファイル:../config/crane-x7_links.csvを読み込みます
5456
...
@@ -175,3 +177,69 @@ for (const auto & [target_id, q_value] : q_list) {
175177
hardware.set_position(links[target_id].dxl_id, q_value);
176178
}
177179
```
180+
181+
## 重力補償トルクをサーボモータに入力する
182+
183+
次のコマンドを実行します。
184+
サンプルを終了する場合はキーボードのEscキーを入力してください。
185+
186+
***安全のためいつでもブレーキモードボタンを押せるように準備してください***
187+
188+
```sh
189+
# CRANE-X7の場合
190+
$ cd bin/
191+
$ ./x7_gravity_compensation
192+
# Sciurus17の場合
193+
$ ./s17_gravity_compensation
194+
```
195+
196+
実行結果（CRANE-X7の場合）
197+
198+
```sh
199+
$ ./x7_gravity_compensation
200+
現在姿勢をもとに重力補償トルクを計算し、CRANE-X7のサーボモータに入力するサンプルです
201+
リンク情報ファイル:../config/crane-x7_links.csvを読み込みます
202+
...
203+
204+
5秒後に重力補償トルクをサーボモータへ入力します
205+
終了する場合はEscキーを押してください
206+
...
207+
```
208+
209+
### 解説
210+
211+
重力補償トルクを計算する関数を使用する場合、
212+
`samples03/include/rt_manipulators_dynamics.hpp`をincludeします。
213+
214+
```cpp
215+
#include "rt_manipulators_dynamics.hpp"
216+
```
217+
218+
重力補償トルク計算関数として、
219+
`samples03_dynamics::gravity_compensation(links, target_id, torque_to_current, q_list)`
220+
があります。
221+
引数にはリンク構成と、手先リンク番号、
222+
電流トルク比(`std::map<unsigned int, double>` または `samples03_dynamics::torque_to_current_t`)、
223+
関節電流の格納先(`std::map<unsigned int, double>` または `kinematics_utils::q_list_t`)を入力します。
224+
225+
```cpp
226+
kinematics_utils::link_id_t target_id = 8;
227+
samples03_dynamics::torque_to_current_t torque_to_current = {
228+
{2, 1.0 / 2.20},
229+
{3, 1.0 / 3.60},
230+
{4, 1.0 / 2.20},
231+
{5, 1.0 / 2.20},
232+
{6, 1.0 / 2.20},
233+
{7, 1.0 / 2.20},
234+
{8, 1.0 / 2.20}
235+
};
236+
237+
kinematics_utils::q_list_t q_list;
238+
239+
// 重力補償トルクを計算する
240+
samples03_dynamics::gravity_compensation(links, target_id, torque_to_current, q_list);
241+
// 関節電流をサーボモータへ設定する
242+
for (const auto & [target_id, q_value] : q_list) {
243+
hardware.set_current(links[target_id].dxl_id, q_value);
244+
}
245+
```

0 commit comments

Comments
 (0)