Ft sensor frame

[ksotebeer]

Proposed solution to #55. Interpret force torque sensor data in the frame specified by header.frame_id from the WrenchStamped message.

[stefanscherzinger]

Thanks @ksotebeer for this contribution!

I've had a closer look and unfortunately, we missed something when discussing this in #55. The calls to setFtSensorReferenceFrame() are important in letting users chose their center of compliance. It's not only about interpreting the force-torque sensor signals correctly (by guessing the right reference frame), but it's also about taking the
offset moments into consideration.

A use case

For instance, when a user has a cordless screwdriver that's mounted somewhere to the robot's end-effector in a weird configuration, they can specify the controller's end-effector to lie exactly in the screwdriver's bit (e.g. with the z-axis aligned). This simplifies the skill to a simple pushing along the specified axis (z-axis in this case). Not transforming the force-torque sensor measurements into that end-effector reference frame introduces these offset moments, and the users need to specify some additional moment in the end-effector frame to move cleanly in contact along the bit axis.

I'm giving it another thought in the next days. We should be able to use some parts of your changes.

[ksotebeer]

I may be missing something, but isn't that handled by the target wrench frame? In your example, the user would user would set the end effector frame at the center of the drill bit like you said, and then the target wrench given by the user would be interpreted in that end effector frame so that's the center of compliance. It's OK if the force torque sensor data is in a different frame because both the sensor wrench and target wrench are transformed into the robot base frame before computing the force error and passing it to the controller.

I believe the offset wrenches you refer to are handled by transforming everything back to the base frame before performing the controller and IK computations. I'm not seeing the need for an extra intermediate frame.

[stefanscherzinger]

@ksotebeer I made a sketch to illustrate the use case.

When the screwdriver's bit is in contact with the screw, the force-torque sensor does not only measure f as applied along the end-effector frame's z-axis, but also r x f as a moment. When performing the controller computations, all wrenches are displayed in (not moved to) the robot base frame's orientation. During control, the applied f will be in equilibrium with the force that the sensor measures, but r x f will not be controlled. As a consequence, the robot will steadily rotate until touching the surface with the sensor frame.

and then the target wrench given by the user would be interpreted in that end effector frame so that's the center of compliance.

It should be the center of compliance, but since the sensor also measures r x f and yields that wrench, the compliance reference frame is perceived as if it stayed in the sensor frame.

It's OK if the force torque sensor data is in a different frame because both the sensor wrench and target wrench are transformed into the robot base frame before computing the force error and passing it to the controller.

It would be ok if both wrenches were moved into the robot base frame (which they are not). They are only displayed (as in rotated) into the robot base frame. We could change this, but users should still be able to set the compliance reference frame to something other than the end-effector frame.

[ksotebeer]

Ahhh it clicked for me now. Thank you for the detailed explanation! Much appreciated!

[ksotebeer]

@stefanscherzinger I pushed some updates to handle the offset moments and control around the center of compliance as discussed while still getting the sensor frame from the Wrench message header. It's the same math as before but the transform computation is done in the sensor callback method the first time a message is received (or in the event the sensor frame changes, which is unlikely in most applications).

I was able to observe the expected behavior in your simulation. With the force controller active, I set a target wrench of 10 N in the x direction (relative to tool0 frame). Then I published fake FTS data of -10 N in the x direction (relative to the sensor_link frame). Due to the offset between sensor_link and tool0, this resulted in a small end effector angular velocity about the y axis. When I ran this test on my previous commit, it resulted in no motion, which proves that the offset wrenches were not originally accounted for as you pointed out.

One thing I'm not quite sure on is if there's a use that I'm not seeing for the compliance_ref_link parameter. As far as I can tell, it accomplishes the same thing for the compliance controller as the end_effector_link parameter does for the force controller.

Let me know what you think and if you're still interested in this approach. If you have a different solution, I won't be offended at all if you want to close this PR. Just been thinking about it so figured I'd give it another go!

[stefanscherzinger]

@ksotebeer

Thanks for your continuous motivation to tackle this issue! It's much appreciated!

I will be busy/absent for roughly two weeks and have a detailed look later.

On a first sight, it looks plausible and we have the previous math back in. Maybe we can even make your test an integration test in this package to cover future adjustments to this mechanism.

What keeps me thinking is the (maybe rare) use case of the Universal Robots driver (branch foxy) that reports the sensor measurements in the robot's base orientation (see also here). I can't check right now what the message header says, but anyhow it's difficult to make use of it. For instance, if it said base_link, we would erroneously introduce offset moments during transformations where we would only have to rotate the measurements. I can't imagine that the measurements really show what a sensor in the robot's base would measure (including offset moments).

---

One thing I'm not quite sure on is if there's a use that I'm not seeing for the compliance_ref_link parameter. As far as I can tell, it accomplishes the same thing for the compliance controller as the end_effector_link parameter does for the force controller.

It allows users to specify where they observe (as in feel) the center of compliance. That accomplishes the same thing as with the cartesian_force_controller, which sets that implicitly to the specified end-effector as discussed before. So, yes, you are right in that point. But this specified end-effector is also the reference frame for the applied target wrenches (for both force and compliance controllers) and the frame for motion tracking in the compliance controller. The distinction between the compliance_ref_link and the end_effector_link in the cartesian_compliance_controller allows users to specify a center of compliance other than the frame with which they want to track tool motion. For instance, an application could benefit from yielding a little in the robot's wrist while drawing a line on a surface with a pen.

[shrutichakraborty]

@ksotebeer I made a sketch to illustrate the use case.

When the screwdriver's bit is in contact with the screw, the force-torque sensor does not only measure f as applied along the end-effector frame's z-axis, but also r x f as a moment. When performing the controller computations, all wrenches are displayed in (not moved to) the robot base frame's orientation. During control, the applied f will be in equilibrium with the force that the sensor measures, but r x f will not be controlled. As a consequence, the robot will steadily rotate until touching the surface with the sensor frame.

and then the target wrench given by the user would be interpreted in that end effector frame so that's the center of compliance.

It should be the center of compliance, but since the sensor also measures r x f and yields that wrench, the compliance reference frame is perceived as if it stayed in the sensor frame.

It's OK if the force torque sensor data is in a different frame because both the sensor wrench and target wrench are transformed into the robot base frame before computing the force error and passing it to the controller.

It would be ok if both wrenches were moved into the robot base frame (which they are not). They are only displayed (as in rotated) into the robot base frame. We could change this, but users should still be able to set the compliance reference frame to something other than the end-effector frame.

Hello @stefanscherzinger, I have a setup as you described in this example. I would like to be able to do force control w.r.t to the tool frame as apposed to the base_link, is this something that is supported?

Thanks!