Oscillations in simulated acceleration

asked 2016-09-02 05:42:45 -0500

Georacer gravatar image

updated 2016-09-05 09:44:32 -0500

Hello everyone,

I am writing some custom aerodynamics plugins and I want to query Gazebo for the linear acceleration (among others).

I have placed this code inside one of my plugins, similar to libgazebo_ros_imu:

  common::Time cur_time = this->world->GetSimTime();
  double dt = this->last_time.Double() - cur_time.Double();
  if (dt != 0)
  {
    tempVect = (this->last_velLin - this->model->GetWorldLinearVel())/dt;
    ignition::math::Vector3d gravity(this->world->Gravity()); // Read gravity acceleration in inertial frame
    tempVect = tempVect - math::Vector3(gravity.X(), gravity.Y(), gravity.Z()); // Subtract gravity
    tempVect = this->modelPose.rot*tempVect; // Rotate acceleration to body frame
    this->modelState.acceleration.linear.x = tempVect.x;
    this->modelState.acceleration.linear.y = tempVect.y;
    this->modelState.acceleration.linear.z = tempVect.z;

    this->last_velLin = this->model->GetWorldLinearVel();
    this->last_time = cur_time;
  }

  this->rosPub.publish(this->modelState);

However, the output of the simulation, e.g. on the z-axis is: image description

There is a 10m/s^2 oscillation around the real value.

I noticed that increasing the simulation step size doesn't solve the problem. Increasing physics CFM value makes the spikes less frequent but of the same amplitude.

There are some related questions here and here

I should note that, despite what is said in the first link, the output of GetWorldLinearAcc() is exactly the same as differentiating the linear velocity.

Glad to provide more details on request, thanks in advance!

EDIT: Switched to Bullet engine. The acceleration trajectory is by far better (although the overall situation is by far worse), since its API seems broken: image description

EDIT 2:

I tried various bodies, with the IMU sensor attached onto them. For testing purposes, I monitored the z-axis, with the following IMU noise profile:

<z>
    <noise type="gaussian">
        <mean>0.0</mean>
        <stddev>0.0</stddev>
        <bias_mean>0.0</bias_mean>
        <bias_stddev>0.000</bias_stddev>
    </noise>
</z>

It seems that the heavier a body is, the more erratic is the calculated acceleration.

  • The coke_can registers about a_z=9.87+-0.03m/s^2
  • A 0.1kg "noisy_imy" model registers about a_z=9.79m/s^2 with sparse 0.08m/s^2 spikes
  • A 1kg "noisy_imy" model registers about a_z=9.75+-0.25m/s^2, sometimes peaking to 0m/s^2

Gravity is set to the default 9.8m/s^2

edit retag flag offensive close merge delete