test_ik_with_cost.cpp
Go to the documentation of this file.
1 /*********************************************************************
2  * Software License Agreement (BSD License)
3  *
4  * Copyright (c) 2008, Willow Garage, Inc.
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  * notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above
14  * copyright notice, this list of conditions and the following
15  * disclaimer in the documentation and/or other materials provided
16  * with the distribution.
17  * * Neither the name of Willow Garage nor the names of its
18  * contributors may be used to endorse or promote products derived
19  * from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *********************************************************************/
34 
35 /* Author: Sachin Chitta */
36 
37 #include <stdio.h>
38 #include <stdlib.h>
39 #include <time.h>
40 #include <ros/ros.h>
41 
42 #include <kinematics_msgs/IKServiceWithCost.h>
43 
44 #include <kinematics_msgs/IKQuery.h>
45 #include <kinematics_msgs/IKService.h>
46 #include <kinematics_msgs/FKService.h>
47 
48 #include <gtest/gtest.h>
49 
50 #define IK_NEAR 1e-4
51 #define IK_NEAR_TRANSLATE 1e-5
52 
53 static const std::string ARM_FK_NAME = "/pr2_ik_right_arm/fk_service";
54 static const std::string ARM_IK_NAME = "/pr2_ik_right_arm/ik_with_cost_service";
55 static const std::string ARM_QUERY_NAME = "/pr2_ik_right_arm/ik_query";
56 
57 static const int NUM_TESTS = 1000;
58 
59 double gen_rand(double min, double max)
60 {
61  int rand_num = rand()%100+1;
62  double result = min + (double)((max-min)*rand_num)/101.0;
63  return result;
64 }
65 
66 bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
67 {
68  if(fabs(v1-v2) > NEAR)
69  return true;
70  return false;
71 }
72 
73 void test()
74 {
75  srand ( time(NULL) ); // initialize random seed
76  ros::NodeHandle rh;
79  ros::ServiceClient query_client = rh.serviceClient<kinematics_msgs::IKQuery>(ARM_QUERY_NAME, true);
80  ros::ServiceClient fk_client = rh.serviceClient<kinematics_msgs::FKService>(ARM_FK_NAME, true);
81  ros::ServiceClient ik_client = rh.serviceClient<kinematics_msgs::IKServiceWithCost>(ARM_IK_NAME, true);
82 
83  // define the service messages
84  kinematics_msgs::IKServiceWithCost::Request request;
85  kinematics_msgs::IKServiceWithCost::Response response;
86 
87  request.data.pose_stamped.header.frame_id = "torso_lift_link";
88  request.data.pose_stamped.header.stamp = ros::Time::now();
89  request.data.pose_stamped.pose.position.x = 0.75;
90  request.data.pose_stamped.pose.position.y = -0.188;
91  request.data.pose_stamped.pose.position.z = 0.0;
92 
93  request.data.pose_stamped.pose.orientation.x = 0.0;
94  request.data.pose_stamped.pose.orientation.y = 0.0;
95  request.data.pose_stamped.pose.orientation.z = 0.0;
96  request.data.pose_stamped.pose.orientation.w = 1.0;
97 
98  request.data.positions.resize(7);
99  request.data.joint_names.resize(7);
100 
101  request.data.joint_names[0] = "r_shoulder_pan_joint";
102  request.data.joint_names[1] = "r_shoulder_lift_joint";
103  request.data.joint_names[2] = "r_upper_arm_roll_joint";
104  request.data.joint_names[3] = "r_elbow_flex_joint";
105  request.data.joint_names[4] = "r_forearm_roll_joint";
106  request.data.joint_names[5] = "r_wrist_flex_joint";
107  request.data.joint_names[6] = "r_wrist_roll_joint";
108 
109  request.link_names.resize(1);
110  request.link_names[0] = "r_wrist_roll_link";
111 
112  bool ik_service_call = ik_client.call(request,response);
113  if(ik_service_call)
114  {
115  for(unsigned int i=0; i < response.solution.size(); i++)
116  ROS_INFO("joint: %s, value: %f",request.data.joint_names[i].c_str(),response.solution[i]);
117  }
118  else
119  {
120  ROS_ERROR("Call was unsuccessful");
121  }
122  ros::shutdown();
123 }
124 
125 int main(int argc, char **argv){
126  ros::init (argc, argv, "pr2_ik_node_test");
127  ros::spinOnce();
128  test();
129 }
ServiceClient serviceClient(const std::string &service_name, bool persistent=false, const M_string &header_values=M_string())
static const int NUM_TESTS
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
double gen_rand(double min, double max)
static const std::string ARM_IK_NAME
bool NOT_NEAR(const double &v1, const double &v2, const double &NEAR)
#define ROS_INFO(...)
static const std::string ARM_QUERY_NAME
void test()
int main(int argc, char **argv)
static Time now()
ROSCPP_DECL void shutdown()
ROSCPP_DECL void spinOnce()
static const std::string ARM_FK_NAME
#define ROS_ERROR(...)
const std::string response
ROSCPP_DECL bool waitForService(const std::string &service_name, int32_t timeout)


pr2_arm_kinematics
Author(s): Sachin Chitta
autogenerated on Wed Jun 5 2019 21:35:32