mirror of
https://github.com/moveit/moveit_task_constructor.git
synced 2025-11-04 14:49:57 +08:00
MoveTo supports attached objects&subframes for ik frame
This commit is contained in:
parent
aee76fee5e
commit
e1216aa8ab
@ -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
|
||||
|
||||
@ -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:
|
||||
|
||||
@ -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
|
||||
|
||||
@ -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);
|
||||
|
||||
Loading…
Reference in New Issue
Block a user