00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
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
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
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
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
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;
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
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
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
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
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
00160
00161
00162
00163
00164 if (!msg.empty())
00165 msg = "\n" + msg + "= ";
00166 if (do_wait_input)
00167 {
00168
00169 }
00170 }
00171
00172 void run_test_ros(bool do_wait_input = false)
00173 {
00174
00175
00176
00177
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
00189
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
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
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
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
00275
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
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
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
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
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
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;
00419 int main(int argc, char* argv[])
00420 {
00421
00422 ros::init(argc, argv, "hironx_ros_client");
00423
00424 AcceptanceTest_Hiro test;
00425 test.run_test_ros();
00426 return 0;
00427 }