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をしてから、少し動かして、セットします。
動きました。