controll GR001 with Python on Edison/PC.

今回の目標

EdisonもしくはPC上のpython環境からGR001を制御する.
これを実現するために以前C言語で作成した通信ライブラリrsxをpythonから利用できる様に拡張します.
拡張したライブラリを利用してpython環境からGR001を制御します.

※ :ライブラリの利用のみを行う場合には# 1.を飛ばしてください.


1. python用通信ライブラリ(rsxpy)のビルド/実行環境を構築

今回はEdison上でビルドします.
boost-pythonがビルドに必要で、ipythonが実行環境として便利なので
これらをインストールします.

1
2
3
4
> sudo apt-get install libboost-python-dev
130[MB]ほど消費されます.
> sudo apt-get install ipython
6.0[MB]ほど消費されます.

2. ライブラリの取得

以前作成したRS301/RS302サーボ向けの通信ライブラリを修正するため
通信ライブラリrsxを取得します.

1
2
3
> git clone git@github.com:takarakasai/rsx.git
> cd rsx
> git checkout feature/for_edison

3. ライブラリ内部の説明

rsxをpythonから利用するためのライブラリrsxpyに拡張します。
これにはboost_pythonを利用します.


3.1 全体構成

rsxpy向けに新たに用意したソースコードは以下のとおりです。
boost_pythonはC++のテンプレートライブラリなので、一旦C++によるラッパ(rsxpp)を作成して
boost_pythonにてpython向けのAPIを作成していきます.

1
2
3
4
./rsx/h/rsxpp/rsxpp.h                   // rsxライブラリのC++ラッパ
./rsx/python/sample/rsx_servo_state.py // pythonによるサンプルプログラム
./rsx/python/src/rsxpy.cc // python向けのラッパクラスの実装
./rsx/python/src/rsxpy.h // python向けのラッパクラスのヘッダ

3.2 C++によるラッパクラスの作成

ここではrsxppというクラスを作成します.

1
2
3
4
5
6
7
8
9
-- h/rsxpp/rsxpp.h
// C言語で作成した通信ライブラリrsxを利用するためrsx.hを読み込みます.
// またシリアル通信向けのEnumを扱えるようにするためhr_serial.hも読み込みます.
extern "C" {
#include "rsx.h"
#include "serial/hr_serial.h"
}

// (略)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
-- h/rsxpp/rsxpp.h
// (略)

// ラッパクラスを定義します.
// rsxライブラリを利用するために必要なrsx構造体を
// DEPSERVO_DECLというマクロを利用してメンバ変数として定義します.
// これをクラスのコンストラクタ内から初期化するようにします.
class rsxpp
{
public:
rsxpp(void) {
DPSERVO_INIT(servo_inst, RSX_INIT);
servo = (rsx*)(&servo_inst);
}

// (略)

// ここにrsxライブリが用意している通信APIを一通りラップする関数を用意します.
// 紙面の都合上全ては記載しませんが、
// ロングパケットでデータを送信するAPIについて以下に例を示します.
errno_t lpkt_mem_write_all
(uint8_t id[/*num*/], uint8_t num ,
uint8_t start_addr, uint8_t size, uint8_t data[/*size*/]) {
ECALL(rsx_lpkt_mem_write_all(servo, id, num, start_addr, size, data));
return EOK;
}

// (略)

protected:
DPSERVO_DECL(servo_inst, kNUM_OF_JOINTS, kMAX_PKT_SIZE, RSX_DECL);
rsx *servo;
}
```

---

## 3.3 Pythonで公開するためのクラスの作成

rsxppクラスを継承したrsxpyクラスを作成します.
rsxppクラスではrsxライブラリのAPIをそのままラップしていますが、
rsxpyクラスではpythonから利用することを考えて、
pythonで扱いやすい型の変数を引数や返り値にしています.

```c++
-- python/src/rsxpy.h
// C++によるラッパクラスrsxppを利用するためのヘッダを読み込みます.
// boost_pythonを利用するためヘッダを読み込みます.
#include "rsxpp.h"
#include "boost/python.hpp"
...
class rsxpy : public dp::rsxpp<kRSXPY_NUM_OF_JOINTS, kRSXPY_MAX_PKT_SIZE> {
...
// ここにrsxppライブリが用意している通信APIを一通りラップする関数を用意します.
// 紙面の都合上全ては記載しませんが、ロングパケットでデータを送信するAPIについて以下に例を示します.
errno_t lpkt_mem_write (bp::list &in_id, uint8_t start_addr, size_t size, bp::list &in_data) {
GET_C_ARRAY(uint8_t, num, id/*[num]*/, in_id);
GET_C_ARRAY_2D(uint8_t, num, size, data/*[num][size]*/, in_data);

ECALL(base::lpkt_mem_write(id, num, start_addr, size, (uint8_t**)data));
return EOK;
}
...
}

