Cytonアームのジェスチャ制御（Kinect編） - 八谷大岳の覚え書きブログ

前回紹介したCytonアームを、人の腕の動きに追従するようにサンプルプログラムを変更してみた。

用意したハードウェアとソフトウェアは次の通り。

【ハードウェア】

Kinect
ノートPC
Cytonアーム300

【ソフトウェア】

Kinect for Windows SDK v1.5（2012年5月時点のバージョン、現在は、2.0が最新）
OpenCV 2.2.0（2010年10月時点のバージョン、現在は、2.4.9が最新）
Microsoft Visual C++ 2010 Express（現在は、2013が最新）
Cyton Gamma 300R2 Viewer_3.3.3.a12de6のサンプルプログラムcytonCommandExample

まず、Kinectから人の右上半身の関節位置を取得し、PC上に描画するプログラムを作ってみた。プログラムコードは、下記のGithubからダウンロードできる。

https://github.com/hirotaka-hachiya/kinect_skeleton_example1

main.cppでは、次のように、Kinectに接続し、人の右上半身の関節位置を、Natural User InterfaceのNUI_SKELETON_FRAME構造体を用いて取得する。そしてOpenCVを用いて、関節位置を線と丸で骨を描画している。なお、main.cppで用いているconnectKinect、drawLine及びdrawPoint関数は、myKinect.cppにて定義されている。

//main.cpp Kinectから右上半身の関節位置を取得し、線と丸で描画するプログラム
#include "myKinect.h"
#include <tchar.h>
#include <iostream>

int _tmain(int argc, _TCHAR* argv[])
{
 //Kinectに接続
 if(!connectKinect()) return 1;
    
 //各フレームでの全員分の骨格情報（スケルトン）などが格納される構造体
 NUI_SKELETON_FRAME SkeletonFrame;

 //キー'q'が押されるまでループ
 while (true)
 {    
  // 右上半身の骨格表示用のOpenCv行列を作成
  cv::Mat img = cv::Mat::zeros(300, 500, CV_8UC3);
 
  //次のフレームの骨格情報を取得
  NuiSkeletonGetNextFrame(0, &SkeletonFrame); 

  //１人目の骨格情報を追跡できているか、または、関節位置を取得できているか？
  if(SkeletonFrame.SkeletonData[0].eTrackingState==NUI_SKELETON_TRACKED
	  || SkeletonFrame.SkeletonData[0].eTrackingState==NUI_SKELETON_POSITION_ONLY)
  {      
   //右上半身の関節位置情報の取得
   const Vector4 hand_r = 
    SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT];
   const Vector4 wrist_r =
    SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_WRIST_RIGHT];
   const Vector4 elbow_r = 
    SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_ELBOW_RIGHT];  
   const Vector4 shoulder_r = 
    SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT];
   const Vector4 shoulder_c =
    SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_CENTER];
   const Vector4 head =
    SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_HEAD];
   const Vector4 spine =
    SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_SPINE];
  
   //右肩を中心とした、相対的な右手のx, y, z座標を標準出力
   std::cout<<"Right Hand:"<<hand_r.x - shoulder_r.x<<", \\
	   "<<hand_r.y-shoulder_r.y<<", "<<hand_r.z-shoulder_r.z<<std::endl;

   //２つの関節間を繋ぐ線を描画
   drawLine(img, wrist_r,  hand_r);
   drawLine(img, elbow_r,  wrist_r);
   drawLine(img, shoulder_r,  elbow_r);
   drawLine(img, shoulder_c,  shoulder_r);
   drawLine(img, head,  shoulder_c);
   drawLine(img, shoulder_c,  spine);
   
   //右手の位置に丸を描画
   drawPoint(img, hand_r);

   //行列を表示
   cv::imshow(" ", img);
  
   //キー'q'が押されたらループを停止
   int key = cv::waitKey(10);
   if(key == 'q'){
    break;
   }
  } 
  // 骨格情報を連続に変化するように変換
  NuiTransformSmooth(&SkeletonFrame,NULL);        
 }

 return 0;
}

kinect_skeleton_example1.slnをビルドし、実行すると、次のように、ウィンドウに右上半身の関節位置が線と丸で描画される。

f:id:hirotaka_hachiya:20140823225040p:plain

次に、右上半身の動きに対してCytonアームが追従するように、cytonCommandExampleのmain.cppに変更を加える。

