Fix resolveConstraintFrames for orientation constraints

[rhaschke]

Address moveit/moveit_task_constructor#619 (comment).
While the old Euler tolerance cannot be adapted to the new link frame, the rotation vector tolerance can!
Also, I noticed that the tolerance vector was interpreted w.r.t. the target frame, while it should be interpreted w.r.t. the given frame_id. The difference can be seen in the following videos. In both cases, frame_id is world and rotation is allowed about the z-axis. However, in the left video, the object rotates about it's own z-axis (parallel to world's x) instead of world's one. Both videos run the same MTC example.

w.r.t. target frame	w.r.t. frame_id

TODO:

• add unit test
  Without this PR, the tolerance is interpreted w.r.t. the target frame, making the unit test fail:

  moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp:506
  Value of: oc.decide(robot_state).satisfied
    Actual: false
  Expected: true
  
  moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp:510
  Value of: oc.decide(robot_state).satisfied
    Actual: true
  Expected: false
  

The old Euler-angles-based tolerance representation is fundamentally broken: it only yields correct results if the target orientation w.r.t. the reference frame is the identity and it is not compatible to subframes.
For this reason, I also suggest to deprecate that representation and making the rotation-vector one (introduced by @JeroenDM in #2402) the default. However, this should be discussed.

[codecov-commenter]

Codecov Report

Attention: Patch coverage is 84.21053% with 6 lines in your changes missing coverage. Please review.

Project coverage is 47.83%. Comparing base (c174715) to head (3a73ec8).
Report is 79 commits behind head on master.

Files with missing lines	Patch %	Lines
..._constraints/test/test_orientation_constraints.cpp	83.34%	4 Missing ⚠️
moveit_core/kinematic_constraints/src/utils.cpp	50.00%	2 Missing ⚠️

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #3656      +/-   ##
==========================================
+ Coverage   47.15%   47.83%   +0.68%     
==========================================
  Files         603      604       +1     
  Lines       58999    61008    +2009     
  Branches     6969     7021      +52     
==========================================
+ Hits        27814    29175    +1361     
- Misses      30773    31415     +642     
- Partials      412      418       +6     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

🚀 New features to boost your workflow:

• ❄️ Test Analytics: Detect flaky tests, report on failures, and find test suite problems.

[captain-yoshi]

Thanks for looking into this. I gave up a long time ago on orientation constraints and switched to cartesian planning when needed. I have added a small review. Did not have a chance to test this, but will try in the comming week.

[amoyalang]

Address moveit/moveit_task_constructor#619 (comment). While the old Euler tolerance cannot be adapted to the new link frame, the rotation vector tolerance can! Also, I noticed that the tolerance vector was interpreted w.r.t. the target frame, while it should be interpreted w.r.t. the given frame_id. The difference can be seen in the following videos. In both cases, frame_id is world and rotation is allowed about the z-axis. However, in the left video, the object rotates about it's own z-axis (parallel to world's x) instead of world's one. Both videos run the same MTC example.

w.r.t. target frame w.r.t. frame_id

TODO:

• add unit test
  Without this PR, the tolerance is interpreted w.r.t. the target frame, making the unit test fail:

  moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp:506
  Value of: oc.decide(robot_state).satisfied
    Actual: false
  Expected: true
  
  moveit_core/kinematic_constraints/test/test_orientation_constraints.cpp:510
  Value of: oc.decide(robot_state).satisfied
    Actual: true
  Expected: false
  

The old Euler-angles-based tolerance representation is fundamentally broken: it only yields correct results if the target orientation w.r.t. the reference frame is the identity and it is not compatible to subframes. For this reason, I also suggest to deprecate that representation and making the rotation-vector one (introduced by @JeroenDM in #2402) the default. However, this should be discussed.

Hello,Why you say the object rotates about it's own z-axis (parallel to world's x) instead of world's one.In the videos,I looked the object is rotating around world's z-axis,it is not parallel to world's x.Is it my understanding that is incorrect?

[amoyalang]

I can't reuse the demo by cpp.I change it basic this demo,I only change the
my code is likes bellow

#include <rclcpp/rclcpp.hpp>
#include <moveit/planning_scene/planning_scene.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit/task_constructor/task.h>
#include <moveit/task_constructor/solvers.h>
#include <moveit/task_constructor/stages.h>
#if __has_include(<tf2_geometry_msgs/tf2_geometry_msgs.hpp>)
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#else
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#endif
#if __has_include(<tf2_eigen/tf2_eigen.hpp>)
#include <tf2_eigen/tf2_eigen.hpp>
#else
#include <tf2_eigen/tf2_eigen.h>
#endif

static const rclcpp::Logger LOGGER = rclcpp::get_logger("mtc_tutorial");
namespace mtc = moveit::task_constructor;

class MTCTaskNode
{
public:
  MTCTaskNode(const rclcpp::NodeOptions& options);

  rclcpp::node_interfaces::NodeBaseInterface::SharedPtr getNodeBaseInterface();

  void doTask();

  void setupPlanningScene();

private:
  // Compose an MTC task from a series of stages.
  mtc::Task createTask();
  mtc::Task createTaskTest();

  mtc::Task task_;
  rclcpp::Node::SharedPtr node_;
};

rclcpp::node_interfaces::NodeBaseInterface::SharedPtr MTCTaskNode::getNodeBaseInterface()
{
  return node_->get_node_base_interface();
}

MTCTaskNode::MTCTaskNode(const rclcpp::NodeOptions& options)
  : node_{ std::make_shared<rclcpp::Node>("mtc_node", options) }
{
}

void MTCTaskNode::setupPlanningScene()
{
  // obstacle
  moveit_msgs::msg::CollisionObject obstacle;
  obstacle.id = "obstacle";
  obstacle.header.frame_id = "world";
  obstacle.primitives.resize(1);
  obstacle.primitives[0].type = shape_msgs::msg::SolidPrimitive::SPHERE;
  obstacle.primitives[0].dimensions = {0.1};
  geometry_msgs::msg::Pose obstacle_pose;
  obstacle_pose.position.set__x(0.3).set__y(0.2).set__z(0.5);
  obstacle_pose.orientation.w = 1.0;
  obstacle.pose = obstacle_pose;

  // object
  moveit_msgs::msg::CollisionObject object;
  object.id = "object";
  object.header.frame_id = "world";
  object.primitives.resize(1);
  object.primitives[0].type = shape_msgs::msg::SolidPrimitive::BOX;
  object.primitives[0].dimensions = {0.1, 0.05, 0.03};
  geometry_msgs::msg::Pose object_pose;
  object_pose.position.set__x(0.30702).set__y(0.0).set__z(0.44);
  object_pose.orientation.w = 0.70711;
  object_pose.orientation.x = 0.70711;
  object.pose = object_pose;

  moveit::planning_interface::PlanningSceneInterface psi;
  psi.applyCollisionObject(object);
  psi.applyCollisionObject(obstacle);

}

void MTCTaskNode::doTask()
{
  task_ = createTaskTest();

  try
  {
    task_.init();
  }
  catch (mtc::InitStageException& e)
  {
    RCLCPP_ERROR_STREAM(LOGGER, e);
    return;
  }

  if (!task_.plan(5 /* max_solutions */))
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Task planning failed");
    return;
  }
  task_.introspection().publishSolution(*task_.solutions().front());

  auto result = task_.execute(*task_.solutions().front());
  if (result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS)
  {
    RCLCPP_ERROR_STREAM(LOGGER, "Task execution failed");
    return;
  }

  return;
}
mtc::Task MTCTaskNode::createTaskTest()
{
  mtc::Task task;
  task.stages()->setName("demo task");
  task.loadRobotModel(node_);

  const auto& arm_group_name = "panda_arm";
  const auto& hand_group_name = "hand";
  const auto& hand_frame = "panda_hand";

  // Set task properties
  task.setProperty("group", arm_group_name);
  task.setProperty("eef", hand_group_name);
  task.setProperty("ik_frame", hand_frame);

  std::cout << "1" << std::endl;
  mtc::Stage* current_state_ptr = nullptr;  // Forward current_state on to grasp pose generator
  auto stage_state_current = std::make_unique<mtc::stages::CurrentState>("current");
  current_state_ptr = stage_state_current.get();
  task.add(std::move(stage_state_current));
  std::cout << "2" << std::endl;
  auto sampling_planner = std::make_shared<mtc::solvers::PipelinePlanner>(node_);
  auto interpolation_planner = std::make_shared<mtc::solvers::JointInterpolationPlanner>();

  auto cartesian_planner = std::make_shared<mtc::solvers::CartesianPath>();
  cartesian_planner->setMaxVelocityScalingFactor(1.0);
  cartesian_planner->setMaxAccelerationScalingFactor(1.0);
  cartesian_planner->setStepSize(.01);
  {
    auto stage = std::make_unique<moveit::task_constructor::stages::ModifyPlanningScene>("attach object");
    stage->attachObject("object","panda_hand");
    task.add(std::move(stage));

  }
  {
    // clang-format off
    auto stage =
        std::make_unique<mtc::stages::MoveRelative>("y+0.4", sampling_planner);
    // clang-format on
    stage->properties().configureInitFrom(mtc::Stage::PARENT, { "group" });
    stage->setIKFrame(hand_frame);
    stage->setTimeout(10);
    stage->properties().set("marker_ns", "y+0.4");
    stage->setGroup("panda_arm");
     // add the constraint
     moveit_msgs::msg::Constraints path_constraints;
     path_constraints.name = "move_to_place_constraints";
 
     path_constraints.orientation_constraints.resize(1);
 
     {
         moveit_msgs::msg::OrientationConstraint &c =
         path_constraints.orientation_constraints[0];
         c.link_name = "object";
         c.header.frame_id = "world";
         c.orientation.w = c.orientation.x = 0.70711;
 
         //When grasping object,X axis is upright, 
         //so the X axis could be rotated,it is tolerancec omplete freedom,ohter tolerances shouble be constrained
         c.absolute_x_axis_tolerance = 0.1;
         c.absolute_y_axis_tolerance = 0.1;
         c.absolute_z_axis_tolerance = 3.14;
         c.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
         c.weight = 1.0;
     } 
     stage->setPathConstraints(path_constraints);
    // Set upward direction
    geometry_msgs::msg::Vector3Stamped vec;
    vec.header.frame_id = "world";
    vec.vector.y = 0.4;
    vec.vector.x = 0.0;
    vec.vector.z = 0.0;
    stage->setDirection(vec);
    task.add(std::move(stage));
  }
  
  return task;
}

