Force Torque sensor (ForceTorqueSensor.cc) does not take into account sensor pose

[osrf-migration]

Original report (archived issue) by Alessio Rocchi (Bitbucket: arocchi).

---

Force readings from the force torque sensor should be expressed wrt the sensor pose, instead of the child link pose.

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

Just to articulate more on this issue: the SDF format documentation states that a ForceTorque sensor is described in SDF by a <force_torque> tag [1], that has an optional tag. This tag has "parent" as a default value (so it is reasonable to suppose that the other possible value for it is "child").

Apparently this frame tag is not taken into account in the ForceTorqueSensor [2], that simply returns the body2force/torque elements of the JointWrench object returned by GetForceTorque(). From the JointWrench [3] documentation apparently this body2force/torque is expressed in the joint Frame, while in GetForceTorque() documentation [4] it appears that it is the ForceTorque applied by the body2 on body1 expressed in the child Frame. It is also not clear to me the point with respect to which the torque is expressed, but I assume that this point is the joint origin.

I can commit time to fix this issue (and prepare a regression test) as soon as the semantics of the <force_torque> tag (and relative ) and of GetForceTorque() method are clarified.

[1] : http://gazebosim.org/sdf/dev.html#force_torque354

[2] : https://github.com/osrf/gazebo/blob/master/gazebo/sensors/ForceTorqueSensor.cc

[3] : http://gazebosim.org/api/dev/classgazebo_1_1physics_1_1JointWrench.html

[4] : http://gazebosim.org/api/dev/classgazebo_1_1physics_1_1Joint.html#a924807d32ef42bd741911ff33e7f3d53

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

Indeed, the <frame> tag is not used.

The <force_torque> is used to instantiate a ForceTorqueSensor. The <frame> tag should indicate the frame in which the force is reported. The frame should the name of a link. If blank, the frame should be the link that contains the sensor.

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

Thanks for the reply.

Given that the ForceTorqueSensor belongs to a Joint, I guess that by "link that contains the sensor" you mean the parent link of the joint that contains the sensor. This would be coherent with the current <frame> SDF documentation, reading "Default: parent" as "Default: the name of the parent link".

To completely define a real 6-axis Force Torque sensor, assuming that a link frame is appropriately placed to match the sensor orientation, two additional information are needed:

• The point with respect to which the torque is expressed. I assume that this point can be hardcoded to be the joint origin, considering how real sensor works and that they can be simulated in Gazebo with a "fake" fixed joint (a revolute joint with zero max and min limits).

• The direction of the wrench, i.e. if it is the one applied by the first body on the second body or the one applied by the second body on the first body. To ensure complete flexibility this would be ideally defined by an additional SDF tag, but I guess that this would require a modification on the SDF format and so it would complicate things.

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

Thanks for the correction, the ForceTorque sensor is a child of Joint, and frame should be the frame (child or parent) in which the wrench values are reported.

You are also correct about the point about which the torque is expressed. The joint origin is the assumed point.

Your last bullet point would require another SDF tag. It's easy to add a tag. Do you have a suggestion for a name?

To recap, we have:

1. <force_torque> : Creates a force-torque sensor located at the origin of the joint to which the sensor is attached.

2. <frame> : The frame in which the wrench values should be reported. Currently we are sending back values in the child-link frame.

3. <applied_link> : This is my placeholder for the new SDF tag which specifies which link the wrench values are referring to.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

To me, it makes the most sense to use the <sensor><pose> tag as an offset and rotation relative to the joint frame (which is relative to the child link). That wouldn't require any custom tags, though it's less flexible. By default it could return the values applied to to the child.

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

Could you explain the use of <sensor><pose> in this context more. We need a way to specify which link the wrench value refers to and in what frame they should be reported.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

Typically the <sensor><pose> is interpreted as a pose offset from the parent, which in this case is the joint. Here's a summary of the hierarchy of pose offsets to define this frame:

<model ...><pose>...</pose>
  <link name='child'><pose>...</pose>
  <joint ...><pose>...</pose>
    <sensor type='force_torque'><pose>…</pose>
model -> child link -> joint -> sensor

As an initial implementation, I would say we should express the wrench on the child link and in a frame offset from the joint frame.

I think that's the simplest and most consistent approach that doesn't involve any new parameters. I would try implementing that first and see if it's sufficient, and add new parameters if there's a compelling reason to do so.

[osrf-migration]

Original comment by John Hsu (Bitbucket: hsu, GitHub: hsu).

---

I think I like the <pose> tag suggested by @scpeters. Normally, a strain gauge is mounted with some pose offset from actual joint location, which should map well to this notation. As @nkoenig mentioned, we can then offer some options (or specify them in our documentation):

• <applied_link>: Which link is the force torque sensor physically mounted on? (parent or child link of the joint?)
• specify what sign convention to use. We could simply mention this in doxygen (e.g. if compression force is measured along the x-axis of the sensor frame, is the x-force component sensor readout positive or negative?).

<model ...><pose>...</pose>
  <link name='child'><pose>...</pose>
  <joint ...><pose>...</pose>
    <sensor type='force_torque'><pose>…</pose><attached_link>[parent|child]</attached_link>
model -> child link -> joint -> sensor

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

I'll try to argue why having a parameter describing the direction of the force/torque is really handy in real use cases.

