srv_kinematics_plugin.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2014, JSK, The University of Tokyo.
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 JSK, The University of Tokyo 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: Dave Coleman, Masaki Murooka */
00036 
00037 #include <moveit/srv_kinematics_plugin/srv_kinematics_plugin.h>
00038 #include <class_loader/class_loader.h>
00039 
00040 // URDF, SRDF
00041 #include <urdf_model/model.h>
00042 #include <srdfdom/model.h>
00043 
00044 #include <moveit/robot_state/conversions.h>
00045 #include <moveit/rdf_loader/rdf_loader.h>
00046 
00047 // Eigen
00048 #include <Eigen/Core>
00049 #include <Eigen/Geometry>
00050 
00051 // register SRVKinematics as a KinematicsBase implementation
00052 CLASS_LOADER_REGISTER_CLASS(srv_kinematics_plugin::SrvKinematicsPlugin, kinematics::KinematicsBase)
00053 
00054 namespace srv_kinematics_plugin
00055 {
00056 SrvKinematicsPlugin::SrvKinematicsPlugin() : active_(false)
00057 {
00058 }
00059 
00060 bool SrvKinematicsPlugin::initialize(const std::string& robot_description, const std::string& group_name,
00061                                      const std::string& base_frame, const std::vector<std::string>& tip_frames,
00062                                      double search_discretization)
00063 {
00064   bool debug = false;
00065 
00066   ROS_INFO_STREAM_NAMED("srv", "SrvKinematicsPlugin initializing");
00067 
00068   setValues(robot_description, group_name, base_frame, tip_frames, search_discretization);
00069 
00070   rdf_loader::RDFLoader rdf_loader(robot_description_);
00071   const boost::shared_ptr<srdf::Model>& srdf = rdf_loader.getSRDF();
00072   const boost::shared_ptr<urdf::ModelInterface>& urdf_model = rdf_loader.getURDF();
00073 
00074   if (!urdf_model || !srdf)
00075   {
00076     ROS_ERROR_NAMED("srv", "URDF and SRDF must be loaded for SRV kinematics solver to work.");  // TODO: is this true?
00077     return false;
00078   }
00079 
00080   robot_model_.reset(new robot_model::RobotModel(urdf_model, srdf));
00081 
00082   joint_model_group_ = robot_model_->getJointModelGroup(group_name);
00083   if (!joint_model_group_)
00084     return false;
00085 
00086   if (debug)
00087   {
00088     std::cout << std::endl << "Joint Model Variable Names: ------------------------------------------- " << std::endl;
00089     const std::vector<std::string> jm_names = joint_model_group_->getVariableNames();
00090     std::copy(jm_names.begin(), jm_names.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
00091     std::cout << std::endl;
00092   }
00093 
00094   // Get the dimension of the planning group
00095   dimension_ = joint_model_group_->getVariableCount();
00096   ROS_INFO_STREAM_NAMED("srv", "Dimension planning group '"
00097                                    << group_name << "': " << dimension_
00098                                    << ". Active Joints Models: " << joint_model_group_->getActiveJointModels().size()
00099                                    << ". Mimic Joint Models: " << joint_model_group_->getMimicJointModels().size());
00100 
00101   // Copy joint names
00102   for (std::size_t i = 0; i < joint_model_group_->getJointModels().size(); ++i)
00103   {
00104     ik_group_info_.joint_names.push_back(joint_model_group_->getJointModelNames()[i]);
00105   }
00106 
00107   if (debug)
00108   {
00109     ROS_ERROR_STREAM_NAMED("temp", "tip links available:");
00110     std::copy(tip_frames_.begin(), tip_frames_.end(), std::ostream_iterator<std::string>(std::cout, "\n"));
00111   }
00112 
00113   // Make sure all the tip links are in the link_names vector
00114   for (std::size_t i = 0; i < tip_frames_.size(); ++i)
00115   {
00116     if (!joint_model_group_->hasLinkModel(tip_frames_[i]))
00117     {
00118       ROS_ERROR_NAMED("srv", "Could not find tip name '%s' in joint group '%s'", tip_frames_[i].c_str(),
00119                       group_name.c_str());
00120       return false;
00121     }
00122     ik_group_info_.link_names.push_back(tip_frames_[i]);
00123   }
00124 
00125   // Choose what ROS service to send IK requests to
00126   ROS_DEBUG_STREAM_NAMED("srv", "Looking for ROS service name on rosparam server with param: "
00127                                     << "/kinematics_solver_service_name");
00128   std::string ik_service_name;
00129   lookupParam("kinematics_solver_service_name", ik_service_name, std::string("solve_ik"));
00130 
00131   // Setup the joint state groups that we need
00132   robot_state_.reset(new robot_state::RobotState(robot_model_));
00133   robot_state_->setToDefaultValues();
00134 
00135   // Create the ROS service client
00136   ros::NodeHandle nonprivate_handle("");
00137   ik_service_client_ = boost::make_shared<ros::ServiceClient>(
00138       nonprivate_handle.serviceClient<moveit_msgs::GetPositionIK>(ik_service_name));
00139   if (!ik_service_client_->waitForExistence(ros::Duration(0.1)))  // wait 0.1 seconds, blocking
00140     ROS_WARN_STREAM_NAMED("srv",
00141                           "Unable to connect to ROS service client with name: " << ik_service_client_->getService());
00142   else
00143     ROS_INFO_STREAM_NAMED("srv", "Service client started with ROS service name: " << ik_service_client_->getService());
00144 
00145   active_ = true;
00146   ROS_DEBUG_NAMED("srv", "ROS service-based kinematics solver initialized");
00147   return true;
00148 }
00149 
00150 bool SrvKinematicsPlugin::setRedundantJoints(const std::vector<unsigned int>& redundant_joints)
00151 {
00152   if (num_possible_redundant_joints_ < 0)
00153   {
00154     ROS_ERROR_NAMED("srv", "This group cannot have redundant joints");
00155     return false;
00156   }
00157   if (redundant_joints.size() > num_possible_redundant_joints_)
00158   {
00159     ROS_ERROR_NAMED("srv", "This group can only have %d redundant joints", num_possible_redundant_joints_);
00160     return false;
00161   }
00162 
00163   return true;
00164 }
00165 
00166 bool SrvKinematicsPlugin::isRedundantJoint(unsigned int index) const
00167 {
00168   for (std::size_t j = 0; j < redundant_joint_indices_.size(); ++j)
00169     if (redundant_joint_indices_[j] == index)
00170       return true;
00171   return false;
00172 }
00173 
00174 int SrvKinematicsPlugin::getJointIndex(const std::string& name) const
00175 {
00176   for (unsigned int i = 0; i < ik_group_info_.joint_names.size(); i++)
00177   {
00178     if (ik_group_info_.joint_names[i] == name)
00179       return i;
00180   }
00181   return -1;
00182 }
00183 
00184 bool SrvKinematicsPlugin::timedOut(const ros::WallTime& start_time, double duration) const
00185 {
00186   return ((ros::WallTime::now() - start_time).toSec() >= duration);
00187 }
00188 
00189 bool SrvKinematicsPlugin::getPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00190                                         std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00191                                         const kinematics::KinematicsQueryOptions& options) const
00192 {
00193   const IKCallbackFn solution_callback = 0;
00194   std::vector<double> consistency_limits;
00195 
00196   return searchPositionIK(ik_pose, ik_seed_state, default_timeout_, solution, solution_callback, error_code,
00197                           consistency_limits, options);
00198 }
00199 
00200 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00201                                            double timeout, std::vector<double>& solution,
00202                                            moveit_msgs::MoveItErrorCodes& error_code,
00203                                            const kinematics::KinematicsQueryOptions& options) const
00204 {
00205   const IKCallbackFn solution_callback = 0;
00206   std::vector<double> consistency_limits;
00207 
00208   return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
00209                           options);
00210 }
00211 
00212 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00213                                            double timeout, const std::vector<double>& consistency_limits,
00214                                            std::vector<double>& solution, moveit_msgs::MoveItErrorCodes& error_code,
00215                                            const kinematics::KinematicsQueryOptions& options) const
00216 {
00217   const IKCallbackFn solution_callback = 0;
00218   return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
00219                           options);
00220 }
00221 
00222 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00223                                            double timeout, std::vector<double>& solution,
00224                                            const IKCallbackFn& solution_callback,
00225                                            moveit_msgs::MoveItErrorCodes& error_code,
00226                                            const kinematics::KinematicsQueryOptions& options) const
00227 {
00228   std::vector<double> consistency_limits;
00229   return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
00230                           options);
00231 }
00232 
00233 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00234                                            double timeout, const std::vector<double>& consistency_limits,
00235                                            std::vector<double>& solution, const IKCallbackFn& solution_callback,
00236                                            moveit_msgs::MoveItErrorCodes& error_code,
00237                                            const kinematics::KinematicsQueryOptions& options) const
00238 {
00239   return searchPositionIK(ik_pose, ik_seed_state, timeout, solution, solution_callback, error_code, consistency_limits,
00240                           options);
00241 }
00242 
00243 bool SrvKinematicsPlugin::searchPositionIK(const geometry_msgs::Pose& ik_pose, const std::vector<double>& ik_seed_state,
00244                                            double timeout, std::vector<double>& solution,
00245                                            const IKCallbackFn& solution_callback,
00246                                            moveit_msgs::MoveItErrorCodes& error_code,
00247                                            const std::vector<double>& consistency_limits,
00248                                            const kinematics::KinematicsQueryOptions& options) const
00249 {
00250   // Convert single pose into a vector of one pose
00251   std::vector<geometry_msgs::Pose> ik_poses;
00252   ik_poses.push_back(ik_pose);
00253 
00254   return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code,
00255                           options);
00256 }
00257 
00258 bool SrvKinematicsPlugin::searchPositionIK(const std::vector<geometry_msgs::Pose>& ik_poses,
00259                                            const std::vector<double>& ik_seed_state, double timeout,
00260                                            const std::vector<double>& consistency_limits, std::vector<double>& solution,
00261                                            const IKCallbackFn& solution_callback,
00262                                            moveit_msgs::MoveItErrorCodes& error_code,
00263                                            const kinematics::KinematicsQueryOptions& options) const
00264 {
00265   // Check if active
00266   if (!active_)
00267   {
00268     ROS_ERROR_NAMED("srv", "kinematics not active");
00269     error_code.val = error_code.NO_IK_SOLUTION;
00270     return false;
00271   }
00272 
00273   // Check if seed state correct
00274   if (ik_seed_state.size() != dimension_)
00275   {
00276     ROS_ERROR_STREAM_NAMED("srv", "Seed state must have size " << dimension_ << " instead of size "
00277                                                                << ik_seed_state.size());
00278     error_code.val = error_code.NO_IK_SOLUTION;
00279     return false;
00280   }
00281 
00282   // Check that we have the same number of poses as tips
00283   if (tip_frames_.size() != ik_poses.size())
00284   {
00285     ROS_ERROR_STREAM_NAMED("srv", "Mismatched number of pose requests (" << ik_poses.size() << ") to tip frames ("
00286                                                                          << tip_frames_.size()
00287                                                                          << ") in searchPositionIK");
00288     error_code.val = error_code.NO_IK_SOLUTION;
00289     return false;
00290   }
00291 
00292   // Create the service message
00293   moveit_msgs::GetPositionIK ik_srv;
00294   ik_srv.request.ik_request.avoid_collisions = true;
00295   ik_srv.request.ik_request.group_name = getGroupName();
00296 
00297   // Copy seed state into virtual robot state and convert into moveit_msg
00298   robot_state_->setJointGroupPositions(joint_model_group_, ik_seed_state);
00299   moveit::core::robotStateToRobotStateMsg(*robot_state_, ik_srv.request.ik_request.robot_state);
00300 
00301   // Load the poses into the request in difference places depending if there is more than one or not
00302   geometry_msgs::PoseStamped ik_pose_st;
00303   ik_pose_st.header.frame_id = base_frame_;
00304   if (tip_frames_.size() > 1)
00305   {
00306     // Load into vector of poses
00307     for (std::size_t i = 0; i < tip_frames_.size(); ++i)
00308     {
00309       ik_pose_st.pose = ik_poses[i];
00310       ik_srv.request.ik_request.pose_stamped_vector.push_back(ik_pose_st);
00311       ik_srv.request.ik_request.ik_link_names.push_back(tip_frames_[i]);
00312     }
00313   }
00314   else
00315   {
00316     ik_pose_st.pose = ik_poses[0];
00317 
00318     // Load into single pose value
00319     ik_srv.request.ik_request.pose_stamped = ik_pose_st;
00320     ik_srv.request.ik_request.ik_link_name = getTipFrames()[0];
00321   }
00322 
00323   ROS_DEBUG_STREAM_NAMED("srv", "Calling service: " << ik_service_client_->getService());
00324   if (ik_service_client_->call(ik_srv))
00325   {
00326     // Check error code
00327     error_code.val = ik_srv.response.error_code.val;
00328     if (error_code.val != error_code.SUCCESS)
00329     {
00330       ROS_DEBUG_NAMED("srv", "An IK that satisifes the constraints and is collision free could not be found.");
00331       switch (error_code.val)
00332       {
00333         // Debug mode for failure:
00334         ROS_DEBUG_STREAM("Request was: \n" << ik_srv.request.ik_request);
00335         ROS_DEBUG_STREAM("Response was: \n" << ik_srv.response.solution);
00336 
00337         case moveit_msgs::MoveItErrorCodes::FAILURE:
00338           ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: FAILURE");
00339           break;
00340         case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
00341           ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: NO IK SOLUTION");
00342           break;
00343         default:
00344           ROS_ERROR_STREAM_NAMED("srv", "Service failed with with error code: " << error_code.val);
00345       }
00346       return false;
00347     }
00348   }
00349   else
00350   {
00351     ROS_ERROR_STREAM("Service call failed to connect to service: " << ik_service_client_->getService());
00352     error_code.val = error_code.FAILURE;
00353     return false;
00354   }
00355 
00356   // Convert the robot state message to our robot_state representation
00357   if (!moveit::core::robotStateMsgToRobotState(ik_srv.response.solution, *robot_state_))
00358   {
00359     ROS_ERROR_STREAM_NAMED("srv", "An error occured converting recieved robot state message into internal robot "
00360                                   "state.");
00361     error_code.val = error_code.FAILURE;
00362     return false;
00363   }
00364 
00365   // Get just the joints we are concerned about in our planning group
00366   robot_state_->copyJointGroupPositions(joint_model_group_, solution);
00367 
00368   // Run the solution callback (i.e. collision checker) if available
00369   if (!solution_callback.empty())
00370   {
00371     ROS_DEBUG_STREAM_NAMED("srv", "Calling solution callback on IK solution");
00372 
00373     // hack: should use all poses, not just the 0th
00374     solution_callback(ik_poses[0], solution, error_code);
00375 
00376     if (error_code.val != error_code.SUCCESS)
00377     {
00378       switch (error_code.val)
00379       {
00380         case moveit_msgs::MoveItErrorCodes::FAILURE:
00381           ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: FAILURE");
00382           break;
00383         case moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION:
00384           ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: NO IK SOLUTION");
00385           break;
00386         default:
00387           ROS_ERROR_STREAM_NAMED("srv", "IK solution callback failed with with error code: " << error_code.val);
00388       }
00389       return false;
00390     }
00391   }
00392 
00393   ROS_INFO_STREAM_NAMED("srv", "IK Solver Succeeded!");
00394   return true;
00395 }
00396 
00397 bool SrvKinematicsPlugin::getPositionFK(const std::vector<std::string>& link_names,
00398                                         const std::vector<double>& joint_angles,
00399                                         std::vector<geometry_msgs::Pose>& poses) const
00400 {
00401   ros::WallTime n1 = ros::WallTime::now();
00402   if (!active_)
00403   {
00404     ROS_ERROR_NAMED("srv", "kinematics not active");
00405     return false;
00406   }
00407   poses.resize(link_names.size());
00408   if (joint_angles.size() != dimension_)
00409   {
00410     ROS_ERROR_NAMED("srv", "Joint angles vector must have size: %d", dimension_);
00411     return false;
00412   }
00413 
00414   ROS_ERROR_STREAM_NAMED("srv", "Forward kinematics not implemented");
00415 
00416   return false;
00417 }
00418 
00419 const std::vector<std::string>& SrvKinematicsPlugin::getJointNames() const
00420 {
00421   return ik_group_info_.joint_names;
00422 }
00423 
00424 const std::vector<std::string>& SrvKinematicsPlugin::getLinkNames() const
00425 {
00426   return ik_group_info_.link_names;
00427 }
00428 
00429 const std::vector<std::string>& SrvKinematicsPlugin::getVariableNames() const
00430 {
00431   return joint_model_group_->getVariableNames();
00432 }
00433 
00434 }  // namespace


moveit_kinematics
Author(s): Dave Coleman , Ioan Sucan , Sachin Chitta
autogenerated on Wed Jan 17 2018 03:32:16