FastPhysicsEngine::getNextKinematicsOnCollision - is angular_avg and r in body frame?

[cdyk]

I'm reading through the collision response code and is a bit puzzled by the line
https://github.com/Microsoft/AirSim/blob/c86c341f512c06d3fbb6acb5170885a21d84cfea/AirLib/include/physics/FastPhysicsEngine.hpp#L153
which I assume calculate the velocity at the contact point. vcur_avg_body is transformed from world frame to body frame in the line above, but for the rest to make sense, must not angular_avg and r be in body frame as well (or the result of the cross product transformed to body frame)?

It is a bit unclear which frames impact point and position (from which r is calculated) is in, but it appears to me that
https://github.com/Microsoft/AirSim/blob/c86c341f512c06d3fbb6acb5170885a21d84cfea/AirLib/include/physics/FastPhysicsEngine.hpp#L131
is in world frame?

[cdyk]

Also, at
https://github.com/Microsoft/AirSim/blob/c86c341f512c06d3fbb6acb5170885a21d84cfea/AirLib/include/physics/FastPhysicsEngine.hpp#L171
I think that the r.cross(normal_body) should be multiplied with body.getInertiaInv() since we apply an angular impulse. Also, I think that the everything besides angular_avg should be transformed back to world frame before adding it to angular_avg. And if r is not in body frame, it should be transformed to body_frame before being used in the cross product.

Similarily at
https://github.com/Microsoft/AirSim/blob/c86c341f512c06d3fbb6acb5170885a21d84cfea/AirLib/include/physics/FastPhysicsEngine.hpp#L185
I also think that body.getInvertialInv() should be used and not 1/mass since this also is an angular impulse. And transformed back to world space.

I might be wrong, however. On flat ground, the orientation of body frame and world frame is roughly the same, so if these are indeed errors, they do not necessarily produce too much errors on the result.

Also I think that
https://github.com/Microsoft/AirSim/blob/c86c341f512c06d3fbb6acb5170885a21d84cfea/AirLib/include/physics/FastPhysicsEngine.hpp#L139
is always identical to is_ground_normal (if is_ground_normal is true, the if on line 140 always hit and ground_collision is set with is_ground_normal. If is_ground_normal is false, either the if doesn't hit and ground_collision is false, or it hits, and ground_collision is set to is_ground_normal (which is in this case false). Optimizer will probably catch this, but reducing it to a single variable might improve readability a bit.

[cdyk]

Found
https://github.com/Microsoft/AirSim/blob/c86c341f512c06d3fbb6acb5170885a21d84cfea/AirLib/include/physics/FastPhysicsEngine.hpp#L410
So I assume angular velocity is in body frame.

[sytelus]

In the FastPhysicsEngine, linear quantities are in world frame while angular quantities are in body frame. This might seem weird but it allows us to eliminate certain calculations because linear force usually starts out in world frame while torques starts out in body frame.

Re line 171: This code is now almost 1.5 year old so my memory is bit hazzy. I think it should be multiplying with inv inertia. As far as I can recall, the inv inertia caused huge problem here because the inertia matrix is diagonal matrix with very small numbers (because of SI units and inertia eq for box). So when you invert inertia matrix, you get matrix with very large diagonal numbers. This caused huge angular velocities during collision. We still have to perfect the collision response (and at the time we cared more about regular dynamics accuracy instead of collision response accuracy) but just removing inv of inertia matrix kind of sort of removed crazy behavior. This is the place more insights from Physics gurus would be appreciated :). Chris Hecker has great doc here and @gafferongames had implemented similar physics here but they don't seem to be affected by this much. So it is also likely may be we are doing something wrong here. I believe same explanation would go for line 185 as well.

Re line 139: Its just different ways of reading the code. The way I thought about this logic is: If collision is normal to any of the body's faces, we want to think of it as collision occurred for the point mass i.e. no angular responses. So in this case, we collided to ground if collision was on the bottom surface of the box. Again, this is another hack in collision response which otherwise produces weird results. Also, in edge cases this won't work (for example, vehicle flipped over and hits ceiling then it would be considered as ground collision).

There is a lot of room to improve collision response. For our purpose collision usually was end of the simulation episode so we didn't cared so much about how accurate was the response. However, there are quite a few folks who are working on collision recovery research. For them, this was deal breaker however we haven't gotten around to take a second look at this. So, if you know how this works, please feel free to suggest any improvements!

[cdyk]

Thanks for your elaboration!

But I assume that collision_info.impact_point and collision_info.positionare in world frame? If so, I think that r should be rotated to body frame before being used in the body-frame calculations at lines 153, 165, 166, 171, 178, 179, and 185?