kinematics_constraint_aware.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002 *
00003 * Software License Agreement (BSD License)
00004 *
00005 *  Copyright (c) 2012, Willow Garage, Inc.
00006 *  All rights reserved.
00007 *
00008 *  Redistribution and use in source and binary forms, with or without
00009 *  modification, are permitted provided that the following conditions
00010 *  are met:
00011 *
00012 *   * Redistributions of source code must retain the above copyright
00013 *     notice, this list of conditions and the following disclaimer.
00014 *   * Redistributions in binary form must reproduce the above
00015 *     copyright notice, this list of conditions and the following
00016 *     disclaimer in the documentation and/or other materials provided
00017 *     with the distribution.
00018 *   * Neither the name of Willow Garage, Inc. nor the names of its
00019 *     contributors may be used to endorse or promote products derived
00020 *     from this software without specific prior written permission.
00021 *
00022 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00023 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00024 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00025 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00026 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00027 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00028 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00029 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00030 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00031 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00032 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00033 *  POSSIBILITY OF SUCH DAMAGE.
00034 *
00035 * Author: Sachin Chitta
00036 *********************************************************************/
00037 
00038 // // ROS msgs
00039 #include <moveit/kinematics_constraint_aware/kinematics_constraint_aware.h>
00040 #include <moveit/robot_model/robot_model.h>
00041 #include <moveit/planning_scene/planning_scene.h>
00042 #include <eigen_conversions/eigen_msg.h>
00043 #include <boost/bind.hpp>
00044 
00045 namespace kinematics_constraint_aware
00046 {
00047 KinematicsConstraintAware::KinematicsConstraintAware(const robot_model::RobotModelConstPtr& kinematic_model,
00048                                                      const std::string& group_name)
00049 {
00050   if (!kinematic_model->hasJointModelGroup(group_name))
00051   {
00052     logError("The group %s does not exist", group_name.c_str());
00053     joint_model_group_ = NULL;
00054     return;
00055   }
00056   kinematic_model_ = kinematic_model;
00057   group_name_ = group_name;
00058   joint_model_group_ = kinematic_model_->getJointModelGroup(group_name);
00059   if (joint_model_group_->getSolverInstance())
00060   {
00061     has_sub_groups_ = false;
00062     sub_groups_names_.push_back(group_name_);
00063   }
00064   else
00065   {
00066     logDebug("No kinematics solver instance defined for group %s", group_name.c_str());
00067     bool is_solvable_group = true;
00068     if (!(joint_model_group_->getSubgroupNames().empty()))
00069     {
00070       const std::vector<std::string> sub_groups_names = joint_model_group_->getSubgroupNames();
00071       for (std::size_t i = 0; i < sub_groups_names.size(); ++i)
00072       {
00073         if (!kinematic_model_->getJointModelGroup(sub_groups_names[i])->getSolverInstance())
00074         {
00075           is_solvable_group = false;
00076           break;
00077         }
00078       }
00079       if (is_solvable_group)
00080       {
00081         logDebug("Group %s is a group for which we can solve IK", joint_model_group_->getName().c_str());
00082         sub_groups_names_ = sub_groups_names;
00083       }
00084       else
00085       {
00086         joint_model_group_ = NULL;
00087         return;
00088       }
00089     }
00090     else
00091     {
00092       joint_model_group_ = NULL;
00093       logInform("No solver allocated for group %s", group_name.c_str());
00094     }
00095     has_sub_groups_ = true;
00096   }
00097   ik_attempts_ = 10;
00098 }
00099 
00100 bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPtr& planning_scene,
00101                                       const kinematics_constraint_aware::KinematicsRequest& request,
00102                                       kinematics_constraint_aware::KinematicsResponse& response) const
00103 {
00104   if (!joint_model_group_)
00105   {
00106     logError("This solver has not been constructed properly");
00107     return false;
00108   }
00109 
00110   if (!planning_scene)
00111   {
00112     logError("Planning scene must be allocated");
00113     return false;
00114   }
00115 
00116   if (!response.solution_)
00117   {
00118     response.solution_.reset(new robot_state::RobotState(planning_scene->getCurrentState()));
00119   }
00120 
00121   ros::WallTime start_time = ros::WallTime::now();
00122   if (request.group_name_ != group_name_)
00123   {
00124     response.error_code_.val = response.error_code_.INVALID_GROUP_NAME;
00125     return false;
00126   }
00127 
00128   // Setup the seed and the values for all other joints in the robot
00129   robot_state::RobotState kinematic_state = *request.robot_state_;
00130   std::vector<std::string> ik_link_names = request.ik_link_names_;
00131 
00132   // Transform request to tip frame if necessary
00133   if (!request.ik_link_names_.empty())
00134   {
00135     for (std::size_t i = 0; i < request.pose_stamped_vector_.size(); ++i)
00136     {
00137       geometry_msgs::PoseStamped tmp_pose = request.pose_stamped_vector_[i];
00138       // The assumption is that this new link is rigidly attached to the tip link for the group
00139       if (!kinematic_model_->getJointModelGroup(sub_groups_names_[i])->hasLinkModel(request.ik_link_names_[i]) &&
00140           kinematic_model_->getJointModelGroup(sub_groups_names_[i])->isLinkUpdated(request.ik_link_names_[i]))
00141       {
00142         tmp_pose.pose = getTipFramePose(planning_scene, kinematic_state, request.pose_stamped_vector_[i].pose,
00143                                         request.ik_link_names_[i], i);
00144         ik_link_names[i] =
00145             kinematic_model_->getJointModelGroup(sub_groups_names_[i])->getSolverInstance()->getTipFrame();
00146       }
00147       else if (!kinematic_model_->getJointModelGroup(sub_groups_names_[i])
00148                     ->canSetStateFromIK(request.ik_link_names_[i]))
00149       {
00150         logError("Could not find IK solver for link %s for group %s", request.ik_link_names_[i].c_str(),
00151                  sub_groups_names_[i].c_str());
00152         return false;
00153       }
00154     }
00155   }
00156 
00157   // Transform the requests to the base frame of the kinematic model
00158   EigenSTL::vector_Affine3d goals =
00159       transformPoses(planning_scene, kinematic_state, request.pose_stamped_vector_, kinematic_model_->getModelFrame());
00160 
00161   robot_state::StateValidityCallbackFn constraint_callback_fn =
00162       boost::bind(&KinematicsConstraintAware::validityCallbackFn, this, planning_scene, request, response, _1, _2);
00163 
00164   bool result = false;
00165   if (has_sub_groups_)
00166   {
00167     result = kinematic_state.getJointStateGroup(group_name_)
00168                  ->setFromIK(goals, ik_link_names, ik_attempts_, request.timeout_.toSec(), constraint_callback_fn);
00169   }
00170   else
00171   {
00172     result =
00173         ik_link_names.empty() ?
00174             kinematic_state.getJointStateGroup(group_name_)
00175                 ->setFromIK(goals[0], ik_attempts_, request.timeout_.toSec(), constraint_callback_fn) :
00176             kinematic_state.getJointStateGroup(group_name_)
00177                 ->setFromIK(goals[0], ik_link_names[0], ik_attempts_, request.timeout_.toSec(), constraint_callback_fn);
00178   }
00179 
00180   if (result)
00181   {
00182     std::vector<double> solution_values;
00183     kinematic_state.getJointStateGroup(group_name_)->getVariableValues(solution_values);
00184     response.solution_->getJointStateGroup(group_name_)->setVariableValues(solution_values);
00185     response.error_code_.val = response.error_code_.SUCCESS;
00186   }
00187 
00188   if (response.error_code_.val == 0)
00189   {
00190     response.error_code_.val = response.error_code_.NO_IK_SOLUTION;
00191   }
00192   return result;
00193 }
00194 
00195 bool KinematicsConstraintAware::validityCallbackFn(const planning_scene::PlanningSceneConstPtr& planning_scene,
00196                                                    const kinematics_constraint_aware::KinematicsRequest& request,
00197                                                    kinematics_constraint_aware::KinematicsResponse& response,
00198                                                    robot_state::JointStateGroup* joint_state_group,
00199                                                    const std::vector<double>& joint_group_variable_values) const
00200 {
00201   joint_state_group->setVariableValues(joint_group_variable_values);
00202 
00203   // Now check for collisions
00204   if (request.check_for_collisions_)
00205   {
00206     collision_detection::CollisionRequest collision_request;
00207     collision_detection::CollisionResult collision_result;
00208     collision_request.group_name = request.group_name_;
00209     planning_scene->checkCollision(collision_request, collision_result, *joint_state_group->getRobotState());
00210     if (collision_result.collision)
00211     {
00212       logDebug("IK solution is in collision");
00213       response.error_code_.val = response.error_code_.GOAL_IN_COLLISION;
00214       return false;
00215     }
00216   }
00217 
00218   // Now check for constraints
00219   if (request.constraints_)
00220   {
00221     kinematic_constraints::ConstraintEvaluationResult constraint_result;
00222     constraint_result =
00223         request.constraints_->decide(*joint_state_group->getRobotState(), response.constraint_eval_results_);
00224     if (!constraint_result.satisfied)
00225     {
00226       logDebug("IK solution violates constraints");
00227       response.error_code_.val = response.error_code_.GOAL_VIOLATES_PATH_CONSTRAINTS;
00228       return false;
00229     }
00230   }
00231 
00232   // Now check for user specified constraints
00233   if (request.constraint_callback_)
00234   {
00235     if (!request.constraint_callback_(joint_state_group, joint_group_variable_values))
00236     {
00237       logDebug("IK solution violates user specified constraints");
00238       response.error_code_.val = response.error_code_.GOAL_VIOLATES_PATH_CONSTRAINTS;
00239       return false;
00240     }
00241   }
00242 
00243   return true;
00244 }
00245 
00246 bool KinematicsConstraintAware::getIK(const planning_scene::PlanningSceneConstPtr& planning_scene,
00247                                       const moveit_msgs::GetConstraintAwarePositionIK::Request& request,
00248                                       moveit_msgs::GetConstraintAwarePositionIK::Response& response) const
00249 {
00250   if (!joint_model_group_)
00251   {
00252     logError("This solver has not been constructed properly");
00253     return false;
00254   }
00255 
00256   if (!planning_scene)
00257   {
00258     logError("Planning scene must be allocated");
00259     return false;
00260   }
00261 
00262   kinematics_constraint_aware::KinematicsRequest kinematics_request;
00263   kinematics_constraint_aware::KinematicsResponse kinematics_response;
00264 
00265   if (!convertServiceRequest(planning_scene, request, kinematics_request, kinematics_response))
00266   {
00267     response.error_code = kinematics_response.error_code_;
00268     return false;
00269   }
00270 
00271   bool result = getIK(planning_scene, kinematics_request, kinematics_response);
00272   response.error_code = kinematics_response.error_code_;
00273   kinematics_response.solution_->getStateValues(response.solution.joint_state);
00274   return result;
00275 }
00276 
00277 bool KinematicsConstraintAware::convertServiceRequest(
00278     const planning_scene::PlanningSceneConstPtr& planning_scene,
00279     const moveit_msgs::GetConstraintAwarePositionIK::Request& request,
00280     kinematics_constraint_aware::KinematicsRequest& kinematics_request,
00281     kinematics_constraint_aware::KinematicsResponse& kinematics_response) const
00282 {
00283   if (request.ik_request.group_name != group_name_)
00284   {
00285     logError("This kinematics solver does not support requests for group: %s", request.ik_request.group_name.c_str());
00286     kinematics_response.error_code_.val = kinematics_response.error_code_.INVALID_GROUP_NAME;
00287     return false;
00288   }
00289 
00290   if (!request.ik_request.pose_stamped_vector.empty() &&
00291       request.ik_request.pose_stamped_vector.size() != sub_groups_names_.size())
00292   {
00293     logError("Number of poses in request: %d must match number of sub groups %d in this group",
00294              request.ik_request.pose_stamped_vector.size(), sub_groups_names_.size());
00295     kinematics_response.error_code_.val = kinematics_response.error_code_.INVALID_GROUP_NAME;
00296     return false;
00297   }
00298 
00299   if (!request.ik_request.ik_link_names.empty() && request.ik_request.ik_link_names.size() != sub_groups_names_.size())
00300   {
00301     logError("Number of ik_link_names in request: %d must match number of sub groups %d in this group or must be zero",
00302              request.ik_request.ik_link_names.size(), sub_groups_names_.size());
00303     kinematics_response.error_code_.val = kinematics_response.error_code_.INVALID_GROUP_NAME;
00304     return false;
00305   }
00306 
00307   if (request.ik_request.ik_link_names.empty() && request.ik_request.ik_link_name != "")
00308     kinematics_request.ik_link_names_.push_back(request.ik_request.ik_link_name);
00309   else
00310     kinematics_request.ik_link_names_ = request.ik_request.ik_link_names;
00311 
00312   if (request.ik_request.pose_stamped_vector.empty())
00313     kinematics_request.pose_stamped_vector_.push_back(request.ik_request.pose_stamped);
00314   else
00315     kinematics_request.pose_stamped_vector_ = request.ik_request.pose_stamped_vector;
00316 
00317   kinematics_request.robot_state_.reset(new robot_state::RobotState(planning_scene->getCurrentState()));
00318   kinematics_request.robot_state_->setStateValues(request.ik_request.robot_state.joint_state);
00319   kinematics_request.constraints_.reset(
00320       new kinematic_constraints::KinematicConstraintSet(kinematic_model_, planning_scene->getTransforms()));
00321   kinematics_request.constraints_->add(request.constraints);
00322   kinematics_request.timeout_ = request.ik_request.timeout;
00323   kinematics_request.group_name_ = request.ik_request.group_name;
00324   kinematics_request.check_for_collisions_ = true;
00325 
00326   kinematics_response.solution_.reset(new robot_state::RobotState(planning_scene->getCurrentState()));
00327 
00328   return true;
00329 }
00330 
00331 EigenSTL::vector_Affine3d KinematicsConstraintAware::transformPoses(
00332     const planning_scene::PlanningSceneConstPtr& planning_scene, const robot_state::RobotState& kinematic_state,
00333     const std::vector<geometry_msgs::PoseStamped>& poses, const std::string& target_frame) const
00334 {
00335   Eigen::Affine3d eigen_pose, eigen_pose_2;
00336   EigenSTL::vector_Affine3d result(poses.size());
00337   bool target_frame_is_root_frame = (target_frame == kinematic_state.getRobotModel()->getModelFrame());
00338   for (std::size_t i = 0; i < poses.size(); ++i)
00339   {
00340     geometry_msgs::Pose pose = poses[i].pose;
00341     tf::poseMsgToEigen(pose, eigen_pose_2);
00342     planning_scene->getTransforms()->transformPose(kinematic_state, poses[i].header.frame_id, eigen_pose_2, eigen_pose);
00343     if (!target_frame_is_root_frame)
00344     {
00345       eigen_pose_2 = planning_scene->getTransforms()->getTransform(kinematic_state, target_frame);
00346       eigen_pose = eigen_pose_2.inverse() * eigen_pose;
00347     }
00348     result[i] = eigen_pose;
00349   }
00350   return result;
00351 }
00352 
00353 geometry_msgs::Pose KinematicsConstraintAware::getTipFramePose(
00354     const planning_scene::PlanningSceneConstPtr& planning_scene, const robot_state::RobotState& kinematic_state,
00355     const geometry_msgs::Pose& pose, const std::string& link_name, unsigned int sub_group_index) const
00356 {
00357   geometry_msgs::Pose result;
00358   Eigen::Affine3d eigen_pose_in, eigen_pose_link, eigen_pose_tip;
00359   std::string tip_name =
00360       kinematic_model_->getJointModelGroup(sub_groups_names_[sub_group_index])->getSolverInstance()->getTipFrame();
00361   tf::poseMsgToEigen(pose, eigen_pose_in);
00362   eigen_pose_link = planning_scene->getTransforms()->getTransform(kinematic_state, link_name);
00363   eigen_pose_tip = planning_scene->getTransforms()->getTransform(kinematic_state, tip_name);
00364   eigen_pose_in = eigen_pose_in * (eigen_pose_link.inverse() * eigen_pose_tip);
00365   tf::poseEigenToMsg(eigen_pose_in, result);
00366   return result;
00367 }
00368 }


moveit_experimental
Author(s): Ioan Sucan , Sachin Chitta , Acorn Pooley , Jorge Nicho
autogenerated on Mon Jul 24 2017 02:21:02