- The added line is THIS COLOR.
- The deleted line is THIS COLOR.
Up:[[Tutorial]] Previous:[[距離センサ]] Next: [[キネクトによるエージェントの操作]]
#contents
*眼球運動 [#r8deca10]
人間エージェントがターゲットのオブジェクトを目で追いかけ、視線の変化と同時に目玉を動かすサンプルを紹介します。
人間が目玉を動かすサンプルを紹介します。
※バージョン111005以降で正常に動作(v121029, v2.0.1以降には未対応)
**コントローラ作成 [#pa99c726]
コントローラを作成します。
**コントローラ [#qd92b4eb]
まずコントローラを作成します。
$ emacs moveEye.cpp
***目玉を動かすコントローラ [#h8d0bf21]
#highlight(cpp){{
#include "ControllerEvent.h"
#include "Controller.h"
#include "Logger.h"
$ cd ~/sigverse-<version>/bin/NewWorld
$ emacs moveEye.cpp
#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 )
moveEye.cpp
using namespace std;
#highlight(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;
}
class MyController : public Controller {
public:
void onInit(InitEvent &evt);
double onAction(ActionEvent&);
void onRecvMsg(RecvMsgEvent &evt);
void onCollision(CollisionEvent &evt);
};
}}
これは人間がターゲット"Toy_0"を目で追いかけ、視線の変化に合わせて黒目を動かすサンプルです。黒目を動かす方法は普通の関節を動かす方法と同じでsetJointAngleを使います。ここで指定した関節名は人間エージェントのそれぞれの関節名に対応する関節軸は[[Joint定義一覧]]を参考にしてください。
void MyController::onInit(InitEvent &evt) {
}
***移動可能なコントローラ [#xfd41c97]
次にビューワーで操作可能なエージェントのコントローラを作ります。
double MyController::onAction(ActionEvent &evt) {
return 1.0;
}
$ emacs move.cpp
move.cpp
#highlight(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;
}
void MyController::onRecvMsg(RecvMsgEvent &evt) {
}}
このコントローラはdynamicsをtrueに設定したときにエージェントを操作することができます。操作方法はビューワーからメッセージを送信してオブジェクトに力を加えます。例えば
z=-500
とメッセージを送信したらz軸のマイナス方向に500(N)の力が働きます。
//取得したメッセージを表示します
string msg = evt.getMsg();
LOG_MSG(("msg : %s", msg.c_str()));
SimObj *my = getObj(myname());
Makefileを修正してコンパイルします。
if(strstr(msg.c_str()," "))
{
// phi theta に分ける
int n = 0;
n = msg.find(" ");
string phi_s = msg.substr(0,n);
string theta_s = msg.substr(n+1);
$ make
double phi = atof(theta_s.c_str());
double theta = atof(phi_s.c_str());
**設定ファイル [#ad97e1b9]
次に設定ファイルを作成します。
***世界ファイル [#ga7fa45a]
// 目玉関節を回転させる
my->setJointAngle("LEYE_JOINT1",DEG2RAD(phi));
my->setJointAngle("REYE_JOINT1",DEG2RAD(phi));
my->setJointAngle("LEYE_JOINT0",DEG2RAD(theta));
my->setJointAngle("REYE_JOINT0",DEG2RAD(theta));
}
}
void MyController::onCollision(CollisionEvent &evt) {
}
$ cd ../
$ emacs xml/eyeWorld.xml
eyeWorld.xml
extern "C" Controller * createController() {
return new MyController;
}
#highlight(xml){{
}}
このコントローラは目玉を動かすサンプルです。
目を動かす方法は普通の関節を動かす方法と同じでsetJointAngleを使います。
横方向の回転角度と縦方向の回転角度をメッセージとして受信すると目玉を動かします。
コンパイルします。
$ ./sigmake.sh moveEye.cpp
**世界ファイル [#k6e08594]
世界ファイルを作成します。
$ emacs moveEye.xml
#highlight(cpp){{
<?xml version="1.0" encoding="utf8"?>
<world name="newworld">
<gravity x="0.0" y="-9.8" z="0.0"/>
<gravity x="0.0" y="-980.7" 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"/>
value="./moveEye.so"/>
<set-attr-value name="dynamics" value="false"/>
<set-attr-value name="x" value="80.0"/>
<set-attr-value name="x" value="0.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>
</instanciate>
</world>
}}
この世界ファイルでは人間、ペンギン(おもちゃ)、テーブル、椅子が登場します。ペンギンとテーブルはdynamicsをtrueに設定しています。ペンギンがテーブルの上に乗っています。このペンギンに先ほど作成したビューワーで操作できるコントローラを割り当てます。
**物理演算形状、大きさの設定 [#rdd21a03]
エージェントまたはエンティティの物理演算用の形状、位置を微調整します。
人間だけが登場するシンプルな世界ファイルです。
$ emacs xml/seSidetabe_B.xml
**実行 [#bfeada84]
<body filename="dummy-body.xml"/>
の下に以下を加えます。
<simpleShape type="box">
<position x="0" y="-10" z="0"/>
<size sx="150" sy="70" sz="80"/>
</simpleShape>
テーブルの見た目の形状と物理演算用の形状を一致させるように微調整を行いました。
**実行 [#n27b7daf]
それでは実行してみましょう。
$ ./sigserver.sh -p 9001 -w xml/eyeWorld.xml
シミュレーションを開始してみると以下のようにテーブルの上にペンギンが乗っていてそれを人間エージェントが見つめています。
$ sigserver.sh -w ./moveEye.xml
#ref(eye_1.jpg)
ビューワーで接続すると人間が立っています。
例えばメッセージ"20 10"と送信すると人間エージェントは横方向に20°、縦方向に10°目玉を回転させます。
次にテーブルの上に乗っているおもちゃを動かしてみましょう。
例えばSIGViewerから"Toy_0"に
z=500
というメッセージを送信します。
するとペンギンはz軸方向に力が加わり、机の上を移動します。
人間はそれを目で追いかけ、同時に黒目を動かします。
#ref(./moveEye.PNG,40%)
#ref(eye_2.jpg)
&br;
#ref(eye_3.jpg)
顔のズーム
次にy軸方向に力を加えてみます。
以下のメッセージを"Toy_0"に送信します。
y=3000
するとy軸方向に力が加わり、おもちゃのペンギンは飛び跳ねます。
人間エージェントはそれを目で追いかけます。
#ref(eye_4.jpg)
#highlight(end)
*Old Version [#bac32fb8]
-[[眼球運動(v1.4.8)]]
Up:[[Tutorial]] Previous:[[距離センサ]] Next: [[キネクトによるエージェントの操作]]