cvl-robot's diary

研究ノート メモメモ https://github.com/dotchang/

hiro_handleとGUIコントローラを接続する(その3)

初稿: 2013/10/8, 最終更新: 2013/10/10

11.GUIコントローラでロボット実機を動かす

XML-RPCサーバ化したhiro_handleからの関節角度の読み込みは上手くできました。次は、関節角度の指令値を送り実際にロボットを動かしてみましょう。

11.1 XML-RPCサーバに関数を追加

hiro_test.pyに、XML-RPCからsetJointAnglesDeg関数呼び出せるように関数の登録をします。インスタンスと関数名を記述するだけです。

server.register_function(hiro.setJointAnglesDeg, "setJointAnglesDeg")

 11.2 関節角度の送信

testApp.hに関節角度送信用ボタン用ブール変数の宣言をします。

ofParameter<bool> rpc_set_angles;

 testApp.cppのtestApp::setup関数内でボタンを作成します。

gui.add(rpc_set_angles.set("set angles", false));

testApp::update()関数内で、ボタンが押されたときの処理を記述します。ボタンが連続して押されないように、ボタンの状態をfalseにします。次に、関節角度を送信するために、setJointAnglesDeg関数に渡す引数を作成します。少し面倒くさいことに、setJointAnglesDegのpose引数は一次元の配列ではなく長さが均一ではない2次元の配列になっています。内訳は、

pose=[

 [chest, pan, tilt],

 [rarm0, rarm1, rarm2, rarm3, rarm4, rarm5],

 [larm0, larm1, larm2, larm3, larm4, larm5],

 [rhand0, rhand1, rhand2, rhand3],

 [lhand0, lhand1, lhand2, lhand3],

]

 です。この形に合うように、argsに配列変数を入れ子で与えていきます。2つ目の引数tmは動作時間の指定で、マニュアルに記述も無いようなのでよくわからないのですが、100の方が早く動作しますから、単位がsecの動作時間ではなく0ー100%の指定のようです。最初は暴走したら怖いので、30ぐらいにしておきましょう。

if(rpc_set_angles){

  rpc_set_angles = false;

  XmlRpc::XmlRpcValue angles, robots, rarms, larms, rhands, lhands, args, res;

  angles.setSize(5);

  robots.setSize(3);

  rarms.setSize(6);

  larms.setSize(6);

  rhands.setSize(4);

  lhands.setSize(4);

  robots[0] = chest; robots[1] = pan; robots[2] = tilt;

  for(int i=0; i<6; i++){ rarms[i] = rarm[i]; }

  for(int i=0; i<6; i++){ larms[i] = larm[i]; }

  for(int i=0; i<4; i++){ rhands[i] = 0.0f; }

  for(int i=0; i<4; i++){ lhands[i] = 0.0f; }

  angles[0] = robots;

  angles[1] = rarms;

  angles[2] = larms;

  angles[3] = rhands;

  angles[4] = lhands;

  args.setSize(2);

  args[0] = angles;

  args[1] = 30;

  c->execute("setJointAnglesDeg", args, res);

}

非常停止ボタンを片手に持って、気をつけて動かします。全身の関節が動きますので、移動量を小さめにして確認しましょう。getJointAnglesDegをしてから、少し動かして、セットします。

動きました。