mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
Merge pull request #304 from v4hn/pr-move-to-tests
Add MoveTo tests & make them pass
This commit is contained in:
commit
79869b856c
@ -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)
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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*/) {
|
||||
|
||||
@ -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));
|
||||
}
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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
113
core/src/utils.cpp
Normal 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
|
||||
@ -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)
|
||||
|
||||
@ -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
7
core/test/move_to.test
Normal 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
170
core/test/test_move_to.cpp
Normal 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();
|
||||
}
|
||||
@ -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
|
||||
|
||||
Loading…
Reference in New Issue
Block a user