search

ros-industrial / ur_modern_driver

(deprecated) ROS 1 driver for CB1 and CB2 controllers with UR5 or UR10 robots from Universal Robots
Apache License 2.0
302 stars 340 forks source link

No frame_id in the wrench message #318

Closed davizinho5 closed 5 years ago

davizinho5 commented 5 years ago

I'm wondering why the frame_id is not set when publishing the wrench message into ROS.

I assume that, as happens in polyscope or URScript, both the force and torque readings are defined w.r.t. the base frame, is this correct?

If yes, why not setting the frame_id to the base_link?

Also if yes, in case I tranform the wrench to the TCP frame, which one should it be in the TF tree? In other words, what is the difference between the tool0 and ee_link? What is the phisical meaning for the orientaiton difference?

gavanderhoorn commented 5 years ago

I'm wondering why the frame_id is not set when publishing the wrench message into ROS.

Probably just an oversight, as even ur_driver didn't do that. Note how the code here is virtually identical to:

https://github.com/ros-industrial/ur_modern_driver/blob/c6637030695fff154d387882761dc3024977a55e/src/ros/rt_publisher.cpp#L38-L48

I assume that, as happens in polyscope or URScript, both the force and torque readings are defined w.r.t. the base frame, is this correct?

yes, the base frame. We're just converting the data coming in over the "realtime interface", so whatever frame is used there is the frame the Wrench is relative to.

If yes, why not setting the frame_id to the base_link?

because base_link is not coincident with base. That would be base. See here.

Also if yes, in case I tranform the wrench to the TCP frame, which one should it be in the TF tree? In other words, what is the difference between the tool0 and ee_link? What is the phisical meaning for the orientaiton difference?

What would you want to transform the wrench to?

tool0 corresponds to "an all-zeros toolframe" (ie: similar to the flange on a UR).

ee_link is a relic kept for bw-compatibility. It does not correspond to anything in the real robot (perhaps an internal axis).

Note that base_link is seldomly coincident with the origin of the "world" or "base" frame of industrial robots. For that reason base was introduced. It's not an official REP (yet), but see Coordinate Frames for Serial Industrial Manipulators for some explanation.

If you could submit a PR against kinetic-devel that fixes this that would be much appreciated.

gavanderhoorn commented 5 years ago

I'm not sure whether things on the ros_control side are correct either:

https://github.com/ros-industrial/ur_modern_driver/blob/c6637030695fff154d387882761dc3024977a55e/src/ros/hardware_interface.cpp#L41

That seems to specify the tcp_link as the frame_id.

@miguelprada: any input?

gavanderhoorn commented 5 years ago

Relates issue in ur_driver: ros-industrial/universal_robot#80.

davizinho5 commented 5 years ago

Relates issue in ur_driver: ros-industrial/universal_robot#80.

Ok, I've taken a look at the issue and I see there was a plan to solve the issue, but for some reason it wasn't, although I cannot figure out the reason reading the conversation.

because base_link is not coincident with base. That would be base. See here.

Ok, I understand that the solution would be to set the wrench frame_id to "base". However, I'll wait to hear if there is any issue from the control side.

What would you want to transform the wrench to?

Sorry, I explained myself very badly. Here, there is an explanation on how to tranform the force/torque sensor data into the TCP frame using URScript. When there is no tool attached to the flange of the arm, the TCP frame is actually the flange frame, and that is the frame I was asking for.

However, in my opinion, the force/torque sensor data should be transformed into the sensor frame, because it is where the actual physicall measurement is being done. Actually, it is quite intriguing why Universal Robots converts the sensor measurements to the base. But I cannot find any information in the UR datasheets on sensor position, and there is not such a frame in the TFtree of the UR arms, so at least the flange frame is a good one (close enough).

tool0 corresponds to "an all-zeros toolframe" (ie: similar to the flange on a UR).

