Create new messages with all fields needed to define a velocity and transform it

[destogl]

This PR proposes a new message type for a full description of velocities, unambiguous definition, and the possibility to transform velocities using TF (this is currently commented out in geometry2 library).

As found in the literature and comments across the geometry2 library, geometry_msgs/TwistStamped is insufficient information for the complete representation.

The new fields should be sufficiently described in the message. If not clear, please provide feedback for improvement.

@tfoote @SteveMacenski @azeey looking forward to your comments.

References:

• See commit ea03b20802e4eff5ae339f6a9042ceef51dd6665 in this repository from 24th Apr. 2010
• https://core.ac.uk/download/pdf/154240607.pdf
• http://hades.mech.northwestern.edu/images/7/7f/MR.pdf Eq'n 3.83

[SteveMacenski]

I think you can remove this particular line

[destogl]

I am with you on this, my only concern is not to confuse people that are moving from Twist as used currently in Nav2 and ros2_control.

[destogl]

Suggested change

	# which is the usual use-case in mobile robotics.
	# as common in mobile robotics.

[azeey]

I'm a little confused. if body_frame_id and header.frame_id are identical, shouldn't the velocity be always zero? i.e, we're asking the velocity of a body relative to itself. On the other hand, we could have body_frame_id and observation_pose (via its frame_id) be identical and header.frame_id be something different from the two. This makes sense to me since it's asking the velocity of the body relative to whatever header.frame_id is but expressed in the body frame.

[destogl]

@tfoote any comments on this?

[tfoote]

The changes look good. I'd like to flesh out the documentation more completely so that people in the future clearly understand the design. As well as confirming that we can confirm that the transform velocity operations have enough semantic information to be restored.

[destogl]

Great, feel free to add any comments. It would be great if we can merge this sometime in February, so we can adjust controllers already for Jazzy.

[audrow]

@tfoote and @destogl, it sounds like this is waiting on documentation. What's the status of this? Also, any thoughts on what exactly should be done before this is ready to go in?

[ahcorde]

I will be working int this feature ros2/geometry2#643 but this PR requires some changes to be operational

• Modify name geometry_msgs/msg/VelocityStamped to geometry_msgs/msg/VelocityStamped.msg
• Include this new message in the CMakeLists.txt

@destogl do you think you can include these changes?

[tfoote]

I think that this is very close, but before merging I'd like to make sure that we have both documentation but also the example implementation that validates that we have actually covered all our bases.

[ahcorde]

I added this PR StoglRobotics-forks#1 to include the required changes

[destogl]

@tfoote what do you mean about documentation? Should be adapted this in the message “header” or do you have something else in mind?

[tfoote]

The names right now are not referencing a standard of notation and talking to others, they have made different assumptions about what each field means. This is coming to a head as we're trying to implement the usage in geometry2. I'm going to try to make a descriptive doc we can use as a baseline. But this message needs to be documented in the comments enough that someone can use that self documenting feature to interpret the data unambiguously.

[destogl]

OK, can you make a proposal?

[ahcorde]

Ey @destogl, @azeey and @SteveMacenski ,

@tfoote added this documentation to detail things ros2/ros2_documentation#4235

Feel free to add any comments

[tfoote]

@ahcorde Thanks for putting it into a PR from my draft.

Having created that writeup I'd like to propose that we change how we're using/naming the different coordinates. First of all I believe that the best analogy to the header.frame_id for other datatypes is the observational frame. This is the frame in which the value of the velocity is being represented. If you transform this representation to a new frame it does not change the meaning of the velocity (the first derivative of position between two entities). And just like other data types they don't loose information about the data when transformed.

So this leaves that we need to be able to express the two entities between which the velocity is being computed. There's object A which we'll take as the moving object $X_A$ and the reference entity $X_B$ such that $V_{A->B} = d/{d t} ( X_A - X_B)$ and all of these values are expressed in the header.frame_id frame.

I would like to suggest that we consider thus just having two frame_ids inside the message which indicate the entities A and B.

With another round of research I think that the standards in academia for these frames are the body frame and the space frame.

• https://modernrobotics.northwestern.edu/nu-gm-book-resource/3-3-2-twists-part-1-of-2/#department
• https://mecharithm.com/learning/lesson/screws-a-geometric-description-of-twists-in-robotics-9
• https://www2.ece.ohio-state.edu/~zhang/RoboticsClass/docs/LN5_RigidBodyVelocity_a.pdf

