Skip to content

Commit e1216aa

Browse files
v4hnv4hn
committed
MoveTo supports attached objects&subframes for ik frame
1 parent aee76fe commit e1216aa

File tree

4 files changed

+84
-31
lines changed

4 files changed

+84
-31
lines changed

‎core/include/moveit/task_constructor/moveit_compat.h‎

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -55,3 +55,20 @@
5555

5656
// use object shape poses relative to a single object pose
5757
#define MOVEIT_HAS_OBJECT_POSE MOVEIT_VERSION_GE(1, 1, 6)
58+
59+
#define MOVEIT_HAS_STATE_RIGID_PARENT_LINK MOVEIT_VERSION_GE(1, 1, 6)
60+
61+
#if !MOVEIT_HAS_STATE_RIGID_PARENT_LINK
62+
#include <moveit/robot_state/robot_state.h>
63+
inline const moveit::core::LinkModel* getRigidlyConnectedParentLinkModel(const moveit::core::RobotState& state,
64+
std::string frame) {
65+
const moveit::core::LinkModel* link{ nullptr };
66+
67+
if (state.hasAttachedBody(frame)) {
68+
link = state.getAttachedBody(frame)->getAttachedLink();
69+
} else if (state.getRobotModel()->hasLinkModel(frame))
70+
link = state.getLinkModel(frame);
71+
72+
return state.getRobotModel()->getRigidlyConnectedParentLinkModel(link);
73+
}
74+
#endif

‎core/include/moveit/task_constructor/stages/move_to.h‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -99,7 +99,7 @@ class MoveTo : public PropagatingEitherWay
9999
bool getJointStateGoal(const boost::any& goal, const core::JointModelGroup* jmg, moveit::core::RobotState& state);
100100
bool getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
101101
Eigen::Isometry3d& target_eigen);
102-
bool getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
102+
bool getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose,
103103
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen);
104104

105105
protected:

‎core/src/stages/move_to.cpp‎

Lines changed: 60 additions & 28 deletions
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,8 @@
4242

4343
#include <moveit/task_constructor/stages/move_to.h>
4444
#include <moveit/task_constructor/cost_terms.h>
45+
#include <moveit/task_constructor/moveit_compat.h>
46+
4547
#include <rviz_marker_tools/marker_creation.h>
4648

