#contents
*Operations on the movement of agents  [#v2485f29]

Describes how to operate the joints of agents or anthropomorphic robots.

#contents
*エージェントの動きに関する操作 [#v2485f29]
人間またはロボットの形をしたエージェントの関節の操作方法を説明します。
** Operations of joints of robots[#f5cb7768]

**関節の操作 [#f5cb7768]
人間が手を上げるサンプルコードを紹介します。
***コントローラの作成 [#oc032119]
NewWorldディレクトリに新しくAgentController.cppを作成します。
Here is  the sample code to change the angle of joint of human hand.

***Creating the Controller [#oc032119]
NewWorld is the directory in which we create AgentController.cpp

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

AgentController.cpp

#highlight(cpp){{
#include "Controller.h"
#include "Logger.h"

#define PI 3.141592

//角度からラジアンに変換します。
//Coverts angles to radians
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 ) 

class AgentController : public Controller
{
public:
  double onAction(ActionEvent &evt);
}; 


//定期的に呼び出される関数です。
//This function is called on regular basis
double AgentController::onAction(ActionEvent &evt)
{
  try {
    // 自分自身を得る
    // get handle for the agent
    SimObj *my = getObj(myname());
    if (!my->dynamics()) { 

     // "LARM_JOINT2" を45度に設定する
     // "LARM_JOINT2" is set to 45 degrees
      my->setJointAngle("LARM_JOINT2", DEG2RAD(45));

   }

 } catch(SimObj::Exception &) {
   ;
 }

 return 5.0;
}


extern "C"  Controller * createController ()
{
  return new AgentController;
}
}}
のサンプルはシミュレーション開始と同時に
my->setJointAngleでエージェントの左腕が上に45°に上がるというものです。
Sample at the start of the simulation
my->setJointAngle  is used to set the angle to 45 degree for left arm of robot.

***コンパイル [#o78ee00f]
次にMakefileを変更します。
***Compiling [#o78ee00f]

Lets change the Makefile.

 $ emacs Makefile

変更前
// #自作コード
// CPP_SRCS = MoveController.cpp
// 
 #オブジェクトファイルの指定
 OBJS = MoveController.so
//
変更後
// #自作コード
// CPP_SRCS = AgentController.cpp/
// 

 #オブジェクトファイルの指定
 #The object is modified to incorporate the changes due to new agent controller
 OBJS = AgentController.so

Makefile

 #SIGVerseソースの場所指定
 #The location of the source of header files for SIGVerse
 SIG_SRC  = /home/<username>/sigverse-<version>/include/sigverse
 
 #オブジェクトファイルの指定
 #The object 
 OBJS     = AgentController.so 
 
 all: $(OBJS)
 
 #コンパイルを行います。
 #Compile
 ./%.so: ./%.cpp
         g++ -DCONTROLLER -DNDEBUG -DUSE_ODE -DdDOUBLE -I$(SIG_SRC) -I$(SIG_SRC)/comm/controller  -fPIC -shared -o $@   $<


コンパイルします。
Compile

 $ make

***世界ファイル作成 [#l5dd7972]
次に世界ファイルを作成します。
***Creating world files [#l5dd7972]
Then create a world file

//SIGVerse起動時に-wオプションで毎回世界ファイルを指定するのは面倒なので、MyWorld.xmlに記述します。

 $ cd ~/sigverse-<version>/bin
 $ ln -s ../share/sigverse/data/xml/ xml
xmlファイルがあるディレクトリまで遠いのでリンクを貼りました。
//$ cp xml/MyWorld.xml xml/MyWorld_org.xml
//念のためデフォルトのMyWorld.xmlファイルを残しておきました。

This glues the link to the directory where the xml file is located

 $ emacs xml/AgentWorld.xml

AgentWorld.xml
#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
 <world name="myworld2">
 
 <!--重力の設定-->
 <!--Set gravity-->
   <gravity x="0.0" y="-9.8" z="0.0"/>  
 
 <!--エージェントMan-niiの設定-->
 <!--object of agent Man-nii-->
   <instanciate class="Man-nii.xml"> 
 
 <!--エージェント名-->
 <!--Agent Name-->
         <set-attr-value name="name" value="man_000"/> 
 
 <!--C++言語の指定-->
 <!--C++ is the programming language-->
         <set-attr-value name="language" value="c++"/> 
 
 <!--オブジェクトファイルAgentController.soの指定-->
 <!--Name and relative path of the object AgentController.so-->
         <set-attr-value name="implementation"
 value="./NewWorld/AgentController.so"/> 
 
 <!--動力学演算をfalseに設定-->
 <!--put the flag of dynamics to false-->
         <set-attr-value name="dynamics" value="false"/> 
 
 <!--エージェントの位置(x,y,z)-->
 <!--setting the initial position of agent (x,y,z)-->
        <set-attr-value name="x" value="0.0"/>
        <set-attr-value name="y" value="50.0"/>
        <set-attr-value name="z" value="0.0"/>
 
  </instanciate>
</world>
}}
エージェントとしてMan-nii.xmlを読み込むように設定し、動力学演算をfalseにしました。動力学演算をtrueにするとエージェントの関節角度などを直接指定することができないので注意が必要です。

***SIGVerse起動 [#g6cf60e0]
それではSIGVerseを起動してみましょう。
The file being read is Man-nii.xml, dynamics is set to false.

***Starting Sigverse  [#g6cf60e0]

So let's start SIGVerse.

 $ ./sigserver.sh -w AgentWorld.xml -p 9001
SIGViewerを起動して見てみると人間のエージェントが手を広げてスタンバイしていると思います。
シミュレーションを開始するとエージェントの左腕が45°上がります。

Upon connecting to the SimServer in SIGViewer and starting the simulation  , the agent's left arm expands with an increased angle of 45 degree .

#ref(man_1.jpg)

今回setJointAngleという関数を用いて"LARM_JOINT2" を45°にする設定を行いました。"LARM_JOINT2"は左肩のz軸での回転として定義された関節の名前です。同様に"LARM_JOINT0","LARM_JOINT1"はそれぞれ左肩のx軸、y軸の回転として定義されています。（エージェント座標系。）これらの関節名はsigverse-<version>/share/sigverse/data/shape/にあるx3dファイル（nii_man.x3d）で定義されています。サンプルモデルのジョイント名は[[Joint定義一覧]]を参照
We use setJointAngle to set the angle of "LARM_JOINT2" to 45 degree."LARM_JOINT2" is the name of the joint defined for rotation of the left shoulder on Z axis.

AgentWorld.xmlファイルはMan-nii.xmlファイルを読み込みMan-nii.xmlファイルは人間型エージェントの形状を記述したnii_man.x3dを読み込んでいます。
Joint names of these files are defined in sigverse-x3d <version> / share / sigverse / data / shape / (nii_man.x3d) .

**エージェントの属性 [#nb731d79]
***属性の確認 [#g984fed9]
SIGViewerでエージェントをクリックすると右のAttributesタブでエージェントの名前や位置(x,y,z)、エージェントの向き(qw,qx,qy,qz)、視点の位置(epx,epy,epz)などの属性を見ることができます。これらの属性の値はxmlファイルで設定されています。
Joint names and join definitions of the sample model described in current Agentworld.xml file are read from file Man_nii.xml which in turn is reading the file nii_man.x3d describing the shape of a human-type agent. 

**Agent Attributes [#nb731d79]
***Checking Attributes [#g984fed9]

Click the Attributes tab . You can check the attributes of the agent as a whole and also part attributes by clicking on the part of agent model . The general attributes are location of the agent in SIGViewer (x, y, z), the orientation of the agent (qw, qx, qy, qz), the position of the viewpoint (epx, epy, epz) etc . The value of these attributes are set in the xml file.


#ref(attr_1.jpg)

Man-nii humanoid agent is an object that inherits from the Agent class. The default value of the Agent class attribute is set in Agent.xml. Let's see the file Agent.xml

人間型エージェントMan-niiはAgentクラスを継承したオブジェクトです。Agentクラスの属性のデフォルトの値はAgent.xmlで設定されています。見てみましょう。

 $ less xml/Agent.xml

#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
 
 <define-class name="Agent" inherit="Entity.xml">
 
 <attr name="language" type="string" group="model"/>
 <attr name="implementation" type="string" group="model"/>
 
 
    <!-- view point(agent coordinate) -->
 <attr name="vpx" type="double" group="viewpoint" value="0.0"/>
 <attr name="vpy" type="double" group="viewpoint" value="0.0"/>
 <attr name="vpz" type="double" group="viewpoint" value="0.0"/>
 
    <!-- view vector(view coordinate) -->
 <attr name="vvx" type="double" group="viewvector" value="0.0"/>
 <attr name="vvy" type="double" group="viewvector" value="0.0"/>
 <attr name="vvz" type="double" group="viewvector" value="-1.0"/>
}}
上で見たvpx,vvxなどの値が設定されているのがわかります。
さらにAgentクラスはEntityクラスを継承しています。Entityクラスは自律動作を行わない物体のクラスです。

***エージェント、エンティティの基本的な属性 [#g14c1ee2]
エンティティ、エージェントの基本的な属性を紹介します。
Furthermore, Agent class has inherited the Entity class. Entity class is the class of objects that do not behave autonomously.

エンティティの属性
|属性名| 意味 | 備考|
|x,y,z|エンティティの位置 |グローバル座標。x軸は原点から緑矢印 ,y軸は赤矢印 z軸は黄色矢印で表示 |
|fx,fy,fz|エンティティにかかる力||
|vx,vy,vz|エンティティの速度||
|qw,qx,qy,qz|エンティティの向き|クオータニオン表記|
|scalex, scaley, scalez|エンティティの大きさ||
***Agents, basic attributes of an entity[#g14c1ee2]

エージェントの属性
|属性名| 意味 | 備考|
|vpx,vpy,vpz|エージェントの視点の位置(ヒューマノイド以外） |エージェント座標、原点はエージェントの位置|
|vvx,vvy,vvz|視線の方向（ヒューマノイド以外）|ベクトル表記|
|elnk1,elnk2,...elnk9|視点（カメラ）を設置するリンク名（ヒューマノイドのみ）|elnk1はデフォルトでHEAD_LINKに設置されている。|
|epx1,epy1,epz1,...epx9, epy9, epz9|視点（カメラ）の位置（ヒューマノイドのみ）|原点はelnkで指定されたリンクの位置|
|evx1,evy1,evz1,...evx9, evy9, evz9|視点（カメラ）の方向（ヒューマノイドのみ）||
|FOV|エージェントの視野角(高さ）||
|voiceReachRadius|エージェント声が届く範囲||
Entities introduces the basic properties of the agent.

これらの属性はすべて世界ファイルで設定が行えます。
**コマンドによるエージェントの操作 [#ta077379]
前回は定期的に呼び出されるonActionで手を上げるサンプルコードを紹介しました。次はSIGViewerからのメッセージを送信することによりエージェントがお辞儀をするようなサンプルコードを紹介します。
Attribute of an entity

まずAgentController.cppを変更します。
| Attribute Name | Meaning | Remarks |
| x, y, z | position of the entity | global coordinates. green arrow from the origin for x-axis, yellow arrow for y-axis red arrow is shown for  z-axis |
| fx, fy, fz | force on the entity | |
| vx, vy, vz | speed of the entity | |
| qw, qx, qy, qz | direction of the entity | Quaternion notation |
| scalex, scaley, scalez | the size of the entity | |

Agent attributes
| Attribute Name | Meaning | Remarks |
| vpx, vpy, vpz | position of the viewpoint of the agent (non-humanoid) | agents coordinate origin is the position of the agent |
| vvx, vvy, vvz | gaze direction (non-humanoid) | vector notation |
| elnk1, elnk2, ... elnk9 | viewpoint (camera) to establish a link name (humanoid only) | elnk1 HEAD_LINK are installed by default. |
| epx1, epy1, epz1, ... epx9, epy9, epz9 | viewpoint (camera) position (humanoid only) | origin is specified by the position of the link elnk |
| evx1, evy1, evz1, ... evx9, evy9, evz9 | viewpoint (camera) direction (humanoid only) | |
| FOV | Agent viewing angle (height) | |
| voiceReachRadius | Agent voices reach | |

These attributes can be set in all the world files.


** Operating agents with a Command [# ta077379] [#z4a13466]

Last time we introduced the sample code that calls function onAction to raise the hands of agent hands regularly. The following provides sample code to bow to the agent by sending a message from SIGViewer.


Introducing changes to AgentController.cpp
 $ cd NewWorld
 $ emacs AgentController.cpp

AgetnController.cpp

#highlight(cpp){{
#include <string>  //stringをインクルード
#include <string>  //include string header files
#include "Controller.h"
#include "Logger.h"
#include "ControllerEvent.h"


#define PI 3.141592
#define DEG2RAD(DEG) ( (PI) * (DEG) / 180.0 ) 

using namespace std;  //利用名前空間の定義
using namespace std;  //Using namespace definitions

class AgentController : public Controller
{
public:
  double onAction(ActionEvent &evt);

  // メッセージを受信したときの関数onRecvMessageの利用を宣言します。
  // Declare the use of onRecvMessage function when it receives a message

  void onRecvMessage(RecvMessageEvent &evt);
};

double AgentController::onAction(ActionEvent &evt)
{
  try {
    SimObj *my = getObj(myname());
    if (!my->dynamics()) { 

      // 左手を下に下げます。
      // Lower down the left.
      my->setJointAngle("LARM_JOINT2", DEG2RAD(-90));

      // 右手を下に下げます。
      // Lower down the right
     my->setJointAngle("RARM_JOINT2", DEG2RAD(90));

    }
  } catch(SimObj::Exception &) {
    ;
  }
  return 5.0;
}


void AgentController::onRecvMessage(RecvMessageEvent &evt)
{

  //自分自身の取得
  //get an handle to agent
  SimObj *my = getObj(myname());

  //string型のメッセージを取得します。
  //string message is received 
  string value(evt.getString(0)); 
  if (value =="Hello"){

    //腰の関節を45°曲げます。
    //bend the hip joint to 45°
    my->setJointAngle("WAIST_JOINT1", DEG2RAD(45));
  } 

}

extern "C"  Controller * createController ()
{
  return new AgentController;
}
}}
onActionで手を下げ、メッセージを受信したときに呼び出される関数onRecvMessageを追加しました。メッセージが"Hello"だったときにお辞儀をします。

コンパイルします。
agent puts its hands down when onAction function is called , onRecvMessage  function is to be called when a message is received. The message "Hello" is passed which triggers the function that makes the agent  to the bow.

Compile.

 $ make

**実行 [#d5c0a11e]
次にエージェントの属性を編集します。
**Run  [#d5c0a11e]

それでは実行してみましょう。
Edit the attributes of the Agent.
So let's run. 

 $ cd ..
 $ ./sigserver.sh -w AgentWorld.xml -p 9001

SIGViewerで接続して、
sendボタンをクリックしてシミュレーションを開始すると、まずonActionが呼び出されて右手と左手を下に下げます。
Connect with SIGViewer, and start the simulation by clicking the send button, lower left and right hands  go down as the funcion onAction is called first.

#ref(man_2.jpg)
***メッセージ送信 [#hb68ba23]
***Send a message [#hb68ba23]

まずSIGViewerの右上にあるCommandタブをクリックします。
一番上の入力欄でvoiceとgeneralが選択できるようになっています。ここではgeneralを選択します。次に送信対象選択ボタンをクリックして画面上のエージェントをクリックします。エージェント名が自動的に入力されると思います。そしてSendMessage引数のところにHelloと入力します。
Click the "Command" tab on the right side of GUI and select "General" .
Type the name of agent as "man_000" , and write "Hello" , without strings .

#ref(man_3.jpg)
入力が完了したら、実行ボタンを押します。

Click the "Execute" button and check the behavior.
#ref(man_4.jpg)
お辞儀をしました。

**クオータニオンによる関節の回転 [#ecbdc67b]
関節の回転はクオータニオンによって設定することもできます。(※バージョン110223以降で動作）
I bowed.

**Joint rotation with quaternion [#ecbdc67b]

関節回転箇所を以下のように修正します。
Rotation of the joint can also be set by the quaternion.


AgentController.cpp

#highlight(cpp:firstline[51]){{
     //腰の関節を45°曲げます。
     //Bend the Hip joint to 45 degree
     my->setJointAngle("WAIST_JOINT1", DEG2RAD(45));
}}
　　　↓
#highlight(cpp:firstline[51]){{
    //腰の関節をx軸周りに90°回転させます。
    //Rotate 90 degree around X axis for the hip joint
    my->setJointQuaternion("WAIST_JOINT1", 0.707, 0.707, 0.0, 0.0);
}}
setJointQuaternionという関数を使って関節のクオータニオンを設定します。引数で関節名の次にクオータニオンのw,x,y,z成分を指定します。

クオータニオンを指定した場合はあらかじめ設定されている関節の回転軸に関係なく回転します。
正常に関節が回転するか確認してみてください。
setJointquaternion  function is called  to set the joint using quaternions. Then the joint names of the quaternion are specified in argument w, x, y,z.

Please try to make sure the joint is rotated correctly. 

#highlight(end)
Front page   New List of pages Search Recent changes   Help   RSS of recent changes