test_ik_with_cost.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 /* Author: Sachin Chitta */
00036 
00037 #include <stdio.h>
00038 #include <stdlib.h>
00039 #include <time.h>
00040 #include <ros/ros.h>
00041 
00042 #include <kinematics_msgs/IKServiceWithCost.h>
00043 
00044 #include <kinematics_msgs/IKQuery.h>
00045 #include <kinematics_msgs/IKService.h>
00046 #include <kinematics_msgs/FKService.h>
00047 
00048 #include <gtest/gtest.h>
00049 
00050 #define IK_NEAR 1e-4
00051 #define IK_NEAR_TRANSLATE 1e-5
00052 
00053 static const std::string ARM_FK_NAME = "/pr2_ik_right_arm/fk_service";
00054 static const std::string ARM_IK_NAME = "/pr2_ik_right_arm/ik_with_cost_service";
00055 static const std::string ARM_QUERY_NAME = "/pr2_ik_right_arm/ik_query";
00056 
00057 static const int NUM_TESTS = 1000;
00058 
00059 double gen_rand(double min, double max)
00060 {
00061   int rand_num = rand()%100+1;
00062   double result = min + (double)((max-min)*rand_num)/101.0;
00063   return result;
00064 }
00065 
00066 bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
00067 {
00068    if(fabs(v1-v2) > NEAR)
00069       return true;
00070    return false;
00071 }
00072 
00073 void test()
00074 {
00075   srand ( time(NULL) ); // initialize random seed
00076   ros::NodeHandle rh;
00077   ros::service::waitForService(ARM_QUERY_NAME);
00078   ros::service::waitForService(ARM_FK_NAME);
00079   ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::IKQuery>(ARM_QUERY_NAME, true);
00080   ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::FKService>(ARM_FK_NAME, true);
00081   ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::IKServiceWithCost>(ARM_IK_NAME, true);
00082 
00083   // define the service messages
00084   kinematics_msgs::IKServiceWithCost::Request request;
00085   kinematics_msgs::IKServiceWithCost::Response response;
00086 
00087   request.data.pose_stamped.header.frame_id = "torso_lift_link";
00088   request.data.pose_stamped.header.stamp    = ros::Time::now();
00089   request.data.pose_stamped.pose.position.x = 0.75;
00090   request.data.pose_stamped.pose.position.y = -0.188;
00091   request.data.pose_stamped.pose.position.z = 0.0;
00092 
00093   request.data.pose_stamped.pose.orientation.x = 0.0;
00094   request.data.pose_stamped.pose.orientation.y = 0.0;
00095   request.data.pose_stamped.pose.orientation.z = 0.0;
00096   request.data.pose_stamped.pose.orientation.w = 1.0;
00097 
00098   request.data.positions.resize(7);
00099   request.data.joint_names.resize(7);
00100 
00101   request.data.joint_names[0] = "r_shoulder_pan_joint";
00102   request.data.joint_names[1] = "r_shoulder_lift_joint";
00103   request.data.joint_names[2] = "r_upper_arm_roll_joint";
00104   request.data.joint_names[3] = "r_elbow_flex_joint";
00105   request.data.joint_names[4] = "r_forearm_roll_joint";
00106   request.data.joint_names[5] = "r_wrist_flex_joint";
00107   request.data.joint_names[6] = "r_wrist_roll_joint";
00108 
00109   request.link_names.resize(1);
00110   request.link_names[0] = "r_wrist_roll_link";
00111 
00112   bool ik_service_call = ik_client.call(request,response);
00113   if(ik_service_call)
00114     {
00115       for(unsigned int i=0; i < response.solution.size(); i++)
00116     ROS_INFO("joint: %s, value: %f",request.data.joint_names[i].c_str(),response.solution[i]);
00117     }
00118   else
00119     {
00120       ROS_ERROR("Call was unsuccessful");
00121     }
00122   ros::shutdown();
00123 }
00124 
00125 int main(int argc, char **argv){
00126   ros::init (argc, argv, "pr2_ik_node_test");
00127   ros::spinOnce();
00128   test();
00129 }


pr2_moveit_plugins
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Thu Jul 4 2019 19:51:07