センサ値を取得しようのバックアップソース(No.7)

*物体情報の取得とデータ処理 [#d815c7cd]
[[サンプルプログラム>]]をダウンロード・コンパイル・実行してください．

このプログラムでは，一つの物体の位置へアームを移動させ，物体を把持し，その重量を計測します．

物体の見た目の情報（位置，色，半径）は天井の視覚センサ（カメラ）で，物体の重量はグリッパの力覚センサで計測できると仮定します．

ただし，簡単化のため，カメラ画像処理は省いており，物体番号を渡すとその位置，色，または，半径を返す関数が用意されています．

まずは，その物体情報を格納するObject構造体を定義しましょう．
 typedef struct{
      int index;
      Position position;
      ColorRGB color;
      double radius;
      double weight;
 } Object;
indexは物体の番号を表すものとします．

get_object_position関数を用いて，1番目の物体の位置情報を取得できます．
 Position temp;
 temp = get_object_position(0);
ここで，関数の引数0は1番目の物体を意味します．

同様にして，色，半径，重量は以下で取得できます．
 object.color = get_object_color_rgb(0);
 object.radius = get_object_radius(0);
 object.weight = get_object_weight(0);

ただし，これらのセンサ値にはノイズが含まれています．

例えば，誤差の大きな位置をアームの目標位置としてしまうと，その後の把持動作に失敗してしまいます．

このノイズを除去するために，MEASUREMENT_STEP回センサ値を取得して，データを平均化しています．

まず，位置を初期化します．
 object.position.x = 0;

ループ中で，MEASUREMENT_STEP回，センサデータを加算します．
 object.position.x += temp.x

そして，MEASUREMENT_STEP + 1回目のループで，それを計測回で除算することで，比較的正確な平均値を得ます．
 object.position.x /=  MEASUREMENT_STEP;

y軸方向も同様です．

*把持動作と重量情報の取得 [#m423d4e9]
MEASUREMENT_STEP + 2回目以降のループでは，取得した位置情報を元に，その物体の重量を計測します．

この処理は煩雑になるため，また，後で繰り返し使えるように，
 approach_and_weight(object.position, get_arm_state(), get_gripper_state());
に関数化しています．

この関数には，アームとグリッパの状態も渡しています．

まず，アームが動いているときに指令を与えてもアームの動作の邪魔になるだけなので，
 if(arm_state != ARM_STATE_STOP) return;
の条件をつけて，以降の処理をアームが止まっているときに限ります．

次に，この関数ではアームは移動，把持，重量測定と解放の三つの動作をする必要があるので，その三つの場合に分けます．

現在，どの処理段階にあるのかはtask_stateに書いておきます．

はじめ，
 TaskState task_state = APPROACH;
であるので，
 set_command_move_arm_to(object.position);
で，物体位置までアームを移動させます．

そして，PICKUPでは，
 set_command_pick_up_object();
で物体を把持します．

最後に，RELEASEでは重量を測定して，と行きたいところですが，上記の把持動作は失敗することがあります．

グリッパの状態を確認して，物体を把持していなければ，再試行します．
 if(gripper_state == GRIPPER_STATE_EMPTY){
      task_state = PICKUP;
 }

把持していれば，重量を測定して，物体を離します．
 if(gripper_state == GRIPPER_STATE_HOLDING){
      weight = get_hold_object_weight();
      set_command_release_object();
      task_state = STOP;
      return(weight);
 }

STOPの処理には何も書いてありませんので，ロボットは停止します．

*練習 [#kc380b0f]
5個すべての物体の重量を計測・表示するプログラムを作成してください．

ただし，重量のセンサ値はガウスノイズを含んでいるので，ノイズ除去をしてください．

**ヒント [#w391efea]
approach_and_weight関数が重量を返したときに，次の物体の処理へ進めましょう．

*課題 [#wace50e8]
N個すべての物体の位置・重量・色・半径をファイル出力するプログラムを作成してください．

各センサ値はガウスノイズを含んでいるので，ノイズ除去後の値をtxtファイルやcsvファイルなどに書き出してください．

物体数Nはプログラム実行後にキーボード入力します．

適宜コメントを入れたり，関数化したりして，他人にもわかりやすいプログラムになるように心がけてください．全くコメントのないものは減点します．

他人のプログラムを写したことが判明した場合は，両者とも0点になります．課題内容に対応していないプログラムにも点をつけません．

**発展課題1 [#oa9e0c6d]
上記の課題中のアームの軌道をグラフ化してください．

アームの位置の取得には，関数
 get_arm_position()
を用います．

グラフ描画ツールには例えば，gnuplot，R，python，Microsoft Excel，OpenOffice Calcがあります．

**発展課題2 [#c35f472d]
最短で移動を終了させるためにはどのようにすればよいか考え，可能ならばプログラムを作成してください．

この課題はセールスマン巡回問題と関連し，難問ですので，後回しでもよいです．

** 提出物 [#k17aaedc]
提出物一式を入れるフォルダの名前は学生番号にしてください．
- プログラムファイル
- 出力した物体情報ファイル
- アーム軌道の図（発展課題1）
- 高速化のアイデアを書いたテキスト（発展課題2）

** 提出日 [#lc267e66]
次回の授業中に，提出プログラムが正しく実行できていることをTAが直接確認します．

その授業日24:00までにCLEで提出物を提出してください．

- 1・2組：1/20
- 3・4組：1/16
センサ値を取得しよう のバックアップソース(No.7)

センサ値を取得しようのバックアップソース(No.7)
