Backup of Eye Movement (old v1.4.8)(No. 4)

Tutorial?

眼球運動 †

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

コントローラ †

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

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

$ 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を使います。ここで指定した関節名は人間エージェントのみが持っている関節で、この関節を動かすことにより目玉を動かすことができます。

↑

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

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

$ 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="./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="./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/moveWorld.xml

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

Backup of Eye Movement (old v1.4.8) (No. 4)