インタラクティブにIKに指令するクライアントを作る

前回IKのサービスを使うクライアントを作りました。

今回はもう少しインタラクティブに操作できるものを作ります。

以前使ったturtle_teleopのクライアントと、前回作ったPythonスクリプトを参考に、

カーソルキーを押すと、押した方向に手先位置を動かすクライアントを試しに作ってみました（動作保証しません）。

view plainprint?

1. #include <ros/ros.h>  
2. #include <signal.h>  
3. #include <termios.h>  
4. #include <stdio.h>  
5. #include <kinematics_msgs/IKQuery.h>  
6. #include <kinematics_msgs/IKService.h>  
7. #include <kinematics_msgs/FKService.h>  
8. #include <ik_trajectory_tutorial/ExecuteCartesianIKTrajectory.h>  
9. #include <vector>  
10.   
11. #define KEYCODE_R (0x43)  
12. #define KEYCODE_L (0x44)  
13. #define KEYCODE_U (0x41)  
14. #define KEYCODE_D (0x42)  
15. #define KEYCODE_Q (0x71)  
16.   
17. #define KEYCODE_P (0x70)  
18. #define KEYCODE_N (0x6e)  
19.   
20. #define INIT_POS_X ( 0.55)  
21. #define INIT_POS_Y (-0.19)  
22. #define INIT_POS_Z ( 0.70)  
23. #define INIT_ORI_X ( 0.02)  
24. #define INIT_ORI_Y (-0.09)  
25. #define INIT_ORI_Z ( 0.0)  
26. #define INIT_ORI_W ( 1.0)  
27.   
28. #define DLENGTH (0.04)  
29.   
30. class TeleopPr2  
31. {  
32. public:  
33.   TeleopPr2();  
34.   void keyLoop();  
35.   void call_ik();  
36. private:  
37.   ros::NodeHandle nh_;  
38.   double dx_, dy_, dz_;  
39.   geometry_msgs::Pose pose_;  
40.   ros::ServiceClient ik_client_;  
41. };  
42.   
43. #define IK_SERVICE_NAME "execute_cartesian_ik_trajectory"  
44.   
45. TeleopPr2::TeleopPr2():  
46.   nh_(),  
47.   pose_(),  
48.   ik_client_(nh_.serviceClient<ik_trajectory_tutorial::executecartesianiktrajectory>(IK_SERVICE_NAME))  
49. {  
50.   dx_=dy_=dz_=0;  
51.   
52.   pose_.position.x = INIT_POS_X;  
53.   pose_.position.y = INIT_POS_Y;  
54.   pose_.position.z = INIT_POS_Z;  
55.   
56.   pose_.orientation.x = INIT_ORI_X;  
57.   pose_.orientation.y = INIT_ORI_Y;  
58.   pose_.orientation.z = INIT_ORI_Z;  
59.   pose_.orientation.w = INIT_ORI_W;  
60.   
61.   
62.   ROS_INFO("Waiting for services to be ready");  
63.   ros::service::waitForService(IK_SERVICE_NAME);  
64.     
65.   ROS_INFO("Services ready");  
66. }  
67.   
68. int kfd = 0;  
69. struct termios cooked, raw;  
70.   
71. void quit(int sig)  
72. {  
73.   tcsetattr(kfd, TCSANOW, &cooked);  
74.   ros::shutdown();  
75.   exit(0);  
76. }  
77.   
78.   
79. int main(int argc, char** argv)  
80. {  
81.   ros::init(argc, argv, "teleop_pr2");  
82.   TeleopPr2 teleop_pr2;  
83.   
84.   signal(SIGINT,quit);  
85.   
86.   teleop_pr2.keyLoop();  
87.     
88.   return(0);  
89. }  
90.   
91.   
92. void TeleopPr2::call_ik()  
93. {  
94.   ik_trajectory_tutorial::ExecuteCartesianIKTrajectory ik_req;  
95.   ik_req.request.header = roslib::Header();  
96.   ik_req.request.header.frame_id = "base_link";  
97.   ik_req.request.header.stamp = ros::Time::now();  
98.   
99.   //ik_req.poses = std::vector<geometry_msgs::pose>;  
100.   ik_req.request.poses.resize(0);  
101.   ik_req.request.poses.push_back(pose_);  
102.   ROS_INFO("Calling IK service");  
103.   printf("q%f %f %f, %f %f %f %f\n",  
104.   pose_.position.x,  
105.   pose_.position.y,  
106.   pose_.position.z,  
107.   pose_.orientation.x,  
108.   pose_.orientation.y,  
109.   pose_.orientation.z,  
110.   pose_.orientation.w);  
111.   ik_client_.call(ik_req);  
112.   
113.   if (ik_req.response.success)  
114.   {  
115.     ROS_INFO("success!");  
116.   }  
117.   else  
118.   {  
119.     ROS_ERROR("fail!");  
120.     //back to last pose  
121.     pose_.position.x -= dx_;  
122.     pose_.position.y -= dy_;  
123.     pose_.position.z -= dz_;  
124.   
125.   }  
126. }  
127.   
128. void TeleopPr2::keyLoop()  
129. {  
130.   char c;  
131.   bool dirty=false;  
132.   
133.   
134.   // get the console in raw mode                                                                
135.   tcgetattr(kfd, &cooked);  
136.   memcpy(&raw, &cooked, sizeof(struct termios));  
137.   raw.c_lflag &=~ (ICANON | ECHO);  
138.   // Setting a new line, then end of file                           
139.   raw.c_cc[VEOL] = 1;  
140.   raw.c_cc[VEOF] = 2;  
141.   tcsetattr(kfd, TCSANOW, &raw);  
142.   
143.   puts("Reading from keyboard");  
144.   puts("---------------------------");  
145.   puts("Use arrow keys to move the right arm.");  
146.   
147.   
148.   for(;;)  
149.   {  
150.     // get the next event from the keyboard    
151.     if(read(kfd, &c, 1) < 0)  
152.     {  
153.       perror("read():");  
154.       exit(-1);  
155.     }  
156.   
157.     dx_=dy_=dz_=0;  
158.   
159.     printf("code=%x\n", c);  
160.   
161.     switch(c)  
162.     {  
163.       case KEYCODE_L:  
164.         ROS_DEBUG("LEFT");  
165.         dy_ += DLENGTH;  
166.         dirty = true;  
167.         break;  
168.       case KEYCODE_R:  
169.         ROS_DEBUG("RIGHT");  
170.         dy_ -= DLENGTH;  
171.         dirty = true;  
172.         break;  
173.       case KEYCODE_U:  
174.         ROS_DEBUG("UP");  
175.         dx_ += DLENGTH;  
176.         dirty = true;  
177.         break;  
178.       case KEYCODE_D:  
179.         ROS_DEBUG("DOWN");  
180.         dx_ -= DLENGTH;  
181.         dirty = true;  
182.         break;  
183.       case KEYCODE_P:  
184.         ROS_DEBUG("P");  
185.         dz_ += DLENGTH;  
186.         dirty = true;  
187.         break;  
188.       case KEYCODE_N:  
189.         ROS_DEBUG("N");  
190.         dz_ -= DLENGTH;  
191.         dirty = true;  
192.         break;  
193.     }  
194.      
195.     if(dirty ==true)  
196.     {  
197.       pose_.position.x += dx_;  
198.       pose_.position.y += dy_;  
199.       pose_.position.z += dz_;  
200.   
201.       call_ik();  
202.       dirty=false;  
203.     }  
204.   }  
205.   
206.   return;  
207. }  

