Skip to content

Commit 4c28a74

Browse files
rr-tom-noblerr-tom-noble
authored
Enhancement/ports moveit 3522 (#3070)
* Makes PTP changes * Makes LIN changes * Makes CIRC changes * Makes CIRC changes * Makes CIRC changes * Makes CIRC changes * Makes LIN changes * Makes LIN changes * Fixes formatting * Replace ROS_WARN with RCLCPP_WARN * Fixes formatting * Fixes RCLCPP_WARN calls * Fixes compiler errors * Fixes if statement * Fixes formatting * Fixes msg type
1 parent 5049e4d commit 4c28a74

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
@@ -97,7 +97,6 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
9797
RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request.");
9898

9999
info.group_name = req.group_name;
100-
std::string frame_id{ robot_model_->getModelFrame() };
101100
moveit::core::RobotState robot_state = scene->getCurrentState();
102101

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

148164
computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
149165

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

‎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
@@ -74,7 +74,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
7474
RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request.");
7575

7676
info.group_name = req.group_name;
77-
std::string frame_id{ robot_model_->getModelFrame() };
7877
moveit::core::RobotState robot_state = scene->getCurrentState();
7978

8079
// goal given in joint space
@@ -103,6 +102,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
103102
// goal given in Cartesian space
104103
else
105104
{
105+
std::string frame_id;
106+
106107
info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
107108
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
108109
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
@@ -115,20 +116,25 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
115116
{
116117
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
117118
}
118-
info.goal_pose = getConstraintPose(req.goal_constraints.front());
119-
}
120119

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

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

134140
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
@@ -233,11 +233,32 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin
233233
// solve the ik
234234
else
235235
{
236-
Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front());
237-
if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
238-
goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
236+
std::string frame_id;
237+
238+
info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
239+
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
240+
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
241+
{
242+
RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of "
243+
"goal. Use model frame as default");
244+
frame_id = robot_model_->getModelFrame();
245+
}
246+
else
247+
{
248+
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
249+
}
250+
251+
// goal pose with optional offset wrt. the planning frame
252+
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
253+
frame_id = robot_model_->getModelFrame();
254+
255+
// check goal pose ik before Cartesian motion plan start
256+
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
257+
info.goal_joint_position))
239258
{
240-
throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose");
259+
std::ostringstream os;
260+
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
261+
throw PtpNoIkSolutionForGoalPose(os.str());
241262
}
242263
}
243264
}

0 commit comments

Comments
 (0)