キネクトによるエージェントの操作(ロボット)
http://www.sigverse.org/wiki/en/index.php?%E3%82%AD%E3%83%8D%E3%82%AF%E3%83%88%E3%81%AB%E3%82%88%E3%82%8B%E3%82%A8%E3%83%BC%E3%82%B8%E3%82%A7%E3%83%B3%E3%83%88%E3%81%AE%E6%93%8D%E4%BD%9C%28%E3%83%AD%E3%83%9C%E3%83%83%E3%83%88%29
[
Front page
] [
Edit
|
Freeze
|
Diff
|
Backup
|
Upload
|
Reload
] [
New
|
List of pages
|
Search
|
Recent changes
|
Help
]
System
Tutorial
License
Links
Old Content
recent(20)
2023-05-29
Tutorial using ROS for ver.3
2023-05-26
Tutorial for ver.3
SIGVerse
2022-12-26
(TurtleBot3)Grasp Automatically
2022-07-04
(HSR)Cleanup Task using Cloud and VR
(HSR)Cleanup Task using Cloud and VR with Playback function
2022-04-05
(TurtleBot3)SLAM
(TurtleBot3)Grasp by Keyboard Operation
2021-12-23
Melodic_(TurtleBot3)Grasp Automatically
Melodic_Tutorial using ROS for ver.3
Noetic_Tutorial using ROS for ver.3
Noetic_(TurtleBot3)Grasp Automatically
2021-12-21
(Turtlebot2)Operate with Keyboard
(Turtlebot2)Follower
2021-08-18
(TIAGo)Cleanup Task
(TIAGo)Operate with Keyboard
(HSR)Cleanup Task
(HSR)Operate with Keyboard
(PR2)Operate with Keyboard
(TurtleBot3)Recognize Human Pointing
キネクトによるエージェントの操作
?
キネクトでヒューマノイドロボットを操作する場合のコントローラ
†
#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; }
Last-modified: 2012-02-17 (Fri) 11:31:46 (4423d)