43#include <kdl/path_line.hpp>
44#include <kdl/trajectory_segment.hpp>
45#include <kdl/utilities/error.h>
46#include <tf2/convert.h>
47#include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
48#include <rclcpp/logger.hpp>
49#include <rclcpp/logging.hpp>
50#include <tf2_eigen/tf2_eigen.hpp>
51#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
70void TrajectoryGeneratorLIN::extractMotionPlanInfo(
const planning_scene::PlanningSceneConstPtr& scene,
74 RCLCPP_DEBUG(getLogger(),
"Extract necessary information from motion plan request.");
80 if (!req.goal_constraints.front().joint_constraints.empty())
84 if (req.goal_constraints.front().joint_constraints.size() !=
85 robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size())
87 std::ostringstream os;
88 os <<
"Number of joints in goal does not match number of joints of group "
89 "(Number joints goal: "
90 << req.goal_constraints.front().joint_constraints.size() <<
" | Number of joints of group: "
91 <<
robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size() <<
')';
92 throw JointNumberMismatch(os.str());
95 for (
const auto& joint_item : req.goal_constraints.front().joint_constraints)
107 info.
link_name = req.goal_constraints.front().position_constraints.front().link_name;
108 if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
109 req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
111 RCLCPP_WARN(
getLogger(),
"Frame id is not set in position/orientation constraints of "
112 "goal. Use model frame as default");
117 frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
122 assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
123 for (
const auto& joint_name :
robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames())
125 auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) };
126 if (it == req.start_state.joint_state.name.cend())
128 std::ostringstream os;
129 os <<
"Could not find joint \"" << joint_name <<
"\" of group \"" << req.group_name
130 <<
"\" in start state of request";
131 throw LinJointMissingInStartState(os.str());
133 size_t index = it - req.start_state.joint_state.name.cbegin();
142 std::map<std::string, double> ik_solution;
146 std::ostringstream os;
147 os <<
"Failed to compute inverse kinematics for link: " << info.
link_name <<
" of goal pose";
148 throw LinInverseForGoalIncalculable(os.str());
152void TrajectoryGeneratorLIN::plan(
const planning_scene::PlanningSceneConstPtr& scene,
154 double sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
157 std::unique_ptr<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose));
160 std::unique_ptr<KDL::VelocityProfile> vp(
167 KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(),
false);
169 moveit_msgs::msg::MoveItErrorCodes error_code;
173 plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
176 std::ostringstream os;
177 os <<
"Failed to generate valid joint trajectory from the Cartesian path";
178 throw LinTrajectoryConversionFailure(os.str(), error_code.val);
182std::unique_ptr<KDL::Path> TrajectoryGeneratorLIN::setPathLIN(
const Eigen::Affine3d& start_pose,
183 const Eigen::Affine3d& goal_pose)
const
185 RCLCPP_DEBUG(
getLogger(),
"Set Cartesian path for LIN command.");
187 KDL::Frame kdl_start_pose, kdl_goal_pose;
188 tf2::transformEigenToKDL(start_pose, kdl_start_pose);
189 tf2::transformEigenToKDL(goal_pose, kdl_goal_pose);
192 KDL::RotationalInterpolation* rot_interpo =
new KDL::RotationalInterpolation_SingleAxis();
194 return std::unique_ptr<KDL::Path>(
195 std::make_unique<KDL::Path_Line>(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius,
true));
This class combines CartesianLimit and JointLimits into on single class.
void printCartesianLimits() const
Prints the cartesian limits set by user. Default values for limits are 0.0.
const cartesian_limits::Params & getCartesianLimits() const
Return the cartesian limits.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr &robot_model, const pilz_industrial_motion_planner::LimitsContainer &planner_limits, const std::string &group_name)
Constructor of LIN Trajectory Generator.
This class is used to extract needed information from motion plan request.
std::map< std::string, double > goal_joint_position
Eigen::Isometry3d start_pose
Eigen::Isometry3d goal_pose
std::map< std::string, double > start_joint_position
Base class of trajectory generators.
const moveit::core::RobotModelConstPtr robot_model_
const pilz_industrial_motion_planner::LimitsContainer planner_limits_
std::unique_ptr< KDL::VelocityProfile > cartesianTrapVelocityProfile(double max_velocity_scaling_factor, double max_acceleration_scaling_factor, const std::unique_ptr< KDL::Path > &path) const
build cartesian velocity profile for the path
rclcpp::Logger getLogger()
rclcpp::Logger getLogger(const std::string &name)
Creates a namespaced logger.
bool generateJointTrajectory(const planning_scene::PlanningSceneConstPtr &scene, const JointLimitsContainer &joint_limits, const KDL::Trajectory &trajectory, const std::string &group_name, const std::string &link_name, const std::map< std::string, double > &initial_joint_position, double sampling_time, trajectory_msgs::msg::JointTrajectory &joint_trajectory, moveit_msgs::msg::MoveItErrorCodes &error_code, bool check_self_collision=false)
Generate joint trajectory from a KDL Cartesian trajectory.
bool computePoseIK(const planning_scene::PlanningSceneConstPtr &scene, const std::string &group_name, const std::string &link_name, const Eigen::Isometry3d &pose, const std::string &frame_id, const std::map< std::string, double > &seed, std::map< std::string, double > &solution, bool check_self_collision=true, const double timeout=0.0)
compute the inverse kinematics of a given pose, also check robot self collision
bool computeLinkFK(const planning_scene::PlanningSceneConstPtr &scene, const std::string &link_name, const std::map< std::string, double > &joint_state, Eigen::Isometry3d &pose)
compute the pose of a link at give robot state
moveit_msgs::msg::MotionPlanRequest MotionPlanRequest
Eigen::Isometry3d getConstraintPose(const geometry_msgs::msg::Point &position, const geometry_msgs::msg::Quaternion &orientation, const geometry_msgs::msg::Vector3 &offset)
Adapt goal pose, defined by position+orientation, to consider offset.