3.3 boost_pythonによるAPIの公開

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
-- h/rsxpp/rsxpp.cc
...
// rs30xの仮想アドレスをpythonに定数として公開するため
include "mmap/rs30x.h"
...
// boost_pythonのおまじないコードです.
BOOST_PYTHON_MODULE( rsxpy ) {
using namespace dp;

// シリアル通信の通信レートを定数として公開します.
boost::python::enum_<hr_baudrate>("HR_BAUDRATE")
.value("HR_B9600" , HR_B9600 )

// 以下定数の公開処理が続きます.
...

// ここからrsxpyクラスをpythonのrsxpyクラスとして公開します.
boost::python::class_<rsxpy>("rsxpy")
// rsxpyクラスの各公開関数をpythonのrsxpyクラスの関数として公開します.
.def("open", &rsxpy::open)
...
.def("lpkt_mem_write", &rsxpy::lpkt_mem_write)
...
;
}

3 . ライブラリのビルド

1
2
3
4
5
6
7
> m  kdir build
> c make ..
> c cmake ..
// PYTHON_API : OFF --> ON に変更します.
// gとcを押下してmakefileを生成します.
// qを押下してccmakeを抜けます.
> make

3. 実行

pythonのインタプリタを起動します.
ipythonだと関数や定数はタブ補完してくれます.

1
2
3
4
5
6
7
8
9
10
11
12
> cd python
> ipython
Python 2.7.6 (default, Mar 22 2014, 22:59:56)
Type "copyright", "credits" or "license" for more information.

IPython 1.2.1 -- An enhanced Interactive Python.
? -> Introduction and overview of IPython's features.
%quickref -> Quick reference.
help -> Python's own help system.

object? -> Details about 'object', use 'object??' for extra details.

In [1]:

ipythonのインタプリタから通信APIを叩いていきます.

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
// ライブラリを読み込んで、通信クラスをインスタンス化します.
IN[*]: import rsxpy
IN[*]: rsx = rsxpy.rsxpy()

// 通信ポートを開きます.
IN[*]: rsx.open("ttyMFD", "1", rsxpy.HR_BAUDRATE.HR_B460800, rsxpy.HR_PARITY.HR_PAR_NONE)
// PCから直接GR001を制御する場合には下記の様に第12引数を読み替えてください.
IN[*]: rsx.open("ttyUSB", "0", rsxpy.HR_BAUDRATE.HR_B460800, rsxpy.HR_PARITY.HR_PAR_NONE)

IN[*]: ids = range(1,21)

// サーボをONにします.
IN[*]: rsx.lpkt_mem_write_all(ids, rsxpy.RSX_RS30X_MEM_ADDR.RSX_RAM_TRQ_ENABLE, [1])

// 各関節の角度を取得します.
IN[*]: ang = list()
IN[*]: for id in ids :
IN[*]: ang.append(rsx.lpkt_mem_read_int16(id, rsxpy.RSX_RS30X_MEM_ADDR.RSX_RAM_PRESENT_POS_L, 1)[0])
IN[*]:

// 1番と4番関節の角度を30度変化させます.
IN[*]: ang[1] += 30 * 10;
IN[*]: ang[4] += 30 * 10;
IN[*]: rsx.lpkt_mem_write_int16(ids, rsxpy.RSX_RS30X_MEM_ADDR.RSX_RAM_GOAL_POS_L, 1, ang)

// サーボをOFFします.
IN[*]: rsx.lpkt_mem_write_all(ids, rsxpy.RSX_RS30X_MEM_ADDR.RSX_RAM_TRQ_ENABLE, [0])