mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
refactor logic to handle ik_frame
fallbacks and verification.
This commit is contained in:
parent
f1fc447e3b
commit
3b835986e3
@ -42,14 +42,25 @@
|
|||||||
#include <type_traits>
|
#include <type_traits>
|
||||||
#include <initializer_list>
|
#include <initializer_list>
|
||||||
|
|
||||||
|
#include <Eigen/Geometry>
|
||||||
|
|
||||||
|
#include <moveit/macros/class_forward.h>
|
||||||
|
|
||||||
|
namespace planning_scene {
|
||||||
|
MOVEIT_CLASS_FORWARD(PlanningScene);
|
||||||
|
}
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
|
|
||||||
namespace core {
|
namespace core {
|
||||||
class LinkModel;
|
MOVEIT_CLASS_FORWARD(LinkModel);
|
||||||
class RobotState;
|
MOVEIT_CLASS_FORWARD(JointModelGroup);
|
||||||
|
MOVEIT_CLASS_FORWARD(RobotState);
|
||||||
} // namespace core
|
} // namespace core
|
||||||
|
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
|
MOVEIT_CLASS_FORWARD(Property);
|
||||||
|
MOVEIT_CLASS_FORWARD(SolutionBase);
|
||||||
|
|
||||||
namespace utils {
|
namespace utils {
|
||||||
|
|
||||||
@ -133,6 +144,10 @@ private:
|
|||||||
|
|
||||||
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
|
||||||
std::string frame);
|
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 utils
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|||||||
@ -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);
|
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
} else {
|
} else {
|
||||||
// Cartesian targets require an IK reference frame
|
// Cartesian targets require an IK reference frame
|
||||||
geometry_msgs::PoseStamped ik_pose_msg;
|
|
||||||
const moveit::core::LinkModel* link;
|
const moveit::core::LinkModel* link;
|
||||||
const boost::any& value = props.get("ik_frame");
|
Eigen::Isometry3d ik_pose_world;
|
||||||
if (value.empty()) { // property undefined
|
|
||||||
// determine IK link from group
|
if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world))
|
||||||
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
|
|
||||||
solution.markAsFailure("missing ik_frame");
|
|
||||||
return false;
|
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?
|
bool use_rotation_distance = false; // measure achieved distance as rotation?
|
||||||
Eigen::Vector3d linear; // linear translation
|
Eigen::Vector3d linear; // linear translation
|
||||||
@ -291,9 +278,7 @@ bool MoveRelative::compute(const InterfaceState& state, planning_scene::Planning
|
|||||||
|
|
||||||
COMPUTE:
|
COMPUTE:
|
||||||
// transform target pose such that ik frame will reach there if link does
|
// transform target pose such that ik frame will reach there if link does
|
||||||
Eigen::Isometry3d ik_pose;
|
target_eigen = target_eigen * scene->getCurrentState().getGlobalLinkTransform(link).inverse() * ik_pose_world;
|
||||||
tf2::fromMsg(ik_pose_msg.pose, ik_pose);
|
|
||||||
target_eigen = target_eigen * ik_pose.inverse();
|
|
||||||
|
|
||||||
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
|
|
||||||
|
|||||||
@ -208,44 +208,10 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
|||||||
// What frame+offset in the robot should go there?
|
// What frame+offset in the robot should go there?
|
||||||
geometry_msgs::PoseStamped ik_pose_msg;
|
geometry_msgs::PoseStamped ik_pose_msg;
|
||||||
|
|
||||||
const boost::any& value = props.get("ik_frame");
|
const moveit::core::LinkModel* link;
|
||||||
|
Eigen::Isometry3d ik_pose_world;
|
||||||
auto get_tip{ [&jmg](std::string& out) {
|
if (!utils::getRobotTipForFrame(props.property("ik_frame"), *scene, jmg, solution, link, ik_pose_world))
|
||||||
// determine IK frame from group
|
|
||||||
std::vector<std::string> tips;
|
|
||||||
jmg->getEndEffectorTips(tips);
|
|
||||||
if (tips.size() != 1) {
|
|
||||||
return false;
|
return false;
|
||||||
}
|
|
||||||
out = tips[0];
|
|
||||||
return true;
|
|
||||||
} };
|
|
||||||
|
|
||||||
if (value.empty()) { // property undefined
|
|
||||||
if (!get_tip(ik_pose_msg.header.frame_id)) {
|
|
||||||
solution.markAsFailure("missing ik_frame");
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
ik_pose_msg.pose.orientation.w = 1.0;
|
|
||||||
} else {
|
|
||||||
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
|
|
||||||
if (ik_pose_msg.header.frame_id.empty() && !get_tip(ik_pose_msg.header.frame_id)) {
|
|
||||||
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
|
|
||||||
return false;
|
|
||||||
} else if (!scene->knowsFrameTransform(ik_pose_msg.header.frame_id)) {
|
|
||||||
std::stringstream ss;
|
|
||||||
ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'";
|
|
||||||
solution.markAsFailure(ss.str());
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
Eigen::Isometry3d ik_pose_world{ [&]() {
|
|
||||||
Eigen::Isometry3d t;
|
|
||||||
tf2::fromMsg(ik_pose_msg.pose, t);
|
|
||||||
t = scene->getFrameTransform(ik_pose_msg.header.frame_id) * t;
|
|
||||||
return t;
|
|
||||||
}() };
|
|
||||||
|
|
||||||
if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) {
|
if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) {
|
||||||
solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
|
solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
|
||||||
@ -263,16 +229,11 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
|
|||||||
add_frame(target, "target frame");
|
add_frame(target, "target frame");
|
||||||
add_frame(ik_pose_world, "ik frame");
|
add_frame(ik_pose_world, "ik frame");
|
||||||
|
|
||||||
const moveit::core::LinkModel* parent{ utils::getRigidlyConnectedParentLinkModel(scene->getCurrentState(),
|
|
||||||
ik_pose_msg.header.frame_id) };
|
|
||||||
|
|
||||||
// transform target pose such that ik frame will reach there if link does
|
// transform target pose such that ik frame will reach there if link does
|
||||||
Eigen::Isometry3d ik_pose;
|
target = target * ik_pose_world.inverse() * scene->getCurrentState().getGlobalLinkTransform(link);
|
||||||
tf2::fromMsg(ik_pose_msg.pose, ik_pose);
|
|
||||||
target = target * ik_pose_world.inverse() * scene->getFrameTransform(parent->getName());
|
|
||||||
|
|
||||||
// plan to Cartesian target
|
// plan to Cartesian target
|
||||||
success = planner_->plan(state.scene(), *parent, target, jmg, timeout, robot_trajectory, path_constraints);
|
success = planner_->plan(state.scene(), *link, target, jmg, timeout, robot_trajectory, path_constraints);
|
||||||
}
|
}
|
||||||
|
|
||||||
// store result
|
// store result
|
||||||
|
|||||||
@ -35,9 +35,14 @@
|
|||||||
|
|
||||||
/* Authors: Michael Goerner, Robert Haschke */
|
/* Authors: Michael Goerner, Robert Haschke */
|
||||||
|
|
||||||
|
#include <tf2_eigen/tf2_eigen.h>
|
||||||
|
|
||||||
#include <moveit/robot_state/robot_state.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/moveit_compat.h>
|
||||||
|
#include <moveit/task_constructor/properties.h>
|
||||||
|
#include <moveit/task_constructor/storage.h>
|
||||||
|
|
||||||
namespace moveit {
|
namespace moveit {
|
||||||
namespace task_constructor {
|
namespace task_constructor {
|
||||||
@ -59,6 +64,50 @@ const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::
|
|||||||
#endif
|
#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 utils
|
||||||
} // namespace task_constructor
|
} // namespace task_constructor
|
||||||
} // namespace moveit
|
} // namespace moveit
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user