4749
namespace moveit {
@@ -142,33 +144,29 @@ bool MoveTo::getJointStateGoal(const boost::any& goal, const moveit::core::Joint
142144
}
143145

144146
bool MoveTo::getPoseGoal(const boost::any& goal, const planning_scene::PlanningScenePtr& scene,
145-
Eigen::Isometry3d& target_eigen) {
147+
Eigen::Isometry3d& target) {
146148
try {
147-
const geometry_msgs::PoseStamped& target = boost::any_cast<geometry_msgs::PoseStamped>(goal);
148-
tf2::fromMsg(target.pose, target_eigen);
149-
149+
const geometry_msgs::PoseStamped& msg = boost::any_cast<geometry_msgs::PoseStamped>(goal);
150+
tf2::fromMsg(msg.pose, target);
150151
// transform target into global frame
151-
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
152-
target_eigen = frame * target_eigen;
152+
target = scene->getFrameTransform(msg.header.frame_id) * target;
153153
} catch (const boost::bad_any_cast&) {
154154
return false;
155155
}
156156
return true;
157157
}
158158

159-
bool MoveTo::getPointGoal(const boost::any& goal, const moveit::core::LinkModel* link,
159+
bool MoveTo::getPointGoal(const boost::any& goal, const Eigen::Isometry3d& ik_pose,
160160
const planning_scene::PlanningScenePtr& scene, Eigen::Isometry3d& target_eigen) {
161161
try {
162162
const geometry_msgs::PointStamped& target = boost::any_cast<geometry_msgs::PointStamped>(goal);
163163
Eigen::Vector3d target_point;
164164
tf2::fromMsg(target.point, target_point);
165-
166165
// transform target into global frame
167-
const Eigen::Isometry3d& frame = scene->getFrameTransform(target.header.frame_id);
168-
target_point = frame * target_point;
166+
target_point = scene->getFrameTransform(target.header.frame_id) * target_point;
169167

170168
// retain link orientation
171-
target_eigen = scene->getCurrentState().getGlobalLinkTransform(link);
169+
target_eigen = ik_pose;
172170
target_eigen.translation() = target_point;
173171
} catch (const boost::bad_any_cast&) {
174172
return false;
@@ -204,47 +202,81 @@ bool MoveTo::compute(const InterfaceState& state, planning_scene::PlanningSceneP
204202
// plan to joint-space target
205203
success = planner_->plan(state.scene(), scene, jmg, timeout, robot_trajectory, path_constraints);
206204
} else { // Cartesian goal
207-
const moveit::core::LinkModel* link;
208-
Eigen::Isometry3d target_eigen;
209-
210-
// Cartesian targets require an IK reference frame
205+
// Where to go?
206+
Eigen::Isometry3d target;
207+
// What frame+offset in the robot should go there?
211208
geometry_msgs::PoseStamped ik_pose_msg;
209+
212210
const boost::any& value = props.get("ik_frame");
211+
212+
auto get_tip{ [&jmg](std::string& out) {
213+
// determine IK frame from group
214+
std::vector<std::string> tips;
215+
jmg->getEndEffectorTips(tips);
216+
if (tips.size() != 1) {
217+
return false;
218+
}
219+
out = tips[0];
220+
return true;
221+
} };
222+
213223
if (value.empty()) { // property undefined
214-
// determine IK link from group
215-
if (!(link = jmg->getOnlyOneEndEffectorTip())) {
224+
if (!get_tip(ik_pose_msg.header.frame_id)) {
216225
solution.markAsFailure("missing ik_frame");
217226
return false;
218227
}
219-
ik_pose_msg.header.frame_id = link->getName();
220228
ik_pose_msg.pose.orientation.w = 1.0;
221229
} else {
222230
ik_pose_msg = boost::any_cast<geometry_msgs::PoseStamped>(value);
223-
if (!(link = robot_model->getLinkModel(ik_pose_msg.header.frame_id))) {
224-
solution.markAsFailure("unknown link for ik_frame: " + ik_pose_msg.header.frame_id);
231+
if (ik_pose_msg.header.frame_id.empty() && !get_tip(ik_pose_msg.header.frame_id)) {
232+
solution.markAsFailure("frame_id of ik_frame is empty and no unique group tip was found");
233+
return false;
234+
} else if (!scene->knowsFrameTransform(ik_pose_msg.header.frame_id)) {
235+
std::stringstream ss;
236+
ss << "ik_frame specified in unknown frame '" << ik_pose_msg << "'";
237+
solution.markAsFailure(ss.str());
225238
return false;
226239
}
227240
}
228241

229-
if (!getPoseGoal(goal, scene, target_eigen) && !getPointGoal(goal, link, scene, target_eigen)) {
242+
Eigen::Isometry3d ik_pose_world{ [&]() {
243+
Eigen::Isometry3d t;
244+
tf2::fromMsg(ik_pose_msg.pose, t);
245+
t = scene->getFrameTransform(ik_pose_msg.header.frame_id) * t;
246+
return t;
247+
}() };
248+
249+
if (!getPoseGoal(goal, scene, target) && !getPointGoal(goal, ik_pose_world, scene, target)) {
230250
solution.markAsFailure(std::string("invalid goal type: ") + goal.type().name());
231251
return false;
232252
}
233253

254+
auto add_frame{ [&](const Eigen::Isometry3d& pose, const char name[]) {
255+
geometry_msgs::PoseStamped msg;
256+
msg.header.frame_id = scene->getPlanningFrame();
257+
msg.pose = tf2::toMsg(pose);
258+
rviz_marker_tools::appendFrame(solution.markers(), msg, 0.1, name);
259+
} };
260+
234261
// visualize plan with frame at target pose and frame at link
235-
geometry_msgs::PoseStamped target;
236-
target.header.frame_id = scene->getPlanningFrame();
237-
target.pose = tf2::toMsg(target_eigen);
238-
rviz_marker_tools::appendFrame(solution.markers(), target, 0.1, "ik frame");
239-
rviz_marker_tools::appendFrame(solution.markers(), ik_pose_msg, 0.1, "ik frame");
262+
add_frame(target, "target frame");
263+
add_frame(ik_pose_world, "ik frame");
264+
265+
const moveit::core::LinkModel* parent {
266+
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
267+
scene->getCurrentState().getRigidlyConnectedParentLinkModel(ik_pose_msg.header.frame_id)
268+
#else
269+
getRigidlyConnectedParentLinkModel(scene->getCurrentState(), ik_pose_msg.header.frame_id)
270+
#endif
271+
};
240272

241273
// transform target pose such that ik frame will reach there if link does
242274
Eigen::Isometry3d ik_pose;
243275
tf2::fromMsg(ik_pose_msg.pose, ik_pose);
244-
target_eigen = target_eigen * ik_pose.inverse();
276+
target = target * ik_pose_world.inverse() * scene->getFrameTransform(parent->getName());
245277

246278
// plan to Cartesian target
247-
success = planner_->plan(state.scene(), *link, target_eigen, jmg, timeout, robot_trajectory, path_constraints);
279+
success = planner_->plan(state.scene(), *parent, target, jmg, timeout, robot_trajectory, path_constraints);
248280
}
249281

250282
// store result

‎core/test/test_move_to.cpp‎

Lines changed: 6 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -126,6 +126,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
126126
aco.object.primitive_poses[0].orientation.w = 1.0;
127127
#endif
128128
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
129+
// If we don't have this, we also don't have subframe support
129130
aco.object.subframe_names.resize(1, "subframe");
130131
aco.object.subframe_poses.resize(1, []() {
131132
geometry_msgs::Pose p;
@@ -136,7 +137,7 @@ moveit_msgs::AttachedCollisionObject createAttachedObject(const std::string& id)
136137
return aco;
137138
}
138139

139-
TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) {
140+
TEST_F(PandaMoveTo, poseIKFrameAttachedTarget) {
140141
const std::string ATTACHED_OBJECT{ "attached_object" };
141142
scene->processAttachedCollisionObjectMsg(createAttachedObject(ATTACHED_OBJECT));
142143

@@ -145,7 +146,9 @@ TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedTarget) {
145146
EXPECT_ONE_SOLUTION;
146147
}
147148

148-
TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) {
149+
#if MOVEIT_HAS_STATE_RIGID_PARENT_LINK
150+
// If we don't have this, we also don't have subframe support
151+
TEST_F(PandaMoveTo, poseIKFrameAttachedSubframeTarget) {
149152
const std::string ATTACHED_OBJECT{ "attached_object" };
150153
const std::string IK_FRAME{ ATTACHED_OBJECT + "/subframe" };
151154

@@ -155,6 +158,7 @@ TEST_F(PandaMoveTo, DISABLED_poseIKFrameAttachedSubframeTarget) {
155158
move_to->setGoal(getFramePoseOfNamedState(scene->getCurrentState(), "ready", IK_FRAME));
156159
EXPECT_ONE_SOLUTION;
157160
}
161+
#endif
158162

159163
int main(int argc, char** argv) {
160164
testing::InitGoogleTest(&argc, argv);

0 commit comments

Comments
 (0)