Box constrains hardly works

[mouwe5]

Affected ROS2 Driver version(s)

2.2.13

Used ROS distribution.

Humble

Which combination of platform is the ROS driver running on.

Ubuntu Linux with standard kernel

How is the UR ROS2 Driver installed.

Build the driver from source and using the UR Client Library from binary

Which robot platform is the driver connected to.

UR E-series robot

Robot SW / URSim version(s)

00

How is the ROS driver used.

Through the robot teach pendant using External Control URCap

Issue details

Summary

We are trying to move the robot from its current state to a goal state with specified XYZ coordinates and orientation using the MoveIt2 C++ API. To ensure the path is as smooth as possible, we are attempting to use box/line constraints.

However, we encounter a problem: even for very simple paths (e.g., moving -20 cm along the Z-axis), the robot often fails to find a path. Occasionally, about once in every 20 attempts, it succeeds and the robot moves, but this success rate is insufficient.

We expect to achieve simple movements like this reliably, and we are open to any suggestions for improvement.

Issue details

We tried several planners in OMPL (PRMkConfigDefault, SBLkConfigDefault, ESTkConfigDefault, LBKPIECEkConfigDefault, BKPIECEkConfigDefault, KPIECEkConfigDefault, RRTkConfigDefault,RRTConnectkConfigDefault, RRTstarkConfigDefault, TRRTkConfigDefault, PRMstarkConfigDefault)
with Kinematics solvers: kdl_kinematics and lma_kinemtaics.

Detailed description help us understand the problem. Code are welcome!

