Servo: Enable usage of transforms published during runtime

[udopa]

Description

Please explain the changes you made, including a reference to the related issue if applicable
This is related to #2891 and enables sending TwistStamped to Servo with frame_ids that were not known during startup resulting in more flexibility. With this approach, the following additional frames can be used (and were tested):

1. Static TF Broadcaster with a frame attached to the endeffector link of the selected movegroup. It should however work for any published frame. This could be potentially dangerous when applying the Twist in a frame far away from the actual control frame. Maybe there is some checking needed in that case.
2. Subframe of Attached Collision Object.

@AndyZe Here is the PR as discussed

The changes are in 3 places:

1. Servo.start()
2. Servo.calculateSingleIteration()
3. Servo.cartesianServoCalcs()

Using only getFrameTransform for getting the transform right did not work for me, this is why I still use the version with the inverse as shown here.

const auto tf_moveit_to_incoming_cmd_frame =
    current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() *
    planning_scene->getFrameTransform(cmd.header.frame_id);

translation_vector = tf_moveit_to_incoming_cmd_frame.linear() * translation_vector;
angular_vector = tf_moveit_to_incoming_cmd_frame.linear() * angular_vector;

The version with robot_state does not recognize the frames from the static tf broadcaster, which is why I did not use it in the end.

const auto tf_moveit_to_incoming_cmd_frame =
    current_state_->getGlobalLinkTransform(parameters_.planning_frame).inverse() *
    robot_state->getframeTransform(cmd.header.frame_id);

Furthermore, I think this code could also be used instead of the transformations (tf_moveit_to_robot_cmd_frame_ and tf_moveit_to_ee_frame_).
As far as I can see it, these transformations are also calculated every single iteration, but the frame ids have to be defined manually before starting servo (in the servo config file). They are also not used any further. Therefore I suppose that tf_moveit_to_robot_cmd_frame_ and tf_moveit_to_ee_frame_ can be completely replaced with a tf_moveit_to_incoming_cmd_frame and a default transformation (in case there is no frame_id declared in the cmd header). This also results in not needing to declare the frames in the config.

Maintainers: What do you think about that?

Checklist

• Required by CI: Code is auto formatted using clang-format
• Extend the tutorials / documentation reference
• Document API changes relevant to the user in the MIGRATION.md notes
• Create tests, which fail without this PR reference
• Include a screenshot if changing a GUI
• While waiting for someone to review your request, please help review another open pull request to support the maintainers

[welcome]

Thanks for helping in improving MoveIt and open source robotics!

[JafarAbdi]

Why keep requesting the whole planning scene on each iteration .? see https://github.com/ros-planning/moveit2/pull/673/files for how we fixed this issue for moveit2

[udopa]

I'm don't know how to do that, can you elaborate a bit more?

[udopa]

This doesn't work. See my other comment here

[AndyZe]

@udopa I'm making some changes to sync with what @JafarAbdi has done. If I have permission, I'll push directly to your branch. If not, I'll open a PR against your branch and it will be up to you to review/merge it

[AndyZe]

Getting pretty close to merge-ready in my opinion. Thanks!

[AndyZe]

To test this, I added a static tf publisher to my Servo launch file:

<node pkg="tf" type="static_transform_publisher" name="test_tf_broadcaster" args="0 0 0 0 0 0 1 ee_link test_link 10" />

And set robot_link_command_frame: test_link. This didn't work for me:

ros.moveit_core.transforms: Unable to transform from frame 'test_link' to frame 'world'. Returning identity.

Is that how you intended for it to be used?

Maybe it only works with subframes, not tf frames. That's still a nice improvement.

[udopa]

To test this, I added a static tf publisher to my Servo launch file:

<node pkg="tf" type="static_transform_publisher" name="test_tf_broadcaster" args="0 0 0 0 0 0 1 ee_link test_link 10" />

And set robot_link_command_frame: test_link. This didn't work for me:

ros.moveit_core.transforms: Unable to transform from frame 'test_link' to frame 'world'. Returning identity.

Is that how you intended for it to be used?

Maybe it only works with subframes, not tf frames. That's still a nice improvement.

I tested both scenarios, with and without the call

planning_scene_monitor_->requestPlanningSceneState();

Without it I was not able to send commands in any attached/published frame. With it, it's possible to use a both. -> I would do the call in every iteration (or maybe every n-th iteration?). I guess this is why your test didn't work.

Testing & Intended way of use

I didn't change anything in the servo configuration file at all. This works because in case the frame is not one of the ones defined in the config file, the calculations will be done here L492. This is the config I used:

