MoveTo supports attached objects&subframes for ik frame

This commit is contained in:
v4hn 2021-10-19 15:52:15 +02:00
parent aee76fee5e
commit e1216aa8ab
4 changed files with 84 additions and 31 deletions

View File

@ -55,3 +55,20 @@
// use object shape poses relative to a single object pose // use object shape poses relative to a single object pose
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6) #define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)
#if !MOVEIT_HAS_STATE_RIGID_PARENT_LINK
#include <moveit/robot_state/robot_state.h>
inline const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
std::string frame) {
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

View File

@ -99,7 +99,7 @@ protected:
bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state); 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, bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d& target_eigen); 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); const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen);
protected: protected:

View File

@ -42,6 +42,8 @@
#include <moveit/task_constructor/stages/move_to.h> #include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h> #include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <rviz_marker_tools/marker_creation.h> #include <rviz_marker_tools/marker_creation.h>
namespace moveit { namespace moveit {
@ -142,33 +144,29 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
} }
bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene, bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
Eigen::Isometry3d& target_eigen) { Eigen::Isometry3d& target) {
try { try {
const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal); const geometry_msgs::PoseStamped& msg = boost::any_cast<geometry_msgs::PoseStamped>(goal);
tf2::fromMsg(target.pose, target_eigen); tf2::fromMsg(msg.pose, target);
// transform target into global frame // transform target into global frame
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id); target = scene->getFrameTransform(msg.header.frame_id) * target;
target_eigen = frame * target_eigen;
} catch (const boost::bad_any_cast&) { } catch (const boost::bad_any_cast&) {
return false; return false;
} }
return true; 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) { const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) {
try { try {
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal); const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
Eigen::Vector3d target_point; Eigen::Vector3d target_point;
tf2::fromMsg(target.point, target_point); tf2::fromMsg(target.point, target_point);
// transform target into global frame // transform target into global frame
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id); target_point = scene->getFrameTransform(target.header.frame_id) * target_point;
target_point = frame * target_point;
// retain link orientation // retain link orientation
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link); target_eigen = ik_pose;
target_eigen.translation() = target_point; target_eigen.translation() = target_point;
} catch (const boost::bad_any_cast&) { } catch (const boost::bad_any_cast&) {
return false; return false;
@ -204,47 +202,81 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
// plan to joint-space target // plan to joint-space target
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 { // Cartesian goal } else { // Cartesian goal
const moveit::core::LinkModel* link; // Where to go?
Eigen::Isometry3d target_eigen; Eigen::Isometry3d target;
// What frame+offset in the robot should go there?
// Cartesian targets require an IK reference frame
geometry_msgs::PoseStamped ik_pose_msg; geometry_msgs::PoseStamped ik_pose_msg;
const boost::any& value = props.get("ik_frame"); const boost::any& value = props.get("ik_frame");
auto get_tip{ [&jmg](std::string& out) {
// determine IK frame from group
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 (value.empty()) { // property undefined
// determine IK link from group if (!get_tip(ik_pose_msg.header.frame_id)) {
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
solution.markAsFailure("missing ik_frame"); 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; ik_pose_msg.pose.orientation.w = 1.0;
} else { } else {
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value); ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) { if (ik_pose_msg.header.frame_id.empty() && !get_tip(ik_pose_msg.header.frame_id)) {
solution.markAsFailure("unknown link for ik_frame: " + 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; return false;
} }
} }
if (!getPoseGoal(goal, scene, target_eigen) && !getPointGoal(goal, link, scene, target_eigen)) { 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)) {
solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name()); solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
return false; 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 // visualize plan with frame at target pose and frame at link
geometry_msgs::PoseStamped target; add_frame(target, "target frame");
target.header.frame_id = scene->getPlanningFrame(); add_frame(ik_pose_world, "ik frame");
target.pose = tf2::toMsg(target_eigen);
rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame"); const moveit::core::LinkModel* parent {
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame"); #if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id)
#else
getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id)
#endif
};
// 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; Eigen::Isometry3d ik_pose;
tf2::fromMsg(ik_pose_msg.pose, ik_pose); tf2::fromMsg(ik_pose_msg.pose, ik_pose);
target_eigen = target_eigen * ik_pose.inverse(); target = target * ik_pose_world.inverse() * scene->getFrameTransform(parent->getName());
// plan to Cartesian target // plan to Cartesian target
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints); success = planner_->plan(state.scene(), *parent, target, jmg, timeout, robot_trajectory, path_constraints);
} }
// store result // store result

View File

@ -126,6 +126,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
aco.object.primitive_poses[0].orientation.w = 1.0; aco.object.primitive_poses[0].orientation.w = 1.0;
#endif #endif
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK #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_names.resize(1, "subframe");
aco.object.subframe_poses.resize(1, []() { aco.object.subframe_poses.resize(1, []() {
geometry_msgs::Pose p; geometry_msgs::Pose p;
@ -136,7 +137,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
return aco; return aco;
} }
TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) { TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) {
const std::string ATTACHED_OBJECT{ "attached_object" }; const std::string ATTACHED_OBJECT{ "attached_object" };
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT)); scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
@ -145,7 +146,9 @@ TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) {
EXPECT_ONE_SOLUTION; EXPECT_ONE_SOLUTION;
} }
TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) { #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 ATTACHED_OBJECT{ "attached_object" };
const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" }; const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" };
@ -155,6 +158,7 @@ TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) {
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME)); move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
EXPECT_ONE_SOLUTION; EXPECT_ONE_SOLUTION;
} }
#endif
int main(int argc, char** argv) { int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv); testing::InitGoogleTest(&argc, argv);