void MotionServerNode::motion_service_callback(const std::shared_ptr<rmw_request_id_t> request_header,
                                               const std::shared_ptr<hvmr_interfaces::srv::Motion::Request> request,
                                               std::shared_ptr<hvmr_interfaces::srv::Motion::Response> response)
{
            auto current_pose = move_group_->getCurrentPose();
            auto get_relative_pose = [current_pose, this](double x, double y, double z) {
                auto target_pose = current_pose;
                target_pose.pose.position.x += x;
                target_pose.pose.position.y += y;
                target_pose.pose.position.z += z;
                visual_tools_->publishSphere(current_pose.pose, rviz_visual_tools::BLUE, 0.05);
                visual_tools_->publishSphere(target_pose.pose, rviz_visual_tools::GREEN, 0.05);
                visual_tools_->trigger();
                return target_pose;
            };
            auto target_pose = get_relative_pose(0.0, 0.0, -0.2).pose;

            this->setBoxConstraints(target_pose);
            move_group_->setPathConstraints(constraints_);
            this->log_constraints();
            this->planTargetPoseTrajectory(target_pose, plan);
            this->visualizeAndExecute(plan, request->do_visualize, request->do_execute);
}
void MotionServerNode::setBoxConstraints(geometry_msgs::msg::Pose &target_pose)
{
    auto current_pose = move_group_->getCurrentPose();

    // Let's try the simple box constraints first!
    moveit_msgs::msg::PositionConstraint box_constraint;
    box_constraint.header.frame_id = move_group_->getPoseReferenceFrame();
    box_constraint.link_name = move_group_->getEndEffectorLink();

    //Compute the box dimensions to encompass both positions
    float box_min_x = std::min(current_pose.pose.position.x, target_pose.position.x) - 0.05;
    float box_max_x = std::max(current_pose.pose.position.x, target_pose.position.x) + 0.05;
    float box_min_y = std::min(current_pose.pose.position.y, target_pose.position.y) - 0.05;
    float box_max_y = std::max(current_pose.pose.position.y, target_pose.position.y) + 0.05;
    float box_min_z = std::min(current_pose.pose.position.z, target_pose.position.z) - 0.05;
    float box_max_z = std::max(current_pose.pose.position.z, target_pose.position.z) + 0.05;

    float box_center_x = (box_min_x + box_max_x) / 2.0;
    float box_center_y = (box_min_y + box_max_y) / 2.0;
    float box_center_z = (box_min_z + box_max_z) / 2.0;

    float box_size_x = box_max_x - box_min_x;
    float box_size_y = box_max_y - box_min_y;
    float box_size_z = box_max_z - box_min_z;

    shape_msgs::msg::SolidPrimitive box;
    box.type = shape_msgs::msg::SolidPrimitive::BOX;
    box.dimensions = {box_size_x, box_size_y, box_size_z};
    box_constraint.constraint_region.primitives.emplace_back(box);

    geometry_msgs::msg::Pose box_pose;
    box_pose.position.x = box_center_x;
    box_pose.position.y = box_center_y;
    box_pose.position.z = box_center_z;
    box_pose.orientation.w = 1.0;
    box_constraint.constraint_region.primitive_poses.emplace_back(box_pose);
    box_constraint.weight = 1.0;

    // Visualize the box constraint
    Eigen::Vector3d box_point_1(box_pose.position.x - box.dimensions[0] / 2, box_pose.position.y - box.dimensions[1] / 2,
                                box_pose.position.z - box.dimensions[2] / 2);
    Eigen::Vector3d box_point_2(box_pose.position.x + box.dimensions[0] / 2, box_pose.position.y + box.dimensions[1] / 2,
                                box_pose.position.z + box.dimensions[2] / 2);
    visual_tools_->publishCuboid(box_point_1, box_point_2, rviz_visual_tools::TRANSLUCENT_DARK);
    visual_tools_->trigger();
    constraints_.position_constraints.emplace_back(box_constraint);
}
void MotionServerNode::planTargetPoseTrajectory(
    geometry_msgs::msg::Pose &pose,
    moveit::planning_interface::MoveGroupInterface::Plan &plan)
{
    geometry_msgs::msg::PoseStamped pose_stamped;
    pose_stamped.header.frame_id = "world";
    pose_stamped.pose = pose;
    move_group_->setPoseTarget(pose);
    this->visualizeCurrentPointAndTargetPoint(pose);
    bool success = (move_group_->plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
    if (!success)
    {
        throw moveit::Exception("Planning trajectory to pose target failed.");
    }

Expected Behavior

Smooth tranjectory between current pose to XYZ.

Actual Behavior

Without constrains the robot is "dancing" and with constarins to most of the time have no solution.

Relevant log output

[move_group-1] [INFO] [1718800030.752053389] [moveit_move_group_default_capabilities.move_action_capability]: Received request
[move_group-1] [INFO] [1718800030.752879409] [moveit_move_group_default_capabilities.move_action_capability]: executing..
[motion_server-1] [INFO] [1718800030.753450186] [move_group_interface]: Planning request accepted
[move_group-1] [INFO] [1718800030.753732404] [moveit_move_group_default_capabilities.move_action_capability]: Planning request received for MoveGroup action. Forwarding to planning pipeline.
[move_group-1] [INFO] [1718800030.754265284] [moveit.ompl_planning.model_based_planning_context]: Planner configuration 'ur_manipulator[RRTkConfigDefault]' will use planner 'geometric::RRT'. Additional configuration parameters will be set when the planner is constructed.
[move_group-1] [ERROR] [1718800046.234870634] [ompl]: ./src/ompl/base/src/SpaceInformation.cpp:468 - State samplers generate constraint unsatisfying states.
[move_group-1] [INFO] [1718800046.603169031] [moveit.ompl_planning.model_based_planning_context]: Unable to solve the planning problem
[move_group-1] [INFO] [1718800046.603590090] [moveit_move_group_default_capabilities.move_action_capability]: Catastrophic failure
[motion_server-1] [INFO] [1718800046.604004504] [move_group_interface]: Planning request aborted
[motion_server-1] [ERROR] [1718800046.604189128] [move_group_interface]: MoveGroupInterface::plan() failed or timeout reached
[motion_server-1] [ERROR] [1718800046.604217101] [moveit_exceptions.exceptions]: Planning trajectory to pose target failed.
[motion_server-1] Exception thrown.
[motion_server-1] [WARN] [1718800046.604252113] [motion_server_node]: Planning trajectory to pose target failed.

Accept Public visibility

• I agree to make this context public

[fmauch]

Thank you for reporting this.

Unfortunately, this package's maintainers aren't MoveIt! experts themselves, so we can't really provide any insightful information. However, if there is anything we can change with the default settings to improve that situations, it might be good to change that here, which is why I will leave this issue open.

I suggest, that you ask somewhere else, e.g. on robotics.stackexchange.com (please also reference this issue there).

[firesurfer]

Did you properly configure the projection_evaluator ?

e.g. in the ompl_planning.yaml for your planning group:

 projection_evaluator: joints(ur_bottom/shoulder_pan_joint,ur_bottom/shoulder_lift_joint,ur_bottom/elbow_joint,ur_bottom/wrist_1_joint,ur_bottom/wrist_2_joint,ur_bottom/wrist_3_joint)

From my understanding constrained planning uses this projection in order to only sampling within the constrain region.
It could be that without the projection it falls back to rejection sampling which could explain the phenomena you are seeing

If you just want to move from to a certain cartesian position it might be worth taking a look at the pilz planner in moveit. Which allows for linear and circular (deterministic!) motions.