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>
55 static const rclcpp::Logger LOGGER =
56 rclcpp::get_logger(
"moveit.pilz_industrial_motion_planner.trajectory_generator_lin");
63 RCLCPP_ERROR(LOGGER,
"Cartesian limits not set for LIN trajectory generator.");
64 throw TrajectoryGeneratorInvalidLimitsException(
"Cartesian limits are not fully set for LIN trajectory generator.");
68 void TrajectoryGeneratorLIN::extractMotionPlanInfo(
const planning_scene::PlanningSceneConstPtr&
scene,
72 RCLCPP_DEBUG(LOGGER,
"Extract necessary information from motion plan request.");
78 if (!req.goal_constraints.front().joint_constraints.empty())
82 if (req.goal_constraints.front().joint_constraints.size() !=
83 robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size())
85 std::ostringstream os;
86 os <<
"Number of joints in goal does not match number of joints of group "
87 "(Number joints goal: "
88 << req.goal_constraints.front().joint_constraints.size() <<
" | Number of joints of group: "
89 <<
robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size() <<
")";
90 throw JointNumberMismatch(os.str());
93 for (
const auto& joint_item : req.goal_constraints.front().joint_constraints)
105 info.
link_name = req.goal_constraints.front().position_constraints.front().link_name;
106 if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
107 req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
109 RCLCPP_WARN(LOGGER,
"Frame id is not set in position/orientation constraints of "
110 "goal. Use model frame as default");
115 frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
120 assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
121 for (
const auto& joint_name :
robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames())
123 auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) };
124 if (it == req.start_state.joint_state.name.cend())
126 std::ostringstream os;
127 os <<
"Could not find joint \"" << joint_name <<
"\" of group \"" << req.group_name
128 <<
"\" in start state of request";
129 throw LinJointMissingInStartState(os.str());
131 size_t index = it - req.start_state.joint_state.name.cbegin();
140 std::map<std::string, double> ik_solution;
144 std::ostringstream os;
145 os <<
"Failed to compute inverse kinematics for link: " << info.
link_name <<
" of goal pose";
146 throw LinInverseForGoalIncalculable(os.str());
150 void TrajectoryGeneratorLIN::plan(
const planning_scene::PlanningSceneConstPtr&
scene,
152 const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
155 std::unique_ptr<KDL::Path> path(setPathLIN(plan_info.start_pose, plan_info.goal_pose));
158 std::unique_ptr<KDL::VelocityProfile> vp(
165 KDL::Trajectory_Segment cart_trajectory(path.get(), vp.get(),
false);
167 moveit_msgs::msg::MoveItErrorCodes error_code;
171 plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
174 std::ostringstream os;
175 os <<
"Failed to generate valid joint trajectory from the Cartesian path";
176 throw LinTrajectoryConversionFailure(os.str(), error_code.val);
180 std::unique_ptr<KDL::Path> TrajectoryGeneratorLIN::setPathLIN(
const Eigen::Affine3d& start_pose,
181 const Eigen::Affine3d& goal_pose)
const
183 RCLCPP_DEBUG(LOGGER,
"Set Cartesian path for LIN command.");
185 KDL::Frame kdl_start_pose, kdl_goal_pose;
186 tf2::transformEigenToKDL(start_pose, kdl_start_pose);
187 tf2::transformEigenToKDL(goal_pose, kdl_goal_pose);
190 KDL::RotationalInterpolation* rot_interpo =
new KDL::RotationalInterpolation_SingleAxis();
192 return std::unique_ptr<KDL::Path>(
193 std::make_unique<KDL::Path_Line>(kdl_start_pose, kdl_goal_pose, rot_interpo, eqradius,
true));
double getMaxRotationalVelocity() const
Return the maximal rotational velocity [rad/s], 0 if nothing was set.
double getMaxTranslationalVelocity() const
Return the maximal translational velocity [m/s], 0 if nothing was set.
This class combines CartesianLimit and JointLimits into on single class.
bool hasFullCartesianLimits() const
Return if this LimitsContainer has defined cartesian limits.
const JointLimitsContainer & getJointLimitContainer() const
Obtain the Joint Limits from the container.
const CartesianLimit & getCartesianLimits() const
Return the cartesian limits.
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_
std::unique_ptr< KDL::VelocityProfile > cartesianTrapVelocityProfile(const double &max_velocity_scaling_factor, const double &max_acceleration_scaling_factor, const std::unique_ptr< KDL::Path > &path) const
build cartesian velocity profile for the path
const pilz_industrial_motion_planner::LimitsContainer planner_limits_
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, const 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.