diff --git a/core/include/moveit/task_constructor/stages/compute_ik.h b/core/include/moveit/task_constructor/stages/compute_ik.h index f2448c3e..e442be81 100644 --- a/core/include/moveit/task_constructor/stages/compute_ik.h +++ b/core/include/moveit/task_constructor/stages/compute_ik.h @@ -10,7 +10,10 @@ #include namespace moveit { -namespace core { MOVEIT_CLASS_FORWARD(RobotState) } +namespace core { +MOVEIT_CLASS_FORWARD(RobotState) +MOVEIT_CLASS_FORWARD(JointModelGroup) +} } namespace moveit { namespace task_constructor { namespace stages { @@ -19,6 +22,7 @@ class ComputeIK : public WrapperBase { public: ComputeIK(const std::string &name, pointer &&child = Stage::pointer()); + void init(const core::RobotModelConstPtr &robot_model); void onNewSolution(const SolutionBase &s) override; void setTimeout(double timeout); diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index 48f1733d..f04d2ce9 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -25,7 +25,7 @@ ComputeIK::ComputeIK(const std::string &name, Stage::pointer &&child) auto& p = properties(); p.declare("timeout", 1.0); p.declare("eef", "name of end-effector group"); - p.declare("group", "", "name of active group (derived from eef if not provided)"); + p.declare("group", "name of active group (derived from eef if not provided)"); p.declare("default_pose", "", "default joint pose of active group (defines cost of IK)"); p.declare("max_ik_solutions", 1); p.declare("ignore_collisions", false); @@ -112,8 +112,62 @@ bool isTargetPoseColliding(const planning_scene::PlanningSceneConstPtr& scene, return res.collision; } +bool validateEEF(const PropertyMap& props, const moveit::core::RobotModelConstPtr& robot_model, + const moveit::core::JointModelGroup*& jmg, std::string* msg) +{ + try { + const std::string& eef = props.get("eef"); + if (!robot_model->hasEndEffector(eef)) { + if (msg) *msg = "Unknown end effector: " + eef; + return false; + } else + jmg = robot_model->getEndEffector(eef); + } catch (const Property::undefined&) { + } + return true; +} +bool validateGroup(const PropertyMap& props, const moveit::core::RobotModelConstPtr& robot_model, + const moveit::core::JointModelGroup* eef_jmg, + const moveit::core::JointModelGroup*& jmg, std::string* msg) +{ + try { + const std::string& group = props.get("group"); + if (!(jmg = robot_model->getJointModelGroup(group))) { + if (msg) *msg = "Unknown group: " + group; + return false; + } + } catch (const Property::undefined&) { + if (eef_jmg) { + // derive group from eef + const auto& parent = eef_jmg->getEndEffectorParentGroup(); + jmg = robot_model->getJointModelGroup(parent.first); + } + } + return true; +} + } // anonymous namespace +void ComputeIK::init(const moveit::core::RobotModelConstPtr& robot_model) +{ + InitStageException errors; + try { WrapperBase::init(robot_model); } catch (InitStageException &e) { errors.append(e); } + + // all properties can be derived from the interface state + // however, if they are defined already now, we validate here + const auto& props = properties(); + const moveit::core::JointModelGroup* eef_jmg = nullptr; + const moveit::core::JointModelGroup* jmg = nullptr; + std::string msg; + + if (!validateEEF(props, robot_model, eef_jmg, &msg)) + errors.push_back(*this, msg); + if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) + errors.push_back(*this, msg); + + if (errors) throw errors; +} + void ComputeIK::onNewSolution(const SolutionBase &s) { assert(s.start() && s.end()); @@ -124,27 +178,39 @@ void ComputeIK::onNewSolution(const SolutionBase &s) properties().performInitFrom(INTERFACE, s.start()->properties(), true); const auto& props = properties(); - const std::string& eef = props.get("eef"); - assert(sandbox_scene->getRobotModel()->hasEndEffector(eef) && "The specified end effector is not defined in the srdf"); - const auto& robot_model = sandbox_scene->getRobotModel(); - const moveit::core::JointModelGroup* eef_jmg = robot_model->getEndEffector(eef); - const std::string& link_name = eef_jmg->getEndEffectorParentGroup().second; + const moveit::core::JointModelGroup* eef_jmg = nullptr; + const moveit::core::JointModelGroup* jmg = nullptr; + std::string msg; - const std::string& group = props.get("group"); - const moveit::core::JointModelGroup* jmg = group.empty() ? robot_model->getJointModelGroup(eef_jmg->getEndEffectorParentGroup().first) - : robot_model->getJointModelGroup(group); + if (!validateEEF(props, robot_model, eef_jmg, &msg)) { + ROS_WARN_STREAM_NAMED("ComputeIK", msg); + return; + } + if (!validateGroup(props, robot_model, eef_jmg, jmg, &msg)) { + ROS_WARN_STREAM_NAMED("ComputeIK", msg); + return; + } + if (!eef_jmg && !jmg) { + ROS_WARN_STREAM_NAMED("ComputeIK", "Neither eef nor group are well defined"); + return; + } + const robot_model::LinkModel* link = eef_jmg ? robot_model->getLinkModel(eef_jmg->getEndEffectorParentGroup().second) + : jmg->getOnlyOneEndEffectorTip(); + if (!link) { + ROS_WARN_STREAM_NAMED("ComputeIK", "Failed to derive IK target link"); + return; + } + const std::string& link_name = link->getName(); robot_state::RobotState& sandbox_state = sandbox_scene->getCurrentStateNonConst(); - const robot_model::LinkModel* link = sandbox_state.getLinkModel(link_name); - if (!link) throw std::runtime_error("requested link '" + link_name + "' does not exist"); // compute target pose w.r.t. link_name geometry_msgs::PoseStamped target_pose_msg = props.get("target_pose"); Eigen::Affine3d target_pose; tf::poseMsgToEigen(target_pose_msg.pose, target_pose); if (!target_pose_msg.header.frame_id.empty() && target_pose_msg.header.frame_id != link_name) { - const robot_model::LinkModel* ref_link = sandbox_state.getLinkModel(target_pose_msg.header.frame_id); + const robot_model::LinkModel* ref_link = robot_model->getLinkModel(target_pose_msg.header.frame_id); if (!ref_link) throw std::runtime_error("requested reference frame '" + target_pose_msg.header.frame_id + "' is not a robot link"); const Eigen::Affine3d link_pose = sandbox_state.getGlobalLinkTransform(link_name);