ik_tests.cpp
Go to the documentation of this file.
1 /********************************************************************************
2 Copyright (c) 2016, TRACLabs, Inc.
3 All rights reserved.
4 
5 Redistribution and use in source and binary forms, with or without modification,
6  are permitted provided that the following conditions are met:
7 
8  1. Redistributions of source code must retain the above copyright notice,
9  this list of conditions and the following disclaimer.
10 
11  2. Redistributions in binary form must reproduce the above copyright notice,
12  this list of conditions and the following disclaimer in the documentation
13  and/or other materials provided with the distribution.
14 
15  3. Neither the name of the copyright holder nor the names of its contributors
16  may be used to endorse or promote products derived from this software
17  without specific prior written permission.
18 
19 THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
20 ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
21 WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED.
22 IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT,
23 INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
24 BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
25 DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
26 LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE
27 OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED
28 OF THE POSSIBILITY OF SUCH DAMAGE.
29 ********************************************************************************/
30 
31 #include <boost/date_time.hpp>
32 #include <trac_ik/trac_ik.hpp>
33 #include <ros/ros.h>
34 #include <kdl/chainiksolverpos_nr_jl.hpp>
35 
36 double fRand(double min, double max)
37 {
38  double f = (double)rand() / RAND_MAX;
39  return min + f * (max - min);
40 }
41 
42 
43 void test(ros::NodeHandle& nh, double num_samples, std::string chain_start, std::string chain_end, double timeout, std::string urdf_param)
44 {
45 
46  double eps = 1e-5;
47 
48  // This constructor parses the URDF loaded in rosparm urdf_param into the
49  // needed KDL structures. We then pull these out to compare against the KDL
50  // IK solver.
51  TRAC_IK::TRAC_IK tracik_solver(chain_start, chain_end, urdf_param, timeout, eps);
52 
53  KDL::Chain chain;
54  KDL::JntArray ll, ul; //lower joint limits, upper joint limits
55 
56  bool valid = tracik_solver.getKDLChain(chain);
57 
58  if (!valid)
59  {
60  ROS_ERROR("There was no valid KDL chain found");
61  return;
62  }
63 
64  valid = tracik_solver.getKDLLimits(ll, ul);
65 
66  if (!valid)
67  {
68  ROS_ERROR("There were no valid KDL joint limits found");
69  return;
70  }
71 
72  assert(chain.getNrOfJoints() == ll.data.size());
73  assert(chain.getNrOfJoints() == ul.data.size());
74 
75  ROS_INFO("Using %d joints", chain.getNrOfJoints());
76 
77 
78  // Set up KDL IK
79  KDL::ChainFkSolverPos_recursive fk_solver(chain); // Forward kin. solver
80  KDL::ChainIkSolverVel_pinv vik_solver(chain); // PseudoInverse vel solver
81  KDL::ChainIkSolverPos_NR_JL kdl_solver(chain, ll, ul, fk_solver, vik_solver, 1, eps); // Joint Limit Solver
82  // 1 iteration per solve (will wrap in timed loop to compare with TRAC-IK)
83 
84 
85  // Create Nominal chain configuration midway between all joint limits
86  KDL::JntArray nominal(chain.getNrOfJoints());
87 
88  for (uint j = 0; j < nominal.data.size(); j++)
89  {
90  nominal(j) = (ll(j) + ul(j)) / 2.0;
91  }
92 
93  // Create desired number of valid, random joint configurations
94  std::vector<KDL::JntArray> JointList;
95  KDL::JntArray q(chain.getNrOfJoints());
96 
97  for (uint i = 0; i < num_samples; i++)
98  {
99  for (uint j = 0; j < ll.data.size(); j++)
100  {
101  q(j) = fRand(ll(j), ul(j));
102  }
103  JointList.push_back(q);
104  }
105 
106 
107  boost::posix_time::ptime start_time;
108  boost::posix_time::time_duration diff;
109 
110  KDL::JntArray result;
111  KDL::Frame end_effector_pose;
112  int rc;
113 
114  double total_time = 0;
115  uint success = 0;
116 
117  ROS_INFO_STREAM("*** Testing KDL with " << num_samples << " random samples");
118 
119  for (uint i = 0; i < num_samples; i++)
120  {
121  fk_solver.JntToCart(JointList[i], end_effector_pose);
122  double elapsed = 0;
123  result = nominal; // start with nominal
124  start_time = boost::posix_time::microsec_clock::local_time();
125  do
126  {
127  q = result; // when iterating start with last solution
128  rc = kdl_solver.CartToJnt(q, end_effector_pose, result);
129  diff = boost::posix_time::microsec_clock::local_time() - start_time;
130  elapsed = diff.total_nanoseconds() / 1e9;
131  }
132  while (rc < 0 && elapsed < timeout);
133  total_time += elapsed;
134  if (rc >= 0)
135  success++;
136 
137  if (int((double)i / num_samples * 100) % 10 == 0)
138  ROS_INFO_STREAM_THROTTLE(1, int((i) / num_samples * 100) << "\% done");
139  }
140 
141  ROS_INFO_STREAM("KDL found " << success << " solutions (" << 100.0 * success / num_samples << "\%) with an average of " << total_time / num_samples << " secs per sample");
142 
143 
144  total_time = 0;
145  success = 0;
146 
147  ROS_INFO_STREAM("*** Testing TRAC-IK with " << num_samples << " random samples");
148 
149  for (uint i = 0; i < num_samples; i++)
150  {
151  fk_solver.JntToCart(JointList[i], end_effector_pose);
152  double elapsed = 0;
153  start_time = boost::posix_time::microsec_clock::local_time();
154  rc = tracik_solver.CartToJnt(nominal, end_effector_pose, result);
155  diff = boost::posix_time::microsec_clock::local_time() - start_time;
156  elapsed = diff.total_nanoseconds() / 1e9;
157  total_time += elapsed;
158  if (rc >= 0)
159  success++;
160 
161  if (int((double)i / num_samples * 100) % 10 == 0)
162  ROS_INFO_STREAM_THROTTLE(1, int((i) / num_samples * 100) << "\% done");
163  }
164 
165  ROS_INFO_STREAM("TRAC-IK found " << success << " solutions (" << 100.0 * success / num_samples << "\%) with an average of " << total_time / num_samples << " secs per sample");
166 
167 
168 
169 }
170 
171 
172 
173 int main(int argc, char** argv)
174 {
175  srand(1);
176  ros::init(argc, argv, "ik_tests");
177  ros::NodeHandle nh("~");
178 
179  int num_samples;
180  std::string chain_start, chain_end, urdf_param;
181  double timeout;
182 
183  nh.param("num_samples", num_samples, 1000);
184  nh.param("chain_start", chain_start, std::string(""));
185  nh.param("chain_end", chain_end, std::string(""));
186 
187  if (chain_start == "" || chain_end == "")
188  {
189  ROS_FATAL("Missing chain info in launch file");
190  exit(-1);
191  }
192 
193  nh.param("timeout", timeout, 0.005);
194  nh.param("urdf_param", urdf_param, std::string("/robot_description"));
195 
196  if (num_samples < 1)
197  num_samples = 1;
198 
199  test(nh, num_samples, chain_start, chain_end, timeout, urdf_param);
200 
201  // Useful when you make a script that loops over multiple launch files that test different robot chains
202  // std::vector<char *> commandVector;
203  // commandVector.push_back((char*)"killall");
204  // commandVector.push_back((char*)"-9");
205  // commandVector.push_back((char*)"roslaunch");
206  // commandVector.push_back(NULL);
207 
208  // char **command = &commandVector[0];
209  // execvp(command[0],command);
210 
211  return 0;
212 }
bool getKDLLimits(KDL::JntArray &lb_, KDL::JntArray &ub_)
#define ROS_FATAL(...)
f
ROSCPP_DECL void init(int &argc, char **argv, const std::string &name, uint32_t options=0)
IMETHOD Vector diff(const Vector &p_w_a, const Vector &p_w_b, double dt=1)
#define ROS_INFO(...)
bool param(const std::string &param_name, T &param_val, const T &default_val) const
int CartToJnt(const KDL::JntArray &q_init, const KDL::Frame &p_in, KDL::JntArray &q_out, const KDL::Twist &bounds=KDL::Twist::Zero())
Eigen::VectorXd data
unsigned int getNrOfJoints() const
void test(ros::NodeHandle &nh, double num_samples, std::string chain_start, std::string chain_end, double timeout, std::string urdf_param)
Definition: ik_tests.cpp:43
double fRand(double min, double max)
Definition: ik_tests.cpp:36
bool getKDLChain(KDL::Chain &chain_)
#define ROS_INFO_STREAM(args)
int main(int argc, char **argv)
Definition: ik_tests.cpp:173
#define ROS_INFO_STREAM_THROTTLE(rate, args)
#define ROS_ERROR(...)
virtual int CartToJnt(const JntArray &q_init, const Frame &p_in, JntArray &q_out)


trac_ik_examples
Author(s): Patrick Beeson
autogenerated on Tue Jun 1 2021 02:38:37