kinematics_speed_and_validity_evaluator.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2013, 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: Ioan Sucan */
00036 
00037 #include <moveit/robot_model_loader/robot_model_loader.h>
00038 #include <moveit/robot_state/robot_state.h>
00039 #include <moveit/profiler/profiler.h>
00040 #include <ros/ros.h>
00041 
00042 static const std::string ROBOT_DESCRIPTION = "robot_description";
00043 
00044 int main(int argc, char** argv)
00045 {
00046   ros::init(argc, argv, "inverse_kinematics_test");
00047 
00048   ros::AsyncSpinner spinner(1);
00049   spinner.start();
00050 
00051   if (argc <= 1)
00052     ROS_ERROR("An argument specifying the group name is needed");
00053   else
00054   {
00055     robot_model_loader::RobotModelLoader rml(ROBOT_DESCRIPTION);
00056     std::string group = argv[1];
00057     ROS_INFO_STREAM("Evaluating IK for " << group);
00058 
00059     const robot_model::JointModelGroup* jmg = rml.getModel()->getJointModelGroup(group);
00060     if (jmg)
00061     {
00062       const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
00063       if (solver)
00064       {
00065         const std::string& tip = solver->getTipFrame();
00066         robot_state::RobotState state(rml.getModel());
00067         state.setToDefaultValues();
00068 
00069         ROS_INFO_STREAM("Tip Frame:  " << solver->getTipFrame());
00070         ROS_INFO_STREAM("Base Frame: " << solver->getBaseFrame());
00071         ROS_INFO_STREAM("IK Timeout: " << solver->getDefaultTimeout());
00072         ROS_INFO_STREAM("Search res: " << solver->getSearchDiscretization());
00073 
00074         unsigned int test_count = 1000;
00075         if (argc > 2)
00076           try
00077           {
00078             test_count = boost::lexical_cast<unsigned int>(argv[2]);
00079           }
00080           catch (...)
00081           {
00082           }
00083 
00084         ROS_INFO("Running %u tests", test_count);
00085 
00086         moveit::tools::Profiler::Start();
00087         for (unsigned int i = 0; i < test_count; ++i)
00088         {
00089           state.setToRandomPositions(jmg);
00090           Eigen::Affine3d pose = state.getGlobalLinkTransform(tip);
00091           state.setToRandomPositions(jmg);
00092           moveit::tools::Profiler::Begin("IK");
00093           state.setFromIK(jmg, pose);
00094           moveit::tools::Profiler::End("IK");
00095           const Eigen::Affine3d& pose_upd = state.getGlobalLinkTransform(tip);
00096           Eigen::Affine3d diff = pose_upd * pose.inverse();
00097           double rot_err = (diff.rotation() - Eigen::Matrix3d::Identity()).norm();
00098           double trans_err = diff.translation().norm();
00099           moveit::tools::Profiler::Average("Rotation error", rot_err);
00100           moveit::tools::Profiler::Average("Translation error", trans_err);
00101           if (rot_err < 1e-3 && trans_err < 1e-3)
00102           {
00103             moveit::tools::Profiler::Event("Valid IK");
00104             moveit::tools::Profiler::Average("Success Rate", 100);
00105           }
00106           else
00107           {
00108             moveit::tools::Profiler::Event("Invalid IK");
00109             moveit::tools::Profiler::Average("Success Rate", 0);
00110           }
00111         }
00112         moveit::tools::Profiler::Stop();
00113         moveit::tools::Profiler::Status();
00114       }
00115       else
00116         ROS_ERROR_STREAM("No kinematics solver specified for group " << group);
00117     }
00118   }
00119 
00120   ros::shutdown();
00121   return 0;
00122 }


planning
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Mon Apr 23 2018 10:25:14