59 robot_state::RobotState* state,
const robot_model::JointModelGroup* jmg,
60 const double* ik_solution)
62 state->setJointGroupPositions(jmg, ik_solution);
64 return (!planning_scene || !planning_scene->
isStateColliding(*state, jmg->getName())) &&
70 moveit_msgs::PositionIKRequest& req, moveit_msgs::RobotState& solution, moveit_msgs::MoveItErrorCodes& error_code,
71 robot_state::RobotState& rs,
const robot_state::GroupStateValidityCallbackFn& constraint)
const 73 const robot_state::JointModelGroup* jmg = rs.getJointModelGroup(req.group_name);
76 robot_state::robotStateMsgToRobotState(req.robot_state, rs);
77 const std::string& default_frame =
context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
79 if (req.pose_stamped_vector.empty() || req.pose_stamped_vector.size() == 1)
81 geometry_msgs::PoseStamped req_pose =
82 req.pose_stamped_vector.empty() ? req.pose_stamped : req.pose_stamped_vector[0];
83 std::string ik_link = (!req.pose_stamped_vector.empty()) ?
84 (req.ik_link_names.empty() ?
"" : req.ik_link_names[0]) :
89 bool result_ik =
false;
91 result_ik = rs.setFromIK(jmg, req_pose.pose, req.attempts, req.timeout.toSec(), constraint);
93 result_ik = rs.setFromIK(jmg, req_pose.pose, ik_link, req.attempts, req.timeout.toSec(), constraint);
97 robot_state::robotStateToRobotStateMsg(rs, solution,
false);
98 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
101 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
104 error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
108 if (req.pose_stamped_vector.size() != req.ik_link_names.size())
109 error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
114 for (std::size_t k = 0; k < req.pose_stamped_vector.size(); ++k)
116 geometry_msgs::PoseStamped msg = req.pose_stamped_vector[k];
121 error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
128 if (rs.setFromIK(jmg, req_poses, req.ik_link_names, req.attempts, req.timeout.toSec(), constraint))
130 robot_state::robotStateToRobotStateMsg(rs, solution,
false);
131 error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
134 error_code.val = moveit_msgs::MoveItErrorCodes::NO_IK_SOLUTION;
140 error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_GROUP_NAME;
144 moveit_msgs::GetPositionIK::Response& res)
146 context_->planning_scene_monitor_->updateFrameTransforms();
153 robot_state::RobotState rs = ls->getCurrentState();
154 kset.add(req.ik_request.constraints, ls->getTransforms());
155 computeIK(req.ik_request, res.solution, res.error_code, rs,
156 boost::bind(&isIKSolutionValid, req.ik_request.avoid_collisions ?
157 static_cast<const planning_scene::PlanningSceneConstPtr&>(ls).get() :
159 kset.empty() ? NULL : &kset, _1, _2, _3));
164 robot_state::RobotState rs =
166 computeIK(req.ik_request, res.solution, res.error_code, rs);
173 moveit_msgs::GetPositionFK::Response& res)
175 if (req.fk_link_names.empty())
177 ROS_ERROR(
"No links specified for FK request");
178 res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
182 context_->planning_scene_monitor_->updateFrameTransforms();
184 const std::string& default_frame =
context_->planning_scene_monitor_->getRobotModel()->getModelFrame();
185 bool do_transform = !req.header.frame_id.empty() &&
186 !robot_state::Transforms::sameFrame(req.header.frame_id, default_frame) &&
187 context_->planning_scene_monitor_->getTFClient();
188 bool tf_problem =
false;
190 robot_state::RobotState rs =
192 robot_state::robotStateMsgToRobotState(req.robot_state, rs);
193 for (std::size_t i = 0; i < req.fk_link_names.size(); ++i)
194 if (rs.getRobotModel()->hasLinkModel(req.fk_link_names[i]))
196 res.pose_stamped.resize(res.pose_stamped.size() + 1);
197 tf::poseEigenToMsg(rs.getGlobalLinkTransform(req.fk_link_names[i]), res.pose_stamped.back().pose);
198 res.pose_stamped.back().header.frame_id = default_frame;
203 res.fk_link_names.push_back(req.fk_link_names[i]);
206 res.error_code.val = moveit_msgs::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE;
207 else if (res.fk_link_names.size() == req.fk_link_names.size())
208 res.error_code.val = moveit_msgs::MoveItErrorCodes::SUCCESS;
210 res.error_code.val = moveit_msgs::MoveItErrorCodes::INVALID_LINK_NAME;
virtual void initialize()
bool isEmpty(const moveit_msgs::Constraints &constr)
bool computeIKService(moveit_msgs::GetPositionIK::Request &req, moveit_msgs::GetPositionIK::Response &res)
std::vector< Eigen::Affine3d, Eigen::aligned_allocator< Eigen::Affine3d > > vector_Affine3d
void poseEigenToMsg(const Eigen::Affine3d &e, geometry_msgs::Pose &m)
void poseMsgToEigen(const geometry_msgs::Pose &m, Eigen::Affine3d &e)
ServiceServer advertiseService(const std::string &service, bool(T::*srv_func)(MReq &, MRes &), T *obj)
static const std::string FK_SERVICE_NAME
MoveGroupContextPtr context_
void computeIK(moveit_msgs::PositionIKRequest &req, moveit_msgs::RobotState &solution, moveit_msgs::MoveItErrorCodes &error_code, robot_state::RobotState &rs, const robot_state::GroupStateValidityCallbackFn &constraint=robot_state::GroupStateValidityCallbackFn()) const
ros::ServiceServer ik_service_
ros::ServiceServer fk_service_
bool performTransform(geometry_msgs::PoseStamped &pose_msg, const std::string &target_frame) const
static const std::string IK_SERVICE_NAME
MoveGroupKinematicsService()
ros::NodeHandle root_node_handle_
ConstraintEvaluationResult decide(const robot_state::RobotState &state, bool verbose=false) const
bool isStateColliding(const std::string &group="", bool verbose=false)
bool computeFKService(moveit_msgs::GetPositionFK::Request &req, moveit_msgs::GetPositionFK::Response &res)
CLASS_LOADER_REGISTER_CLASS(Dog, Base)