From 40ca35085a1157d1af57e4918a62bb6d3bd0ac35 Mon Sep 17 00:00:00 2001 From: Robert Haschke Date: Sun, 15 Apr 2018 20:38:03 +0200 Subject: [PATCH] cleanup --- .../moveit/task_constructor/properties.h | 19 +- .../task_constructor/stages/move_relative.h | 39 +- .../moveit/task_constructor/stages/pick.h | 14 +- core/src/stages/move_relative.cpp | 336 ++++++++---------- core/src/stages/pick.cpp | 6 +- 5 files changed, 196 insertions(+), 218 deletions(-) diff --git a/core/include/moveit/task_constructor/properties.h b/core/include/moveit/task_constructor/properties.h index 95c127bb..33c33375 100644 --- a/core/include/moveit/task_constructor/properties.h +++ b/core/include/moveit/task_constructor/properties.h @@ -287,15 +287,16 @@ void PropertyMap::set(const std::string& name, const boost::any& val // provide a serialization method for std::map template std::ostream& operator<<(std::ostream& os, const std::map& m) { - os << "{"; - bool first = true; - for (const auto& pair : m) { - if (!first) - os << ", "; - os << pair.first << " : " << pair.second; - first = false; - } - os << "}"; + os << "{"; + bool first = true; + for (const auto& pair : m) { + if (!first) + os << ", "; + os << pair.first << " : " << pair.second; + first = false; + } + os << "}"; + return os; } } // namespace task_constructor diff --git a/core/include/moveit/task_constructor/stages/move_relative.h b/core/include/moveit/task_constructor/stages/move_relative.h index 14748322..d9a1d062 100644 --- a/core/include/moveit/task_constructor/stages/move_relative.h +++ b/core/include/moveit/task_constructor/stages/move_relative.h @@ -55,24 +55,42 @@ public: bool computeForward(const InterfaceState& from) override; bool computeBackward(const InterfaceState& to) override; - void setGroup(const std::string& group); - void setLink(const std::string& link); + void setGroup(const std::string& group) { + setProperty("group", group); + } + void setLink(const std::string& link) { + setProperty("link", link); + } /// set minimum / maximum distance to move - void setMinDistance(double distance); - void setMaxDistance(double distance); - void setMinMaxDistance(double min_distance, double max_distance); + void setMinDistance(double distance) { + setProperty("min_distance", distance); + } + void setMaxDistance(double distance) { + setProperty("max_distance", distance); + } + void setMinMaxDistance(double min_distance, double max_distance) { + setProperty("min_distance", min_distance); + setProperty("max_distance", max_distance); + } - void setRelativeJointSpaceGoal(const std::map& goal); - - void setPathConstraints(moveit_msgs::Constraints path_constraints){ + void setPathConstraints(moveit_msgs::Constraints path_constraints) { setProperty("path_constraints", std::move(path_constraints)); } /// perform twist motion on specified link - void along(const geometry_msgs::TwistStamped& twist); + void along(const geometry_msgs::TwistStamped& twist) { + setProperty("twist", twist); + } /// translate link along given direction - void along(const geometry_msgs::Vector3Stamped& direction); + void along(const geometry_msgs::Vector3Stamped& direction) { + setProperty("direction", direction); + } + /// move specified joint variables by given amount + void about(const std::map& goal) { + setProperty("joints", goal); + } + protected: bool compute(const InterfaceState& state, planning_scene::PlanningScenePtr &scene, @@ -80,7 +98,6 @@ protected: protected: solvers::PlannerInterfacePtr planner_; - bool joint_space_goal_ = false; }; } } } diff --git a/core/include/moveit/task_constructor/stages/pick.h b/core/include/moveit/task_constructor/stages/pick.h index 9d566154..65ae301a 100644 --- a/core/include/moveit/task_constructor/stages/pick.h +++ b/core/include/moveit/task_constructor/stages/pick.h @@ -86,10 +86,10 @@ public: void setApproachRetract(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); + void setLiftPlace(const geometry_msgs::TwistStamped& motion, double min_distance, double max_distance); - - void setRelativeJointSpaceGoal(const std::map& relative_joint_space_goal); + void setLiftPlace(const std::map& joints); }; @@ -109,10 +109,9 @@ public: double min_distance, double max_distance) { setLiftPlace(motion, min_distance, max_distance); } - - void setRelativeJointSpaceLiftGoal(const std::map& relative_joint_space_goal) { - setRelativeJointSpaceGoal(relative_joint_space_goal); - } + void setLiftMotion(const std::map& joints) { + setLiftPlace(joints); + } }; @@ -132,6 +131,9 @@ public: double min_distance, double max_distance) { setLiftPlace(motion, min_distance, max_distance); } + void setPlaceMotion(const std::map& joints) { + setLiftPlace(joints); + } }; diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index dc2b3340..badf8af6 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -52,52 +52,17 @@ MoveRelative::MoveRelative(const std::string& name, const solvers::PlannerInterf p.declare("marker_ns", "", "marker namespace"); p.declare("group", "name of planning group"); p.declare("link", "", "link to move (default is tip of jmg)"); - p.declare("min_distance", "minimum distance to move"); - p.declare("max_distance", "maximum distance to move"); + p.declare("min_distance", -1.0, "minimum distance to move"); + p.declare("max_distance", 0.0, "maximum distance to move"); p.declare("twist", "Cartesian twist transform"); p.declare("direction", "Cartesian translation direction"); - p.declare>("relative_joint_space_goal", "Relative joint space goal"); + p.declare>("joints", "Relative joint space goal"); p.declare("path_constraints", moveit_msgs::Constraints(), "constraints to maintain during trajectory"); } -void MoveRelative::setRelativeJointSpaceGoal(const std::map& goal){ - setProperty("relative_joint_space_goal", goal); -} - -void MoveRelative::setGroup(const std::string& group){ - setProperty("group", group); -} - -void MoveRelative::setLink(const std::string& link){ - setProperty("link", link); -} - -void MoveRelative::setMinDistance(double distance){ - setProperty("min_distance", distance); -} - -void MoveRelative::setMaxDistance(double distance){ - setProperty("max_distance", distance); -} - -void MoveRelative::setMinMaxDistance(double min_distance, double max_distance){ - setProperty("min_distance", min_distance); - setProperty("max_distance", max_distance); -} - -void MoveRelative::along(const geometry_msgs::TwistStamped &twist) -{ - setProperty("twist", twist); -} - -void MoveRelative::along(const geometry_msgs::Vector3Stamped &direction) -{ - setProperty("direction", direction); -} - void MoveRelative::init(const moveit::core::RobotModelConstPtr& robot_model) { PropagatingEitherWay::init(robot_model); @@ -114,154 +79,172 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning const std::string& group = props.get("group"); const moveit::core::JointModelGroup* jmg = scene->getRobotModel()->getJointModelGroup(group); if (!jmg) { - ROS_WARN_STREAM("MoveRelative: invalid joint model group: " << group); + ROS_WARN_STREAM_NAMED("MoveRelative", "Invalid joint model group: " << group); return false; } // only allow single target - size_t count_goals = props.countDefined({"twist", "direction", "relative_joint_space_goal"}); + size_t count_goals = props.countDefined({"twist", "direction", "joints"}); if (count_goals != 1) { - if (count_goals == 0) ROS_WARN("MoveRelative: no goal defined"); - else ROS_WARN("MoveRelative: cannot plan to multiple goals"); + if (count_goals == 0) ROS_WARN_NAMED("MoveRelative", "No goal defined"); + else ROS_WARN_NAMED("MoveRelative", "Cannot plan to multiple goals"); return false; } - // Cartesian targets require the link name - // TODO: use ik_frame property as in ComputeIK - 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("MoveRelative: no or invalid link name specified: " << link_name); - return false; - } + double max_distance = props.get("max_distance"); + double min_distance = props.get("min_distance"); + const auto& path_constraints = props.get("path_constraints"); - Property prop = props.property("max_distance"); - double max_distance = prop.value().empty() ? 0.0 : std::abs(boost::any_cast(prop.value())); - prop = props.property("min_distance"); - double min_distance = prop.value().empty() ? -1.0 : boost::any_cast(prop.value()); + robot_trajectory::RobotTrajectoryPtr robot_trajectory; + bool success = false; - bool use_rotation_distance = false; // measure achieved distance as rotation? - Eigen::Vector3d linear; // linear translation - Eigen::Vector3d angular; // angular rotation - double linear_norm = 0.0, angular_norm = 0.0; - - Eigen::Affine3d target_eigen; - Eigen::Affine3d link_pose = scene->getFrameTransform(link_name); // take a copy here, pose will change on success - - boost::any goal = props.get("twist"); + boost::any goal = props.get("joints"); if (!goal.empty()) { - const geometry_msgs::TwistStamped& target = boost::any_cast(goal); - const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id); - tf::vectorMsgToEigen(target.twist.linear, linear); - tf::vectorMsgToEigen(target.twist.angular, angular); - - linear_norm = linear.norm(); - angular_norm = angular.norm(); - if (angular_norm > std::numeric_limits::epsilon()) - angular /= angular_norm; // normalize angular - use_rotation_distance = linear_norm < std::numeric_limits::epsilon(); - - // use max distance? - if (max_distance > 0.0) { - double scale; - if (!use_rotation_distance) // non-zero linear motion defines distance - scale = max_distance / linear_norm; - else if (angular_norm > std::numeric_limits::epsilon()) - scale = max_distance / angular_norm; - linear *= scale; - linear_norm *= scale; - angular_norm *= scale; + const auto& accepted = jmg->getJointModelNames(); + auto& robot_state = scene->getCurrentStateNonConst(); + const auto& joints = boost::any_cast>(goal); + for (auto j : joints) { + int index = robot_state.getRobotModel()->getVariableIndex(j.first); + auto jm = scene->getRobotModel()->getJointModel(index); + if (std::find(accepted.begin(), accepted.end(), j.first) == accepted.end()) { + ROS_WARN_STREAM_NAMED("MoveRelative", "Ignoring joint " << j.first << " that is not part of group " << group); + continue; + } + robot_state.setVariablePosition(index, robot_state.getVariablePosition(index) + j.second); + robot_state.enforceBounds(jm); + } + robot_state.update(); + success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); + } else { + // Cartesian targets require the link name + // TODO: use ik_frame property as in ComputeIK + 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_NAMED("MoveRelative", "No or invalid link name specified: " << link_name); + return false; } - // invert direction? - if (dir == BACKWARD) { - linear *= -1.0; - angular *= -1.0; + bool use_rotation_distance = false; // measure achieved distance as rotation? + Eigen::Vector3d linear; // linear translation + Eigen::Vector3d angular; // angular rotation + double linear_norm = 0.0, angular_norm = 0.0; + + Eigen::Affine3d target_eigen; + Eigen::Affine3d link_pose = scene->getFrameTransform(link_name); // take a copy here, pose will change on success + + boost::any goal = props.get("twist"); + if (!goal.empty()) { + const geometry_msgs::TwistStamped& target = boost::any_cast(goal); + const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id); + tf::vectorMsgToEigen(target.twist.linear, linear); + tf::vectorMsgToEigen(target.twist.angular, angular); + + linear_norm = linear.norm(); + angular_norm = angular.norm(); + if (angular_norm > std::numeric_limits::epsilon()) + angular /= angular_norm; // normalize angular + use_rotation_distance = linear_norm < std::numeric_limits::epsilon(); + + // use max distance? + if (max_distance > 0.0) { + double scale; + if (!use_rotation_distance) // non-zero linear motion defines distance + scale = max_distance / linear_norm; + else if (angular_norm > std::numeric_limits::epsilon()) + scale = max_distance / angular_norm; + linear *= scale; + linear_norm *= scale; + angular_norm *= scale; + } + + // invert direction? + if (dir == BACKWARD) { + linear *= -1.0; + angular *= -1.0; + } + + // compute absolute transform for link + linear = frame_pose.linear() * linear; + angular = frame_pose.linear() * angular; + target_eigen = link_pose; + target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular); + target_eigen.translation() += linear; } - // compute absolute transform for link - linear = frame_pose.linear() * linear; - angular = frame_pose.linear() * angular; - target_eigen = link_pose; - target_eigen.linear() = target_eigen.linear() * Eigen::AngleAxisd(angular_norm, link_pose.linear().transpose() * angular); - target_eigen.translation() += linear; - } + goal = props.get("direction"); + if (!goal.empty()) { + const geometry_msgs::Vector3Stamped& target = boost::any_cast(goal); + const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id); + tf::vectorMsgToEigen(target.vector, linear); - goal = props.get("direction"); - if (!goal.empty()) { - const geometry_msgs::Vector3Stamped& target = boost::any_cast(goal); - const Eigen::Affine3d& frame_pose = scene->getFrameTransform(target.header.frame_id); - tf::vectorMsgToEigen(target.vector, linear); + // use max distance? + if (max_distance > 0.0) { + linear.normalize(); + linear *= max_distance; + } + linear_norm = linear.norm(); - // use max distance? - if (max_distance > 0.0) { - linear.normalize(); - linear *= max_distance; + // invert direction? + if (dir == BACKWARD) + linear *= -1.0; + + // compute absolute transform for link + linear = frame_pose.linear() * linear; + target_eigen = link_pose; + target_eigen.translation() += linear; } - linear_norm = linear.norm(); - // invert direction? - if (dir == BACKWARD) - linear *= -1.0; + success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); - // compute absolute transform for link - linear = frame_pose.linear() * linear; - target_eigen = link_pose; - target_eigen.translation() += linear; - } - - goal = props.get("relative_joint_space_goal"); - if (!goal.empty()) { - - const auto &start_state = scene->getCurrentState(); - auto& current_state = scene->getCurrentStateNonConst(); - joint_space_goal_ = true; - - const std::map& relative_joint_position_map = boost::any_cast>(goal); - - for (auto j : relative_joint_position_map) { - double new_pos = (start_state.getVariablePosition(j.first) + j.second); - current_state.setVariablePosition(j.first, new_pos); - - auto jm = scene->getRobotModel()->getJointModel(j.first); - current_state.enforceBounds(jm); - current_state.update(true); - } - } - - robot_trajectory::RobotTrajectoryPtr robot_trajectory; - bool success = false; - - if (joint_space_goal_) { - success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, - props.get("path_constraints")); - } else { - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, - props.get("path_constraints")); - } - - // min_distance reached? - if (min_distance > 0.0) { - double distance = 0.0; - if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { - const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint(); - Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link); - if (use_rotation_distance) { - Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); - distance = rotation.angle(); - } else - distance = (reached_pose.translation() - link_pose.translation()).norm(); + // min_distance reached? + if (min_distance > 0.0) { + double distance = 0.0; + if (robot_trajectory && robot_trajectory->getWayPointCount() > 0) { + const robot_state::RobotState& reached_state = robot_trajectory->getLastWayPoint(); + Eigen::Affine3d reached_pose = reached_state.getGlobalLinkTransform(link); + if (use_rotation_distance) { + Eigen::AngleAxisd rotation(reached_pose.linear() * link_pose.linear().transpose()); + distance = rotation.angle(); + } else + distance = (reached_pose.translation() - link_pose.translation()).norm(); + } + success = distance >= min_distance; + if (!success) { + char msg[100]; + snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance); + trajectory.setName(msg); + } + } else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case + success = true; } - success = distance >= min_distance; - if (!success) { - char msg[100]; - snprintf(msg, sizeof(msg), "min_distance not reached (%.3g < %.3g)", distance, min_distance); - trajectory.setName(msg); + + // add an arrow marker + visualization_msgs::Marker m; + // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" + m.ns = props.get("marker_ns"); + if (!m.ns.empty()) { + m.header.frame_id = scene->getPlanningFrame(); + if (linear_norm > 1e-3) { + // +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder) + rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN + : rviz_marker_tools::RED); + rviz_marker_tools::makeArrow(m, linear_norm); + auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear); + Eigen::Vector3d pos(link_pose.translation()); + if (dir == BACKWARD) { + // flip arrow direction + quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()); + // arrow tip at goal_pose + pos += quat * Eigen::Vector3d(-linear_norm, 0, 0); + } + tf::pointEigenToMsg(pos, m.pose.position); + tf::quaternionEigenToMsg(quat, m.pose.orientation); + trajectory.markers().push_back(m); + } } - } else if (min_distance == 0.0) { // if min_distance is zero, we succeed in any case - success = true; } // store result @@ -271,31 +254,6 @@ bool MoveRelative::compute(const InterfaceState &state, planning_scene::Planning trajectory.setTrajectory(robot_trajectory); } - // add an arrow marker - visualization_msgs::Marker m; - // +1 TODO: make "marker" a common property of all stages. However, I would stick with "marker_ns" - m.ns = props.get("marker_ns"); - if (!m.ns.empty()) { - m.header.frame_id = scene->getPlanningFrame(); - if (linear_norm > 1e-3) { - // +1 TODO: arrow could be split into "valid" and "invalid" part (as red cylinder) - rviz_marker_tools::setColor(m.color, success ? rviz_marker_tools::LIME_GREEN - : rviz_marker_tools::RED); - rviz_marker_tools::makeArrow(m, linear_norm); - auto quat = Eigen::Quaterniond::FromTwoVectors(Eigen::Vector3d::UnitX(), linear); - Eigen::Vector3d pos(link_pose.translation()); - if (dir == BACKWARD) { - // flip arrow direction - quat = quat * Eigen::AngleAxisd(M_PI, Eigen::Vector3d::UnitY()); - // arrow tip at goal_pose - pos += quat * Eigen::Vector3d(-linear_norm, 0, 0); - } - tf::pointEigenToMsg(pos, m.pose.position); - tf::quaternionEigenToMsg(quat, m.pose.orientation); - trajectory.markers().push_back(m); - } - } - return success; } diff --git a/core/src/stages/pick.cpp b/core/src/stages/pick.cpp index ac7f6eac..4b418c5d 100644 --- a/core/src/stages/pick.cpp +++ b/core/src/stages/pick.cpp @@ -83,10 +83,10 @@ void PickPlaceBase::setLiftPlace(const geometry_msgs::TwistStamped& motion, doub p.set("max_distance", max_distance); } -void PickPlaceBase::setRelativeJointSpaceGoal(const std::map& relative_joint_space_goal) +void PickPlaceBase::setLiftPlace(const std::map& joints) { - auto& p = lift_stage_->properties(); - p.set("relative_joint_space_goal", relative_joint_space_goal); + auto& p = lift_stage_->properties(); + p.set("joints", joints); } } } }