diff --git a/core/include/moveit/task_constructor/moveit_compat.h b/core/include/moveit/task_constructor/moveit_compat.h index 5024861f..91e7d12b 100644 --- a/core/include/moveit/task_constructor/moveit_compat.h +++ b/core/include/moveit/task_constructor/moveit_compat.h @@ -55,3 +55,5 @@ // use object shape poses relative to a single object pose #define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6) + +#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6) diff --git a/core/include/moveit/task_constructor/stage.h b/core/include/moveit/task_constructor/stage.h index 79c27837..7a25fa4b 100644 --- a/core/include/moveit/task_constructor/stage.h +++ b/core/include/moveit/task_constructor/stage.h @@ -84,7 +84,7 @@ enum InterfaceFlag GENERATE = WRITES_PREV_END | WRITES_NEXT_START, }; -using InterfaceFlags = Flags; +using InterfaceFlags = utils::Flags; /** invert interface such that * - new end can connect to old start diff --git a/core/include/moveit/task_constructor/stages/fixed_state.h b/core/include/moveit/task_constructor/stages/fixed_state.h index 59a434cb..2ace2726 100644 --- a/core/include/moveit/task_constructor/stages/fixed_state.h +++ b/core/include/moveit/task_constructor/stages/fixed_state.h @@ -48,7 +48,7 @@ namespace stages { class FixedState : public Generator { public: - FixedState(const std::string& name = "initial state"); + FixedState(const std::string& name = "initial state", planning_scene::PlanningScenePtr = nullptr); void setState(const planning_scene::PlanningScenePtr& scene); void reset() override; diff --git a/core/include/moveit/task_constructor/stages/move_to.h b/core/include/moveit/task_constructor/stages/move_to.h index 98bf0f57..5968017b 100644 --- a/core/include/moveit/task_constructor/stages/move_to.h +++ b/core/include/moveit/task_constructor/stages/move_to.h @@ -99,7 +99,7 @@ protected: bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state); bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen); - bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link, + bool getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen); protected: diff --git a/core/include/moveit/task_constructor/utils.h b/core/include/moveit/task_constructor/utils.h index ed83596a..302d4780 100644 --- a/core/include/moveit/task_constructor/utils.h +++ b/core/include/moveit/task_constructor/utils.h @@ -32,15 +32,38 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Authors: Robert Haschke - Desc: Project-agnostic utility classes +/* Authors: Robert Haschke, Michael 'v4hn' Goerner + Desc: Miscellaneous utilities */ #pragma once +#include #include #include +#include + +#include + +namespace planning_scene { +MOVEIT_CLASS_FORWARD(PlanningScene); +} + +namespace moveit { + +namespace core { +MOVEIT_CLASS_FORWARD(LinkModel); +MOVEIT_CLASS_FORWARD(JointModelGroup); +MOVEIT_CLASS_FORWARD(RobotState); +} // namespace core + +namespace task_constructor { +MOVEIT_CLASS_FORWARD(Property); +MOVEIT_CLASS_FORWARD(SolutionBase); + +namespace utils { + /** template class to compose flags from enums in a type-safe fashion */ template class Flags @@ -118,3 +141,13 @@ private: }; #define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags; + +const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, + std::string frame); + +bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, + const moveit::core::JointModelGroup* jmg, SolutionBase& solution, + const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame); +} // namespace utils +} // namespace task_constructor +} // namespace moveit diff --git a/core/src/CMakeLists.txt b/core/src/CMakeLists.txt index 263c5033..d37ea27c 100644 --- a/core/src/CMakeLists.txt +++ b/core/src/CMakeLists.txt @@ -5,6 +5,7 @@ add_library(${PROJECT_NAME} ${PROJECT_INCLUDE}/introspection.h ${PROJECT_INCLUDE}/marker_tools.h ${PROJECT_INCLUDE}/merge.h + ${PROJECT_INCLUDE}/moveit_compat.h ${PROJECT_INCLUDE}/properties.h ${PROJECT_INCLUDE}/stage.h ${PROJECT_INCLUDE}/stage_p.h @@ -27,6 +28,7 @@ add_library(${PROJECT_NAME} stage.cpp storage.cpp task.cpp + utils.cpp solvers/planner_interface.cpp solvers/cartesian_path.cpp diff --git a/core/src/solvers/joint_interpolation.cpp b/core/src/solvers/joint_interpolation.cpp index a0aa2202..35b3935f 100644 --- a/core/src/solvers/joint_interpolation.cpp +++ b/core/src/solvers/joint_interpolation.cpp @@ -40,6 +40,8 @@ #include #include +#include + namespace moveit { namespace task_constructor { namespace solvers { @@ -53,9 +55,9 @@ void JointInterpolationPlanner::init(const core::RobotModelConstPtr& /*robot_mod bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, const planning_scene::PlanningSceneConstPtr& to, - const moveit::core::JointModelGroup* jmg, double timeout, + const moveit::core::JointModelGroup* jmg, double /*timeout*/, robot_trajectory::RobotTrajectoryPtr& result, - const moveit_msgs::Constraints& path_constraints) { + const moveit_msgs::Constraints& /*path_constraints*/) { const auto& props = properties(); // Get maximum joint distance @@ -95,12 +97,38 @@ bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr return true; } -bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& /*from*/, - const moveit::core::LinkModel& /*link*/, const Eigen::Isometry3d& /*target_eigen*/, - const moveit::core::JointModelGroup* /*jmg*/, double /*timeout*/, - robot_trajectory::RobotTrajectoryPtr& /*result*/, - const moveit_msgs::Constraints& /*path_constraints*/) { - throw std::runtime_error("Not yet implemented"); +bool JointInterpolationPlanner::plan(const planning_scene::PlanningSceneConstPtr& from, + const moveit::core::LinkModel& link, const Eigen::Isometry3d& target_eigen, + const moveit::core::JointModelGroup* jmg, double timeout, + robot_trajectory::RobotTrajectoryPtr& result, + const moveit_msgs::Constraints& path_constraints) { + const auto start_time = std::chrono::steady_clock::now(); + + auto to{ from->diff() }; + + kinematic_constraints::KinematicConstraintSet constraints{ to->getRobotModel() }; + constraints.add(path_constraints, from->getTransforms()); + + auto is_valid{ [&constraints, &to](moveit::core::RobotState* robot_state, const moveit::core::JointModelGroup* jmg, + const double* joint_values) -> bool { + robot_state->setJointGroupPositions(jmg, joint_values); + robot_state->update(); + return to->isStateValid(*robot_state, constraints, jmg->getName()); + } }; + + if (!to->getCurrentStateNonConst().setFromIK(jmg, target_eigen, link.getName(), timeout, is_valid)) { + // TODO(v4hn): planners need a way to add feedback to failing plans + // in case of an invalid solution feedback should include unwanted collisions or violated constraints + ROS_WARN_NAMED("JointInterpolationPlanner", "IK failed for pose target"); + return false; + } + to->getCurrentStateNonConst().update(); + + timeout = std::chrono::duration(std::chrono::steady_clock::now() - start_time).count(); + if (timeout <= 0.0) + return false; + + return plan(from, to, jmg, timeout, result, path_constraints); } } // namespace solvers } // namespace task_constructor diff --git a/core/src/stages/compute_ik.cpp b/core/src/stages/compute_ik.cpp index b86450cb..0e5a6b55 100644 --- a/core/src/stages/compute_ik.cpp +++ b/core/src/stages/compute_ik.cpp @@ -287,31 +287,17 @@ void ComputeIK::compute() { ik_pose_msg = boost::any_cast(value); Eigen::Isometry3d ik_pose; tf2::fromMsg(ik_pose_msg.pose, ik_pose); - if (robot_model->hasLinkModel(ik_pose_msg.header.frame_id)) { - link = robot_model->getLinkModel(ik_pose_msg.header.frame_id); - } else { - const robot_state::AttachedBody* attached = - scene->getCurrentState().getAttachedBody(ik_pose_msg.header.frame_id); - if (!attached) { - ROS_WARN_STREAM_NAMED("ComputeIK", "Unknown frame: " << ik_pose_msg.header.frame_id); - return; - } - const EigenSTL::vector_Isometry3d& tf = -#if MOVEIT_HAS_OBJECT_POSE - attached->getShapePosesInLinkFrame(); -#else - attached->getFixedTransforms(); -#endif - if (tf.empty()) { - ROS_WARN_STREAM_NAMED("ComputeIK", "Attached body doesn't have shapes."); - return; - } - // prepend link - link = attached->getAttachedLink(); - ik_pose = tf[0] * ik_pose; + + if (!scene->getCurrentState().knowsFrameTransform(ik_pose_msg.header.frame_id)) { + ROS_WARN_STREAM_NAMED("ComputeIK", "ik frame unknown in robot: '" << ik_pose_msg.header.frame_id << "'"); + return; } + ik_pose = scene->getCurrentState().getFrameTransform(ik_pose_msg.header.frame_id) * ik_pose; + + link = utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id); + // transform target pose such that ik frame will reach there if link does - target_pose = target_pose * ik_pose.inverse(); + target_pose = target_pose * ik_pose.inverse() * scene->getCurrentState().getFrameTransform(link->getName()); } // validate placed link for collisions @@ -323,7 +309,7 @@ void ComputeIK::compute() { // markers used for failures std::deque failure_markers; // frames at target pose and ik frame - rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "ik frame"); + rviz_marker_tools::appendFrame(failure_markers, target_pose_msg, 0.1, "target frame"); rviz_marker_tools::appendFrame(failure_markers, ik_pose_msg, 0.1, "ik frame"); // visualize placed end-effector auto appender = [&failure_markers](visualization_msgs::Marker& marker, const std::string& /*name*/) { diff --git a/core/src/stages/fixed_state.cpp b/core/src/stages/fixed_state.cpp index fddc9e4d..71ad60ad 100644 --- a/core/src/stages/fixed_state.cpp +++ b/core/src/stages/fixed_state.cpp @@ -43,7 +43,8 @@ namespace moveit { namespace task_constructor { namespace stages { -FixedState::FixedState(const std::string& name) : Generator(name) { +FixedState::FixedState(const std::string& name, planning_scene::PlanningScenePtr scene) + : Generator(name), scene_(scene) { setCostTerm(std::make_unique(0.0)); } diff --git a/core/src/stages/move_relative.cpp b/core/src/stages/move_relative.cpp index 4271337d..dbf5ab7f 100644 --- a/core/src/stages/move_relative.cpp +++ b/core/src/stages/move_relative.cpp @@ -193,24 +193,11 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); } else { // Cartesian targets require an IK reference frame - geometry_msgs::PoseStamped ik_pose_msg; const moveit::core::LinkModel* link; - const boost::any& value = props.get("ik_frame"); - if (value.empty()) { // property undefined - // determine IK link from group - if (!(link = jmg->getOnlyOneEndEffectorTip())) { - solution.markAsFailure("missing ik_frame"); - return false; - } - ik_pose_msg.header.frame_id = link->getName(); - ik_pose_msg.pose.orientation.w = 1.0; - } else { - ik_pose_msg = boost::any_cast(value); - if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { - solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id); - return false; - } - } + Eigen::Isometry3d ik_pose_world; + + if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world)) + return false; bool use_rotation_distance = false; // measure achieved distance as rotation? Eigen::Vector3d linear; // linear translation @@ -291,9 +278,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning COMPUTE: // transform target pose such that ik frame will reach there if link does - Eigen::Isometry3d ik_pose; - tf2::fromMsg(ik_pose_msg.pose, ik_pose); - target_eigen = target_eigen * ik_pose.inverse(); + target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world; success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); diff --git a/core/src/stages/move_to.cpp b/core/src/stages/move_to.cpp index f44bd702..3270a831 100644 --- a/core/src/stages/move_to.cpp +++ b/core/src/stages/move_to.cpp @@ -42,6 +42,9 @@ #include #include +#include +#include + #include namespace moveit { @@ -142,33 +145,29 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint } bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, - Eigen::Isometry3d& target_eigen) { + Eigen::Isometry3d& target) { try { - const geometry_msgs::PoseStamped& target = boost::any_cast(goal); - tf2::fromMsg(target.pose, target_eigen); - + const geometry_msgs::PoseStamped& msg = boost::any_cast(goal); + tf2::fromMsg(msg.pose, target); // transform target into global frame - const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id); - target_eigen = frame * target_eigen; + target = scene->getFrameTransform(msg.header.frame_id) * target; } catch (const boost::bad_any_cast&) { return false; } return true; } -bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link, +bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose, const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) { try { const geometry_msgs::PointStamped& target = boost::any_cast(goal); Eigen::Vector3d target_point; tf2::fromMsg(target.point, target_point); - // transform target into global frame - const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id); - target_point = frame * target_point; + target_point = scene->getFrameTransform(target.header.frame_id) * target_point; // retain link orientation - target_eigen = scene->getCurrentState().getGlobalLinkTransform(link); + target_eigen = ik_pose; target_eigen.translation() = target_point; } catch (const boost::bad_any_cast&) { return false; @@ -204,47 +203,37 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP // plan to joint-space target success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints); } else { // Cartesian goal - const moveit::core::LinkModel* link; - Eigen::Isometry3d target_eigen; - - // Cartesian targets require an IK reference frame + // Where to go? + Eigen::Isometry3d target; + // What frame+offset in the robot should go there? geometry_msgs::PoseStamped ik_pose_msg; - const boost::any& value = props.get("ik_frame"); - if (value.empty()) { // property undefined - // determine IK link from group - if (!(link = jmg->getOnlyOneEndEffectorTip())) { - solution.markAsFailure("missing ik_frame"); - return false; - } - ik_pose_msg.header.frame_id = link->getName(); - ik_pose_msg.pose.orientation.w = 1.0; - } else { - ik_pose_msg = boost::any_cast(value); - if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { - solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id); - return false; - } - } - if (!getPoseGoal(goal, scene, target_eigen) && !getPointGoal(goal, link, scene, target_eigen)) { + const moveit::core::LinkModel* link; + Eigen::Isometry3d ik_pose_world; + if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world)) + return false; + + if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) { solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name()); return false; } + auto add_frame{ [&](const Eigen::Isometry3d& pose, const char name[]) { + geometry_msgs::PoseStamped msg; + msg.header.frame_id = scene->getPlanningFrame(); + msg.pose = tf2::toMsg(pose); + rviz_marker_tools::appendFrame(solution.markers(), msg, 0.1, name); + } }; + // visualize plan with frame at target pose and frame at link - geometry_msgs::PoseStamped target; - target.header.frame_id = scene->getPlanningFrame(); - target.pose = tf2::toMsg(target_eigen); - rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame"); - rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); + add_frame(target, "target frame"); + add_frame(ik_pose_world, "ik frame"); // transform target pose such that ik frame will reach there if link does - Eigen::Isometry3d ik_pose; - tf2::fromMsg(ik_pose_msg.pose, ik_pose); - target_eigen = target_eigen * ik_pose.inverse(); + target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link); // plan to Cartesian target - success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); + success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints); } // store result diff --git a/core/src/utils.cpp b/core/src/utils.cpp new file mode 100644 index 00000000..0a4e7e0e --- /dev/null +++ b/core/src/utils.cpp @@ -0,0 +1,113 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2017, Hamburg University + * Copyright (c) 2017, Bielefeld University + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Bielefeld University nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Authors: Michael Goerner, Robert Haschke */ + +#include + +#include +#include + +#include +#include +#include + +namespace moveit { +namespace task_constructor { +namespace utils { + +const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state, + std::string frame) { +#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK + return state.getRigidlyConnectedParentLinkModel(frame); +#else + const moveit::core::LinkModel* link{ nullptr }; + + if (state.hasAttachedBody(frame)) { + link = state.getAttachedBody(frame)->getAttachedLink(); + } else if (state.getRobotModel()->hasLinkModel(frame)) + link = state.getLinkModel(frame); + + return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link); +#endif +} + +bool getRobotTipForFrame(const Property& property, const planning_scene::PlanningScene& scene, + const moveit::core::JointModelGroup* jmg, SolutionBase& solution, + const moveit::core::LinkModel*& robot_link, Eigen::Isometry3d& tip_in_global_frame) { + auto get_tip = [&jmg]() -> const moveit::core::LinkModel* { + // determine IK frame from group + std::vector tips; + jmg->getEndEffectorTips(tips); + if (tips.size() != 1) { + return nullptr; + } + return tips[0]; + }; + + if (property.value().empty()) { // property undefined + robot_link = get_tip(); + if (!robot_link) { + solution.markAsFailure("missing ik_frame"); + return false; + } + tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link); + } else { + auto ik_pose_msg = boost::any_cast(property.value()); + if (ik_pose_msg.header.frame_id.empty()) { + if (!(robot_link = get_tip())) { + solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found"); + return false; + } + tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); + tip_in_global_frame = scene.getCurrentState().getGlobalLinkTransform(robot_link) * tip_in_global_frame; + } else if (scene.knowsFrameTransform(ik_pose_msg.header.frame_id)) { + robot_link = getRigidlyConnectedParentLinkModel(scene.getCurrentState(), ik_pose_msg.header.frame_id); + tf2::fromMsg(ik_pose_msg.pose, tip_in_global_frame); + tip_in_global_frame = scene.getFrameTransform(ik_pose_msg.header.frame_id) * tip_in_global_frame; + } else { + std::stringstream ss; + ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'"; + solution.markAsFailure(ss.str()); + return false; + } + } + + return true; +} + +} // namespace utils +} // namespace task_constructor +} // namespace moveit diff --git a/core/test/CMakeLists.txt b/core/test/CMakeLists.txt index 4ccd5278..808e7b0d 100644 --- a/core/test/CMakeLists.txt +++ b/core/test/CMakeLists.txt @@ -9,17 +9,29 @@ if (CATKIN_ENABLE_TESTING) add_library(gtest_utils gtest_value_printers.cpp models.cpp stage_mockups.cpp) target_link_libraries(gtest_utils ${PROJECT_NAME}) - macro(mtc_add_test TYPE FILE) - string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${FILE}) - string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME}) - _catkin_add_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${FILE}) - target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages ${ARGN} gtest_utils gtest_main) + macro(mtc_add_test TYPE) + # Split ARGN into .test file and other sources + set(TEST_FILE ${ARGN}) + set(SOURCES ${ARGN}) + list(FILTER TEST_FILE INCLUDE REGEX "\.test$") + list(FILTER SOURCES EXCLUDE REGEX "\.test$") + # Determine TARGET name from first source file + list(GET SOURCES 0 TEST_NAME) + string(REGEX REPLACE "\.cpp$" "" TEST_NAME ${TEST_NAME}) + string(REGEX REPLACE "_" "-" TEST_NAME ${TEST_NAME}) + # Configure build target + if(TEST_FILE) # Add rostest if .test file was specified + _add_rostest_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${TEST_FILE} ${SOURCES}) + else() + _catkin_add_google_test(${TYPE} ${PROJECT_NAME}-${TEST_NAME} ${SOURCES}) + endif() + target_link_libraries(${PROJECT_NAME}-${TEST_NAME} ${PROJECT_NAME} ${PROJECT_NAME}_stages gtest_utils gtest_main) endmacro() macro(mtc_add_gtest) - mtc_add_test("gtest" ${ARGN}) + mtc_add_test("gtest" ${ARGN}) endmacro() macro(mtc_add_gmock) - mtc_add_test("gmock" ${ARGN}) + mtc_add_test("gmock" ${ARGN}) endmacro() mtc_add_gtest(test_stage.cpp) @@ -33,6 +45,8 @@ if (CATKIN_ENABLE_TESTING) mtc_add_gmock(test_cost_queue.cpp) mtc_add_gmock(test_interface_state.cpp) + mtc_add_gtest(test_move_to.cpp move_to.test) + # building these integration tests works without moveit config packages add_executable(pick_ur5 pick_ur5.cpp) target_link_libraries(pick_ur5 ${PROJECT_NAME}_stages gtest) diff --git a/core/test/models.cpp b/core/test/models.cpp index 9d514b1c..377df953 100644 --- a/core/test/models.cpp +++ b/core/test/models.cpp @@ -18,6 +18,6 @@ RobotModelPtr getModel() { } moveit::core::RobotModelPtr loadModel() { - robot_model_loader::RobotModelLoader loader; + static robot_model_loader::RobotModelLoader loader; return loader.getModel(); } diff --git a/core/test/move_to.test b/core/test/move_to.test new file mode 100644 index 00000000..105e936a --- /dev/null +++ b/core/test/move_to.test @@ -0,0 +1,7 @@ + + + + + + + diff --git a/core/test/test_move_to.cpp b/core/test/test_move_to.cpp new file mode 100644 index 00000000..1fbc512b --- /dev/null +++ b/core/test/test_move_to.cpp @@ -0,0 +1,170 @@ +#include "models.h" + +#include +#include +#include +#include +#include + +#include + +#include + +#include +#include + +#include +#include + +using namespace moveit::task_constructor; +using namespace planning_scene; +using namespace moveit::core; + +constexpr double TAU{ 2 * M_PI }; + +// provide a basic test fixture that prepares a Task +struct PandaMoveTo : public testing::Test +{ + Task t; + stages::MoveTo* move_to; + PlanningScenePtr scene; + + PandaMoveTo() { + t.setRobotModel(loadModel()); + + scene = std::make_shared(t.getRobotModel()); + scene->getCurrentStateNonConst().setToDefaultValues(); + scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), + "extended"); + t.add(std::make_unique("start", scene)); + + auto move = std::make_unique("move", std::make_shared()); + move_to = move.get(); + move_to->setGroup("panda_arm"); + t.add(std::move(move)); + } +}; + +#define EXPECT_ONE_SOLUTION \ + { \ + EXPECT_TRUE(t.plan()); \ + EXPECT_EQ(t.solutions().size(), 1u); \ + } + +TEST_F(PandaMoveTo, namedTarget) { + move_to->setGoal("ready"); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, mapTarget) { + move_to->setGoal(std::map{ { "panda_joint1", TAU / 8 }, { "panda_joint2", TAU / 8 } }); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, stateTarget) { + move_to->setGoal([] { + moveit_msgs::RobotState state; + state.is_diff = true; + state.joint_state.name = { "panda_joint1", "panda_joint2" }; + state.joint_state.position = { TAU / 8, TAU / 8 }; + return state; + }()); + EXPECT_ONE_SOLUTION; +} + +geometry_msgs::PoseStamped getFramePoseOfNamedState(RobotState state, std::string pose, std::string frame) { + state.setToDefaultValues(state.getRobotModel()->getJointModelGroup("panda_arm"), pose); + auto frame_eigen{ state.getFrameTransform(frame) }; + geometry_msgs::PoseStamped p; + p.header.frame_id = state.getRobotModel()->getModelFrame(); + p.pose = tf2::toMsg(frame_eigen); + return p; +} + +TEST_F(PandaMoveTo, pointTarget) { + geometry_msgs::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") }; + + geometry_msgs::PointStamped point; + point.header = pose.header; + point.point = pose.pose.position; + move_to->setGoal(point); + + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, poseTarget) { + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8")); + EXPECT_ONE_SOLUTION; +} + +TEST_F(PandaMoveTo, poseIKFrameLinkTarget) { + const std::string IK_FRAME{ "panda_hand" }; + move_to->setIKFrame(IK_FRAME); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + EXPECT_ONE_SOLUTION; +} + +moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) { + moveit_msgs::AttachedCollisionObject aco; + aco.link_name = "panda_hand"; + aco.object.header.frame_id = aco.link_name; + aco.object.operation = aco.object.ADD; + aco.object.id = id; + aco.object.primitives.resize(1, [] { + shape_msgs::SolidPrimitive p; + p.type = p.SPHERE; + p.dimensions.resize(1); + p.dimensions[p.SPHERE_RADIUS] = 0.01; + return p; + }()); +#if MOVEIT_HAS_OBJECT_POSE + aco.object.pose.position.z = 0.2; + aco.object.pose.orientation.w = 1.0; +#else + aco.object.primitive_poses.resize(1); + aco.object.primitive_poses[0].position.z = 0.2; + aco.object.primitive_poses[0].orientation.w = 1.0; +#endif +#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK + // If we don't have this, we also don't have subframe support + aco.object.subframe_names.resize(1, "subframe"); + aco.object.subframe_poses.resize(1, [] { + geometry_msgs::Pose p; + p.orientation.w = 1.0; + return p; + }()); +#endif + return aco; +} + +TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) { + const std::string ATTACHED_OBJECT{ "attached_object" }; + scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + + move_to->setIKFrame(ATTACHED_OBJECT); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", ATTACHED_OBJECT)); + EXPECT_ONE_SOLUTION; +} + +#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK +// If we don't have this, we also don't have subframe support +TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) { + const std::string ATTACHED_OBJECT{ "attached_object" }; + const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; + + scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); + + move_to->setIKFrame(IK_FRAME); + move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); + EXPECT_ONE_SOLUTION; +} +#endif + +int main(int argc, char** argv) { + testing::InitGoogleTest(&argc, argv); + ros::init(argc, argv, "move_to_test"); + ros::AsyncSpinner spinner(1); + spinner.start(); + + return RUN_ALL_TESTS(); +} diff --git a/visualization/motion_planning_tasks/src/pluginlib_factory.h b/visualization/motion_planning_tasks/src/pluginlib_factory.h index a5992724..77b91206 100644 --- a/visualization/motion_planning_tasks/src/pluginlib_factory.h +++ b/visualization/motion_planning_tasks/src/pluginlib_factory.h @@ -150,7 +150,7 @@ public: } template void addBuiltInClass(const QString& name, const QString& description) { - addBuiltInClass("Built Ins", name, description, []() { return new Derived(); }); + addBuiltInClass("Built Ins", name, description, [] { return new Derived(); }); } /** @brief Instantiate and return a instance of a subclass of Type using our