## MoveIt properties
move_group_name:  welding_endeffector  # Often 'manipulator' or 'arm'
planning_frame: base_link  # The MoveIt planning frame. Often 'base_link' or 'world'

## Other frames
ee_frame_name: wrist_3_link  # The name of the end effector link, used to return the EE pose
robot_link_command_frame:  base  # commands must be given in the frame of a robot link. Usually either the base or end effector

For using the new feature I published a frame using the static tf broadcaster from inside a ros node and then send the commands. I also made sure to first start all servo components and publish the frame afterwards, so that servo should not know the new frame at the beginning. So basically like you did.

[AndyZe]

I'm going to be out for a week. So don't think I'm ignoring this PR- I consider it pretty important.

[gautz]

@AndyZe any further work required for this PR to be merged? Any hint why the CI is failing?

[gautz]

btw would this #2918 possibly simplify / make this PR obsolete? @v4hn

[v4hn]

btw would this #2918 possibly simplify / make this PR obsolete? @v4hn

I don't think these are much related, feature-wise. I think it's a nice idea to support arbitrary frames and that's what this PR seems to aim at. Looking at the patch I'm not convinced locking the scene that often is a good idea at all, but I can't review this properly right now. My new helper function just simplifies transforming poses for attached objects into poses for a link the RobotModel knows about. Possibly, this could be done here as well, but it would be a different approach.

[AndyZe]

@udopa it seems like the MoveIt getFrameTransform() function does not work well with externally-published tf's. I made an issue here for MoveIt1 and somebody already linked me to a patch for MoveIt2.

For me, the easiest path is to bring this functionality you've requested into MoveIt2. But if you want to follow through with this PR, I'll still review it.

I'm kind of curious how you said it was working for you and if I missed something when testing, though.

[codecov]

Codecov Report

Merging #2894 (de70f92) into master (ab42a1d) will decrease coverage by 0.43%.
The diff coverage is 88.24%.

@@            Coverage Diff             @@
##           master    #2894      +/-   ##
==========================================
- Coverage   61.56%   61.14%   -0.42%     
==========================================
  Files         370      370              
  Lines       33147    33159      +12     
==========================================
- Hits        20405    20271     -134     
- Misses      12742    12888     +146     

Impacted Files	Coverage Δ
moveit_ros/moveit_servo/src/servo.cpp	64.20% <85.72%> (+0.98%)	⬆️
moveit_ros/moveit_servo/src/servo_calcs.cpp	38.05% <90.00%> (-25.84%)	⬇️
moveit_ros/moveit_servo/src/low_pass_filter.cpp	63.64% <0.00%> (-31.81%)	⬇️
.../ompl_interface/src/detail/constrained_sampler.cpp	43.91% <0.00%> (-17.07%)	⬇️
...on/pick_place/src/approach_and_translate_stage.cpp	74.06% <0.00%> (-0.63%)	⬇️
...dl_kinematics_plugin/src/kdl_kinematics_plugin.cpp	77.91% <0.00%> (-0.37%)	⬇️
...nning_scene_monitor/src/planning_scene_monitor.cpp	67.25% <0.00%> (+0.50%)	⬆️
...ccupancy_map_monitor/src/occupancy_map_monitor.cpp	30.25% <0.00%> (+0.62%)	⬆️
...meterization/work_space/pose_model_state_space.cpp	81.77% <0.00%> (+1.77%)	⬆️

---

Continue to review full report at Codecov.

Legend - Click here to learn more
Δ = absolute <relative> (impact), ø = not affected, ? = missing data
Powered by Codecov. Last update ab42a1d...de70f92. Read the comment docs.

[udopa]

@AndyZe I did some more testing and realized, that it only works on with my own code. I will try to push a working minimal example for you in the upcoming days.

[AndyZe]

Cool. Feel free to wipe out my commits on your branch, or close this PR and open a new one, if you need to.

[udopa]

The example i have it not yet working because somehow servo_server_sub_pub doesn't get recognized. I will take a look at it shortly :)

[udopa]

@AndyZe I finally realized why it worked for me and didn't for you. Additionally to the published frame with a static_transform_broadcaster i attached an object to the scene. This somehow forces the scene to update all external frames. I guess the "update call" of the psm is somewhere in the add_object function.

You should be able to test my code by running the cpp_frame_testing.launch file. Of course you still need to have a robot cell running (as always). I did my testing with a custom UR10e cell, but it should also work with any cell. Maybe you need to rename the endeffectors a bit. Let me know if you need assistance in getting it to work

[udopa]

@AndyZe Any news on this issue? Do you need some more information? Quick feedback would be good