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
|
// 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
|
||||||
|
|||||||
@ -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:
|
||||||
|
|||||||
@ -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
|
||||||
|
|||||||
@ -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);
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user