Fix Collision::GetWorldPose for non-canonical links (and friction directions, #2068)

#2702 Merged at e792330
Repository
Branch
collision_pose_noncanonical
Repository
Branch
gazebo7
Author
  1. Steven Peters
Reviewers
Description

It was reported that setting a body-fixed friction direction is not working (issue #2068). This was confusing to me, since we have an extensive test of friction direction parameters in physics_friction.cc:

friction direction test

It turns out that friction direction parameters are indeed broken in certain cases, and it is related to the Collision::GetWorldPose function that is called in ODEPhysics::Collide. Many thanks to @Andrea Ruzzenenti for pointing this out and my apologies for not understanding it sooner.

I believe the problem is that Collision::GetWorldPose is only working properly for the collisions that are attached to canonical links in a model, which is the case with the boxes in the test shown above. To show the test failure, I added a non-canonical link to the collision_pose_offset test world and a failing expectation to INTEGRATION_physics_collision. I also added a pair of spheres to the friction_dir_test world with asymmetric friction and a body-fixed friction direction. The spheres have no friction in the initial direction that they are sliding and will slide straight ahead, but by applying an angular velocity about the vertical axis, the friction directions should change the the spheres should start rolling. I will attach additional animations showing the failure case and the fixed case.

The fix is in 57c8d6f, which is to mark collisions of all links as dirty when parent link gets updated, instead of just for canonical links.

While nosing around in Entity.cc, I also saw that poses were not published for canonical links, which seemed weird to me, so I fixed the logic in 14473f4.

  • Issues #2068: Directional friction still broken. resolved

Comments (12)

  1. Steven Peters author

    here's a close up of the spheres rotating without the fix:

    broken.gif

    the canonical link starts to roll as it should, while the non-canonical link continues spinning vertically

  2. Steven Peters author

    I just found a little more explanation of the changes to Entity.cc in my email drafts folder:

    With each simulation timestep, the physics engine calls SetWorldPose on each link that moves, and Entity::SetWorldPose redirects using a function pointer to different SetWorldPose variants, like SetWorldPoseModel, SetWorldPoseCanonicalLink, and SetWorldPoseDefault. Collision::GetWorldPose was working for collisions attached to the canonical link of a model because SetWorldPoseCanonicalLink was iterating over child collisions and calling Collision::SetWorldPoseDirty. To give this functionality to non-canonical links, I moved that loop to SetWorldPoseDefault and added a call to SetWorldPoseDefault from the start of SetWorldPoseCanonicalLink (57c8d6f).

      1. Martin Pecka

        Hmm, but in the changelogs the news about this were placed in the 7.x.x parts. That's why I didn't notice it. Isn't that wrong? Or don't I just understand the semantics of the changelogs?