[Servo] Fix twist transformation

[ibrahiminfinite]

Description

Fixes #2150

This was partly addressed in #2224, but the input ordering of the twist command to the tf2::doTransform was wrong.
Since Servo used (linear, angular) and tf2::doTransform uses (angular, linear).

---

Assumptions about the frames used in the context of twist commands:

• The frame specified by the parameter planning_frame is always stationary.
• The command maybe given in any stationary frame or the end effector frame.
• Any other moving frames other than ee frame will also still transform, but not sure if the resulting velocities will be accurate.

[codecov]

Codecov Report

Patch coverage: 10.00% and project coverage change: -0.01% ⚠️

Comparison is base (a2f0183) 50.71% compared to head (ebff3d3) 50.69%.

Additional details and impacted files

@@            Coverage Diff             @@
##             main    #2301      +/-   ##
==========================================
- Coverage   50.71%   50.69%   -0.01%     
==========================================
  Files         386      386              
  Lines       31914    31920       +6     
==========================================
- Hits        16182    16179       -3     
- Misses      15732    15741       +9     

Files Changed	Coverage Δ
moveit_ros/moveit_servo/src/servo.cpp	63.35% <10.00%> (-2.77%)	⬇️

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

[sea-bass]

I'll admit, I don't fully understand this logic and new parameter, so I've taken a look and commented more on the code side of things rather than the math.

Regardless, this will need some detailed documentation to make sense of it for users, because if I can't figure it out and I've been looking at servo for a few months, I'm not confident that the average user will be able to figure out easily either.

[ibrahiminfinite]

Regardless, this will need some detailed documentation to make sense of it for users, because if I can't figure it out and I've been looking at servo for a few months, I'm not confident that the average user will be able to figure out easily either.

Yeah, will be updating the Servo tutorial page with a good explanation. Did not want to add it inline.

[henningkayser]

I don't understand why we would need this option at all. The PlanningSceneMonitor listens to and keeps track of all transforms (has its own transform buffer), and should probably be used instead of lookupTransform(). It also shouldn't make a difference if a frame is stationary or not just in order to transform it to the planning frame.

[ibrahiminfinite]

I don't understand why we would need this option at all. The PlanningSceneMonitor listens to and keeps track of all transforms (has its own transform buffer), and should probably be used instead of lookupTransform(). It also shouldn't make a difference if a frame is stationary or not just in order to transform it to the planning frame.

There is no real difference in the computation for when the command is in moving frame.
However the frame_id we specify in the command is the reference frame, if I am not mistaken.

So when the twist command given is expressed in the moving frame basis , we cannot use the moving frame as the frame_id since velocity of the moving frame in the moving frame would be zero.

Does that make sense?

[AndyZe]

@henningkayser the basic idea is, if there's a lever arm between two poses, then an angular velocity will also cause a translational velocity. You can read all about it in #2150

[ibrahiminfinite]

Not sure why the "HaltForSingularity" unit test fails on Humble.
The changes in this PR are not related that function.

[AndyZe]

Let's have a video chat. I feel like there is an XY communication issue going on here.

[ibrahiminfinite]

Removed the complexity in favour of documentation.

[AndyZe]

Code looks good, i'll approve after testing it.

[sea-bass]

Following @AndyZe 's need to test, I decided to test this against MoveIt Studio and found a bug...

I set the twist commands to be in the end effector frame, along a single axis, but they are leading to combined translation and rotation.

2023-08-16.16-43-04.mp4

Looking at the tf2::doTransform code here (https://docs.ros2.org/foxy/api/tf2_geometry_msgs/namespacetf2.html), how does this work with an Eigen vector? Do you mind linking me to the code?

[sea-bass]

Behold, I found a PR by @AndyZe which says this, in fact, is translation first and rotation later!

https://github.com/ros2/geometry2/pull/406/files

[AndyZe]

That helps explain my confusion. lol

[sea-bass]

I guess the unit/integration tests still pass because we're passing all the commands in the planning frame itself, so the transform is a no-op.

Would suggest making some tests that use alternative frames, e.g., the end effector, in the upcoming work we discussed offline

[sea-bass]

BTW reverted to main and this issue goes away. The frame conversions work correctly. So I actually think we should just close this PR and move on to the next thing.

[ibrahiminfinite]

LOL, This is getting quite confusing.

Behold, I found a PR by @AndyZe which says this, in fact, is translation first and rotation later!

https://github.com/ros2/geometry2/pull/406/files

I believe the documentation there is wrong.

This is the equation being used in that tf2::doTransform() method, based on this ,the angular velocities must come first.

I set the twist commands to be in the end effector frame, along a single axis, but they are leading to combined translation and rotation.

I think it might be because the ee_frame parameter was still pointing to panda_link8 instead of panda_hand. The panda_link8 frame has a rotation relative to panda_hand. This change was originally in the PR but got reverted when I reset.

This is panda_hand

And this is panda_link8

Also turns out, the pose resulting from the application of the twist is computed incorrectly here: https://github.com/ros-planning/moveit2/blob/a2f0183297284ed43ef5d435b4e11b414ec21876/moveit_ros/moveit_servo/src/utils/command.cpp#L236

The ik_solver->getTipFrame() points to panda_link8 while the the pose we are interested in is of panda_hand.
I think this needs to be replaced with tf2 transforms as well rather than what is available in RobotState.

EDIT : Not sure about the above since the IK solver wouldn't know how to resolve the pose.
Is there a way we can use panda_hand as the IK tip ?

EDIT 2:

I think it might be because the ee_frame parameter was still pointing to panda_link8 instead of panda_hand. The panda_link8 frame has a rotation relative to panda_hand. This change was originally in the PR but got reverted when I reset.

So basically the command was being given in a moving frame that is not the end effector !!
This is what we talked about offline.

[sea-bass]

Hmm, now I too am confused.

According to the code (not the comments) @ibrahiminfinite is right in that the rotation block seems to come first.

Also, remember that in my example I am using a UR5e robot, not a Panda, so the actual frames used in the Panda servo config do not apply. But you're right -- this was a case where a moving frame was being used as the frame_id.

But then, in MoveIt Studio I get the correct results before this PR, so something else might be up. It might boil down to expectations:

• Wrong equations used in Servo twist transformations #2150 explicitly mentions that if there is a lever, applying a rotation twist will lead to translation as well. This, in fact, is what I'm seeing with this PR, so in that sense it might be correct
• However, in my case I actually want the twist to be applied in the end effector frame without translating the end effector. In order words, I want the rotation-only part, but not the translation, if I'm commanding a rotation-only twist.

How do we reconcile that some users (like myself) want the rotation-only part of the twist, whereas others (like those who reported #2150) want the actual translation+rotation portion of twisting the EE frame w.r.t. the base frame?

EDIT: I guess this was the original stationary frame parameter that was in the first draft of this PR, wasn't it? The thing that differentiates whether we use equation 3.83 or 3.84? I may finally understand this, but have perhaps taken us on a big circle...

[ibrahiminfinite]

• However, in my case I actually want the twist to be applied in the end effector frame without translating the end effector. In order words, I want the rotation-only part, but not the translation, if I'm commanding a rotation-only twist.

moveit/moveit#2891 This was already reported earlier.

[AndyZe]

I think we might be getting back to the original idea of not making any changes in code. Provide the transformation options in a header file. Point to the header file from the tutorial and an advanced user can do as they please.