Merge pull request #304 from v4hn/pr-move-to-tests

Add MoveTo tests & make them pass
This commit is contained in:
Michael Görner 2021-11-12 23:14:57 +01:00 committed by GitHub
commit 79869b856c
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
17 changed files with 438 additions and 108 deletions

View File

@ -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)

View File

@ -84,7 +84,7 @@ enum InterfaceFlag
GENERATE = WRITES_PREV_END | WRITES_NEXT_START,
};
using InterfaceFlags = Flags<InterfaceFlag>;
using InterfaceFlags = utils::Flags<InterfaceFlag>;
/** invert interface such that
* - new end can connect to old start

View File

@ -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;

View File

@ -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:

View File

@ -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 <string>
#include <type_traits>
#include <initializer_list>
#include <Eigen/Geometry>
#include <moveit/macros/class_forward.h>
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 <typename Enum>
class Flags
@ -118,3 +141,13 @@ private:
};
#define DECLARE_FLAGS(Flags, Enum) using Flags = QFlags<Enum>;
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

View File

@ -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

View File

@ -40,6 +40,8 @@
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/trajectory_processing/iterative_time_parameterization.h>
#include <chrono>
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<double>(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

View File

@ -287,31 +287,17 @@ void ComputeIK::compute() {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(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);
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;
}
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;
}
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<visualization_msgs::Marker> 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*/) {

View File

@ -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<cost::Constant>(0.0));
}

View File

@ -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");
Eigen::Isometry3d ik_pose_world;
if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world))
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<geometry_msgs::PoseStamped>(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;
}
}
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);

View File

@ -42,6 +42,9 @@
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/utils.h>
#include <rviz_marker_tools/marker_creation.h>
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<geometry_msgs::PoseStamped>(goal);
tf2::fromMsg(target.pose, target_eigen);
const geometry_msgs::PoseStamped& msg = boost::any_cast<geometry_msgs::PoseStamped>(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<geometry_msgs::PointStamped>(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<geometry_msgs::PoseStamped>(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

113
core/src/utils.cpp Normal file
View File

@ -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 <tf2_eigen/tf2_eigen.h>
#include <moveit/robot_state/robot_state.h>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/task_constructor/properties.h>
#include <moveit/task_constructor/storage.h>
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<const moveit::core::LinkModel*> 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<geometry_msgs::PoseStamped>(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

View File

@ -9,11 +9,23 @@ 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})
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})
_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)
# 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})
@ -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)

View File

@ -18,6 +18,6 @@ RobotModelPtr getModel() {
}
moveit::core::RobotModelPtr loadModel() {
robot_model_loader::RobotModelLoader loader;
static robot_model_loader::RobotModelLoader loader;
return loader.getModel();
}

7
core/test/move_to.test Normal file
View File

@ -0,0 +1,7 @@
<launch>
<include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
<arg name="load_robot_description" value="true"/>
</include>
<test pkg="moveit_task_constructor_core" type="moveit_task_constructor_core-test-move-to" test-name="move_to"/>
</launch>

170
core/test/test_move_to.cpp Normal file
View File

@ -0,0 +1,170 @@
#include "models.h"
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/stages/fixed_state.h>
#include <moveit/task_constructor/solvers/joint_interpolation.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <moveit/planning_scene/planning_scene.h>
#include <tf2_eigen/tf2_eigen.h>
#include <moveit_msgs/RobotState.h>
#include <geometry_msgs/PoseStamped.h>
#include <ros/console.h>
#include <gtest/gtest.h>
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<PlanningScene>(t.getRobotModel());
scene->getCurrentStateNonConst().setToDefaultValues();
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"),
"extended");
t.add(std::make_unique<stages::FixedState>("start", scene));
auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
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<std::string, double>{ { "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();
}

View File

@ -150,7 +150,7 @@ public:
}
template <class Derived>
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