move_arm: move_arm: regression_test_cylinder

00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  * 
00004  *  Copyright (c) 2008, Willow Garage, Inc.
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 Willow Garage nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     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  *********************************************************************/
00036 
00037 /* \author: Ioan Sucan */
00038 
00039 #include <ros/ros.h>
00040 #include <tf/tf.h>
00041 #include <actionlib/client/simple_action_client.h>
00042 
00043 #include <stdio.h>
00044 #include <stdlib.h>
00045 #include <time.h>
00046 #include <boost/thread.hpp>
00047 #include <ros/ros.h>
00048 #include <gtest/gtest.h>
00049 #include <iostream>
00050 
00051 #include "regression_test_arm_helpers.h"
00052 
00053 void spinThread()
00054 {
00055   ros::spin();
00056 }
00057 
00058 TEST(MoveArm, goToPoseGoal)
00059 {
00060   ros::NodeHandle nh;
00061   actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm(nh, "move_right_arm");
00062   boost::thread spin_thread(&spinThread);
00063 
00064   move_arm.waitForServer();
00065   ROS_INFO("Connected to server");
00066   move_arm::MoveArmGoal goalA;
00067 
00068   //getting a monitor so that we can track the configuration of the arm
00069   planning_environment::RobotModels rm("robot_description");
00070   EXPECT_TRUE(rm.loadedModels());
00071     
00072   tf::TransformListener tf;
00073   planning_environment::KinematicModelStateMonitor km(&rm, &tf);
00074   km.waitForState();
00075   //should have the state at this point  
00076 
00077   goalA.goal_constraints.set_pose_constraint_size(1);
00078   goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
00079   goalA.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
00080   
00081   //starting configuration
00082   //-position [x, y, z]    = [0.77025, -.18, 0.73]
00083   //-rotation [x, y, z, w] = [0, -0.05, 0, 0]
00084 
00085   goalA.goal_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X 
00086     + arm_navigation_msgs::PoseConstraint::POSITION_Y 
00087     + arm_navigation_msgs::PoseConstraint::POSITION_Z
00088     + arm_navigation_msgs::PoseConstraint::ORIENTATION_R 
00089     + arm_navigation_msgs::PoseConstraint::ORIENTATION_P 
00090     + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
00091   
00092   goalA.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
00093   goalA.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
00094   goalA.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
00095   goalA.goal_constraints.pose_constraint[0].pose.pose.position.z = 0.84;
00096     
00097   goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
00098   goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
00099   goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
00100   goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
00101 
00102   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.01;
00103   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.01;
00104   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
00105   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.01;
00106   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.01;
00107   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.01;
00108 
00109   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.01;
00110   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.01;
00111   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.01;
00112   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.01;
00113   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.01;
00114   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.01;
00115 
00116   goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
00117 
00118   goalA.contacts.resize(2);
00119   goalA.contacts[0].links.push_back("r_gripper_l_finger_link");
00120   goalA.contacts[0].links.push_back("r_gripper_r_finger_link");
00121   goalA.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
00122   goalA.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
00123   
00124   goalA.contacts[0].depth = 0.04;
00125   goalA.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
00126   goalA.contacts[0].bound.dimensions.push_back(0.3);
00127   goalA.contacts[0].pose = goalA.goal_constraints.pose_constraint[0].pose;
00128   
00129   goalA.contacts[1].links.push_back("r_gripper_palm_link");
00130   goalA.contacts[1].links.push_back("r_wrist_roll_link");
00131   goalA.contacts[1].depth = 0.02;
00132   goalA.contacts[1].bound.type = arm_navigation_msgs::Object::SPHERE;
00133   goalA.contacts[1].bound.dimensions.push_back(0.2);
00134   goalA.contacts[1].pose = goalA.goal_constraints.pose_constraint[0].pose;
00135   
00136   if (nh.ok())
00137   {
00138     bool finished_within_time = false;
00139     move_arm.sendGoal(goalA);
00140     finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
00141     
00142     EXPECT_TRUE(finished_within_time);
00143     if (!finished_within_time)
00144     {
00145       move_arm.cancelGoal();
00146       ROS_INFO("Timed out achieving goal A");
00147     }
00148     else
00149     {
00150       actionlib::SimpleClientGoalState state = move_arm.getState();
00151       ROS_INFO("Action finished: %s",state.toString().c_str());
00152       EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
00153 
00154       double dist_pose;
00155       double dist_angle;
00156       
00157       diffConfig(km,goalA,dist_pose,dist_angle);
00158 
00159       //close enough - summed tolerances
00160       EXPECT_TRUE(dist_pose < .005+.005+.01);
00161       EXPECT_TRUE(dist_angle < .005*3);
00162 
00163       EXPECT_TRUE(finalStateMatchesGoal(km,goalA));
00164     
00165     }
00166   }
00167 
00168   nh.setParam( "/move_right_arm/long_range_only", true);
00169 
00170   move_arm::MoveArmGoal goalB;
00171   
00172   //starting configuration
00173   //-position [x, y, z]    = [0.77025, -.18, 0.73]
00174   //-rotation [x, y, z, w] = [0, -0.05, 0, 0]
00175 
00176   goalB.goal_constraints.set_pose_constraint_size(1);
00177   goalB.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
00178   goalB.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
00179   goalB.goal_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X 
00180     + arm_navigation_msgs::PoseConstraint::POSITION_Y 
00181     + arm_navigation_msgs::PoseConstraint::POSITION_Z
00182     + arm_navigation_msgs::PoseConstraint::ORIENTATION_R 
00183     + arm_navigation_msgs::PoseConstraint::ORIENTATION_P 
00184     + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
00185   
00186   goalB.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
00187   goalB.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
00188   goalB.goal_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
00189   goalB.goal_constraints.pose_constraint[0].pose.pose.position.z = 0.5;
00190   
00191   goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
00192   goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
00193   goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
00194   goalB.goal_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
00195   
00196   goalB.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.05;
00197   goalB.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.05;
00198   goalB.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.05;
00199   goalB.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.05;
00200   goalB.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.05;
00201   goalB.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.05;
00202 
00203   goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.05;
00204   goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.05;
00205   goalB.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.05;
00206   goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.05;
00207   goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.05;
00208   goalB.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.05;
00209 
00210   goalB.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
00211 
00212   goalB.path_constraints.set_pose_constraint_size(1);
00213   goalB.path_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
00214   goalB.path_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
00215   goalB.path_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X 
00216     + arm_navigation_msgs::PoseConstraint::POSITION_Y
00217     + arm_navigation_msgs::PoseConstraint::POSITION_Z
00218     + arm_navigation_msgs::PoseConstraint::ORIENTATION_R
00219     + arm_navigation_msgs::PoseConstraint::ORIENTATION_P; 
00220 //     + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
00221   goalB.path_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
00222   goalB.path_constraints.pose_constraint[0].pose.pose.position.x = 0.60;
00223   goalB.path_constraints.pose_constraint[0].pose.pose.position.y = -0.25;
00224   goalB.path_constraints.pose_constraint[0].pose.pose.position.z = .45;
00225     
00226   goalB.path_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
00227   goalB.path_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
00228   goalB.path_constraints.pose_constraint[0].pose.pose.orientation.z = 0;
00229   goalB.path_constraints.pose_constraint[0].pose.pose.orientation.w = 1.0;
00230 
00231   goalB.path_constraints.pose_constraint[0].position_tolerance_above.x = 0.1;
00232   goalB.path_constraints.pose_constraint[0].position_tolerance_above.y = 0.1;
00233   goalB.path_constraints.pose_constraint[0].position_tolerance_below.x = 0.1;
00234   goalB.path_constraints.pose_constraint[0].position_tolerance_below.y = 0.1;
00235   goalB.path_constraints.pose_constraint[0].position_tolerance_above.z = 10.0;
00236   goalB.path_constraints.pose_constraint[0].position_tolerance_below.z = 0.1;
00237 
00238   goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.1;
00239   goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.1;
00240 //   goalB.path_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.05;
00241   goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.1;
00242   goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.1;
00243 //   goalB.path_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.05;
00244 
00245   goalB.path_constraints.pose_constraint[0].orientation_importance = 0.4;
00246 
00247   goalB.contacts.resize(2);
00248   goalB.contacts[0].links.push_back("r_gripper_l_finger_link");
00249   goalB.contacts[0].links.push_back("r_gripper_r_finger_link");
00250   goalB.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
00251   goalB.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
00252   
00253   goalB.contacts[0].depth = 0.04;
00254   goalB.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
00255   goalB.contacts[0].bound.dimensions.push_back(0.3);
00256   goalB.contacts[0].pose = goalB.goal_constraints.pose_constraint[0].pose;
00257   
00258   goalB.contacts[1].links.push_back("r_gripper_palm_link");
00259   goalB.contacts[1].links.push_back("r_wrist_roll_link");
00260   goalB.contacts[1].depth = 0.02;
00261   goalB.contacts[1].bound.type = arm_navigation_msgs::Object::SPHERE;
00262   goalB.contacts[1].bound.dimensions.push_back(0.2);
00263   goalB.contacts[1].pose = goalB.goal_constraints.pose_constraint[0].pose;
00264   
00265   if (nh.ok())
00266   {
00267     move_arm.sendGoal(goalB);
00268 
00269     ros::Time start_time = ros::Time::now();
00270     ros::Duration elapsed(0.0);
00271 
00272     //trying ticks in case time gets wonky
00273     unsigned int ticks=0;
00274 
00275     while(1) {
00276       
00277       bool result_during_cycle = move_arm.waitForResult(ros::Duration(.2));
00278       
00279       //got some result before time out
00280       if(result_during_cycle) {
00281         break;
00282       }
00283       
00284       elapsed = ros::Time::now()-start_time;
00285 
00286       std::cout << "Time " << elapsed.toSec() << std::endl;
00287 
00288       //checking if we've gone over max time - if so bail
00289       if(elapsed.toSec() > 10.0 || ticks++ > 50) break;
00290 
00291       goalB.path_constraints.pose_constraint[0].pose.pose.orientation.x = 0;
00292       goalB.path_constraints.pose_constraint[0].pose.pose.orientation.y = 0;
00293 
00294       //check that the path obeys constraints
00295       EXPECT_TRUE(currentStateSatisfiesPathConstraints(km,goalB));
00296       
00297     }
00298 
00299     if (elapsed.toSec() > 10.0)
00300     {
00301       move_arm.cancelGoal();
00302       ROS_INFO("Timed out achieving goal A");
00303       EXPECT_TRUE(elapsed.toSec() < 10.0);
00304     }
00305     else
00306     {
00307       actionlib::SimpleClientGoalState state = move_arm.getState();
00308       ROS_INFO("Action finished: %s",state.toString().c_str());
00309       EXPECT_TRUE(state == actionlib::SimpleClientGoalState::SUCCEEDED);
00310 
00311       double dist_pose;
00312       double dist_angle;
00313       
00314       diffConfig(km,goalA,dist_pose,dist_angle);
00315 
00316       //close enough - summed tolerances
00317       EXPECT_TRUE(dist_pose < .01+.01+.002);
00318       EXPECT_TRUE(dist_angle < .05*3);
00319 
00320       EXPECT_TRUE(finalStateMatchesGoal(km,goalB));
00321     }
00322   }
00323 
00324   ros::shutdown();
00325   spin_thread.join();
00326 }
00327 
00328 int main(int argc, char **argv){
00329   testing::InitGoogleTest(&argc, argv);
00330   ros::init (argc, argv, "move_arm_regression_test");
00331   return RUN_ALL_TESTS();
00332 }
regression_test_cylinder_constraint.cpp
