refactor logic to handle ik_frame

fallbacks and verification.
This commit is contained in:
v4hn 2021-11-12 14:02:46 +01:00
parent f1fc447e3b
commit 3b835986e3
4 changed files with 77 additions and 67 deletions

View File

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

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); 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())) { return false;
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;
}
}
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);

View File

@ -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 return false;
std::vector<std::string> tips;
jmg->getEndEffectorTips(tips);
if (tips.size() != 1) {
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

View File

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