#include <ros/ros.h>
#include <signal.h>
#include <termios.h>
#include <stdio.h>
#include <kinematics_msgs/IKQuery.h>
#include <kinematics_msgs/IKService.h>
#include <kinematics_msgs/FKService.h>
#include <ik_trajectory_tutorial/ExecuteCartesianIKTrajectory.h>
#include <vector>

#define KEYCODE_R (0x43)
#define KEYCODE_L (0x44)
#define KEYCODE_U (0x41)
#define KEYCODE_D (0x42)
#define KEYCODE_Q (0x71)

#define KEYCODE_P (0x70)
#define KEYCODE_N (0x6e)

#define INIT_POS_X ( 0.55)
#define INIT_POS_Y (-0.19)
#define INIT_POS_Z ( 0.70)
#define INIT_ORI_X ( 0.02)
#define INIT_ORI_Y (-0.09)
#define INIT_ORI_Z ( 0.0)
#define INIT_ORI_W ( 1.0)

#define DLENGTH (0.04)

class TeleopPr2
{
public:
  TeleopPr2();
  void keyLoop();
  void call_ik();
private:
  ros::NodeHandle nh_;
  double dx_, dy_, dz_;
  geometry_msgs::Pose pose_;
  ros::ServiceClient ik_client_;
};

#define IK_SERVICE_NAME "execute_cartesian_ik_trajectory"