And our message will represent a body twist if the header.frame_id == body_frame_id and we'll match the spacial twist if header.frame_id == space_frame_id.

We don't actually need the full PoseStamped to represent any fixed offsets in the position as they will effectively drop out when the derivative in time happens.

[tfoote]

Here's my concrete suggestions for changes to follow those external conventions. I think that following those as closely as possible will hopefully decrease the amount of confusion in the long run.

[tfoote]

Suggested change

	PoseStamped observation_pose
	string space_frame_id

[tfoote]

Suggested change

	PoseStamped observation_pose
	string reference_frame_id

Changing this to reference as this is the frame of reference to which the velocity is being measured.

[tfoote]

Suggested change

	# This expresses the timestamped velocity of a frame 'body_frame_id' in the reference frame 'header.frame_id' expressed from arbitrary observation pose 'observation_pose'.
	# This expresses the velocity of a frame 'body_frame_id' in the space frame 'space_frame_id' expressed from arbitrary observation pose 'header.frame_id' at time `header.stamp`.

[tfoote]

Suggested change

	# This expresses the timestamped velocity of a frame 'body_frame_id' in the reference frame 'header.frame_id' expressed from arbitrary observation pose 'observation_pose'.
	# This expresses the timestamped velocity vector of a frame 'body_frame_id' in the reference frame 'reference_frame_id' transformed to be expressed from arbitrary observation frame 'header.frame_id'.

[tfoote]

Suggested change

	# which is the usual use-case in mobile robotics.
	# which is the usual use-case in mobile robotics and is also known as a body twist.

[tfoote]

Suggested change

	# - 'body_frame_id' and 'header.frame_id' are identical, i.e., the velocity is observed and defined in the local coordinates system of the body
	# - If the 'body_frame_id' and 'header.frame_id' are identical, i.e., the velocity is observed and defined in the local coordinates system of the body

[tfoote]

Suggested change

	# - observation pose is not defined, i.e., it is assumed that the observation pose is the origin of the 'header.frame_id';
	# - If the 'space_frame_id' and 'header.frame_id' are identical, i.e., the velocity is observed and defined in the local coordinates system of the spacial frame.

[tfoote]

Suggested change

	# - if the observation pose is an established frame, you may set its frame_id only in the pose (since the relative pose to that frame is Identity);
	# This is also known as a spacial twist.

[tfoote]

Thanks to @azeey for following up and flagging that this isn't actually spacial twist. Using the reference https://www.cse.lehigh.edu/~trink/Courses/RoboticsII/reading/murray-li-sastry-94-complete.pdf Pages 54-55
"A Mathematical Introduction to
Robotic Manipulation
Richard M. Murray
California Institute of Technology
Zexiang Li
Hong Kong University of Science and Technology
S. Shankar Sastry
University of California, Berkeley
"

So I will update my suggestions based on that.

When the frame in which the twist is represented is the same as the reference frame then what we're representing the body twist offset to that frame and not changing the math to be a spacial twist.

And to that end we're simply transforming the body twist to the new coordinate frame.

So to pull this all back together we will represent a body twist between body_frame_id and reference_frame_id and then transform it to be represented in the observational frame identified by header.frame_id I'll update my changes accordingly.

[tfoote]

Suggested change

	# This expresses the timestamped velocity of a frame 'body_frame_id' in the reference frame 'header.frame_id' expressed from arbitrary observation pose 'observation_pose'.
	# This expresses the timestamped velocity vector of a frame 'body_frame_id' in the reference frame 'reference_frame_id' transformed to be expressed from arbitrary observation frame 'header.frame_id'.

[tfoote]

Suggested change

# The message is often used in simplified versions where:

With the removal of the PoseStamped and only using the frame_ids this optional version is no longer necessary.

[tfoote]

Suggested change

# - observation pose is not defined, i.e., it is assumed that the observation pose is the origin of the 'header.frame_id';

Also removed

[tfoote]

Suggested change

# - if the observation pose is an established frame, you may set its frame_id only in the pose (since the relative pose to that frame is Identity);

Also removed

[ahcorde]

I created a new PR #240 to include the suggestions that Tully made

[tfoote]

#240 had been merged closing this one. 5.3.3 is available in rolling testing now with these changes ros/rosdistro#40550