Gazebo | Ignition | Community
Ask Your Question

DRCSIM: simulated force measurements in wrong coordinates

asked 2013-02-25 22:55:31 -0500

cga gravatar image

updated 2013-02-25 22:57:07 -0500

If I am not mistaken, the current simulated force torque measurements are in the inertial frame (world coordinates), referenced to the center of mass of the foot.

For the DRC robot, the simulated measured forces and torques should be represented in the foot link frame (body coordinates), not in the inertial frame (world coordinates). This is how real force torque sensors work. They move with the foot, and have no idea which way is up, or North (no orientation estimate).

If you were really anal about simulating the force torque sensor, you would reference the simulated force measurements to some point (the center?) on the bottom of the foot.

Padawan Chris

edit retag flag offensive close merge delete

2 Answers

Sort by ยป oldest newest most voted

answered 2013-03-19 18:29:30 -0500

hsu gravatar image

updated 2013-03-20 00:56:03 -0500

Hi Patrick, thanks for pointing this out. I updated your ticket with frame references.

Also, the high frequency noise can be filtered out with very crude low pass filtering experiment, for example,

      static double e = 0.99;
      physics::JointWrench wrench = this->lWristJoint->GetForceTorque(i0);
      forceTorqueSensorsMsg->l_hand.force.x  =
        e * forceTorqueSensorsMsg->l_hand.force.x +
        (1.0 - e) * wrench.body1Force.x;
      forceTorqueSensorsMsg->l_hand.force.y  =
        e * forceTorqueSensorsMsg->l_hand.force.y +
        (1.0 - e) * wrench.body1Force.y;
      forceTorqueSensorsMsg->l_hand.force.z  =
        e * forceTorqueSensorsMsg->l_hand.force.z +
        (1.0 - e) * wrench.body1Force.z;
      forceTorqueSensorsMsg->l_hand.torque.x =
        e * forceTorqueSensorsMsg->l_hand.torque.x +
        (1.0 - e) * wrench.body1Torque.x;
      forceTorqueSensorsMsg->l_hand.torque.y =
        e * forceTorqueSensorsMsg->l_hand.torque.y +
        (1.0 - e) * wrench.body1Torque.y;
      forceTorqueSensorsMsg->l_hand.torque.z =
        e * forceTorqueSensorsMsg->l_hand.torque.z +
        (1.0 - e) * wrench.body1Torque.z;

then the values seems more reasonable (see attached snapshot of rxplot).


Here's with the robot swaying back and forth: rxplots of hand hand force torque values

Here's with the robot pinned: rxplot pinned

edit flag offensive delete link more


Thanks John, but here's the issue from a User's perspective. You are doing this in the AtlasPlugin.cpp. I don't have access to that in the competition, so I have to run such a filter on the robot machine, which requires a node of my own listening to this message at 1000Hz and publishing filtered data. This isn't a huge issue, but if the BDI sensor has onboard filtering, this really should be done in AtlasPlugin to match the real sensor output, not by users.

pbeeson gravatar imagepbeeson ( 2013-03-20 08:09:59 -0500 )edit

We strongly prefer that only anti-aliasing filtering be done by Gazebo. If Gazebo does filtering, this needlessly complicates whole body Kalman filtering for the robot. A simple solution is to publish both raw and filtered versions of the sensor values.

cga gravatar imagecga ( 2013-03-20 10:03:16 -0500 )edit

I wonder if it would help if Gazebo did internal filter based on interations (start after 80% of iterations), and published that as raw. That might smooth out some of the unreasonable noise that we see on sensor, while retaining the "raw" per time stamp performance. The filtering would start fresh each time step. (The underlying assumption is that we are not having gross motions after 80% of iterations, and only converging to eliminate minor penetrations of collision objects.)

dcconner gravatar imagedcconner ( 2013-03-20 13:26:35 -0500 )edit

To be clear, I agree with Chris, that only "raw" sensor data should be published at 1kHz, unless the physical BDI sensor has filtering built into it. In that case, the plugin should mimic whatever the behavior of that system is.

dcconner gravatar imagedcconner ( 2013-03-20 13:34:07 -0500 )edit

How about adding a continuous first order low pass filter, off by default, but user can set the time constant (within Nyquist freq.) to activate it if desired?

hsu gravatar imagehsu ( 2013-03-21 21:21:04 -0500 )edit

Fine with me.

cga gravatar imagecga ( 2013-03-21 22:01:55 -0500 )edit

I think the real answer is to try and model the noise as well as the real sensor. Obviously the underlying physics plays a large part in the noise of these sensors, so until the physics settles down I think having having an option to filter noise would be good.

pbeeson gravatar imagepbeeson ( 2013-03-22 09:13:20 -0500 )edit

For the continuous filtering, I presume you are referring to output filtering. Have you looked into the suggestion of filtering within a time step to cut down on the "noise" strictly due to physics simulation errors. An always on filter wouldn't work for that, as I think you wouldn't want to start filtering until after the physics has stabilized a bit, and it should start fresh every time step. The biggest issues we see are the forces/noise due to collision interactions with objects/joints.

dcconner gravatar imagedcconner ( 2013-03-22 11:01:11 -0500 )edit

The displayed forces are at a hand, not a foot, correct? What's going on in foot forces as the robot sways?

John Nagle gravatar imageJohn Nagle ( 2013-07-24 00:02:51 -0500 )edit

answered 2013-02-26 02:47:43 -0500

hsu gravatar image

Hi Chris, thanks for pointing out this inconsistency. I've created a pull request to rotate the output of simulated force torque sensors into perspective link frames. In this update, the reference point remains at the joint anchor location (we'll confirm with BDI the current locations and post any updates in the issue tracker if an additional change is required).

edit flag offensive delete link more


What is the origin/frame of reference for this? It seems like it's not rfoot link or lfoot link. Also, the lhand and rhand torque and force values (all except Force Z) seems to just be wrong. Getting wildly fluctuating values whenever we start the atlassandiahands.launch simple world (no motion, just unpinned and standing there, arms outstretched).

pbeeson gravatar imagepbeeson ( 2013-03-19 16:55:49 -0500 )edit

Question Tools


Asked: 2013-02-25 22:55:31 -0500

Seen: 1,515 times

Last updated: Mar 20 '13