int main(int argc, char** argv)
{
  rclcpp::init(argc, argv);

  rclcpp::NodeOptions options;
  options.automatically_declare_parameters_from_overrides(true);

  auto mtc_task_node = std::make_shared<MTCTaskNode>(options);
  rclcpp::executors::MultiThreadedExecutor executor;

  auto spin_thread = std::make_unique<std::thread>([&executor, &mtc_task_node]() {
    executor.add_node(mtc_task_node->getNodeBaseInterface());
    executor.spin();
    executor.remove_node(mtc_task_node->getNodeBaseInterface());
  });

  mtc_task_node->setupPlanningScene();
  mtc_task_node->doTask();

  spin_thread->join();
  rclcpp::shutdown();
  return 0;
}

I can't plan task normal.This is the vedio which my demo.

eSearch-2025-05-26-11-50-44-346.mp4

when I change like this:Use panda_link0 as frame_id,panda_hand as target_frame,it is plan fail also.

     {
         moveit_msgs::msg::OrientationConstraint &c =
         path_constraints.orientation_constraints[0];
        //  c.link_name = "object";
        //  c.header.frame_id = "world";
        //  c.orientation.w = c.orientation.x = 0.70711;

        c.link_name = "panda_hand";
        c.header.frame_id = "panda_link0";

         tf2::Quaternion q;
         q.setRPY(M_PI, 0, 0);    

        c.orientation.x = q.x();
        c.orientation.y = q.y();
        c.orientation.z = q.z();
        c.orientation.w = q.w();
         c.absolute_x_axis_tolerance = 0.1;
         c.absolute_y_axis_tolerance = 0.1;
         c.absolute_z_axis_tolerance = 3.14;
         c.parameterization = moveit_msgs::msg::OrientationConstraint::ROTATION_VECTOR;
         c.weight = 1.0;
     } 

eSearch-2025-05-26-14-24-22-949.mp4