From 17e487be062048b412179e44ec1fece778ce1305 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 15 Apr 2018 18:05:06 +0200 Subject: [PATCH] cleanup - indentation: space -> tabs - only consider joints of JMG --- .../moveit/task_constructor/stages/move_to.h | 12 +- core/src/stages/move_to.cpp | 180 ++++++++++-------- core/src/stages/simple_grasp.cpp | 2 +- 3 files changed, 108 insertions(+), 86 deletions(-) diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 4a545952..e482a9d2 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -44,6 +44,9 @@ #include #include +namespace moveit { namespace core { +class RobotState; +} } namespace moveit { namespace task_constructor { namespace stages { class MoveTo : public PropagatingEitherWay { @@ -61,10 +64,10 @@ public: void setGoal(const geometry_msgs::PoseStamped& pose); /// move link to given point, keeping current orientation void setGoal(const geometry_msgs::PointStamped& point); - /// move joint model group to given named pose - void setGoal(const std::string& joint_pose); - /// move joints specified in msg to their target values - void setGoal(const moveit_msgs::RobotState& robot_state); + /// move joint model group to given named pose + void setGoal(const std::string& named_joint_pose); + /// move joints specified in msg to their target values + void setGoal(const moveit_msgs::RobotState& robot_state); void setPathConstraints(moveit_msgs::Constraints path_constraints){ setProperty("path_constraints", std::move(path_constraints)); @@ -73,6 +76,7 @@ public: protected: bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, SubTrajectory &trajectory, Direction dir); + bool getJointStateGoal(moveit::core::RobotState& state, const core::RobotModelConstPtr& robot_model); protected: solvers::PlannerInterfacePtr planner_; diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index 9b74bbfb..acf4d8f5 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -55,8 +55,8 @@ MoveTo::MoveTo(const std::string& name, const solvers::PlannerInterfacePtr& plan p.declare("pose", "Cartesian target pose"); p.declare("point", "Cartesian target point"); - p.declare("named_joint_pose", "named joint pose"); - p.declare("joint_pose", "joint pose as robot state message"); + p.declare("named_joint_pose", "named joint pose"); + p.declare("joint_pose", "joint pose"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); @@ -80,48 +80,64 @@ void MoveTo::setGoal(const geometry_msgs::PointStamped &point) setProperty("point", point); } -void MoveTo::setGoal(const std::string &joint_pose) +void MoveTo::setGoal(const std::string &named_joint_pose) { - setProperty("named_joint_pose", joint_pose); + setProperty("named_joint_pose", named_joint_pose); } -void MoveTo::setGoal(const moveit_msgs::RobotState &robot_state) +void MoveTo::setGoal(const moveit_msgs::RobotState& robot_state) { - setProperty("joint_pose", robot_state); + setProperty("joint_pose", robot_state); } void MoveTo::init(const moveit::core::RobotModelConstPtr& robot_model) { - InitStageException errors; - PropagatingEitherWay::init(robot_model); planner_->init(robot_model); - const auto& props = properties(); + // Check if named_joint_pose is well-defined in JMG. + robot_state::RobotState state(robot_model); + getJointStateGoal(state, robot_model); +} - // check if named_joint_pose is set. if so, convert to robot state msg - boost::any goal = props.get("named_joint_pose"); - if (!goal.empty()) { - const std::string& named_joint_pose = boost::any_cast(goal); - robot_state::RobotState state(robot_model); +bool MoveTo::getJointStateGoal(moveit::core::RobotState& state, + const robot_model::RobotModelConstPtr& robot_model) { + const auto& props = properties(); + if (props.countDefined({"named_joint_pose", "joint_pose"}) == 2) + throw InitStageException(*this, "Cannot define both, named_joint_pose and joint_pose"); - const std::string& group = props.get("group"); - const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); + const std::string& group = props.get("group"); + const moveit::core::JointModelGroup* jmg = robot_model->getJointModelGroup(group); + if (!jmg) + throw InitStageException(*this, "Unknown joint model group: " + group); - if (!state.setToDefaultValues(jmg, named_joint_pose)) { - ROS_WARN("MoveTo: unknown joint pose '%s' for jmg '%s'.", named_joint_pose.c_str(), group.c_str()); - errors.push_back(*this, "MoveTo: unknown joint pose."); - } else { - ROS_INFO("MoveTo: setting joint pose '%s' for jmg '%s' as goal.", named_joint_pose.c_str(), group.c_str()); + boost::any goal = props.get("named_joint_pose"); + if (!goal.empty()) { + const std::string& named_joint_pose = boost::any_cast(goal); + if (!state.setToDefaultValues(jmg, named_joint_pose)) + throw InitStageException(*this, "Unknown joint pose:" + named_joint_pose); + return true; + } - moveit_msgs::RobotState state_msg; - robot_state::robotStateToRobotStateMsg(state, state_msg); + goal = props.get("joint_pose"); + if (!goal.empty()) { + const moveit_msgs::RobotState& msg = boost::any_cast(goal); + if (!msg.is_diff) + throw InitStageException(*this, "Expecting a diff state"); - setGoal(state_msg); - } - } + // validate specified joints + const auto& accepted = jmg->getJointModelNames(); + for (const auto &name : msg.joint_state.name) + if (std::find(accepted.begin(), accepted.end(), name) == accepted.end()) + throw InitStageException(*this, "Joint '" + name + "' is not part of group '" + group + "'"); + for (const auto &name : msg.multi_dof_joint_state.joint_names) + if (std::find(accepted.begin(), accepted.end(), name) == accepted.end()) + throw InitStageException(*this, "Joint '" + name + "' is not part of group '" + group + "'"); - if (errors) throw errors; + moveit::core::robotStateMsgToRobotState(msg, state, false); + return true; + } + return false; } bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningScenePtr& scene, @@ -134,76 +150,78 @@ bool MoveTo::compute(const InterfaceState &state, planning_scene::PlanningSceneP const std::string& group = props.get("group"); const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); if (!jmg) { - ROS_WARN_STREAM("MoveTo: invalid joint model group: " << group); - return false; - } - - // only allow single target - size_t count_goals = props.countDefined({"joint_pose", "pose", "point"}); - if (count_goals != 1) { - if (count_goals == 0) ROS_WARN("MoveTo: no goal defined"); - else ROS_WARN("MoveTo: cannot plan to multiple goals"); + ROS_WARN_STREAM_NAMED("MoveTo", "Invalid joint model group: " << group); return false; } const auto& path_constraints = props.get("path_constraints"); - robot_trajectory::RobotTrajectoryPtr robot_trajectory; bool success = false; - boost::any goal = props.get("joint_pose"); - if (!goal.empty()) { - const moveit_msgs::RobotState& target_state = boost::any_cast(goal); - scene->setCurrentState(target_state); - success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); - } else { - // Cartesian targets require the link name - std::string link_name = props.get("link"); - const moveit::core::LinkModel* link; - if (link_name.empty()) - link_name = solvers::getEndEffectorLink(jmg); - if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { - ROS_WARN_STREAM("MoveTo: no or invalid link name specified: " << link_name); - return false; - } + try { + size_t cartesian_goals = props.countDefined({"pose", "point"}); + if (getJointStateGoal(scene->getCurrentStateNonConst(), scene->getRobotModel())) { + if (cartesian_goals > 0) + ROS_WARN_NAMED("MoveTo", "Ignoring Cartesian goals in favour of joint space goal"); + // plan to joint-space target + success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + } else if (cartesian_goals == 1) { + const moveit::core::LinkModel* link; + Eigen::Affine3d target_eigen; - Eigen::Affine3d target_eigen; + // Cartesian targets require the link name + // TODO: use ik_frame property as in ComputeIK + std::string link_name = props.get("link"); + if (link_name.empty()) + link_name = solvers::getEndEffectorLink(jmg); + if (link_name.empty() || !(link = scene->getRobotModel()->getLinkModel(link_name))) { + ROS_WARN_STREAM_NAMED("MoveTo", "No or invalid link name specified: " << link_name); + return false; + } - goal = props.get("pose"); - if (!goal.empty()) { - const geometry_msgs::PoseStamped& target = boost::any_cast(goal); - tf::poseMsgToEigen(target.pose, target_eigen); + boost::any goal = props.get("pose"); + if (!goal.empty()) { + const geometry_msgs::PoseStamped& target = boost::any_cast(goal); + tf::poseMsgToEigen(target.pose, target_eigen); - // transform target into global frame - const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id); - target_eigen = frame * target_eigen; + // transform target into global frame + const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id); + target_eigen = frame * target_eigen; - // frame at target pose - rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame"); + // frame at target pose + rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame"); - // frame at link - geometry_msgs::PoseStamped pose_msg; - pose_msg.header.frame_id = link_name; - pose_msg.pose.orientation.w = 1.0; - rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); - } + // frame at link + geometry_msgs::PoseStamped pose_msg; + pose_msg.header.frame_id = link_name; + pose_msg.pose.orientation.w = 1.0; + rviz_marker_tools::appendFrame(solution.markers(), pose_msg, 0.1, "ik frame"); + } - goal = props.get("point"); - if (!goal.empty()) { - const geometry_msgs::PointStamped& target = boost::any_cast(goal); - Eigen::Vector3d target_point; - tf::pointMsgToEigen(target.point, target_point); + goal = props.get("point"); + if (!goal.empty()) { + const geometry_msgs::PointStamped& target = boost::any_cast(goal); + Eigen::Vector3d target_point; + tf::pointMsgToEigen(target.point, target_point); - // transform target into global frame - const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id); - target_point = frame * target_point; + // transform target into global frame + const Eigen::Affine3d& frame = scene->getFrameTransform(target.header.frame_id); + target_point = frame * target_point; - // retain link orientation - target_eigen = scene->getCurrentState().getGlobalLinkTransform(link_name); - target_eigen.translation() = target_point; - } + // retain link orientation + target_eigen = scene->getCurrentState().getGlobalLinkTransform(link_name); + target_eigen.translation() = target_point; + } - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + // plan to Cartesian target + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + } else if (cartesian_goals > 1) + ROS_WARN_NAMED("MoveTo", "Cannot plan to multiple goals"); + else if (cartesian_goals == 0) + ROS_WARN_NAMED("MoveTo", "No goal defined"); + } catch (const InitStageException &e) { + ROS_WARN_STREAM_NAMED("MoveTo", e.what()); + return false; } // store result diff --git a/core/src/stages/simple_grasp.cpp b/core/src/stages/simple_grasp.cpp index 72f1db6c..7bc7ab75 100644 --- a/core/src/stages/simple_grasp.cpp +++ b/core/src/stages/simple_grasp.cpp @@ -94,7 +94,7 @@ SimpleGraspBase::SimpleGraspBase(const std::string& name, bool forward) return boost::any(jmg->getName()); }); insert(std::move(move), insertion_position); - exposePropertyOfChildAs(insertion_position, "named_joint_pose", forward ? "grasp" : "pregrasp"); + exposePropertyOfChildAs(insertion_position, "named_joint_pose", forward ? "grasp" : "pregrasp"); } { auto attach = std::make_unique(forward ? "attach object" : "detach object");