acceptancetest_hironx.cpp
Go to the documentation of this file.
00001 /*
00002  * Software License Agreement (BSD License)
00003  *
00004  * Copyright (c) 2014, TORK (Tokyo Opensource Robotics Kyokai Association)
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions
00009  * are met:
00010  *
00011  *  * Redistributions of source code must retain the above copyright
00012  *    notice, this list of conditions and the following disclaimer.
00013  *  * Redistributions in binary form must reproduce the above
00014  *    copyright notice, this list of conditions and the following
00015  *    disclaimer in the documentation and/or other materials provided
00016  *    with the distribution.
00017  *  * Neither the name of JSK Lab, University of Tokyo. nor the
00018  *    names of its contributors may be used to endorse or promote products
00019  *    derived from this software without specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00035 #include <iostream>
00036 #include <string>
00037 #include <vector>
00038 
00039 #include <ros/ros.h>
00040 #include <ros_client.cpp>
00041 #include <testutil/abst_acceptancetest.hpp>
00042 #include <testutil/acceptancetest_ros.hpp>
00043 
00044 #include <sstream>
00045 
00046 template<class T>
00047   std::string to_string(T a)
00048   {
00049     std::stringstream ss;
00050     ss << a;
00051     return ss.str();
00052   }
00053 
00054 class AcceptanceTest_Hiro
00055 {
00056   /*
00057    This class holds methods that can be used for verification of the robot
00058    Kawada Industries' dual-arm robot Hiro (and the same model of other robots
00059    e.g. NEXTAGE OPEN).
00060 
00061    This test class is:
00062    - Intended to be run as nosetests, ie. roscore isn't available by itself.
00063    - Almost all the testcases don't run without an access to a robot running.
00064 
00065    Above said, combined with a rostest that launches roscore and robot (either
00066    simulation or real) hopefully these testcases can be run, both in batch
00067    manner by rostest and in nosetest manner.
00068 
00069    Prefix for methods 'at' means 'acceptance test'.
00070 
00071    All time arguments are second.
00072 
00073    */
00074 public:
00075   typedef std::vector<double> DVEC;
00076   typedef std::vector<DVEC> DMAT;
00077 
00078   static const double positions_larm_deg_up[6];
00079   static const double positions_larm_deg_down[6];
00080   static const double positions_rarm_deg_down[6];
00081   static const double positions_larm_deg_up_sync[6];
00082   static const double positions_rarm_deg_up_sync[6];
00083   static const double rotation_angles_head_1[2][2];
00084   static const double rotation_angles_head_2[2][2];
00085   static const double positions_torso_deg[2][1];
00086   static const double R_Z_SMALLINCREMENT;
00087   // WORKSPACE; x near, y far
00088   static const double pos_l_x_near_y_far[3];
00089   static const double rpy_l_x_near_y_far[3];
00090   static const double pos_r_x_near_y_far[3];
00091   static const double rpy_r_x_near_y_far[3];
00092   // workspace; x far, y far
00093   static const double pos_l_x_far_y_far[3];
00094   static const double rpy_l_x_far_y_far[3];
00095   static const double pos_r_x_far_y_far[3];
00096   static const double rpy_r_x_far_y_far[3];
00097 
00098   static const double TASK_DURATION_DEFAULT;
00099   static const double TASK_DURATION_TORSO;
00100   static const double DURATION_EACH_SMALLINCREMENT;
00101   static const double TASK_DURATION_HEAD;
00102   static const int SLEEP_TIME;
00103   static const int SLEEP_OVERWRITE;
00104 
00105   static const int DURATION_TOTAL_SMALLINCREMENT;  // second
00106 
00107   DVEC POSITIONS_LARM_DEG_UP;
00108   DVEC POSITIONS_LARM_DEG_DOWN;
00109   DVEC POSITIONS_RARM_DEG_DOWN;
00110   DVEC POSITIONS_LARM_DEG_UP_SYNC;
00111   DVEC POSITIONS_RARM_DEG_UP_SYNC;
00112   DMAT ROTATION_ANGLES_HEAD_1;
00113   DMAT ROTATION_ANGLES_HEAD_2;
00114   DMAT POSITIONS_TORSO_DEG;
00115 
00116   // WORKSPACE; X NEAR, Y FAR
00117   DVEC POS_L_X_NEAR_Y_FAR;
00118   DVEC RPY_L_X_NEAR_Y_FAR;
00119   DVEC POS_R_X_NEAR_Y_FAR;
00120   DVEC RPY_R_X_NEAR_Y_FAR;
00121   // WORKSPACE; X FAR, Y FAR
00122   DVEC POS_L_X_FAR_Y_FAR;
00123   DVEC RPY_L_X_FAR_Y_FAR;
00124   DVEC POS_R_X_FAR_Y_FAR;
00125   DVEC RPY_R_X_FAR_Y_FAR;
00126 
00127 #define INIT_VEC(name, num) name, name+num
00128 #define VEC_COPY(from, to, N1, N2) for (int i = 0; i < N1; ++i) for (int j = 0; j < N2; ++j)to[i][j] = from[i][j];
00129   AcceptanceTest_Hiro(std::string name = "Hironx(Robot)0", std::string url = "") :
00130       rtm_robotname(name), rtm_url(url), ros(ROS_Client()), acceptance_ros_client(ros),
00131       POSITIONS_LARM_DEG_UP(INIT_VEC(positions_larm_deg_up, 6)),
00132       POSITIONS_LARM_DEG_DOWN(INIT_VEC(positions_larm_deg_down, 6)),
00133       POSITIONS_RARM_DEG_DOWN(INIT_VEC(positions_rarm_deg_down, 6)),
00134       POSITIONS_LARM_DEG_UP_SYNC(INIT_VEC(positions_larm_deg_up_sync, 6)),
00135       POSITIONS_RARM_DEG_UP_SYNC(INIT_VEC(positions_rarm_deg_up_sync, 6)),
00136 
00137       ROTATION_ANGLES_HEAD_1(2, DVEC(2)), ROTATION_ANGLES_HEAD_2(2, DVEC(2)), POSITIONS_TORSO_DEG(2, DVEC(1)),
00138 
00139       // WORKSPACE, X NEAR, Y FAR
00140       POS_L_X_NEAR_Y_FAR(INIT_VEC(pos_l_x_near_y_far, 3)), RPY_L_X_NEAR_Y_FAR(INIT_VEC(rpy_l_x_near_y_far, 3)),
00141       POS_R_X_NEAR_Y_FAR(INIT_VEC(pos_r_x_near_y_far, 3)), RPY_R_X_NEAR_Y_FAR(INIT_VEC(rpy_r_x_near_y_far, 3)),
00142       // WORKSPACE, X FAR, Y FAR
00143       POS_L_X_FAR_Y_FAR(INIT_VEC(pos_l_x_far_y_far, 3)), RPY_L_X_FAR_Y_FAR(INIT_VEC(rpy_l_x_far_y_far, 3)),
00144       POS_R_X_FAR_Y_FAR(INIT_VEC(pos_r_x_far_y_far, 3)), RPY_R_X_FAR_Y_FAR(INIT_VEC(rpy_r_x_far_y_far, 3))
00145   {
00146     VEC_COPY(rotation_angles_head_1, ROTATION_ANGLES_HEAD_1, 2, 2)
00147     VEC_COPY(rotation_angles_head_2, ROTATION_ANGLES_HEAD_2, 2, 2)
00148     VEC_COPY(positions_torso_deg, POSITIONS_TORSO_DEG, 2, 1)
00149   }
00150 
00151   std::string retName()
00152   {
00153     return rtm_robotname;
00154   }
00155 
00156   void wait_input(std::string msg, bool do_wait_input = false)
00157   {
00158     /*
00159      @type msg: str
00160      @param msg: To be printed on prompt.
00161      @param do_wait_input: If True python commandline waits for any input
00162      to proceed.
00163      */
00164     if (!msg.empty())
00165       msg = "\n" + msg + "= ";
00166     if (do_wait_input)
00167     {
00168       // exception
00169     }
00170   }
00171 
00172   void run_test_ros(bool do_wait_input = false)
00173   {
00174     /*
00175      Run by ROS exactly the same actions that run_tests_rtm performs.
00176      @param do_wait_input: If True, the user will be asked to hit any key
00177      before each task to proceed.
00178      */
00179     ROS_INFO("********RUN_TEST_ROS************");
00180     run_tests(acceptance_ros_client, do_wait_input);
00181   }
00182 
00183   void run_tests(AbstAcceptanceTest& test_client, bool do_wait_input = false)
00184   {
00185     std::string msg_type_client = "";
00186     if (typeid(test_client) == typeid(AcceptanceTestRos))
00187       msg_type_client = "(ROS) ";
00188     // else if(typeid(test_client) == typeid(AcceptanceTestRTM))
00189     //     msg_type_client = "(RTM) ";
00190     std::string msg = msg_type_client;
00191 
00192     test_client.go_initpos();
00193 
00194     std::string msg_task = "TASK-1. Move each arm separately to the given pose by passing joint space.";
00195     msg = msg_type_client + msg_task;
00196     wait_input(msg, do_wait_input);
00197     move_armbyarm_jointangles(test_client);
00198 
00199     msg_task =
00200         "TASK-1. Move each arm separately to the given pose by passing pose in hand space "
00201         "(i.e. orthogonal coordinate of eef).";
00202     ROS_INFO("%s", msg_task.c_str());
00203     msg = msg_type_client + msg_task;
00204     wait_input(msg, do_wait_input);
00205     move_armbyarm_pose(test_client);
00206 
00207     msg_task = "TASK-2. Move both arms at the same time using relative distance and angle from the current pose.";
00208     ROS_INFO("%s", msg_task.c_str());
00209     msg = msg_type_client + msg_task;
00210     wait_input(msg, do_wait_input);
00211     movearms_together(test_client);
00212 
00213     msg_task = "TASK-3. Move arm with very small increment (0.1mm/sec).\n\tRight hand 3 cm upward over 30 seconds.";
00214     msg = msg_type_client + msg_task;
00215     wait_input(msg, do_wait_input);
00216     set_pose_relative(test_client);
00217 
00218     msg_task = "In the beginning you\"ll see the displacement of the previous task."
00219         "\nTASK-4. Move head using Joint angles in degree.";
00220     ROS_INFO("%s", msg_task.c_str());
00221     msg = msg_type_client + msg_task;
00222     wait_input(msg, do_wait_input);
00223     move_head(test_client);
00224 
00225     msg_task = "TASK-5. Rotate torso to the left and right 130 degree.";
00226     ROS_INFO("%s", msg_task.c_str());
00227     msg = msg_type_client + msg_task;
00228     wait_input(msg, do_wait_input);
00229     move_torso(test_client);
00230 
00231     msg_task = std::string("TASK-6. Overwrite ongoing action.\n\t6-1."
00232                            "While rotating torso toward left, it getscanceled and rotate toward "
00233                            "right.\n\t6-2. While lifting left hand, right hand also tries to reach "
00234                            "the same height that gets cancelled so that it stays lower than the left hand.");
00235     ROS_INFO("%s", msg_task.c_str());
00236     msg = msg_type_client + msg_task;
00237     wait_input(msg, do_wait_input);
00238     overwrite_torso(test_client);
00239     // task6-2
00240     overwrite_arm(test_client);
00241 
00242     msg_task = "TASK-7. Show the capable workspace on front side of the robot.";
00243     msg = msg_type_client + msg_task;
00244     wait_input(msg, do_wait_input);
00245     show_workspace(test_client);
00246   }
00247 
00248   void move_armbyarm_jointangles(AbstAcceptanceTest& test_client)
00249   {
00250     /*
00251      @type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest
00252      */
00253     test_client.go_initpos(7.0);
00254     std::string arm = AbstAcceptanceTest::GRNAME_LEFT_ARM;
00255     test_client.set_joint_angles(arm, POSITIONS_LARM_DEG_UP, "Task1 " + to_string(arm));
00256 
00257     arm = AbstAcceptanceTest::GRNAME_RIGHT_ARM;
00258     test_client.set_joint_angles(arm, POSITIONS_RARM_DEG_DOWN, "Task1 " + to_string(arm));
00259     ros::Duration(SLEEP_TIME).sleep();
00260   }
00261 
00262   void move_armbyarm_pose(AbstAcceptanceTest& test_client)
00263   {
00264     ROS_INFO("** _move_armbyarm_pose isn\"t yet implemented");
00265   }
00266 
00267   void movearms_together(AbstAcceptanceTest& test_client)
00268   {
00269     // @type test_client: hironx_ros_robotics.abst_acceptancetest.AbstAcceptanceTest
00270     test_client.go_initpos();
00271     std::string arm = AbstAcceptanceTest::GRNAME_LEFT_ARM;
00272     test_client.set_joint_angles(arm, POSITIONS_LARM_DEG_UP_SYNC, to_string(arm), TASK_DURATION_DEFAULT, false);
00273     /*
00274      task2; Under current implementation, left arm
00275      always moves first, followed immediately by right arm")
00276      */
00277     arm = AbstAcceptanceTest::GRNAME_RIGHT_ARM;
00278     test_client.set_joint_angles(arm, POSITIONS_RARM_DEG_DOWN, to_string(arm), TASK_DURATION_DEFAULT, false);
00279     ros::Duration(SLEEP_TIME).sleep();
00280   }
00281 
00282   void set_pose_relative(AbstAcceptanceTest& test_client)
00283   {
00284     test_client.go_initpos();
00285     double delta = R_Z_SMALLINCREMENT;
00286     double t = DURATION_EACH_SMALLINCREMENT;
00287     double t_total_sec = DURATION_TOTAL_SMALLINCREMENT;
00288     int total_increment = t_total_sec / t;
00289     std::string msg_1 = "Right arm is moving upward with " + to_string(delta) + "mm increment per " + to_string(t)
00290         + "s.";
00291     std::string msg_2 = " (Total " + to_string(delta * total_increment) + "cm over " + to_string(t_total_sec) + "s).";
00292     ROS_INFO("%s", (msg_1 + msg_2).c_str());
00293     for (int i = 0; i < total_increment; ++i)
00294     {
00295       std::string msg_eachloop = to_string(i);
00296       msg_eachloop += "th loop;";
00297       test_client.set_pose_relative(AbstAcceptanceTest::GRNAME_RIGHT_ARM, 0, 0, delta, 0, 0, 0, msg_eachloop, t, true);
00298     }
00299   }
00300 
00301   void move_head(AbstAcceptanceTest& test_client)
00302   {
00303     test_client.go_initpos();
00304 
00305     for (std::vector<std::vector<double> >::iterator positions = ROTATION_ANGLES_HEAD_1.begin();
00306         positions != ROTATION_ANGLES_HEAD_1.end(); ++positions)
00307     {
00308       test_client.set_joint_angles(AbstAcceptanceTest::GRNAME_HEAD, *positions, "(1);", TASK_DURATION_HEAD);
00309     }
00310     for (std::vector<std::vector<double> >::iterator positions = ROTATION_ANGLES_HEAD_2.begin();
00311         positions != ROTATION_ANGLES_HEAD_2.end(); ++positions)
00312     {
00313       test_client.set_joint_angles(AbstAcceptanceTest::GRNAME_HEAD, *positions, "(2);", TASK_DURATION_HEAD);
00314     }
00315   }
00316 
00317   void move_torso(AbstAcceptanceTest& test_client)
00318   {
00319     test_client.go_initpos();
00320     for (std::vector<std::vector<double> >::iterator positions = POSITIONS_TORSO_DEG.begin();
00321         positions != POSITIONS_TORSO_DEG.end(); ++positions)
00322     {
00323       test_client.set_joint_angles(AbstAcceptanceTest::GRNAME_TORSO, *positions);
00324     }
00325   }
00326 
00327   void overwrite_torso(AbstAcceptanceTest& test_client)
00328   {
00329     test_client.go_initpos();
00330     test_client.set_joint_angles(AbstAcceptanceTest::GRNAME_TORSO, POSITIONS_TORSO_DEG[0], "(1)", TASK_DURATION_TORSO,
00331                                  false);
00332 
00333     ros::Duration(SLEEP_OVERWRITE).sleep();
00334 
00335     test_client.set_joint_angles(AbstAcceptanceTest::GRNAME_TORSO, POSITIONS_TORSO_DEG[1], "(2)", TASK_DURATION_TORSO,
00336                                  true);
00337 
00338     ros::Duration(SLEEP_OVERWRITE).sleep();
00339   }
00340 
00341   void overwrite_arm(AbstAcceptanceTest& test_client)
00342   {
00343     test_client.go_initpos();
00344     test_client.set_joint_angles(AbstAcceptanceTest::GRNAME_LEFT_ARM, POSITIONS_LARM_DEG_UP_SYNC, "(1)",
00345                                  TASK_DURATION_DEFAULT, false);
00346     test_client.set_joint_angles(
00347         AbstAcceptanceTest::GRNAME_RIGHT_ARM,
00348         POSITIONS_RARM_DEG_UP_SYNC,
00349         std::string("(2) begins. Overwrite previous arm command.\n\t"
00350                     "In the beginning both arm starts to move to the\n\tsame height"
00351                     "but to the left arm interrupting\ncommand is sent and it goes downward."),
00352         TASK_DURATION_DEFAULT, false);
00353 
00354     ros::Duration(SLEEP_OVERWRITE).sleep();
00355 
00356     test_client.set_joint_angles(AbstAcceptanceTest::GRNAME_LEFT_ARM, POSITIONS_LARM_DEG_DOWN, "(3)",
00357                                  TASK_DURATION_DEFAULT, false);
00358 
00359     ros::Duration((int)TASK_DURATION_DEFAULT).sleep();
00360   }
00361 
00362   void show_workspace(AbstAcceptanceTest& test_client)
00363   {
00364     test_client.go_initpos();
00365     std::string msg = "; ";
00366 
00367     std::string larm = AbstAcceptanceTest::GRNAME_LEFT_ARM;
00368     std::string rarm = AbstAcceptanceTest::GRNAME_RIGHT_ARM;
00369     // X near, Y far.
00370     test_client.set_pose(larm, POS_L_X_NEAR_Y_FAR, RPY_L_X_NEAR_Y_FAR, msg + to_string(larm), TASK_DURATION_DEFAULT,
00371                          false);
00372     test_client.set_pose(rarm, POS_R_X_NEAR_Y_FAR, RPY_R_X_NEAR_Y_FAR, msg + to_string(rarm), TASK_DURATION_DEFAULT,
00373                          true);
00374     // X far, Y far.
00375     test_client.set_pose(larm, POS_L_X_FAR_Y_FAR, RPY_L_X_FAR_Y_FAR, msg + to_string(larm), TASK_DURATION_DEFAULT,
00376                          false);
00377     test_client.set_pose(rarm, POS_R_X_FAR_Y_FAR, RPY_R_X_FAR_Y_FAR, msg + to_string(rarm), TASK_DURATION_DEFAULT,
00378                          true);
00379   }
00380 
00381 private:
00382   const std::string rtm_robotname;
00383   const std::string rtm_url;
00384 
00385   ROS_Client ros;
00386   AcceptanceTestRos acceptance_ros_client;
00387 };
00388 
00389 // init parameters
00390 const double AcceptanceTest_Hiro::positions_larm_deg_up[6] = {-4.697, -2.012, -117.108, -17.180, 29.146, -3.739};
00391 const double AcceptanceTest_Hiro::positions_larm_deg_down[6] = {6.196, -5.311, -73.086, -15.287, -12.906, -2.957};
00392 const double AcceptanceTest_Hiro::positions_rarm_deg_down[6] = {-4.949, -3.372, -80.050, 15.067, -7.734, 3.086};
00393 const double AcceptanceTest_Hiro::positions_larm_deg_up_sync[6] = {-4.695, -2.009, -117.103, -17.178, 29.138, -3.738};
00394 const double AcceptanceTest_Hiro::positions_rarm_deg_up_sync[6] = {4.695, -2.009, -117.103, 17.178, 29.138, 3.738};
00395 const double AcceptanceTest_Hiro::rotation_angles_head_1[2][2] = { {0.1, 0.0}, {50.0, 10.0}};
00396 const double AcceptanceTest_Hiro::rotation_angles_head_2[2][2] = { {-50, -10}, {0, 0}};
00397 const double AcceptanceTest_Hiro::positions_torso_deg[2][1] = { {130.0}, {-130.0}};
00398 
00399 const double AcceptanceTest_Hiro::R_Z_SMALLINCREMENT = 0.0001;
00400 // Workspace; X near, Y far
00401 const double AcceptanceTest_Hiro::pos_l_x_near_y_far[3] = {0.326, 0.474, 1.038};
00402 const double AcceptanceTest_Hiro::rpy_l_x_near_y_far[3] = {-3.075, -1.569, 3.074};
00403 const double AcceptanceTest_Hiro::pos_r_x_near_y_far[3] = {0.326, -0.472, 1.048};
00404 const double AcceptanceTest_Hiro::rpy_r_x_near_y_far[3] = {3.073, -1.569, -3.072};
00405 // workspace; x far, y far
00406 const double AcceptanceTest_Hiro::pos_l_x_far_y_far[3] = {0.47548142379781055, 0.17430276793604782, 1.0376878025614884};
00407 const double AcceptanceTest_Hiro::rpy_l_x_far_y_far[3] = {-3.075954857224205, -1.5690261926181046, 3.0757659493049574};
00408 const double AcceptanceTest_Hiro::pos_r_x_far_y_far[3] = {0.4755337947019357, -0.17242322190721648, 1.0476395479774052};
00409 const double AcceptanceTest_Hiro::rpy_r_x_far_y_far[3] = {3.0715850722714944, -1.5690204449882248, -3.071395243174742};
00410 
00411 const double AcceptanceTest_Hiro::TASK_DURATION_DEFAULT = 4.0;
00412 const double AcceptanceTest_Hiro::DURATION_EACH_SMALLINCREMENT = 0.1;
00413 const double AcceptanceTest_Hiro::TASK_DURATION_TORSO = AcceptanceTest_Hiro::TASK_DURATION_DEFAULT;
00414 const double AcceptanceTest_Hiro::TASK_DURATION_HEAD = AcceptanceTest_Hiro::TASK_DURATION_DEFAULT;
00415 const int AcceptanceTest_Hiro::SLEEP_TIME = 2;
00416 const int AcceptanceTest_Hiro::SLEEP_OVERWRITE = 2;
00417 
00418 const int AcceptanceTest_Hiro::DURATION_TOTAL_SMALLINCREMENT = 30;  // Second.
00419 int main(int argc, char* argv[])
00420 {
00421   // Init the ROS node
00422   ros::init(argc, argv, "hironx_ros_client");
00423 
00424   AcceptanceTest_Hiro test;
00425   test.run_test_ros();
00426   return 0;
00427 }


hironx_ros_bridge
Author(s): Kei Okada
autogenerated on Sun Sep 13 2015 23:21:39