Update subframe of collision object?

edit

Hello everyone,

I'm trying to update the subframe of a collision object, but this gives me quite some struggles. To clarify a bit: I am simulating picking an apple. The apple is modelled as a sphere which has a subframe positioned at the spheres' center, with the z-axis always pointing upward towards the stem. To ease approach for the robot, I want to rotate the subframe around the z-axis and have one axis approximately pointing towards the robot base link. I am using ROS2 Foxy and MoveIt2 on Ubuntu 20.04, with Rviz for visualisation.

The situation is as follows:

• I spawn a sphere-shaped collision object, with a certain orientation. I also give it a subframe with the exact same orientation. This works nicely and I can plan a path using this subframe.
• After identifying the target apple, I want to rotate the subframe of the apple around its z-axis. I manage to extract the collision object and subframe information from the planning scene interface, and then do some linear algebra. This is not the problem. However, when I try to apply this subframe to the collision object again, this does not work, I can see that the path planning still uses the initial subframe.
• It is possible to remove the apple from the planning scene and create a new one with the new subframe, but that seems quite a bit of unnecessary actions for what I aim to do.

So far, I tried to update the subframe using both object.operation = object.MOVE and object.operation = object.APPEND, but this does not rotate the frame. I also tried this with adding the current pose as well, and also with ADD, but all with no result.

moveit_msgs::msg::CollisionObject object;
object.header.frame_id = "base_link";
object.id = "object_name";
object.subframe_names.resize(1);
object.subframe_poses.resize(1);
object.subframe_names[0] = "middle";
object.subframe_poses[0].position.x = pos_x; 
object.subframe_poses[0].position.y = pos_y;
object.subframe_poses[0].position.z = pos_z; 
object.subframe_poses[0].orientation = tf2::toMsg(transformed_quaternion);
object.operation = object.MOVE;
psi.applyCollisionObject(object);

Does anyone have an idea whether this is possible and if so, how? I hope this question and information is clear enough, thanks a lot in advance!