How to use MTC to construct a complex z-axis-free-rotate trajectory

[Demonmasterlqx]

• The ROS2 version: ros2 jazzy
• Ubuntu 24.04

The trajectory includes a linear movement of 100 mm along the X-axis, followed by a linear movement of 100 mm along the Y-axis, and finally a 90-degree rotation around a certain axis. Throughout this process, the robot arm's Z-axis needs to be free to rotate to achieve good passability and successful planning.

I have already tried to write it like this

using namespace moveit::task_constructor;

FreeAxisPoseGenerator::FreeAxisPoseGenerator(const std::string & name, Stage* monitored):
    stages::GeneratePose(name)
{
    setMonitoredStage(monitored);

    auto & p = properties();
    p.declare<geometry_msgs::msg::PoseStamped>("target_pose", "Target pose around which to generate free-axis poses");
    p.declare<Eigen::Vector3d>("free_axis", Eigen::Vector3d::UnitZ(), "Free axis around which to generate poses");
    p.declare<double>("min_angle", -M_PI,"Minimum rotation angle (radians)");
    p.declare<double>("max_angle", M_PI, "Maximum rotation angle (radians)");
    p.declare<unsigned int>("angle_step_num", 10, "Number of rotation angle steps");

}

void FreeAxisPoseGenerator::init(const moveit::core::RobotModelConstPtr& robot_model)
{
	InitStageException errors;
	try {
		stages::GeneratePose::init(robot_model);
	} catch (InitStageException& e) {
		errors.append(e);
	}

    const auto& props = properties();

    // check the min_angle and max_angle
    double min_angle = props.get<double>("min_angle");
    double max_angle = props.get<double>("max_angle");
    if (min_angle > max_angle) {
        errors.push_back(*this, "min_angle should be less than or equal to max_angle");
    }

    // check the angle_step_num
    unsigned int step_num = props.get<unsigned int>("angle_step_num");
    if (step_num == 0) {
        errors.push_back(*this, "angle_step_num should be greater than 0");
    }

    Eigen::Vector3d free_axis = props.get<Eigen::Vector3d>("free_axis");
    if (free_axis.norm() < 1e-9) {
        errors.push_back(*this, "free_axis should be a non-zero vector");
    }

    if (errors) {
        throw errors;
    }

}

void FreeAxisPoseGenerator::setTargetPose(const geometry_msgs::msg::PoseStamped& target_pose)
{
    properties().set("target_pose", target_pose);
}

void FreeAxisPoseGenerator::setTarget(const std::string& target_frame)
{
    geometry_msgs::msg::PoseStamped target_pose;
    target_pose.header.frame_id = target_frame;
    target_pose.pose.orientation.w = 1.0;
    setTargetPose(target_pose);
}

void FreeAxisPoseGenerator::setFreeAxis(const Eigen::Vector3d& axis)
{
    properties().set("free_axis", axis.normalized());
}

void FreeAxisPoseGenerator::setRotationAngleRange(double min_angle, double max_angle)
{
    properties().set("min_angle", min_angle);
    properties().set("max_angle", max_angle);
}

void FreeAxisPoseGenerator::setRotationStepNum(unsigned int step)
{
    properties().set("angle_step_num", step);
}

void FreeAxisPoseGenerator::onNewSolution(const SolutionBase& s)
{

	planning_scene::PlanningSceneConstPtr scene = s.end()->scene();

	const auto& props = properties();

    // check the target_pose frame in the current planning scene
    geometry_msgs::msg::PoseStamped target_pose = props.get<geometry_msgs::msg::PoseStamped>("target_pose");
    if (!scene->knowsFrameTransform(target_pose.header.frame_id)) {
        const std::string msg = "Target pose frame '" + target_pose.header.frame_id + "' is not known in the current planning scene";
        spawn(InterfaceState{ scene }, SubTrajectory::failure(msg));
        return;
    }

	upstream_solutions_.push(&s);

}

void FreeAxisPoseGenerator::compute(){

	if (upstream_solutions_.empty())
		return;
	planning_scene::PlanningScenePtr scene = upstream_solutions_.pop()->end()->scene()->diff();

    // get the nessary properties
    const auto& props = properties();
    geometry_msgs::msg::PoseStamped target_pose = props.get<geometry_msgs::msg::PoseStamped>("target_pose");
    Eigen::Vector3d free_axis = props.get<Eigen::Vector3d>("free_axis");
    double min_angle = props.get<double>("min_angle");
    double max_angle = props.get<double>("max_angle");
    unsigned int step_num = props.get<unsigned int>("angle_step_num");

    if (free_axis.norm() < 1e-9) {
        spawn(InterfaceState{ scene }, SubTrajectory::failure("free_axis should be a non-zero vector"));
        return;
    }

    free_axis.normalize();

    Eigen::Isometry3d target_pose_tf = Eigen::Isometry3d::Identity();
    tf2::fromMsg(target_pose.pose, target_pose_tf);

    const double angle_step = (step_num > 1) ? (max_angle - min_angle) / static_cast<double>(step_num - 1) : 0.0;

    for (unsigned int i = 0; i < step_num; ++i) {
        const double angle = (step_num > 1) ? (min_angle + static_cast<double>(i) * angle_step) : min_angle;

        // Rotate in target frame, i.e. free_axis is interpreted in target_pose local coordinates.
        const Eigen::Isometry3d rotated_pose_tf = target_pose_tf * Eigen::AngleAxisd(angle, free_axis);

        InterfaceState state(scene);
        geometry_msgs::msg::PoseStamped out_pose = target_pose;
        out_pose.pose = tf2::toMsg(rotated_pose_tf);

        state.properties().set("target_pose", out_pose);

        SubTrajectory trajectory;
        trajectory.setCost(0.0);
        trajectory.setComment("free-axis angle: " + std::to_string(angle));
        rviz_marker_tools::appendFrame(trajectory.markers(), out_pose, 0.1, "free-axis frame");

        spawn(std::move(state), std::move(trajectory));
    }

}

However, the effect is not very good, and the complexity is too high. There will also be problems such as the failure of planning due to the inability to connect the front-end and back-end.

I want to know how to generally implement such a complex trajectory with reduced degrees of freedom in a better way?