眼球運動(v1.4.8)

Up:Tutorial?　　　Previous：距離センサ　　　 Next: キネクトによるエージェントの操作　　　　　　

眼球運動

眼球運動 †

人間エージェントがターゲットのオブジェクトを目で追いかけ、視線の変化と同時に目玉を動かすサンプルを紹介します。

※バージョン111005以降で正常に動作(v121029, v2.0.1以降には未対応)

↑

コントローラ †

まずコントローラを作成します。

↑

目玉を動かすコントローラ †

$ cd ~/sigverse-<version>/bin/NewWorld
$ emacs moveEye.cpp

moveEye.cpp

#include <Controller.h>
 #include <ControllerEvent.h>
 #include <Logger.h>
 #include <ViewImage.h>
 #include <math.h>
 
 #define PI 3.141592
 #define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )
 
 using namespace std;
 
 class RobotController : public Controller
 {
 public:
   void onInit(InitEvent &evt);
   double onAction(ActionEvent &evt);
 
 private:
 
 };
 
 void RobotController::onInit(InitEvent &evt)
 {
   //椅子の取得
   SimObj *chair = getObj("chair_0");  Vector3d chpos;
   //椅子を動かします。
   chair->getPosition(chpos);
   chair->setPosition(chpos.x()-20, chpos.y(), chpos.z());
 
   //自分の取得
   SimObj *my = getObj(myname());
   //座ります。
   Vector3d mypos;
   my->getPosition(mypos);
   my->setPosition(mypos.x() , mypos.y() - 15, mypos.z());
   my->setJointAngle("LARM_JOINT2", DEG2RAD(-90));
   my->setJointAngle("RARM_JOINT2", DEG2RAD(90));
   my->setJointAngle("LLEG_JOINT4", DEG2RAD(90));
   my->setJointAngle("RLEG_JOINT4", DEG2RAD(90));
   my->setJointAngle("LLEG_JOINT2", DEG2RAD(-90));
   my->setJointAngle("RLEG_JOINT2", DEG2RAD(-90));
 }
 
 double RobotController::onAction(ActionEvent &evt)
 {
   //視線で追いかけるターゲットのオブジェクトを取得します。
   SimObj *toy = getObj("Toy_0");
 
   //ターゲットの位置を取得します。
   Vector3d tpos;
   toy->getPosition(tpos);
 
   //自分の視点の位置を取得します。(顔の位置と同じ場合）
   Vector3d hpos;
   SimObj *my = getObj(myname());
   CParts *head = my->getParts("HEAD_LINK");
   head->getPosition(hpos);
 
   //自分の視点からターゲットオブジェクト方向のベクトル（絶対座標)を計算します。
   Vector3d evec(  tpos.x() - hpos.x(), tpos.y() - hpos.y(), tpos.z() - hpos.z());
 
   //自分の回転を得ます
   Rotation r;
   r.setQuaternion(my->qw(), -1*my->qx(), -1*my->qy(), -1*my->qz());
 
   //エージェント座標に変換します。
   Vector3d rot;
   r.apply(evec,rot);
 
   //自分の視線ベクトルをターゲットオブジェクトの方向に設定します。
   double x = rot.x();
   double y = rot.y();
   double z = rot.z();
   my->evx1(x);
   my->evy1(y);
   my->evz1(z);
 
   //視線ベクトルに合わせて目玉を動かします。
   double phi   = asin(x/sqrt(x*x+z*z));
   double theta = -1*asin(y/sqrt(y*y+z*z));
   my->setJointAngle("LEYE_JOINT1",phi);
   my->setJointAngle("REYE_JOINT1",phi);
   my->setJointAngle("LEYE_JOINT0",theta);
   my->setJointAngle("REYE_JOINT0",theta);
 
   return 0.1;
 }
 
 extern "C" Controller * createController ()
 {
   return new RobotController;
 }

これは人間がターゲット"Toy_0"を目で追いかけ、視線の変化に合わせて黒目を動かすサンプルです。黒目を動かす方法は普通の関節を動かす方法と同じでsetJointAngleを使います。ここで指定した関節名は人間エージェントのそれぞれの関節名に対応する関節軸はJoint定義一覧を参考にしてください。

↑

移動可能なコントローラ †

次にビューワーで操作可能なエージェントのコントローラを作ります。

$ emacs move.cpp

move.cpp

#include <string>
 #include "Controller.h"
 #include "ControllerEvent.h"
 #include "Logger.h"
 
 using namespace std;
 
 class MoveController : public Controller {
 public:
 
   double onAction(ActionEvent&);
   void onRecvMessage(RecvMessageEvent &evt);
 
 };
 
 
 double MoveController::onAction(ActionEvent &evt) {
   return 1.0;
 }
 
 void MoveController::onRecvMessage(RecvMessageEvent &evt)
 {
   int size = evt.getSize();
   if (size>0)
     { 
 
       //取得したメッセージを表示します。
       string msg = evt.getString(0);
       LOG_MSG(("msg : %s", msg.c_str()));
       SimObj *my = getObj(myname());
 
       if(strstr(msg.c_str(),"="))
         { 
 
           //メッセージを方向と力に分けます
           string axis;
           string msg_force;
           int n = 0;
           n = msg.find("=");
           axis = msg.substr(0,n);
           msg_force = msg.substr(n+1);
 
           //力を加えます。
           double force = atof(msg_force.c_str());
           if(strcmp(axis.c_str(), "x") == 0){
             my->setForce(force,0,0);
           }
           if(strcmp(axis.c_str(), "y") == 0){
             my->setForce(0,force,0);
           }
           if(strcmp(axis.c_str(), "z") == 0){
             my->setForce(0,0,force);
           }
         }
     }
 }
 
 //自身のインスタンスをSIGVerseに返します。
 extern "C" Controller * createController() {
   return new MoveController;
 }