The use case I was thinking of is not the simulation of Joint Torque sensors (i.e. strain gauges mounted on a link near to a joint), but simulation of 6-axis Force/Torque (FT) sensors mounted in the middle of a link.

Consider for example the case of this FT sensor mounted in the middle of the end/effector link of an industrial robot (a typical setup for force control):

To properly simulate this setup, we have to describe the 6-axis F/T sensor as a fixed joint (i.e. in gazebo a revolute joint with 0 max and min limits). The direction of the measured force/torque depends on internal F/T sensor configuration and on how the F/T sensor is mounted on the robot, but it is safe to assume that this cannot be changed on the real robot (for example for backward compatibility with existing software).

If is not possible to specify Force/Torque direction in the SDF sensor description, it is then necessary to account for this real/simulation mismatch in the middleware interface (if any) or user software. This is not convenient because it is then impossible to test exactly the same software in simulation and on the real robot.

Furthermore this will make the SDF an incomplete description of the robot, that requires additional information to properly describe the real world robot.

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

@scpeters Your suggestion would only move the frame of the force-torque sensor, it would not specify the applied_link which is necessary for a complete description.

@traversaro , can you suggest SDF tags that you'd like to add or remove?

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

I suggest to name the additional child tag <direction> instead of <applied_link>.

A more verbose alternative could be <measure_direction>.

Its value must be one of the following:

parent_to_child if the measured force torque is the one applied by parent on the child ,

child_to_parent if the measured force torque is the one applied by the child on the parent .

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

@traversaro Thanks for the detailed explanation. I had been thinking that you could switch the parent/child relationship of the joint to change the directionality of the sensor, but that's not a good idea now that I think about it.

I like parent_to_child and child_to_parent.

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

@scpeters yes, requiring to invert the joint to change the direction of the sensor would be quite tedious to do if you import the model from urdf (that is a requirement for us because our inertial parameter identification software outputs its results in a urdf model).

I did a first try on solving this issue in my branch issue_940: https://bitbucket.org/traversaro/gazebo/commits/branch/issue_940 (measure_direction support is commented out because it need a change in sdformat).

I prepared a test under test/regression/940_force_torque_sensor_frame that checks the returned values for various f/t sensors placed between two (fixed) stacked cylinders (and also between the cylinders and the torques as an additional test), and compare the obtained measure against analytically obtained values.
The only physics engine that consistently pass the test is ode. All the other engines have issues:

• bullet normally passes the test, but randomly it measures some crazy torque when it is expected to measure zero torque (this behavior is apparently independent from the number of simulation steps I do before checking the measure). Could be related to https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/issues/619 (implement implicit stiffness and damping in bullet #619) ?
• simbody measure is simply broken.
• dart report the correct measure, but It fails because it expresses some of the wrenches returned by method GetForceTorque in the wrong frame.

The output of a test run when bullet fails : http://pastebin.com/raw.php?i=dZNZT2Fs

The output of a test run when bullet succeeds : http://pastebin.com/raw.php?i=bNvpy14s

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

I have created an initial couple of PRs both for sdformat and gazebo.
There are still a couple of issues:

• The test fails for bullet, simbody and dart. This is related to a bug in GetForceTorque() implementation in this physics engines, so I guess I can open different issues for tracking this different problems. In the meanwhile I can disable tests for this physics engines.
• The sensor pose tag is completely ignored, and this may be counterintuitive compared with the behavior of other sensors. A possible solution could be to add a sensor option to the frame tag, but in this case only the rotational part of the pose tag would be considered.

[osrf-migration]

Original comment by Steve Peters (Bitbucket: Steven Peters, GitHub: scpeters).

---

There seems to be a race condition in bullet that causes it to pass / fail different parts of the test randomly. The simbody and dart failures appear consistent, probably related to incorrect reference frames.

I think we should use the sensor pose tag as an offset from whichever frame is specified in the frame tag. Actually, instead of having an option to use the joint frame, it could be called the sensor frame, and use the joint frame plus the sensor pose offset. I can write a patch for what this suggestion would look like.

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

In this case we have to update also the semantics of pose tag for a sensor:
https://bitbucket.org/osrf/sdformat/src/049841be931a197717837930295102d33f8896a0/sdf/1.5/sensor.sdf?at=default#cl-26

Apparently it is considering only link sensor, not joint sensors as the force_torque.

We have to mention that for joint sensors the sensor pose is relative to the joint frame, not to the parent link frame.

[osrf-migration]

Original comment by Silvio Traversaro (Bitbucket: traversaro).

---

Given that the two pull requests (https://bitbucket.org/osrf/sdformat/pull-request/98/add-new-tag-measure_direction-to/diff and https://osrf-migration.github.io/gazebo-gh-pages/#!/osrf/gazebo/pull-request/1076/add-proper-flexibility-to-force_torque) to solve this issue have been merged I guess we can close this issue.

In the next days I will open new issues to track the bugs in bullet, dart and simbody GetForceTorque() implementation.

I forgot I was not the original author of the issue. @arocchi the issue was solved, can you close it?

[osrf-migration]

Original comment by Alessio Rocchi (Bitbucket: arocchi).

---

• changed state from "new" to "resolved"

[osrf-migration]

Original comment by Nate Koenig (Bitbucket: Nathan Koenig).

---

• changed state from "resolved" to "closed"