Ask Your Question

# Revision history [back]

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 body_1_force/body_1_torque 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

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 body_1_force/body_1_torque 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, 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
19 11 16 0 12000000
19 12 16 0 13000000
19 13 16 0 14000000
19 14 16 0 15000000
19 15 16 0 16000000
20 0 16 0 1000000
20 1 16 0 2000000
20 2 16 0 3000000
20 3 16 0 4000000
20 4 16 0 5000000
20 5 16 0 6000000
20 6 16 0 7000000
20 7 16 0 8000000
20 8 16 0 9000000
20 9 16 0 10000000
20 10 16 0 11000000
20 11 16 0 12000000
20 12 16 0 13000000
20 13 16 0 14000000
20 14 16 0 15000000
20 15 16 0 16000000


and

39 0 16 0 1000000
39 1 16 0 2000000
39 2 16 0 3000000
39 3 16 0 4000000
39 4 16 0 5000000
39 5 16 0 6000000
39 6 16 0 7000000
39 7 16 0 8000000
39 8 16 0 9000000
39 9 16 0 10000000
39 10 16 0 11000000
39 11 16 0 12000000
39 12 16 0 13000000
39 13 16 0 14000000
39 14 16 0 15000000
39 15 16 0 16000000
40 0 29 0 17000000
40 1 29 0 18000000
40 2 29 0 19000000
40 3 29 0 20000000
40 4 29 0 21000000
40 5 29 0 22000000
40 6 29 0 23000000
40 7 29 0 24000000
40 8 29 0 25000000
40 9 29 0 26000000
40 10 29 0 27000000
40 11 29 0 28000000
40 12 29 0 29000000
40 13 29 0 30000000
40 14 29 0 31000000
40 15 29 0 32000000
40 16 29 0 33000000
40 17 29 0 34000000
40 18 29 0 35000000
40 19 29 0 36000000
40 20 29 0 37000000
40 21 29 0 38000000
40 22 29 0 39000000
40 23 29 0 40000000
40 24 29 0 41000000
40 25 29 0 42000000
40 26 29 0 43000000
40 27 29 0 44000000
40 28 29 0 45000000


Why are the nanosecond fields varying within a single update, and going resetted? Seconds seem to roll over appropriately:

488 0 31 0 998000000
488 1 31 0 999000000
488 2 31 1 0
488 3 31 1 1000000
488 4 31 1 2000000


Thanks again,
Chris

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 body_1_force/body_1_torque 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
19 11 16 0 12000000
19 12 16 0 13000000
19 13 16 0 14000000
19 14 16 0 15000000
19 15 16 0 16000000
20 0 16 0 1000000
20 1 16 0 2000000
20 2 16 0 3000000
20 3 16 0 4000000
20 4 16 0 5000000
20 5 16 0 6000000
20 6 16 0 7000000
20 7 16 0 8000000
20 8 16 0 9000000
20 9 16 0 10000000
20 10 16 0 11000000
20 11 16 0 12000000
20 12 16 0 13000000
20 13 16 0 14000000
20 14 16 0 15000000
20 15 16 0 16000000


and

39 0 16 0 1000000
39 1 16 0 2000000
39 2 16 0 3000000
39 3 16 0 4000000
39 4 16 0 5000000
39 5 16 0 6000000
39 6 16 0 7000000
39 7 16 0 8000000
39 8 16 0 9000000
39 9 16 0 10000000
39 10 16 0 11000000
39 11 16 0 12000000
39 12 16 0 13000000
39 13 16 0 14000000
39 14 16 0 15000000
39 15 16 0 16000000
40 0 29 0 17000000
40 1 29 0 18000000
40 2 29 0 19000000
40 3 29 0 20000000
40 4 29 0 21000000
40 5 29 0 22000000
40 6 29 0 23000000
40 7 29 0 24000000
40 8 29 0 25000000
40 9 29 0 26000000
40 10 29 0 27000000
40 11 29 0 28000000
40 12 29 0 29000000
40 13 29 0 30000000
40 14 29 0 31000000
40 15 29 0 32000000
40 16 29 0 33000000
40 17 29 0 34000000
40 18 29 0 35000000
40 19 29 0 36000000
40 20 29 0 37000000
40 21 29 0 38000000
40 22 29 0 39000000
40 23 29 0 40000000
40 24 29 0 41000000
40 25 29 0 42000000
40 26 29 0 43000000
40 27 29 0 44000000
40 28 29 0 45000000


Why are the nanosecond fields varying within a single update, and going getting resetted? Seconds seem to roll over appropriately:

488 0 31 0 998000000
488 1 31 0 999000000
488 2 31 1 0
488 3 31 1 1000000
488 4 31 1 2000000


Thanks again,
Chris