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
#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 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:

View File

@ -42,6 +42,8 @@
#include <moveit/task_constructor/stages/move_to.h>
#include <moveit/task_constructor/cost_terms.h>
#include <moveit/task_constructor/moveit_compat.h>
#include <rviz_marker_tools/marker_creation.h>
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,
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 +202,81 @@ 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");
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
// determine IK link from group
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
if (!get_tip(ik_pose_msg.header.frame_id)) {
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);
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;
}
}
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());
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");
const moveit::core::LinkModel* parent {
#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
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->getFrameTransform(parent->getName());
// 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

View File

@ -126,6 +126,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
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;
@ -136,7 +137,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
return aco;
}
TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) {
TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) {
const std::string ATTACHED_OBJECT{ "attached_object" };
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
@ -145,7 +146,9 @@ TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) {
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 IK_FRAME{ ATTACHED_OBJECT + "/subframe" };
@ -155,6 +158,7 @@ TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) {
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
EXPECT_ONE_SOLUTION;
}
#endif
int main(int argc, char** argv) {
testing::InitGoogleTest(&argc, argv);