Diff of 物体を持つ動作_v2.1.0

http://www.sigverse.org/wiki/jp/?%E7%89%A9%E4%BD%93%E3%82%92%E6%8C%81%E3%81%A4%E5%8B%95%E4%BD%9C_v2.1.0

[ Front page ]   [ Edit | Diff | Backup | Upload | Reload ]   [ New | List of pages | Search | Recent changes | Help ]

• The added line is THIS COLOR.
• The deleted line is THIS COLOR.
• Go to 物体を持つ動作_v2.1.0.
• Deleting diff of 物体を持つ動作_v2.1.0

---

Up:[[Tutorial]]　　 　Previous：[[エージェントの衝突]]　　 Next:[[BVHファイルの読み込み]]　　　

----

#contents

*物体を持つ動作 [#k9e46a26]

※このチュートリアルはv2.1.0以降対応しています。

ここではロボット型エージェントが物体を持つ動作をするサンプルを紹介します。

**コントローラ作成 [#p75fcc04]
物体を持って移動するコントローラを作成します。
 $ cd ~/MyWorld
 $ emacs grasp.cpp

grasp.cpp

#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);
  void onRecvMsg(RecvMsgEvent &evt);
  void onCollision(CollisionEvent &evt);

private:
  //物体をつかむ関数Graspの利用を宣言します
  void Grasp(string obj, const char *arm);

  //graspフラグ
  bool grasp;
  string grasp_obj;

  //graspするパーツ
  const char* grasp_parts;

  //衝突フラグ
  bool Colli;
};

void RobotController::onInit(InitEvent &evt)
{
  Colli = false;
  grasp = false;

  //右手でgrasp
  grasp_parts = "RARM_LINK7";
}

double RobotController::onAction(ActionEvent &evt)
{
  //graspがtrueのときgraspし続けます
  if( grasp == true){
    Grasp(grasp_obj, grasp_parts);
  }
  return 0.1;
}

void RobotController::onRecvMsg(RecvMsgEvent &evt)
{
  //取得したメッセージを表示します
  string msg = evt.getMsg();
  LOG_MSG(("msg : %s", msg.c_str()));
  SimObj *my = getObj(myname());

  //" "(スペース)が含まれるメッセージを受信
  if(strstr(msg.c_str()," "))
    {
      //メッセージをスペースの前と後(Joint名と角度)に分けます
      string msg_j;
      string angle_str;
      int n = 0;
      n = msg.find(" ");
      msg_j = msg.substr(0,n);
      angle_str = msg.substr(n+1);

      //受け取ったJoint名と角度を表示します
      LOG_MSG(("Joint Name : %s", msg_j.c_str()));
      LOG_MSG(("Joint Angle : %s", angle_str.c_str()));

      //Jointを回転させます
      double angle = atof(angle_str.c_str());
      my->setJointAngle(msg_j.c_str(), DEG2RAD(angle));
    }
  //メッセージ"rotation"を受信するとエージェントの体全体が回転します
  if (strcmp(msg.c_str(), "rotation") == 0){

    //回転角を設定します
    int dy = 45;

    //y軸周りの自分の回転を得ます（クオータニオン）
    Rotation rot;
    my->getRotation(rot);

    //くオータニオンから回転角(ラジアン)を導出します
    double theta = 2*asin(rot.qy());

    //体全体を回転させます
    double y = theta + DEG2RAD(45);
    if( y >= PI) {
      y = y - 2 * PI;
    }
    my->setAxisAndAngle(0, 1.0, 0, y);
  }

  //メッセージ"move"を受信すると自分の向いている方向に進みます
  if (strcmp(msg.c_str(), "move") == 0){

    //自分の位置を得ます
    Vector3d pos;
    my->getPosition(pos);

    //y軸周りの自分の回転を得ます（クオータニオン）
    Rotation rot;
    my->getRotation(rot);

    //クオータニオンから回転角を導出します。
    double theta = 2*asin(rot.qy());

    //移動距離を初期化します
    double dx = 0.0;
    double dz = 0.0;

    //移動する速度を設定します
    double vel = 10;

    //移動する方向を決定します
    dx = sin(theta) * vel;
    dz = cos(theta) * vel;

    //移動します。
    my->setPosition( pos.x() + dx, pos.y() , pos.z() + dz );
  }

  //つかんでいる物体を離します
  if (strcmp(msg.c_str(), "release") == 0){
    grasp = false;
    grasp_obj = "";
  }
}


//衝突時に呼び出されます。
void RobotController::onCollision(CollisionEvent &evt) {

  if (Colli == false){
    typedef CollisionEvent::WithC C;

    //触れたエンティティの名前を得ます。
    const std::vector<std::string> & with = evt.getWith();

    // 衝突した自分のパーツを得ます.
    const std::vector<std::string> & mparts = evt.getMyParts();

    //　衝突したエンティティでループします.
    for(int i = 0; i < with.size(); i++){

      //右手、右腕に衝突した場合
      if(strcmp(mparts[i].c_str(),"RARM_LINK7") == 0 || strcmp(mparts[i].c_str(),"RARM_LINK4") == 0){

	LOG_MSG(("collide!"));
        //触ったエンティティをつかみます。
        Grasp(with[i],grasp_parts);
        Colli = true;
      }
    }
  }
}

//物体をつかむ関数Grasp
void RobotController::Grasp( string obj,const char* hand)
{
  //自分とつかむ物体を得ます。
  SimObj *my = getObj(myname());
  SimObj *g_obj = getObj(obj.c_str());

  if(!g_obj->dynamics()){
    //自分の手のパーツを得ます。
    CParts * parts = my->getParts(hand);
    if (parts != NULL){

      //自分の手の位置とつかむ物体の位置を得ます。
      Vector3d hand_pos;
      Vector3d obj_pos;
      parts->getPosition(hand_pos);
      g_obj->getPosition(obj_pos);

      //物体をつかみます。
      if (hand_pos.x() != obj_pos.x() || hand_pos.y() != obj_pos.y() || hand_pos.z() != obj_pos.z()){
        g_obj->setPosition(hand_pos.x(), hand_pos.y(), hand_pos.z());
        LOG_MSG(("grasp %s \n", obj.c_str()));

        //graspフラグをtrueにします。
        grasp = true;
        grasp_obj = obj;

      }
    }
  }
}
extern "C" Controller * createController ()
{
  return new RobotController;
}
}}
これはエージェントがエンティティに触れる（衝突）するとその触れたエンティティをつかむというサンプルです。
***エージェントの操作方法 [#u3025f1a]
エージェントに"move"のメッセージを送信すると前に進み、"rotation"を送信すると体の向きを変えます。
また、"「関節名」 「角度」"というメッセージを送信するとエージェントは関節を指定した角度に曲げます。

例）　腰の関節をx軸（エージェント座標）を中心に45°曲げる場合は"WAIST_JOINT1 45"と送信します。JOINT名の定義は[[こちら>Joint定義一覧]]を参照ください。

***掴む [#v27d193d]
このサンプルではGraspという関数を新しく作成しました。この関数の1番目の引数に物体をつかむパーツの名前（右手の場合はRARM_LINK7、左手の場合はLARM_LINK7）、2番目の引数に掴む対象となるエンティティの名前を指定します。一度掴むとgraspフラグがtrueとなり、エージェントが移動しても手から離れません。

***離す [#y77ddd4d]
エージェントに"release"というメッセージを送信するとエージェントは掴んでいる物体を離します。
　
**コンパイル [#n4de9a65]
 $ ./sigmake.sh grasp.cpp

**世界ファイル作成 [#ad481758]
次に世界ファイルを作成します。視覚に関する操作で使った世界ファイルとほぼ同じものを使います。

 $ emacs grasp.xml

grasp.xml

#highlight(xml){{
<?xml version="1.0" encoding="utf8"?>
<world name="myworld5">

  <gravity x="0.0" y="-980.7" z="0.0"/>

  <!--エージェントRobot-niiの設定-->
  <instanciate class="Robot-nii.xml">

    <!--エージェント名-->
    <set-attr-value name="name" value="robot_000"/>

    <!--C++言語の指定-->
    <set-attr-value name="language" value="c++"/>

    <!--コントローラの指定-->
    <set-attr-value name="implementation"
                    value="./grasp.so"/>

    <!--動力学演算をfalseに設定-->
    <set-attr-value name="dynamics" value="false"/>

    <!--エージェントの位置(x,y,z)-->
    <set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="54.0"/>
    <set-attr-value name="z" value="-40.0"/>

    <!--カメラのID番号,リンク名、方向、位置の設定-->
    <camera id="1"
            link="HEAD_LINK"
            direction="0 -1 1"
            position="0.0 0.0 5.0"/>

    <set-attr-value name="collision" value="true"/>
  </instanciate>

  <!--リビングルーム-->
  <instanciate class="seTV.xml">
    <set-attr-value name="name" value="TV_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-20.0"/>
    <set-attr-value name="y" value="87.5"/>
    <set-attr-value name="z" value="-250.0"/>
    <set-attr-value name="visStateAttrName" value="switch"/>
    <set-attr-value name="switch" value="on"/>
    <set-attr-value name="collision" value="true"/>
  </instanciate>

  <instanciate class="seDoll_Bear.xml">
    <set-attr-value name="name" value="kuma_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="0.0"/>
    <set-attr-value name="y" value="9.9"/>
    <set-attr-value name="z" value="0.0"/>
    <set-attr-value name="collision" value="true"/>
  </instanciate>

  <instanciate class="seToy_D.xml">
    <set-attr-value name="name" value="penguin_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="50.0"/>
    <set-attr-value name="y" value="6.15"/>
    <set-attr-value name="z" value="-40.0"/>
    <set-attr-value name="collision" value="true"/>
  </instanciate>

  <instanciate class="seSofa_2seater.xml">
    <set-attr-value name="name" value="sofa_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-200.0"/>
    <set-attr-value name="y" value="31.85"/>
    <set-attr-value name="z" value="-100.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="collision" value="true"/>
  </instanciate>

  <instanciate class="seTVbass_B.xml">
    <set-attr-value name="name" value="TVdai_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-20.0"/>
    <set-attr-value name="y" value="25.1"/>
    <set-attr-value name="z" value="-250.0"/>
    <set-attr-value name="collision" value="true"/>
  </instanciate>

  <instanciate class="sePlant_B.xml">
    <set-attr-value name="name" value="ki_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="100.0"/>
    <set-attr-value name="y" value="56.5"/>
    <set-attr-value name="z" value="-250.0"/>
    <set-attr-value name="collision" value="true"/>
  </instanciate>

  <instanciate class="seSidetable_B.xml">
    <set-attr-value name="name" value="sidetable_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-100.0"/>
    <set-attr-value name="y" value="16.0"/>
    <set-attr-value name="z" value="-100.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="collision" value="true"/>
  </instanciate>
  <instanciate class="seApple.xml">
    <set-attr-value name="name" value="apple_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="-50.0"/>
    <set-attr-value name="y" value="3.875"/>
    <set-attr-value name="z" value="30.0"/>
    <set-attr-value name="collision" value="true"/>
  </instanciate>

  <instanciate class="seOrange.xml">
    <set-attr-value name="name" value="orange_0"/>
    <set-attr-value name="dynamics" value="false"/>
    <set-attr-value name="x" value="70.0"/>
    <set-attr-value name="y" value="2.215"/>
    <set-attr-value name="z" value="-30.0"/>
    <set-attr-value name="collision" value="true"/>
  </instanciate>

</world>
}}
**実行 [#rca7a9d2]
それでは実行してみましょう。
 $ sigserver.sh  -w ./grasp.xml
SIGViewerでシミュレーションサーバに接続してシミュレーションを開始します。
"move"　"rotation"　"関節名 角度"等のメッセージを送信するとエージェントは移動したり関節を曲げたりすることができます。これらを組み合わせて右手で何かエンティティに触れてみてください。
エージェントは物体を掴みます。

※衝突判定は物理演算用の単純形状で行っているため、見た目は衝突しているのになかなかgraspできない場合があります。



#ref(物体を持つ動作(v2.0系)/grasp_1.PNG,40%)

（※注意）SIGViewerで使われているGUIが日本語キーボードに対応していないため、アンダースコアを入力できないことがあります。その場合はwindowsのキーボードの設定を英語に設定しておくと入力できるようになります。


#ref(物体を持つ動作(v2.0系)/key_1.PNG)

そして一度掴んだら移動したり関節を曲げたりしても手を離さないことを確認してください。

#ref(物体を持つ動作(v2.0系)/grasp_2.PNG,40%)

最後に離したい場所でメッセージ"release"を送信して手を離します。

#ref(物体を持つ動作(v2.0系)/grasp_3.PNG,40%)

エージェントが移動したり、手を動かしたりしても掴んだオブジェクトは付いてこなくなります。

#highlight(end)

*Old Version [#la93d438]
-[[物体を持つ動作(v2.0系)]]
-[[物体を持つ動作(v120330, v1.4.8)]]

----

Up:[[Tutorial]]　　 　Previous：[[エージェントの衝突]]　　　　Next:[[BVHファイルの読み込み]]

       

Site admin: inamura

PukiWiki 1.5.0 Copyright © 2001-2006 PukiWiki Developers Team. License is GPL.
Based on "PukiWiki" 1.3 by yu-ji. Powered by PHP 5.5.9-1ubuntu4.29. HTML convert time: 0.002 sec.