41 #include <kdl/rotational_interpolation_sa.hpp>
42 #include <kdl/trajectory_segment.hpp>
43 #include <kdl/utilities/error.h>
44 #include <kdl/utilities/utility.h>
46 #include <tf2_eigen_kdl/tf2_eigen_kdl.hpp>
47 #include <rclcpp/logger.hpp>
48 #include <rclcpp/logging.hpp>
49 #include <tf2_eigen/tf2_eigen.hpp>
50 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
54 static const rclcpp::Logger LOGGER =
55 rclcpp::get_logger(
"moveit.pilz_industrial_motion_planner.trajectory_generator_circ");
63 throw TrajectoryGeneratorInvalidLimitsException(
64 "Cartesian limits are not fully set for CIRC trajectory generator.");
70 if (!(req.path_constraints.name ==
"interim" || req.path_constraints.name ==
"center"))
72 std::ostringstream os;
73 os <<
"No path constraint named \"interim\" or \"center\" found (found "
74 "unknown constraint: "
75 <<
"\"req.path_constraints.name\""
77 throw UnknownPathConstraintName(os.str());
80 if (req.path_constraints.position_constraints.size() != 1)
82 throw NoPositionConstraints(
"CIRC trajectory generator needs valid a position constraint");
85 if (req.path_constraints.position_constraints.front().constraint_region.primitive_poses.size() != 1)
87 throw NoPrimitivePose(
"CIRC trajectory generator needs valid a primitive pose");
91 void TrajectoryGeneratorCIRC::extractMotionPlanInfo(
const planning_scene::PlanningSceneConstPtr&
scene,
93 TrajectoryGenerator::MotionPlanInfo& info)
const
95 RCLCPP_DEBUG(LOGGER,
"Extract necessary information from motion plan request.");
97 info.group_name = req.group_name;
101 if (!req.goal_constraints.front().joint_constraints.empty())
104 info.link_name = req.path_constraints.position_constraints.front().link_name;
107 throw UnknownLinkNameOfAuxiliaryPoint(
"Unknown link name of CIRC auxiliary point");
110 if (req.goal_constraints.front().joint_constraints.size() !=
111 robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size())
113 std::ostringstream os;
114 os <<
"Number of joint constraint = " << req.goal_constraints.front().joint_constraints.size()
115 <<
" not equal to active joints of group = "
116 <<
robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size();
117 throw NumberOfConstraintsMismatch(os.str());
120 for (
const auto& joint_item : req.goal_constraints.front().joint_constraints)
122 info.goal_joint_position[joint_item.joint_name] = joint_item.position;
130 info.link_name = req.goal_constraints.front().position_constraints.front().link_name;
131 if (req.goal_constraints.front().position_constraints.front().header.frame_id.empty() ||
132 req.goal_constraints.front().orientation_constraints.front().header.frame_id.empty())
134 RCLCPP_WARN(LOGGER,
"Frame id is not set in position/orientation constraints of "
135 "goal. Use model frame as default");
140 frame_id = req.goal_constraints.front().position_constraints.front().header.frame_id;
145 assert(req.start_state.joint_state.name.size() == req.start_state.joint_state.position.size());
146 for (
const auto& joint_name :
robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames())
148 auto it{ std::find(req.start_state.joint_state.name.cbegin(), req.start_state.joint_state.name.cend(), joint_name) };
149 if (it == req.start_state.joint_state.name.cend())
151 std::ostringstream os;
152 os <<
"Could not find joint \"" << joint_name <<
"\" of group \"" << req.group_name
153 <<
"\" in start state of request";
154 throw CircJointMissingInStartState(os.str());
156 size_t index = it - req.start_state.joint_state.name.cbegin();
157 info.start_joint_position[joint_name] = req.start_state.joint_state.position[index];
163 std::map<std::string, double> ik_solution;
168 std::ostringstream os;
169 os <<
"Failed to compute inverse kinematics for link: " << info.link_name <<
" of goal pose";
170 throw CircInverseForGoalIncalculable(os.str());
174 info.circ_path_point.first = req.path_constraints.name;
175 if (!req.goal_constraints.front().position_constraints.empty())
177 const moveit_msgs::msg::Constraints& goal = req.goal_constraints.front();
178 info.circ_path_point.second =
180 req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
181 goal.orientation_constraints.front().orientation, goal.position_constraints.front().target_point_offset)
187 tf2::fromMsg(req.path_constraints.position_constraints.front().constraint_region.primitive_poses.front().position,
189 info.circ_path_point.second = circ_path_point;
193 void TrajectoryGeneratorCIRC::plan(
const planning_scene::PlanningSceneConstPtr&
scene,
195 const double& sampling_time, trajectory_msgs::msg::JointTrajectory& joint_trajectory)
197 std::unique_ptr<KDL::Path> cart_path(setPathCIRC(plan_info));
198 std::unique_ptr<KDL::VelocityProfile> vel_profile(
205 KDL::Trajectory_Segment cart_trajectory(cart_path.get(), vel_profile.get(),
false);
207 moveit_msgs::msg::MoveItErrorCodes error_code;
211 plan_info.link_name, plan_info.start_joint_position, sampling_time, joint_trajectory,
214 throw CircTrajectoryConversionFailure(
"Failed to generate valid joint trajectory from the Cartesian path",
219 std::unique_ptr<KDL::Path> TrajectoryGeneratorCIRC::setPathCIRC(
const MotionPlanInfo& info)
const
221 RCLCPP_DEBUG(LOGGER,
"Set Cartesian path for CIRC command.");
223 KDL::Frame start_pose, goal_pose;
224 tf2::transformEigenToKDL(info.start_pose, start_pose);
225 tf2::transformEigenToKDL(info.goal_pose, goal_pose);
227 const auto& eigen_path_point = info.circ_path_point.second;
228 const KDL::Vector path_point{ eigen_path_point.x(), eigen_path_point.y(), eigen_path_point.z() };
241 if (info.circ_path_point.first ==
"center")
250 catch (KDL::Error_MotionPlanning_Circle_No_Plane& e)
252 std::ostringstream os;
253 os <<
"Failed to create path object for circle." << e.Description();
254 throw CircleNoPlane(os.str());
256 catch (KDL::Error_MotionPlanning_Circle_ToSmall& e)
258 std::ostringstream os;
259 os <<
"Failed to create path object for circle." << e.Description();
260 throw CircleToSmall(os.str());
264 std::ostringstream os;
265 os <<
"Failed to create path object for circle." << e.
Description();
266 throw CenterPointDifferentRadius(os.str());
const char * Description() const override
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.
static std::unique_ptr< KDL::Path > circleFromCenter(const KDL::Frame &start_pose, const KDL::Frame &goal_pose, const KDL::Vector ¢er_point, double eqradius)
set the path circle from start, goal and center point
static std::unique_ptr< KDL::Path > circleFromInterim(const KDL::Frame &start_pose, const KDL::Frame &goal_pose, const KDL::Vector &interim_point, double eqradius)
set circle from start, goal and interim point
TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr &robot_model, const pilz_industrial_motion_planner::LimitsContainer &planner_limits, const std::string &group_name)
Constructor of CIRC Trajectory Generator.
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_
Vec3fX< details::Vec3Data< double > > Vector3d
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.