Skip to content

Commit 7132ddd

Browse files
mergify[bot]mergify[bot]
authored
Enhancement/ports moveit 3522 (backport #3070) (#3074)
1 parent 0179460 commit 7132ddd

File tree

3 files changed

+85
-38
lines changed

3 files changed

+85
-38
lines changed

‎moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp‎

Lines changed: 42 additions & 22 deletions
Original file line numberDiff line numberDiff line change
@@ -95,7 +95,6 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
9595
RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request.");
9696

9797
info.group_name = req.group_name;
98-
std::string frame_id{ robot_model_->getModelFrame() };
9998
moveit::core::RobotState robot_state = scene->getCurrentState();
10099

101100
// goal given in joint space
@@ -128,6 +127,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
128127
// goal given in Cartesian space
129128
else
130129
{
130+
std::string frame_id;
131131
info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
132132
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
133133
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
@@ -140,39 +140,59 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
140140
{
141141
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
142142
}
143-
info.goal_pose = getConstraintPose(req.goal_constraints.front());
143+
144+
// goal pose with optional offset wrt. the planning frame
145+
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
146+
frame_id = robot_model_->getModelFrame();
147+
148+
// check goal pose ik before Cartesian motion plan starts
149+
std::map<std::string, double> ik_solution;
150+
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
151+
ik_solution))
152+
{
153+
// LCOV_EXCL_START
154+
std::ostringstream os;
155+
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
156+
throw CircInverseForGoalIncalculable(os.str());
157+
// LCOV_EXCL_STOP // not able to trigger here since lots of checks before
158+
// are in place
159+
}
144160
}
145161

146162
computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
147163

148-
// check goal pose ik before Cartesian motion plan starts
149-
std::map<std::string, double> ik_solution;
150-
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
151-
ik_solution))
164+
// center point with wrt. the planning frame
165+
std::string center_point_frame_id;
166+
167+
info.circ_path_point.first = req.path_constraints.name;
168+
if (req.path_constraints.position_constraints.front().header.frame_id.empty())
152169
{
153-
// LCOV_EXCL_START
154-
std::ostringstream os;
155-
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
156-
throw CircInverseForGoalIncalculable(os.str());
157-
// LCOV_EXCL_STOP // not able to trigger here since lots of checks before
158-
// are in place
170+
RCLCPP_WARN(LOGGER, "Frame id is not set in position constraints of "
171+
"path. Use model frame as default");
172+
center_point_frame_id = robot_model_->getModelFrame();
159173
}
160-
info.circ_path_point.first = req.path_constraints.name;
174+
else
175+
{
176+
center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id;
177+
}
178+
179+
Eigen::Isometry3d center_point_pose;
180+
tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(),
181+
center_point_pose);
182+
183+
center_point_pose = scene->getFrameTransform(center_point_frame_id) * center_point_pose;
184+
161185
if (!req.goal_constraints.front().position_constraints.empty())
162186
{
163187
const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front();
164-
info.circ_path_point.second =
165-
getConstraintPose(
166-
req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
167-
goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset)
168-
.translation();
188+
geometry_msgs::msg::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.translation()));
189+
info.circ_path_point.second = getConstraintPose(center_point, goal.orientation_constraints.front().orientation,
190+
goal.position_constraints.front().target_point_offset)
191+
.translation();
169192
}
170193
else
171194
{
172-
Eigen::Vector3d circ_path_point;
173-
tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
174-
circ_path_point);
175-
info.circ_path_point.second = circ_path_point;
195+
info.circ_path_point.second = center_point_pose.translation();
176196
}
177197
}
178198

‎moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp‎

Lines changed: 18 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -72,7 +72,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
7272
RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request.");
7373

7474
info.group_name = req.group_name;
75-
std::string frame_id{ robot_model_->getModelFrame() };
7675
moveit::core::RobotState robot_state = scene->getCurrentState();
7776

7877
// goal given in joint space
@@ -101,6 +100,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
101100
// goal given in Cartesian space
102101
else
103102
{
103+
std::string frame_id;
104+
104105
info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
105106
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
106107
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
@@ -113,20 +114,25 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
113114
{
114115
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
115116
}
116-
info.goal_pose = getConstraintPose(req.goal_constraints.front());
117-
}
118117

119-
computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
118+
// goal pose with optional offset wrt. the planning frame
119+
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
120+
frame_id = robot_model_->getModelFrame();
120121

121-
// check goal pose ik before Cartesian motion plan starts
122-
std::map<std::string, double> ik_solution;
123-
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
124-
ik_solution))
125-
{
126-
std::ostringstream os;
127-
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
128-
throw LinInverseForGoalIncalculable(os.str());
122+
// check goal pose ik before Cartesian motion plan starts
123+
std::map<std::string, double> ik_solution;
124+
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
125+
ik_solution))
126+
{
127+
std::ostringstream os;
128+
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
129+
throw LinInverseForGoalIncalculable(os.str());
130+
}
129131
}
132+
133+
// Ignored return value because at this point the function should always
134+
// return 'true'.
135+
computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
130136
}
131137

132138
void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene,

‎moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp‎

Lines changed: 25 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -227,11 +227,32 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin
227227
// solve the ik
228228
else
229229
{
230-
Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front());
231-
if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
232-
goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
230+
std::string frame_id;
231+
232+
info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
233+
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
234+
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
235+
{
236+
RCLCPP_WARN(LOGGER, "Frame id is not set in position/orientation constraints of "
237+
"goal. Use model frame as default");
238+
frame_id = robot_model_->getModelFrame();
239+
}
240+
else
241+
{
242+
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
243+
}
244+
245+
// goal pose with optional offset wrt. the planning frame
246+
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
247+
frame_id = robot_model_->getModelFrame();
248+
249+
// check goal pose ik before Cartesian motion plan start
250+
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
251+
info.goal_joint_position))
233252
{
234-
throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose");
253+
std::ostringstream os;
254+
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
255+
throw PtpNoIkSolutionForGoalPose(os.str());
235256
}
236257
}
237258
}

0 commit comments

Comments
 (0)