ee_link is a relic kept for bw-compatibility. It does not correspond to anything in the real robot (perhaps an internal axis).

Thanks for the explanation. In my opinion it would be nice do add this information at least as comments in the xacro file because it is a bit confusing.

gavanderhoorn commented 5 years ago

Ok, I've taken a look at the issue and I see there was a plan to solve the issue, but for some reason it wasn't, although I cannot figure out the reason reading the conversation.

it was closed without resolution as the issue was reported against ur_driver, which is the old driver and will not be updated any more.

because base_link is not coincident with base. That would be base. See here.

Ok, I understand that the solution would be to set the wrench frame_id to "base". However, I'll wait to hear if there is any issue from the control side.

I believe the ros_control hardware_interface should also use the base frame instead of the tcp one. It uses the same incoming data.

What would you want to transform the wrench to?

Sorry, I explained myself very badly. Here, there is an explanation on how to tranform the force/torque sensor data into the TCP frame using URScript. When there is no tool attached to the flange of the arm, the TCP frame is actually the flange frame, and that is the frame I was asking for.

However, in my opinion, the force/torque sensor data should be transformed into the sensor frame, because it is where the actual physicall measurement is being done. Actually, it is quite intriguing why Universal Robots converts the sensor measurements to the base.

the job of the driver is to make the data available in a ROS compatible way. But we ideally do not change the data, other than to convert it into ROS-specific representations.

If a consumer of that data then wishes to transform it (in the mathematical sense) for some reason, that would be fine, but it's not a responsibility of the driver.

But I cannot find any information in the UR datasheets on sensor position, and there is not such a frame in the TFtree of the UR arms, so at least the flange frame is a good one (close enough).

Afaik, for non e-series robots, there is no sensor. It's all back-calculated from motor currents. For e-series robots there is a sensor, and the location should be available somewhere. Perhaps you could email UR support to get that information, and then we can embed it into the models.

tool0 corresponds to "an all-zeros toolframe" (ie: similar to the flange on a UR). ee_link is a relic kept for bw-compatibility. It does not correspond to anything in the real robot (perhaps an internal axis).

Thanks for the explanation. In my opinion it would be nice do add this information at least as comments in the xacro file because it is a bit confusing.

things are in a state of flux.