このコントローラはdynamicsをtrueに設定したときにエージェントを操作することができます。操作方法はビューワーからメッセージを送信してオブジェクトに力を加えます。例えば

z=-500

とメッセージを送信したらz軸のマイナス方向に500(N)の力が働きます。

Makefileを修正してコンパイルします。

$ make

↑

設定ファイル †

次に設定ファイルを作成します。

↑

世界ファイル †

$ cd ../
$ emacs xml/eyeWorld.xml

eyeWorld.xml

<?xml version="1.0" encoding="utf8"?>
 <world name="newworld">
 
   <gravity x="0.0" y="-9.8" z="0.0"/>
 
   <instanciate class="Man-nii.xml">
     <set-attr-value name="name" value="man_0"/>
     <set-attr-value name="language" value="c++"/>
     <set-attr-value name="implementation"
                     value="./NewWorld/moveEye.so"/>
     <set-attr-value name="dynamics" value="false"/>
     <set-attr-value name="x" value="80.0"/>
     <set-attr-value name="y" value="60.0"/>
     <set-attr-value name="z" value="0.0"/>
 
     <set-attr-value name="qw" value="0.707"/>
     <set-attr-value name="qx" value="0.0"/>
     <set-attr-value name="qy" value="-0.707"/>
     <set-attr-value name="qz" value="0.0"/>
   </instanciate>
 
   <instanciate class="seToy_D.xml">
      <set-attr-value name="name" value="Toy_0"/>
      <set-attr-value name="language" value="c++"/>
      <set-attr-value name="implementation"
                      value="./NewWorld/move.so"/>
      <set-attr-value name="dynamics" value="true"/>
      <set-attr-value name="x" value="0.0"/>
      <set-attr-value name="y" value="74.0"/>
      <set-attr-value name="z" value="5.0"/>
     <set-attr-value name="mass" value="1.0"/>
   </instanciate>
 
   <instanciate class="seSidetable_B.xml">
     <set-attr-value name="name" value="table_0"/>
     <set-attr-value name="dynamics" value="true"/>
 
     <set-attr-value name="x" value="0.0"/>
     <set-attr-value name="y" value="45.0"/>
     <set-attr-value name="z" value="0.0"/>
 
     <set-attr-value name="qw" value="0.707"/>
     <set-attr-value name="qx" value="0.0"/>
     <set-attr-value name="qy" value="0.707"/>
     <set-attr-value name="qz" value="0.0"/>
 
     <set-attr-value name="scalex" value="2.0"/>
     <set-attr-value name="scaley" value="2.0"/>
     <set-attr-value name="scalez" value="2.0"/>
     <set-attr-value name="mass" value="10.0"/>
   </instanciate>
 
   <instanciate class="seChair_A_c01.xml">
     <set-attr-value name="name" value="chair_0"/>
     <set-attr-value name="dynamics" value="false"/>
 
     <set-attr-value name="x" value="120.0"/>
     <set-attr-value name="y" value="43.0"/>
     <set-attr-value name="z" value="0.0"/>
 
     <set-attr-value name="qw" value="0.707"/>
     <set-attr-value name="qx" value="0.0"/>
     <set-attr-value name="qy" value="0.707"/>
     <set-attr-value name="qz" value="0.0"/>
   </instanciate>
</world>

この世界ファイルでは人間、ペンギン（おもちゃ）、テーブル、椅子が登場します。ペンギンとテーブルはdynamicsをtrueに設定しています。ペンギンがテーブルの上に乗っています。このペンギンに先ほど作成したビューワーで操作できるコントローラを割り当てます。

↑

物理演算形状、大きさの設定 †

エージェントまたはエンティティの物理演算用の形状、位置を微調整します。

$ emacs xml/seSidetabe_B.xml

 <body filename="dummy-body.xml"/>

の下に以下を加えます。

 <simpleShape type="box">
   <position x="0" y="-10" z="0"/>
   <size sx="150" sy="70" sz="80"/>
 </simpleShape>

テーブルの見た目の形状と物理演算用の形状を一致させるように微調整を行いました。

↑

実行 †

それでは実行してみましょう。

$ ./sigserver.sh -p 9001 -w xml/eyeWorld.xml

シミュレーションを開始してみると以下のようにテーブルの上にペンギンが乗っていてそれを人間エージェントが見つめています。

次にテーブルの上に乗っているおもちゃを動かしてみましょう。例えばSIGViewerから"Toy_0"に

z=500

というメッセージを送信します。するとペンギンはｚ軸方向に力が加わり、机の上を移動します。人間はそれを目で追いかけ、同時に黒目を動かします。

　　　　　　顔のズーム

次にy軸方向に力を加えてみます。以下のメッセージを"Toy_0"に送信します。

y=3000

するとy軸方向に力が加わり、おもちゃのペンギンは飛び跳ねます。人間エージェントはそれを目で追いかけます。