Contact sensor: 6 axis force/torque sensing

edit

Here is the code to print out the force and torque information:

      for (unsigned int j = 0; j < contacts.contact(i).wrench_size(); ++j)      
        {                                                                       
          std::cout << j << " wrench between [" <<                              
            contacts.contact(i).wrench(j).body_1_name() << "] and [" <<         
            contacts.contact(i).wrench(j).body_2_name() << "]\n";               
          std::cout << "  body_1_force: "                                       
                    << contacts.contact(i).wrench(j).body_1_force().x() << " "  
                    << contacts.contact(i).wrench(j).body_1_force().y() << " "  
                    << contacts.contact(i).wrench(j).body_1_force().z() << "\n";                                                                               
          std::cout << "  body_2_force: "                                       
                    << contacts.contact(i).wrench(j).body_2_force().x() << " "  
                    << contacts.contact(i).wrench(j).body_2_force().y() << " "  
                    << contacts.contact(i).wrench(j).body_2_force().z() << "\n";                                                                               
          std::cout << "  body_1_torque: "                                      
                    << contacts.contact(i).wrench(j).body_1_torque().x() << " "
                    << contacts.contact(i).wrench(j).body_1_torque().y() << " "
                    << contacts.contact(i).wrench(j).body_1_torque().z() << "\n";                                                                              
          std::cout << "  body_2_torque: "                                      
                    << contacts.contact(i).wrench(j).body_2_torque().x() << " "
                    << contacts.contact(i).wrench(j).body_2_torque().y() << " "
                    << contacts.contact(i).wrench(j).body_2_torque().z() << "\n";                                                                              
        }                                                                       

These numbers are similar to gztopic echo /gazebo/default/box/link/my_contact

But the numbers are very noisy.

6.967 body_1_f/t: 1.88961 -0.588002 2.94002 1.176 0.525202 -0.650803
6.967 body_1_f/t: -1.88961 -0.588002 2.94002 1.176 -0.525202 0.650803
6.968 body_1_f/t: 0.423902 1.37199 6.85998 -2.74398 3.21804 -0.474047
6.968 body_1_f/t: -0.423902 1.37199 6.85998 -2.74398 -3.21804 0.474047

The first number is time. We get two non-zero body1force/body1torque readings per simulation step. As you can see, the readings on the same time step often cancel each other.

Averaging the readings from the start of the simulation, I get results like:

1.823: average:    -4.21216e-13 0.00652219 9.7963 -0.0130444 -8.42498e-13 4.81264e-17
...
13.947: average:    2.28457e-14 -0.000353792 9.79939 0.000707584 4.56434e-14 5.26419e-17
...
54.692: average:    6.79372e-14 -0.00105193 9.80068 0.00210386 1.35823e-13 4.91202e-17
...
64.643: average:    3.22536e-14 -0.000499399 9.79989 0.000998798 6.44554e-14 4.85676e-17

I suppose this helps us understand why ODE is such an "approximate" simulator. The "weight" of the box appears to be around 9.8 Newtons (G = 9.8), but with substantial error. The Y force and X torque are also troubling.

Is there any way to average contact forces and torques during a simulation step to at least reduce the noise?

Thanks,
Chris

*** Update ********

The timing of information is strange. Printing out:

      std::cout << count << " " << i << " " <<
        contacts.contact_size() << " " <<
        contacts.contact(i).time().sec() << " " <<
        contacts.contact(i).time().nsec() << "\n";

where count is a count of calls to void ContactPlugin::OnUpdate(), and i is from

  for (unsigned int i = 0; i < contacts.contact_size(); ++i)

generates:

19 0 16 0 1000000
19 1 16 0 2000000
19 2 16 0 3000000
19 3 16 0 4000000
19 4 16 0 5000000
19 5 16 0 6000000
19 6 16 0 7000000
19 7 16 0 8000000
19 8 16 0 9000000
19 9 16 0 10000000
19 10 16 0 11000000 ...