DRCSIM: simulated force measurements in wrong coordinates

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

cga
223 ●41 ●46 ●54

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

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

add a comment

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

hsu
1873 ●14 ●19

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

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).

atlas<em>sandia</em>hands.launch

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

Comments

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 ( 2013-03-20 08:09:59 -0600 )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 ( 2013-03-20 10:03:16 -0600 )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 ( 2013-03-20 13:26:35 -0600 )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 ( 2013-03-20 13:34:07 -0600 )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 ( 2013-03-21 21:21:04 -0600 )edit

Fine with me.

cga ( 2013-03-21 22:01:55 -0600 )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 ( 2013-03-22 09:13:20 -0600 )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 ( 2013-03-22 11:01:11 -0600 )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 ( 2013-07-24 00:02:51 -0600 )edit

add a comment

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 ( 2013-03-19 16:55:49 -0600 )edit

DRCSIM: simulated force measurements in wrong coordinates

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

DRCSIM: simulated force measurements in wrong coordinates edit

2 Answers

Comments

Comments

Question Tools

Stats

Related questions

DRCSIM: simulated force measurements in wrong coordinates