If/when that candidate REP is accepted (it hasn't been submitted yet), we could just claim compliance with the REP and that would be it.

miguelprada commented 5 years ago

First of all, there are two different "frames" that need to be considered while talking about wrenches:

This is somewhat analogue to what happens with twists, which is very well explained in this paper.

I've been doing some tests with a UR5 and I think its behavior is:

The WrenchStamped message type in ROS is not capable of representing all the information required to properly interpret the semantics of the included values. In general, and considering how frame_id is used elsewhere, I would argue for using that field to express the coordinate frame in which the numerical values are expressed. That makes the fact that the reference point is the origin of whichever currently configured TCP implicit.

Then, if any user wants to transform the wrench to a different reference point or express it in a different coordinate frame, any half-decent library to work with geometric primitives (e.g. KDL) should easily allow changing the reference point and/or frame at will.

TL;DR; I think we should do wrench_msg.header.frame_id = prefix + "base", and the same for the ros_control FT sensor handle.

gavanderhoorn commented 5 years ago

Thanks for your analysis @miguelprada. This what was I had understood as well.

@davizinho5: I've tagged this issue with the wrid19 label as it seems like a good issue to work on during World ROS-Industrial Day 2019.

gavanderhoorn commented 5 years ago

324 should take care of this.

davizinho5 commented 5 years ago

Thanks for taking a look at this.

And, @miguelprada , thanks for the explanation, I was misunderstanding the info sent in the /wrench topic, I though it was the actual sensor values.

gavanderhoorn commented 5 years ago

Re-opening as we'll close it when the PR gets merged.

gavanderhoorn commented 5 years ago

@davizinho5 wrote:

I though it was the actual sensor values.

I'm slightly confused by this, as I also have this impression but you now seem to suggest that is not the case.

@miguelprada: any thoughts?

miguelprada commented 5 years ago

I don't think any sensor values are being directly output here.

In the CB-series, that's straightforward, since there's no FT sensor at all, and the wrench is an estimation based on the required current in the motors.

Since a transformation to the currently configured TCP is also happening under the hood, even in the e-series, the values are not necessarily the actual sensor values either.

gavanderhoorn commented 5 years ago

Ah, wait. Then I misunderstood @davizinho5's comment.

davizinho5 commented 5 years ago

@gavanderhoorn

Re-opening as we'll close it when the PR gets merged.

Sorry, I though it had been accepted.

I'm slightly confused by this, as I also have this impression but you now seem to suggest that is not the case.

All my messages assumed that UR gave the actual values given by the force/torque sensor in the wrist of the URe-series. This is because I tend to forget that there exist a previous CB-series which does not include a F/T sensor.

However, what actually happens is that UR uses the data from the F/T sensor, joint values, weight and center of mass of your tool and maybe some other information (like accelerometer in the wrist or current of the motors) to estimate the /wrench that the arm is applying at the TCP.

miguelprada commented 5 years ago

However, what actually happens is that UR uses the data from the F/T sensor, joint values, weight and center of mass of your tool and maybe some other information (like accelerometer in the wrist or current of the motors) to estimate the /wrench that the arm is applying at the TCP.

Correct, the weight and center of mass of the configured tool is filtered out as well.

fmauch commented 5 years ago

Sorry to roll that up again, but this was running around in my mind last night: Wouldn't it be sane to publish the wrench at tool0_controller? As far as I understand it, the wrench published by the robot reflects the FTS measurements (or estimations) at the configured tcp, rotated to the axes of the base frame.

So rotating it back to the tool frame orientation and publishing it in the tool frame should be correct. If the tcp is configured on the TP, tool0_controller follows that configuration.

At least that's the way I set it up in the new driver

davizinho5 commented 5 years ago

I completely agree with you.

However, and I'm just quoting @gavanderhoorn from some comments above:

the job of the driver is to make the data available in a ROS compatible way. But we ideally do not change the data, other than to convert it into ROS-specific representations.

If a consumer of that data then wishes to transform it (in the mathematical sense) for some reason, that would be fine, but it's not a responsibility of the driver.

miguelprada commented 5 years ago

I don't think there's anything incorrect in what you propose, but I'm with @gavanderhoorn on this one. If the robot provides the information expressed in the robot base frame coordinates, the driver should only make that information available to the user.

I don't see why we should assume the end-user will want the data expressed in the tool cordinate frame.

And the same goes for the new driver. I would vote for getting rid of that transformation.

fmauch commented 5 years ago

Thanks for clarifying this. I'll put that part to the documentation then.

gavanderhoorn commented 5 years ago

I like this discussion, and wanted to add just one comment: my main rationale for not doing what @fmauch suggests (ie: transform wrench back to tcp) is that it would be different from what the controller reports.

There could be reasons for doing such a thing, but they'd have to be good, as diverging from whatever conventions and/or standards the controller uses will also mean that we'd invalidate any experience and intuition a user has.

That means having to document it ourselves, instead of being able to just say "it's exactly as the robot controller reports it".

Edit: if we'd really want to, we could be nice to users and make this all configurable. The "base frame" is already configurable, so we'd have to add the transform to the driver.

fmauch commented 5 years ago

Edit: if we'd really want to, we could be nice to users and make this all configurable. The "base frame" is already configurable, so we'd have to add the transform to the driver.

I'd like that.

gavanderhoorn commented 5 years ago

Closing this as #324 has addressed the immediate problem reported by @davizinho5.

@fmauch: it would be great if you could open an issue to track the enhancement of making the transform to (a) tcp frame configurable.