Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Enhancement/ports moveit 3522 (backport #3070) #3075

Closed
wants to merge 1 commit into from
Closed

Enhancement/ports moveit 3522 (backport #3070) #3075

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
1 commit
Select commit
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
67 changes: 46 additions & 21 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,11 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request.");

info.group_name = req.group_name;
<<<<<<< HEAD
std::string frame_id{ robot_model_->getModelFrame() };
=======
moveit::core::RobotState robot_state = scene->getCurrentState();
>>>>>>> 4c28a7434 (Enhancement/ports moveit 3522 (#3070))

// goal given in joint space
if (!req.goal_constraints.front().joint_constraints.empty())
Expand Down Expand Up @@ -123,6 +127,7 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
// goal given in Cartesian space
else
{
std::string frame_id;
info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
Expand All @@ -135,7 +140,23 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
{
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
}
info.goal_pose = getConstraintPose(req.goal_constraints.front());

// goal pose with optional offset wrt. the planning frame
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
frame_id = robot_model_->getModelFrame();

// check goal pose ik before Cartesian motion plan starts
std::map<std::string, double> ik_solution;
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
ik_solution))
{
// LCOV_EXCL_START
std::ostringstream os;
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw CircInverseForGoalIncalculable(os.str());
// LCOV_EXCL_STOP // not able to trigger here since lots of checks before
// are in place
}
}

assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
Expand All @@ -155,34 +176,38 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni

computeLinkFK(scene, info.link_name, info.start_joint_position, info.start_pose);

// check goal pose ik before Cartesian motion plan starts
std::map<std::string, double> ik_solution;
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
ik_solution))
// center point with wrt. the planning frame
std::string center_point_frame_id;

info.circ_path_point.first = req.path_constraints.name;
if (req.path_constraints.position_constraints.front().header.frame_id.empty())
{
// LCOV_EXCL_START
std::ostringstream os;
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw CircInverseForGoalIncalculable(os.str());
// LCOV_EXCL_STOP // not able to trigger here since lots of checks before
// are in place
RCLCPP_WARN(getLogger(), "Frame id is not set in position constraints of "
"path. Use model frame as default");
center_point_frame_id = robot_model_->getModelFrame();
}
info.circ_path_point.first = req.path_constraints.name;
else
{
center_point_frame_id = req.path_constraints.position_constraints.front().header.frame_id;
}

Eigen::Isometry3d center_point_pose;
tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front(),
center_point_pose);

center_point_pose = scene->getFrameTransform(center_point_frame_id) * center_point_pose;

if (!req.goal_constraints.front().position_constraints.empty())
{
const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front();
info.circ_path_point.second =
getConstraintPose(
req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset)
.translation();
geometry_msgs::msg::Point center_point = tf2::toMsg(Eigen::Vector3d(center_point_pose.translation()));
info.circ_path_point.second = getConstraintPose(center_point, goal.orientation_constraints.front().orientation,
goal.position_constraints.front().target_point_offset)
.translation();
}
else
{
Eigen::Vector3d circ_path_point;
tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
circ_path_point);
info.circ_path_point.second = circ_path_point;
info.circ_path_point.second = center_point_pose.translation();
}
}

Expand Down
27 changes: 26 additions & 1 deletion moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,11 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
RCLCPP_DEBUG(LOGGER, "Extract necessary information from motion plan request.");

info.group_name = req.group_name;
<<<<<<< HEAD
std::string frame_id{ robot_model_->getModelFrame() };
=======
moveit::core::RobotState robot_state = scene->getCurrentState();
>>>>>>> 4c28a7434 (Enhancement/ports moveit 3522 (#3070))

// goal given in joint space
if (!req.goal_constraints.front().joint_constraints.empty())
Expand Down Expand Up @@ -98,6 +102,8 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
// goal given in Cartesian space
else
{
std::string frame_id;

info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
Expand All @@ -110,9 +116,23 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
{
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
}
info.goal_pose = getConstraintPose(req.goal_constraints.front());

// goal pose with optional offset wrt. the planning frame
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
frame_id = robot_model_->getModelFrame();

// check goal pose ik before Cartesian motion plan starts
std::map<std::string, double> ik_solution;
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
ik_solution))
{
std::ostringstream os;
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw LinInverseForGoalIncalculable(os.str());
}
}

<<<<<<< HEAD
assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
for (const auto& joint_name : robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames())
{
Expand Down Expand Up @@ -141,6 +161,11 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw LinInverseForGoalIncalculable(os.str());
}
=======
// Ignored return value because at this point the function should always
// return 'true'.
computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
>>>>>>> 4c28a7434 (Enhancement/ports moveit 3522 (#3070))
}

void TrajectoryGeneratorLIN::plan(const planning_scene::PlanningSceneConstPtr& scene,
Expand Down
29 changes: 25 additions & 4 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -234,11 +234,32 @@ void TrajectoryGeneratorPTP::extractMotionPlanInfo(const planning_scene::Plannin
// solve the ik
else
{
Eigen::Isometry3d goal_pose = getConstraintPose(req.goal_constraints.front());
if (!computePoseIK(scene, req.group_name, req.goal_constraints.at(0).position_constraints.at(0).link_name,
goal_pose, robot_model_->getModelFrame(), info.start_joint_position, info.goal_joint_position))
std::string frame_id;

info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
{
RCLCPP_WARN(getLogger(), "Frame id is not set in position/orientation constraints of "
"goal. Use model frame as default");
frame_id = robot_model_->getModelFrame();
}
else
{
frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
}

// goal pose with optional offset wrt. the planning frame
info.goal_pose = scene->getFrameTransform(frame_id) * getConstraintPose(req.goal_constraints.front());
frame_id = robot_model_->getModelFrame();

// check goal pose ik before Cartesian motion plan start
if (!computePoseIK(scene, info.group_name, info.link_name, info.goal_pose, frame_id, info.start_joint_position,
info.goal_joint_position))
{
throw PtpNoIkSolutionForGoalPose("No IK solution for goal pose");
std::ostringstream os;
os << "Failed to compute inverse kinematics for link: " << info.link_name << " of goal pose";
throw PtpNoIkSolutionForGoalPose(os.str());
}
}
}
Expand Down
Loading