mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
add move MoveTo tests
(partially disabled because broken)
This commit is contained in:
parent
86fe752d43
commit
aee76fee5e
@ -4,6 +4,7 @@
|
|||||||
#include <moveit/task_constructor/stages/move_to.h>
|
#include <moveit/task_constructor/stages/move_to.h>
|
||||||
#include <moveit/task_constructor/stages/fixed_state.h>
|
#include <moveit/task_constructor/stages/fixed_state.h>
|
||||||
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
#include <moveit/task_constructor/solvers/joint_interpolation.h>
|
||||||
|
#include <moveit/task_constructor/moveit_compat.h>
|
||||||
|
|
||||||
#include <moveit/planning_scene/planning_scene.h>
|
#include <moveit/planning_scene/planning_scene.h>
|
||||||
|
|
||||||
@ -33,7 +34,8 @@ struct PandaMoveTo : public testing::Test
|
|||||||
|
|
||||||
scene = std::make_shared<PlanningScene>(t.getRobotModel());
|
scene = std::make_shared<PlanningScene>(t.getRobotModel());
|
||||||
scene->getCurrentStateNonConst().setToDefaultValues();
|
scene->getCurrentStateNonConst().setToDefaultValues();
|
||||||
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "ready");
|
scene->getCurrentStateNonConst().setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"),
|
||||||
|
"extended");
|
||||||
t.add(std::make_unique<stages::FixedState>("start", scene));
|
t.add(std::make_unique<stages::FixedState>("start", scene));
|
||||||
|
|
||||||
auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
|
auto move = std::make_unique<stages::MoveTo>("move", std::make_shared<solvers::JointInterpolationPlanner>());
|
||||||
@ -50,7 +52,7 @@ struct PandaMoveTo : public testing::Test
|
|||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PandaMoveTo, namedTarget) {
|
TEST_F(PandaMoveTo, namedTarget) {
|
||||||
move_to->setGoal("extended");
|
move_to->setGoal("ready");
|
||||||
EXPECT_ONE_SOLUTION;
|
EXPECT_ONE_SOLUTION;
|
||||||
}
|
}
|
||||||
|
|
||||||
@ -70,29 +72,87 @@ TEST_F(PandaMoveTo, stateTarget) {
|
|||||||
EXPECT_ONE_SOLUTION;
|
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) {
|
TEST_F(PandaMoveTo, pointTarget) {
|
||||||
move_to->setGoal([this]() {
|
geometry_msgs::PoseStamped pose{ getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8") };
|
||||||
RobotState state{ scene->getCurrentState() };
|
|
||||||
state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended");
|
|
||||||
auto frame{ state.getFrameTransform("panda_link8") };
|
|
||||||
geometry_msgs::PointStamped point;
|
geometry_msgs::PointStamped point;
|
||||||
point.header.frame_id = scene->getPlanningFrame();
|
point.header = pose.header;
|
||||||
point.point = tf2::toMsg(Eigen::Vector3d{ frame.translation() });
|
point.point = pose.pose.position;
|
||||||
return point;
|
move_to->setGoal(point);
|
||||||
}());
|
|
||||||
EXPECT_ONE_SOLUTION;
|
EXPECT_ONE_SOLUTION;
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST_F(PandaMoveTo, poseTarget) {
|
TEST_F(PandaMoveTo, poseTarget) {
|
||||||
move_to->setGoal([this]() {
|
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", "panda_link8"));
|
||||||
RobotState state{ scene->getCurrentState() };
|
EXPECT_ONE_SOLUTION;
|
||||||
state.setToDefaultValues(t.getRobotModel()->getJointModelGroup("panda_arm"), "extended");
|
}
|
||||||
auto frame{ state.getFrameTransform("panda_link8") };
|
|
||||||
geometry_msgs::PoseStamped pose;
|
TEST_F(PandaMoveTo, poseIKFrameLinkTarget) {
|
||||||
pose.header.frame_id = scene->getPlanningFrame();
|
const std::string IK_FRAME{ "panda_hand" };
|
||||||
pose.pose = tf2::toMsg(frame);
|
move_to->setIKFrame(IK_FRAME);
|
||||||
return pose;
|
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
|
||||||
|
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, DISABLED_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;
|
||||||
|
}
|
||||||
|
|
||||||
|
TEST_F(PandaMoveTo, DISABLED_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;
|
EXPECT_ONE_SOLUTION;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user