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を制御する場合には下記の様に第1、2引数を読み替えてください.
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])