TeleopPr2::TeleopPr2():
  nh_(),
  pose_(),
  ik_client_(nh_.serviceClient<ik_trajectory_tutorial::executecartesianiktrajectory>(IK_SERVICE_NAME))
{
  dx_=dy_=dz_=0;

  pose_.position.x = INIT_POS_X;
  pose_.position.y = INIT_POS_Y;
  pose_.position.z = INIT_POS_Z;

  pose_.orientation.x = INIT_ORI_X;
  pose_.orientation.y = INIT_ORI_Y;
  pose_.orientation.z = INIT_ORI_Z;
  pose_.orientation.w = INIT_ORI_W;


  ROS_INFO("Waiting for services to be ready");
  ros::service::waitForService(IK_SERVICE_NAME);
  
  ROS_INFO("Services ready");
}

int kfd = 0;
struct termios cooked, raw;

void quit(int sig)
{
  tcsetattr(kfd, TCSANOW, &cooked);
  ros::shutdown();
  exit(0);
}


int main(int argc, char** argv)
{
  ros::init(argc, argv, "teleop_pr2");
  TeleopPr2 teleop_pr2;

  signal(SIGINT,quit);

  teleop_pr2.keyLoop();
  
  return(0);
}


void TeleopPr2::call_ik()
{
  ik_trajectory_tutorial::ExecuteCartesianIKTrajectory ik_req;
  ik_req.request.header = roslib::Header();
  ik_req.request.header.frame_id = "base_link";
  ik_req.request.header.stamp = ros::Time::now();

  //ik_req.poses = std::vector<geometry_msgs::pose>;
  ik_req.request.poses.resize(0);
  ik_req.request.poses.push_back(pose_);
  ROS_INFO("Calling IK service");
  printf("q%f %f %f, %f %f %f %f\n",
  pose_.position.x,
  pose_.position.y,
  pose_.position.z,
  pose_.orientation.x,
  pose_.orientation.y,
  pose_.orientation.z,
  pose_.orientation.w);
  ik_client_.call(ik_req);

  if (ik_req.response.success)
  {
    ROS_INFO("success!");
  }
  else
  {
    ROS_ERROR("fail!");
    //back to last pose
    pose_.position.x -= dx_;
    pose_.position.y -= dy_;
    pose_.position.z -= dz_;

  }
}

void TeleopPr2::keyLoop()
{
  char c;
  bool dirty=false;


  // get the console in raw mode                                                              
  tcgetattr(kfd, &cooked);
  memcpy(&raw, &cooked, sizeof(struct termios));
  raw.c_lflag &=~ (ICANON | ECHO);
  // Setting a new line, then end of file                         
  raw.c_cc[VEOL] = 1;
  raw.c_cc[VEOF] = 2;
  tcsetattr(kfd, TCSANOW, &raw);

  puts("Reading from keyboard");
  puts("---------------------------");
  puts("Use arrow keys to move the right arm.");


  for(;;)
  {
    // get the next event from the keyboard  
    if(read(kfd, &c, 1) < 0)
    {
      perror("read():");
      exit(-1);
    }

    dx_=dy_=dz_=0;

    printf("code=%x\n", c);

    switch(c)
    {
      case KEYCODE_L:
        ROS_DEBUG("LEFT");
        dy_ += DLENGTH;
        dirty = true;
        break;
      case KEYCODE_R:
        ROS_DEBUG("RIGHT");
        dy_ -= DLENGTH;
        dirty = true;
        break;
      case KEYCODE_U:
        ROS_DEBUG("UP");
        dx_ += DLENGTH;
        dirty = true;
        break;
      case KEYCODE_D:
        ROS_DEBUG("DOWN");
        dx_ -= DLENGTH;
        dirty = true;
        break;
      case KEYCODE_P:
        ROS_DEBUG("P");
        dz_ += DLENGTH;
        dirty = true;
        break;
      case KEYCODE_N:
        ROS_DEBUG("N");
        dz_ -= DLENGTH;
        dirty = true;
        break;
    }
   
    if(dirty ==true)
    {
      pose_.position.x += dx_;
      pose_.position.y += dy_;
      pose_.position.z += dz_;

      call_ik();
      dirty=false;
    }
  }

  return;
}

静止画だとちょっと説明しずらいですが、
インタラクティブにロボットを操作できるものができました。

僕はロボットの上位系を作るのが好きなので、
だんだん楽しくなってきました。

反応速度にかなり問題がありますが、シミュレータが遅いからなのか、計算が悪いのか、
ちょっと分かりません。
手先の場所によっては、ちょっと動かすだけで手首がぐるぐるしちゃったりしていて、
まだIKの安定感がないですね。

ソースは以下に置きましたので、そっちをDLしてみても結構です。
http://code.google.com/p/otl-ros-pkg/