kinematics_service_capability.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2012, 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 "kinematics_service_capability.h"
00038 #include <moveit/robot_state/conversions.h>
00039 #include <moveit/kinematic_constraints/utils.h>
00040 #include <eigen_conversions/eigen_msg.h>
00041 #include <moveit/move_group/capability_names.h>
00042 
00043 move_group::MoveGroupKinematicsService::MoveGroupKinematicsService() : MoveGroupCapability("KinematicsService")
00044 {
00045 }
00046 
00047 void move_group::MoveGroupKinematicsService::initialize()
00048 {
00049   fk_service_ =
00050       root_node_handle_.advertiseService(FK_SERVICE_NAME, &MoveGroupKinematicsService::computeFKService, this);
00051   ik_service_ =
00052       root_node_handle_.advertiseService(IK_SERVICE_NAME, &MoveGroupKinematicsService::computeIKService, this);
00053 }
00054 
00055 namespace
00056 {
00057 bool isIKSolutionValid(const planning_scene::PlanningScene* planning_scene,
00058                        const kinematic_constraints::KinematicConstraintSet* constraint_set,
00059                        robot_state::RobotState* state, const robot_model::JointModelGroup* jmg,
00060                        const double* ik_solution)
00061 {
00062   state->setJointGroupPositions(jmg, ik_solution);
00063   state->update();
00064   return (!planning_scene || !planning_scene->isStateColliding(*state, jmg->getName())) &&
00065          (!constraint_set || constraint_set->decide(*state).satisfied);
00066 }
00067 }
00068 
00069 void move_group::MoveGroupKinematicsService::computeIK(
00070     moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, moveit_msgs::MoveItErrorCodes& error_code,
00071     robot_state::RobotState& rs, const robot_state::GroupStateValidityCallbackFn& constraint) const
00072 {
00073   const robot_state::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name);
00074   if (jmg)
00075   {
00076     robot_state::robotStateMsgToRobotState(req.robot_state, rs);
00077     const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
00078 
00079     if (req.pose_stamped_vector.empty() || req.pose_stamped_vector.size() == 1)
00080     {
00081       geometry_msgs::PoseStamped req_pose =
00082           req.pose_stamped_vector.empty() ? req.pose_stamped : req.pose_stamped_vector[0];
00083       std::string ik_link = (!req.pose_stamped_vector.empty()) ?
00084                                 (req.ik_link_names.empty() ? "" : req.ik_link_names[0]) :
00085                                 req.ik_link_name;
00086 
00087       if (performTransform(req_pose, default_frame))
00088       {
00089         bool result_ik = false;
00090         if (ik_link.empty())
00091           result_ik = rs.setFromIK(jmg, req_pose.pose, req.attempts, req.timeout.toSec(), constraint);
00092         else
00093           result_ik = rs.setFromIK(jmg, req_pose.pose, ik_link, req.attempts, req.timeout.toSec(), constraint);
00094 
00095         if (result_ik)
00096         {
00097           robot_state::robotStateToRobotStateMsg(rs, solution, false);
00098           error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00099         }
00100         else
00101           error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00102       }
00103       else
00104         error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
00105     }
00106     else
00107     {
00108       if (req.pose_stamped_vector.size() != req.ik_link_names.size())
00109         error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
00110       else
00111       {
00112         bool ok = true;
00113         EigenSTL::vector_Affine3d req_poses(req.pose_stamped_vector.size());
00114         for (std::size_t k = 0; k < req.pose_stamped_vector.size(); ++k)
00115         {
00116           geometry_msgs::PoseStamped msg = req.pose_stamped_vector[k];
00117           if (performTransform(msg, default_frame))
00118             tf::poseMsgToEigen(msg.pose, req_poses[k]);
00119           else
00120           {
00121             error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
00122             ok = false;
00123             break;
00124           }
00125         }
00126         if (ok)
00127         {
00128           if (rs.setFromIK(jmg, req_poses, req.ik_link_names, req.attempts, req.timeout.toSec(), constraint))
00129           {
00130             robot_state::robotStateToRobotStateMsg(rs, solution, false);
00131             error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00132           }
00133           else
00134             error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
00135         }
00136       }
00137     }
00138   }
00139   else
00140     error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
00141 }
00142 
00143 bool move_group::MoveGroupKinematicsService::computeIKService(moveit_msgs::GetPositionIK::Request& req,
00144                                                               moveit_msgs::GetPositionIK::Response& res)
00145 {
00146   context_->planning_scene_monitor_->updateFrameTransforms();
00147 
00148   // check if the planning scene needs to be kept locked; if so, call computeIK() in the scope of the lock
00149   if (req.ik_request.avoid_collisions || !kinematic_constraints::isEmpty(req.ik_request.constraints))
00150   {
00151     planning_scene_monitor::LockedPlanningSceneRO ls(context_->planning_scene_monitor_);
00152     kinematic_constraints::KinematicConstraintSet kset(ls->getRobotModel());
00153     robot_state::RobotState rs = ls->getCurrentState();
00154     kset.add(req.ik_request.constraints, ls->getTransforms());
00155     computeIK(req.ik_request, res.solution, res.error_code, rs,
00156               boost::bind(&isIKSolutionValid, req.ik_request.avoid_collisions ?
00157                                                   static_cast<const planning_scene::PlanningSceneConstPtr&>(ls).get() :
00158                                                   NULL,
00159                           kset.empty() ? NULL : &kset, _1, _2, _3));
00160   }
00161   else
00162   {
00163     // compute unconstrained IK, no lock to planning scene maintained
00164     robot_state::RobotState rs =
00165         planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
00166     computeIK(req.ik_request, res.solution, res.error_code, rs);
00167   }
00168 
00169   return true;
00170 }
00171 
00172 bool move_group::MoveGroupKinematicsService::computeFKService(moveit_msgs::GetPositionFK::Request& req,
00173                                                               moveit_msgs::GetPositionFK::Response& res)
00174 {
00175   if (req.fk_link_names.empty())
00176   {
00177     ROS_ERROR("No links specified for FK request");
00178     res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
00179     return true;
00180   }
00181 
00182   context_->planning_scene_monitor_->updateFrameTransforms();
00183 
00184   const std::string& default_frame = context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
00185   bool do_transform = !req.header.frame_id.empty() &&
00186                       !robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) &&
00187                       context_->planning_scene_monitor_->getTFClient();
00188   bool tf_problem = false;
00189 
00190   robot_state::RobotState rs =
00191       planning_scene_monitor::LockedPlanningSceneRO(context_->planning_scene_monitor_)->getCurrentState();
00192   robot_state::robotStateMsgToRobotState(req.robot_state, rs);
00193   for (std::size_t i = 0; i < req.fk_link_names.size(); ++i)
00194     if (rs.getRobotModel()->hasLinkModel(req.fk_link_names[i]))
00195     {
00196       res.pose_stamped.resize(res.pose_stamped.size() + 1);
00197       tf::poseEigenToMsg(rs.getGlobalLinkTransform(req.fk_link_names[i]), res.pose_stamped.back().pose);
00198       res.pose_stamped.back().header.frame_id = default_frame;
00199       res.pose_stamped.back().header.stamp = ros::Time::now();
00200       if (do_transform)
00201         if (!performTransform(res.pose_stamped.back(), req.header.frame_id))
00202           tf_problem = true;
00203       res.fk_link_names.push_back(req.fk_link_names[i]);
00204     }
00205   if (tf_problem)
00206     res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
00207   else if (res.fk_link_names.size() == req.fk_link_names.size())
00208     res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
00209   else
00210     res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
00211   return true;
00212 }
00213 
00214 #include <class_loader/class_loader.h>
00215 CLASS_LOADER_REGISTER_CLASS(move_group::MoveGroupKinematicsService, move_group::MoveGroupCapability)


move_group
Author(s): Ioan Sucan , Sachin Chitta
autogenerated on Wed Jun 19 2019 19:24:36