Skip to content

Commit

Permalink
Enhancement/ports moveit 3522 (#3070)
Browse files Browse the repository at this point in the history 
* 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
  • Loading branch information
@rr-tom-noble
rr-tom-noble authored Nov 8, 2024
1 parent 5049e4d commit 4c28a74
Show file tree
Hide file tree
Showing 3 changed files with 85 additions and 38 deletions.
64 changes: 42 additions & 22 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -97,7 +97,6 @@ void TrajectoryGeneratorCIRC::extractMotionPlanInfo(const planning_scene::Planni
RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request.");

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

// goal given in joint space
Expand Down Expand Up @@ -130,6 +129,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 @@ -142,39 +142,59 @@ 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
}
}

computeLinkFK(robot_state, 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
30 changes: 18 additions & 12 deletions moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,6 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
RCLCPP_DEBUG(getLogger(), "Extract necessary information from motion plan request.");

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

// goal given in joint space
Expand Down Expand Up @@ -103,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 @@ -115,20 +116,25 @@ 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());
}

computeLinkFK(robot_state, info.link_name, info.start_joint_position, info.start_pose);
// 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());
// 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());
}
}

// 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);
}

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
Original file line number Diff line number Diff line change
Expand Up @@ -233,11 +233,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

0 comments on commit 4c28a74

Please sign in to comment.