mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-09-27 00:29:13 +08:00
Add unittest cartesianCollisionMinMaxDistance (#538)
... to illustrate that MoveRelative's min-max constraint fails with PipelinePlanners (e.g. Pilz) returning a partially invalid trajectory: MTC does not truncate the trajectory to its valid part and thus fails, even if the valid part fits the given min-max range. This logic is only supported for the CartesianPath planner for now. Co-authored-by: Robert Haschke <rhaschke@techfak.uni-bielefeld.de>
This commit is contained in:
parent
92efc14043
commit
3b4ea48c18
@ -1,5 +1,5 @@
|
||||
<launch>
|
||||
<include file="$(find moveit_resources_panda_moveit_config)/launch/planning_context.launch">
|
||||
<include file="$(find moveit_resources_panda_moveit_config)/launch/move_group.launch">
|
||||
<arg name="load_robot_description" value="true"/>
|
||||
</include>
|
||||
|
||||
|
@ -4,6 +4,7 @@
|
||||
#include <moveit/task_constructor/stages/move_relative.h>
|
||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||
#include <moveit/task_constructor/solvers/cartesian_path.h>
|
||||
#include <moveit/task_constructor/solvers/pipeline_planner.h>
|
||||
|
||||
#include <moveit/planning_scene/planning_scene.h>
|
||||
#include <moveit/robot_trajectory/robot_trajectory.h>
|
||||
@ -13,6 +14,10 @@
|
||||
|
||||
#include <gtest/gtest.h>
|
||||
|
||||
#ifndef TYPED_TEST_SUITE // for Melodic
|
||||
#define TYPED_TEST_SUITE(SUITE, TYPES) TYPED_TEST_CASE(SUITE, TYPES)
|
||||
#endif
|
||||
|
||||
using namespace moveit::task_constructor;
|
||||
using namespace planning_scene;
|
||||
using namespace moveit::core;
|
||||
@ -20,16 +25,33 @@ using namespace moveit::core;
|
||||
constexpr double TAU{ 2 * M_PI };
|
||||
constexpr double EPS{ 5e-5 };
|
||||
|
||||
template <typename Planner>
|
||||
std::shared_ptr<Planner> create();
|
||||
template <>
|
||||
solvers::CartesianPathPtr create<solvers::CartesianPath>() {
|
||||
return std::make_shared<solvers::CartesianPath>();
|
||||
}
|
||||
template <>
|
||||
solvers::PipelinePlannerPtr create<solvers::PipelinePlanner>() {
|
||||
auto p = std::make_shared<solvers::PipelinePlanner>("pilz_industrial_motion_planner");
|
||||
p->setPlannerId("LIN");
|
||||
p->setProperty("max_velocity_scaling_factor", 0.1);
|
||||
p->setProperty("max_acceleration_scaling_factor", 0.1);
|
||||
return p;
|
||||
}
|
||||
|
||||
// provide a basic test fixture that prepares a Task
|
||||
template <typename Planner = solvers::CartesianPath>
|
||||
struct PandaMoveRelative : public testing::Test
|
||||
{
|
||||
Task t;
|
||||
stages::MoveRelative* move;
|
||||
PlanningScenePtr scene;
|
||||
std::shared_ptr<Planner> planner;
|
||||
|
||||
const JointModelGroup* group;
|
||||
|
||||
PandaMoveRelative() {
|
||||
PandaMoveRelative() : planner(create<Planner>()) {
|
||||
t.setRobotModel(loadModel());
|
||||
|
||||
group = t.getRobotModel()->getJointModelGroup("panda_arm");
|
||||
@ -39,20 +61,20 @@ struct PandaMoveRelative : public testing::Test
|
||||
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
|
||||
t.add(std::make_unique<stages::FixedState>("start", scene));
|
||||
|
||||
auto move_relative = std::make_unique<stages::MoveRelative>("move", std::make_shared<solvers::CartesianPath>());
|
||||
auto move_relative = std::make_unique<stages::MoveRelative>("move", planner);
|
||||
move_relative->setGroup(group->getName());
|
||||
move = move_relative.get();
|
||||
t.add(std::move(move_relative));
|
||||
}
|
||||
};
|
||||
using PandaMoveRelativeCartesian = PandaMoveRelative<solvers::CartesianPath>;
|
||||
|
||||
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, [] {
|
||||
moveit_msgs::CollisionObject createObject(const std::string& id, const geometry_msgs::Pose& pose) {
|
||||
moveit_msgs::CollisionObject co;
|
||||
co.header.frame_id = "panda_hand";
|
||||
co.operation = co.ADD;
|
||||
co.id = id;
|
||||
co.primitives.resize(1, [] {
|
||||
shape_msgs::SolidPrimitive p;
|
||||
p.type = p.SPHERE;
|
||||
p.dimensions.resize(1);
|
||||
@ -60,10 +82,23 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
|
||||
return p;
|
||||
}());
|
||||
|
||||
co.pose = pose;
|
||||
return co;
|
||||
}
|
||||
|
||||
moveit_msgs::CollisionObject createObject(const std::string& id) {
|
||||
geometry_msgs::Pose p;
|
||||
p.position.x = 0.1;
|
||||
p.orientation.w = 1.0;
|
||||
aco.object.pose = p;
|
||||
|
||||
return createObject(id, p);
|
||||
}
|
||||
|
||||
moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id) {
|
||||
moveit_msgs::AttachedCollisionObject aco;
|
||||
aco.link_name = "panda_hand";
|
||||
aco.object = createObject(id);
|
||||
|
||||
return aco;
|
||||
}
|
||||
|
||||
@ -79,13 +114,13 @@ void expect_const_position(const SolutionBaseConstPtr& solution, const std::stri
|
||||
}
|
||||
}
|
||||
|
||||
#define EXPECT_CONST_POSITION(...) \
|
||||
{ \
|
||||
SCOPED_TRACE("expect_constant_position(" #__VA_ARGS__ ")"); \
|
||||
expect_const_position(__VA_ARGS__); \
|
||||
#define EXPECT_CONST_POSITION(...) \
|
||||
{ \
|
||||
SCOPED_TRACE("expect_const_position(" #__VA_ARGS__ ")"); \
|
||||
expect_const_position(__VA_ARGS__); \
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveRelative, cartesianRotateEEF) {
|
||||
TEST_F(PandaMoveRelativeCartesian, cartesianRotateEEF) {
|
||||
move->setDirection([] {
|
||||
geometry_msgs::TwistStamped twist;
|
||||
twist.header.frame_id = "world";
|
||||
@ -97,7 +132,7 @@ TEST_F(PandaMoveRelative, cartesianRotateEEF) {
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), group->getOnlyOneEndEffectorTip()->getName());
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveRelative, cartesianCircular) {
|
||||
TEST_F(PandaMoveRelativeCartesian, cartesianCircular) {
|
||||
const std::string tip = "panda_hand";
|
||||
auto offset = Eigen::Translation3d(0, 0, 0.1);
|
||||
move->setIKFrame(offset, tip);
|
||||
@ -112,7 +147,7 @@ TEST_F(PandaMoveRelative, cartesianCircular) {
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), tip, Eigen::Isometry3d(offset));
|
||||
}
|
||||
|
||||
TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
|
||||
TEST_F(PandaMoveRelativeCartesian, cartesianRotateAttachedIKFrame) {
|
||||
const std::string attached_object{ "attached_object" };
|
||||
scene->processAttachedCollisionObjectMsg(createAttachedObject(attached_object));
|
||||
move->setIKFrame(attached_object);
|
||||
@ -128,6 +163,35 @@ TEST_F(PandaMoveRelative, cartesianRotateAttachedIKFrame) {
|
||||
EXPECT_CONST_POSITION(move->solutions().front(), attached_object);
|
||||
}
|
||||
|
||||
using PlannerTypes = ::testing::Types<solvers::CartesianPath, solvers::PipelinePlanner>;
|
||||
TYPED_TEST_SUITE(PandaMoveRelative, PlannerTypes);
|
||||
TYPED_TEST(PandaMoveRelative, cartesianCollisionMinMaxDistance) {
|
||||
if (std::is_same<TypeParam, solvers::PipelinePlanner>::value)
|
||||
GTEST_SKIP(); // Pilz PipelinePlanner current fails this test (see #538)
|
||||
|
||||
const std::string object{ "object" };
|
||||
geometry_msgs::Pose object_pose;
|
||||
object_pose.position.z = 0.4;
|
||||
object_pose.orientation.w = 1.0;
|
||||
|
||||
this->scene->processCollisionObjectMsg(createObject(object, object_pose));
|
||||
|
||||
this->move->setIKFrame("panda_hand");
|
||||
geometry_msgs::Vector3Stamped v;
|
||||
v.header.frame_id = "panda_hand";
|
||||
v.vector.z = 0.5;
|
||||
this->move->setDirection(v);
|
||||
EXPECT_FALSE(this->t.plan()) << "Plan should fail due to collision";
|
||||
|
||||
this->t.reset();
|
||||
v.vector.z = 1.0;
|
||||
this->move->setDirection(v);
|
||||
this->move->setMinMaxDistance(0.1, 0.5);
|
||||
EXPECT_TRUE(this->t.plan()) << "Plan should succeed due to min distance reached";
|
||||
auto trajectory = std::dynamic_pointer_cast<const SubTrajectory>(this->move->solutions().front());
|
||||
EXPECT_TRUE(this->scene->isPathValid(*trajectory->trajectory(), "panda_arm", false));
|
||||
}
|
||||
|
||||
int main(int argc, char** argv) {
|
||||
testing::InitGoogleTest(&argc, argv);
|
||||
ros::init(argc, argv, "move_relative_test");
|
||||
|
Loading…
Reference in New Issue
Block a user