Backup of キネクトによるエージェントの操作(ロボット)(No. 1)

List of Backups
View the diff.
View the diff current.
View the source.
Go to キネクトによるエージェントの操作(ロボット).
- 1 (2012-02-17 (Fri) 11:27:24)

キネクトによるエージェントの操作?

highlight(): the language, , is not supported.

{{

#include <string>

#include "Controller.h"

#include "Logger.h"

#include "ControllerEvent.h"

#define PI 3.141592

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

using namespace std;

class AgentController : public Controller { public:

 double onAction(ActionEvent &evt);
 void onRecvMessage(RecvMessageEvent &evt);
 void onInit(InitEvent &evt);

private:

 //初期位置
 double m_posx;
 double m_posy;
 double m_posz;
 double m_range;
 //データ数（関節数）最大値
 int m_maxsize;

};

void AgentController::onInit(InitEvent &evt) {

 SimObj *my = getObj(myname());
 m_posx = my->x();
 m_posy = my->y();
 m_posz = my->z();
 m_range = 0.1;
 m_maxsize = 15;
 my->setJointQuaternion("LARM_JOINT2",0.707, 0.0, 0.0, 0.707, true);
 my->setJointQuaternion("RARM_JOINT2",-0.707, 0.0, 0.0, 0.707, true);

}

double AgentController::onAction(ActionEvent &evt) {

 return 10.0;

}

void AgentController::onRecvMessage(RecvMessageEvent &evt) {

 //自分自身の取得
 SimObj *my = getObj(myname());

 //メッセージ取得
 char *all_msg = (char*)evt.getString(0);

 char *msg = strtok(all_msg," ");
 if(strcmp(msg,"KINECT_DATA") == 0)
   {
     int i = 0;
     while(true)
       {
         i++;
         if(i == m_maxsize+1) break;
         char *type = strtok(NULL,":");

         //体の位置
         if(strcmp(type,"POSITION") == 0)
           {
             double x = atof(strtok(NULL,","));
             double y = atof(strtok(NULL,","));
             double z = atof(strtok(NULL," "));
             my->setPosition(m_posx+x,m_posy+y,m_posz+z);
             continue;
           }

         //体全体の回転
         else if(strcmp(type,"WAIST") == 0)
           {
             double w = atof(strtok(NULL,","));
             double x = atof(strtok(NULL,","));
             double y = atof(strtok(NULL,","));
             double z = atof(strtok(NULL," "));
             my->setAxisAndAngle(x,y,z,acos(w)*2);
             continue;
           }

         else if(strcmp(type,"END") == 0)  break;

         //関節の回転
         else
           {
             double w = atof(strtok(NULL,","));
             double x = atof(strtok(NULL,","));
             double y = atof(strtok(NULL,","));
             double z = atof(strtok(NULL," "));
             double angle = acos(w)*2;
             double tmp = sin(angle/2);
             double vx = x/tmp;
             double vy = y/tmp;
             double vz = z/tmp;
             double len = sqrt(vx*vx+vy*vy+vz*vz);
             if(len < (1 - m_range) || (1 + m_range) < len) continue;
             my->setJointQuaternion(type,w,x,y,z);
             continue;
           }
       }
   }

}

extern "C" Controller * createController () {

 return new AgentController;

} }}

Backup of キネクトによるエージェントの操作(ロボット) (No. 1)