test_ik_with_cost.cpp
Go to the documentation of this file.
00001 //Software License Agreement (BSD License)
00002 
00003 //Copyright (c) 2008, Willow Garage, Inc.
00004 //All rights reserved.
00005 
00006 //Redistribution and use in source and binary forms, with or without
00007 //modification, are permitted provided that the following conditions
00008 //are met:
00009 
00010 // * Redistributions of source code must retain the above copyright
00011 //   notice, this list of conditions and the following disclaimer.
00012 // * Redistributions in binary form must reproduce the above
00013 //   copyright notice, this list of conditions and the following
00014 //   disclaimer in the documentation and/or other materials provided
00015 //   with the distribution.
00016 // * Neither the name of Willow Garage, Inc. nor the names of its
00017 //   contributors may be used to endorse or promote products derived
00018 //   from this software without specific prior written permission.
00019 
00020 //THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00021 //"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00022 //LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00023 //FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00024 //COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00025 //INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00026 //BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00027 //LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00028 //CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00029 //LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00030 //ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031 //POSSIBILITY OF SUCH DAMAGE.
00032 
00033 #include <stdio.h>
00034 #include <stdlib.h>
00035 #include <time.h>
00036 #include <ros/ros.h>
00037 
00038 #include <kinematics_msgs/IKServiceWithCost.h>
00039 
00040 #include <kinematics_msgs/IKQuery.h>
00041 #include <kinematics_msgs/IKService.h>
00042 #include <kinematics_msgs/FKService.h>
00043 
00044 #include <gtest/gtest.h>
00045 
00046 #define IK_NEAR 1e-4
00047 #define IK_NEAR_TRANSLATE 1e-5
00048 
00049 static const std::string ARM_FK_NAME = "/pr2_ik_right_arm/fk_service";
00050 static const std::string ARM_IK_NAME = "/pr2_ik_right_arm/ik_with_cost_service";
00051 static const std::string ARM_QUERY_NAME = "/pr2_ik_right_arm/ik_query";
00052 
00053 static const int NUM_TESTS = 1000;
00054 
00055 double gen_rand(double min, double max)
00056 {
00057   int rand_num = rand()%100+1;
00058   double result = min + (double)((max-min)*rand_num)/101.0;
00059   return result;
00060 }
00061 
00062 bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
00063 {
00064    if(fabs(v1-v2) > NEAR)
00065       return true;
00066    return false;
00067 }
00068 
00069 void test()
00070 {
00071   srand ( time(NULL) ); // initialize random seed
00072   ros::NodeHandle rh;
00073   ros::service::waitForService(ARM_QUERY_NAME);
00074   ros::service::waitForService(ARM_FK_NAME);
00075   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::IKQuery>(ARM_QUERY_NAME, true);
00076   ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::FKService>(ARM_FK_NAME, true);
00077   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::IKServiceWithCost>(ARM_IK_NAME, true);
00078 
00079   // define the service messages
00080   kinematics_msgs::IKServiceWithCost::Request request;
00081   kinematics_msgs::IKServiceWithCost::Response response;
00082 
00083   request.data.pose_stamped.header.frame_id = "torso_lift_link";
00084   request.data.pose_stamped.header.stamp    = ros::Time::now();
00085   request.data.pose_stamped.pose.position.x = 0.75;
00086   request.data.pose_stamped.pose.position.y = -0.188;
00087   request.data.pose_stamped.pose.position.z = 0.0;
00088 
00089   request.data.pose_stamped.pose.orientation.x = 0.0;
00090   request.data.pose_stamped.pose.orientation.y = 0.0;
00091   request.data.pose_stamped.pose.orientation.z = 0.0;
00092   request.data.pose_stamped.pose.orientation.w = 1.0;
00093 
00094   request.data.positions.resize(7);
00095   request.data.joint_names.resize(7);
00096 
00097   request.data.joint_names[0] = "r_shoulder_pan_joint";
00098   request.data.joint_names[1] = "r_shoulder_lift_joint";
00099   request.data.joint_names[2] = "r_upper_arm_roll_joint";
00100   request.data.joint_names[3] = "r_elbow_flex_joint";
00101   request.data.joint_names[4] = "r_forearm_roll_joint";
00102   request.data.joint_names[5] = "r_wrist_flex_joint";
00103   request.data.joint_names[6] = "r_wrist_roll_joint";
00104 
00105   request.link_names.resize(1);
00106   request.link_names[0] = "r_wrist_roll_link";
00107 
00108   bool ik_service_call = ik_client.call(request,response);
00109   if(ik_service_call)
00110     {
00111       for(unsigned int i=0; i < response.solution.size(); i++)
00112     ROS_INFO("joint: %s, value: %f",request.data.joint_names[i].c_str(),response.solution[i]);
00113     }
00114   else
00115     {
00116       ROS_ERROR("Call was unsuccessful");
00117     }
00118   ros::shutdown();
00119 }
00120 
00121 int main(int argc, char **argv){
00122   ros::init (argc, argv, "pr2_ik_node_test");
00123   ros::spinOnce();
00124   test();
00125 }


pr2_moveit_plugins
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Oct 6 2014 11:14:04