if (performExample == 6)
{
   //Pick and place example
   RC_CHECK(cytonCommands.pickAndPlaceExample(cytonVersion));
}

の後に、下記のコードを追加する。

//-------------------------------
// kinectジェスチャ制御　開始
if (performExample == 7)
{
   float hand_r_rx, hand_r_ry, hand_r_rz=0.15;

   //Kinectに接続
   if(!connectKinect()) return 1;
    
   //各フレームでの全員分の骨格情報（スケルトン）などが格納される構造体
   NUI_SKELETON_FRAME SkeletonFrame;

   //キー'q'が押されるまでループ
   while (true)
   {    
      // 右上半身の骨格表示用のOpenCv行列を作成
      cv::Mat img = cv::Mat::zeros(300, 500, CV_8UC3);
 
      //次のフレームの骨格情報を取得
      NuiSkeletonGetNextFrame(0, &SkeletonFrame); 

      //１人目の骨格情報を追跡できているか、または、関節位置情報を取得できているか？
      if(SkeletonFrame.SkeletonData[0].eTrackingState==NUI_SKELETON_TRACKED
         || SkeletonFrame.SkeletonData[0].eTrackingState==NUI_SKELETON_POSITION_ONLY)
      {      
      //右上半身の関節位置情報の取得
      const Vector4 hand_r = 
       SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_HAND_RIGHT];
      const Vector4 wrist_r =
       SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_WRIST_RIGHT];
      const Vector4 elbow_r = 
       SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_ELBOW_RIGHT];  
      const Vector4 shoulder_r = 
       SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_RIGHT];
      const Vector4 shoulder_c =
       SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_SHOULDER_CENTER];
      const Vector4 head =
       SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_HEAD];
      const Vector4 spine =
       SkeletonFrame.SkeletonData[0].SkeletonPositions[NUI_SKELETON_POSITION_SPINE];
  
      //右肩を中心とした、相対的な右手のx, y, z座標を標準出力
      std::cout<<"Right Hand:"<<hand_r.x - shoulder_r.x<<", \\
            "<<hand_r.y-shoulder_r.y<<", "<<hand_r.z-shoulder_r.z<<std::endl;

      //関節間を繋ぐ線を描画
      drawLine(img, wrist_r,  hand_r);
      drawLine(img, elbow_r,  wrist_r);
      drawLine(img, shoulder_r,  elbow_r);
      drawLine(img, shoulder_c,  shoulder_r);
      drawLine(img, head,  shoulder_c);
      drawLine(img, shoulder_c,  spine);
   
      //右手の位置に丸を描画
      drawPoint(img, hand_r);

      //行列を表示
      cv::imshow("右上半身の関節位置", img);
  
      //キー'q'が押されたらループを停止
      int key = cv::waitKey(10);
      if(key == 'q'){
         break;
      }

      //右肩を基準にした右手の相対座標
      hand_r_rx = hand_r.x - shoulder_r.x;
      hand_r_ry = (shoulder_r.z-hand_r.z)/2;

      //cytonアームの制御
      EcCoordinateSystemTransformation desiredPose;
      desiredPose.setTranslation(EcVector(hand_r_rx,hand_r_ry,hand_r_rz));
      RC_CHECK(cytonCommands.pointMovementExample(desiredPose));      
     }
     // 骨格情報を連続に変化するように変換
     NuiTransformSmooth(&SkeletonFrame,NULL); 
   }
}
// kinectジェスチャ制御　終了
//-------------------------------

Cytonアームの指先の位置の目標値をhand_r_rx, hand_r_ry, hand_r_rz変数で表している。
z軸方向の位置（高さ）は、hand_r_rz=0.15により、15cmで固定にしている。
x及びy軸方向の位置は、Kinectから取得した、人の右肩を基準にした右手の相対的な座標により決定している。

      //右肩を基準にした右手の相対座標
      hand_r_rx = hand_r.x - shoulder_r.x;
      hand_r_ry = (shoulder_r.z-hand_r.z)/2;

そして、EcCoordinateSystemTransformationクラスのsetTranslationメソッドを用いて、
Cytonアームの指先が、位置hand_r_rx, hand_r_ry, hand_r_rzに移動するように逆キネマティックス制御をしている。

      //cytonアームの制御
      EcCoordinateSystemTransformation desiredPose;
      desiredPose.setTranslation(EcVector(hand_r_rx,hand_r_ry,hand_r_rz));
      RC_CHECK(cyt