regression_test_fake_ompl_detect_collision.cpp
Go to the documentation of this file.
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 #include <move_arm/MoveArmAction.h>
00043 #include <planning_environment/monitors/kinematic_model_state_monitor.h>
00044 #include <planning_models/kinematic_state.h>
00045 
00046 #include <stdio.h>
00047 #include <stdlib.h>
00048 #include <time.h>
00049 #include <boost/thread.hpp>
00050 #include <ros/ros.h>
00051 #include <gtest/gtest.h>
00052 
00053 void setupGoal(const std::vector<std::string> &names, move_arm::MoveArmGoal &goal)
00054 {
00055     goal.goal_constraints.joint_constraint.resize(names.size());
00056     for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
00057     {
00058         goal.goal_constraints.joint_constraint[i].header.stamp = ros::Time::now();
00059         goal.goal_constraints.joint_constraint[i].header.frame_id = "/base_link";
00060         goal.goal_constraints.joint_constraint[i].joint_name = names[i];
00061         goal.goal_constraints.joint_constraint[i].value.resize(1);
00062         goal.goal_constraints.joint_constraint[i].tolerance_above.resize(1);
00063         goal.goal_constraints.joint_constraint[i].tolerance_below.resize(1);
00064         goal.goal_constraints.joint_constraint[i].value[0] = 0.0;
00065         goal.goal_constraints.joint_constraint[i].tolerance_below[0] = 0.0;
00066         goal.goal_constraints.joint_constraint[i].tolerance_above[0] = 0.0;
00067     }
00068 
00069     goal.contacts.resize(1);
00070     goal.contacts[0].links.push_back("r_gripper_l_finger_link");
00071     goal.contacts[0].links.push_back("r_gripper_r_finger_link");
00072     goal.contacts[0].links.push_back("r_gripper_l_finger_tip_link");
00073     goal.contacts[0].links.push_back("r_gripper_r_finger_tip_link");
00074     goal.contacts[0].links.push_back("r_gripper_palm_link");
00075     goal.contacts[0].links.push_back("r_wrist_roll_link");
00076     goal.contacts[0].depth = 0.04;
00077     goal.contacts[0].bound.type = arm_navigation_msgs::Object::SPHERE;
00078     goal.contacts[0].bound.dimensions.push_back(0.5);
00079 
00080     goal.contacts[0].pose.header.stamp = ros::Time::now();
00081     goal.contacts[0].pose.header.frame_id = "/base_link";
00082     goal.contacts[0].pose.pose.position.x = 1;
00083     goal.contacts[0].pose.pose.position.y = 0;
00084     goal.contacts[0].pose.pose.position.z = 0.5;
00085 
00086     goal.contacts[0].pose.pose.orientation.x = 0;
00087     goal.contacts[0].pose.pose.orientation.y = 0;
00088     goal.contacts[0].pose.pose.orientation.z = 0;
00089     goal.contacts[0].pose.pose.orientation.w = 1;
00090 }
00091 
00092 void goalToState(const move_arm::MoveArmGoal &goal, planning_models::KinematicState &sp)
00093 {
00094     for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size() ; ++i)
00095     {
00096         sp.setParamsJoint(&goal.goal_constraints.joint_constraint[i].value[0],
00097                           goal.goal_constraints.joint_constraint[i].joint_name);
00098     }
00099 }
00100 
00101 tf::Transform effPosition(const planning_environment::KinematicModelStateMonitor &km, const move_arm::MoveArmGoal &goal)
00102 {
00103     planning_models::KinematicState sp(*km.getRobotState());
00104     goalToState(goal, sp);
00105     km.getKinematicModel()->computeTransforms(sp.getParams());
00106     return km.getKinematicModel()->getJoint("r_wrist_roll_joint")->after->globalTrans;
00107 }
00108 
00109 void diffConfig(const planning_environment::KinematicModelStateMonitor &km, move_arm::MoveArmGoal &goal)
00110 {
00111   std::vector<std::string> names(7);
00112     names[0] = "r_shoulder_pan_joint";
00113     names[1] = "r_shoulder_lift_joint";
00114     names[2] = "r_upper_arm_roll_joint";
00115     names[3] = "r_elbow_flex_joint";
00116     names[4] = "r_forearm_roll_joint";
00117     names[5] = "r_wrist_flex_joint";
00118     names[6] = "r_wrist_roll_joint";
00119     for (unsigned int i = 0 ; i < goal.goal_constraints.joint_constraint.size(); ++i)
00120     {
00121         std::cout << "  " << goal.goal_constraints.joint_constraint[i].joint_name << " = "
00122                   << goal.goal_constraints.joint_constraint[i].value[0] - km.getRobotState()->getParamsJoint(goal.goal_constraints.joint_constraint[i].joint_name)[0]
00123                   << std::endl;
00124     }
00125 
00126     tf::Transform pose1 = effPosition(km, goal);
00127     move_arm::MoveArmGoal temp;
00128 
00129     planning_models::KinematicState sp(*(km.getRobotState()));
00130     sp.enforceBounds();
00131     for (unsigned int i = 0 ; i < names.size() ; ++i)
00132     {
00133       temp.goal_constraints.joint_constraint[i].value[0] =
00134         sp.getParamsJoint(temp.goal_constraints.joint_constraint[i].joint_name)[0];
00135     }
00136     tf::Transform pose2 = effPosition(km, temp);
00137     std::cout << std::endl;
00138     double dist = pose1.getOrigin().distance(pose2.getOrigin());
00139     std::cout << "  -position distance: " << dist << std::endl;
00140     double angle = pose1.getRotation().angle(pose2.getRotation());
00141     std::cout << "  -rotation distance: " << angle << std::endl;
00142 }
00143 
00144 void spinThread()
00145 {
00146   ros::spin();
00147 }
00148 
00149 TEST(MoveArm, goToPoseGoal)
00150 {
00151   ros::NodeHandle nh;
00152   actionlib::SimpleActionClient<move_arm::MoveArmAction> move_arm(nh, "move_right_arm");
00153   boost::thread spin_thread(&spinThread);
00154 
00155   nh.setParam( "/move_right_arm/long_range_only", true);
00156 
00157   move_arm.waitForServer();
00158   ROS_INFO("Connected to server");
00159   move_arm::MoveArmGoal goalA;
00160 
00161   //getting a monitor so that we can track the configuration of the arm
00162   planning_environment::RobotModels rm("robot_description");
00163   EXPECT_TRUE(rm.loadedModels());
00164     
00165   tf::TransformListener tf;
00166   planning_environment::KinematicModelStateMonitor km(&rm, &tf);
00167   km.waitForState();
00168   //should have the state at this point
00169   
00170   goalA.goal_constraints.set_pose_constraint_size(1);
00171   goalA.goal_constraints.pose_constraint[0].pose.header.stamp = ros::Time::now();
00172   goalA.goal_constraints.pose_constraint[0].pose.header.frame_id = "base_link";
00173   
00174   //-position [x, y, z]    = [0.413446, 0.0124666, 0.206998]
00175   //-rotation [x, y, z, w] = [0.132744, 0.888279, 0.288412, 0.331901]
00176 
00177   goalA.goal_constraints.pose_constraint[0].type = arm_navigation_msgs::PoseConstraint::POSITION_X 
00178     + arm_navigation_msgs::PoseConstraint::POSITION_Y 
00179     + arm_navigation_msgs::PoseConstraint::POSITION_Z
00180     + arm_navigation_msgs::PoseConstraint::ORIENTATION_R 
00181     + arm_navigation_msgs::PoseConstraint::ORIENTATION_P 
00182     + arm_navigation_msgs::PoseConstraint::ORIENTATION_Y;
00183   
00184   goalA.goal_constraints.pose_constraint[0].link_name = "r_wrist_roll_link";
00185   goalA.goal_constraints.pose_constraint[0].pose.pose.position.x = 0.413446;
00186   goalA.goal_constraints.pose_constraint[0].pose.pose.position.y = 0.0124666;
00187   goalA.goal_constraints.pose_constraint[0].pose.pose.position.z = 0.206998;
00188     
00189   goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.x = .132744;
00190   goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.y = .888279;
00191   goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.z = .288412;
00192   goalA.goal_constraints.pose_constraint[0].pose.pose.orientation.w = .331901;
00193 
00194   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.003;
00195   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.003;
00196   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.003;
00197   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.003;
00198   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.003;
00199   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.003;
00200 
00201   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.x = 0.005;
00202   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.y = 0.005;
00203   goalA.goal_constraints.pose_constraint[0].position_tolerance_above.z = 0.01;
00204   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.x = 0.005;
00205   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.y = 0.005;
00206   goalA.goal_constraints.pose_constraint[0].position_tolerance_below.z = 0.005;
00207   
00208   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.x = 0.005;
00209   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.y = 0.005;
00210   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_above.z = 0.005;
00211   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.x = 0.005;
00212   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.y = 0.005;
00213   goalA.goal_constraints.pose_constraint[0].orientation_tolerance_below.z = 0.005;  
00214 
00215   goalA.goal_constraints.pose_constraint[0].orientation_importance = 0.2;
00216 
00217   if (nh.ok())
00218   {
00219     bool finished_within_time = false;
00220     move_arm.sendGoal(goalA);
00221     finished_within_time = move_arm.waitForResult(ros::Duration(10.0));
00222     
00223     diffConfig(km,goalA);
00224 
00225     EXPECT_TRUE(finished_within_time);
00226     if (!finished_within_time)
00227     {
00228       move_arm.cancelGoal();
00229       ROS_INFO("Timed out achieving goal A");
00230     }
00231     else
00232     {
00233       actionlib::SimpleClientGoalState state = move_arm.getState();
00234       ROS_INFO("Action finished: %s",state.toString().c_str());
00235     }
00236   }
00237 
00238   //now we check that we actually achieved the configuration within the tolerances
00239   diffConfig(km,goalA);
00240 
00241   ros::shutdown();
00242   spin_thread.join();
00243 }
00244 
00245 int main(int argc, char **argv){
00246   testing::InitGoogleTest(&argc, argv);
00247   ros::init (argc, argv, "move_arm_regression_test");
00248   return RUN_ALL_TESTS();
00249 }


move_arm
Author(s): Ioan Sucan, Sachin Chitta(sachinc@willowgarage.com)
autogenerated on Thu Dec 12 2013 11:09:39