Bullet compatibility

[captain-yoshi]

Is the Bullet collision library compatible with MTC?

From my understanding by following the moveit tutorials, only this line of code is needed to choose Bullet over FCL in MTC. With this line, is FCL replaced completly with Bullet?

# current_state.cpp
include <moveit/collision_detection_bullet/collision_detector_allocator_bullet.h>

void CurrentState::compute() {
    scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
    scene_->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create());

When planning with MTC, it plans for a certain amount of time before running in a segfault:

Thread 239 "bob_planner_pp" received signal SIGSEGV, Segmentation fault.
[Switching to Thread 0x7fffd188e700 (LWP 943260)]
0x00007ffff2129c46 in ?? () from /lib/x86_64-linux-gnu/libBulletCollision.so.2.88
(gdb) backtrace
#0  0x00007ffff2129c46 in ?? () from /lib/x86_64-linux-gnu/libBulletCollision.so.2.88
#1  0x00007ffff212aac6 in btDbvt::update(btDbvtNode*, btDbvtAabbMm&) () from /lib/x86_64-linux-gnu/libBulletCollision.so.2.88
#2  0x00007ffff212ac3e in btDbvt::update(btDbvtNode*, btDbvtAabbMm&, btVector3 const&, float) () from /lib/x86_64-linux-gnu/libBulletCollision.so.2.88
#3  0x00007ffff212c8b1 in btDbvtBroadphase::setAabb(btBroadphaseProxy*, btVector3 const&, btVector3 const&, btDispatcher*) () from /lib/x86_64-linux-gnu/libBulletCollision.so.2.88
#4  0x00007ffff4f78ed0 in collision_detection_bullet::BulletBVHManager::setCollisionObjectsTransform(std::__cxx11::basic_string<char, std::char_traits<char>, std::allocator<char> > const&, Eigen::Transform<double, 3, 1, 0> const&) () from /home/captain-yoshi/ws/ros/catkin_ws/devel/lib/libmoveit_collision_detection_bullet.so.1.1.1
#5  0x00007ffff4f740b3 in collision_detection::CollisionEnvBullet::checkSelfCollisionHelper(collision_detection::CollisionRequest const&, collision_detection::CollisionResult&, moveit::core::RobotState const&, collision_detection::AllowedCollisionMatrix const*) const () from /home/captain-yoshi/ws/ros/catkin_ws/devel/lib/libmoveit_collision_detection_bullet.so.1.1.1
#6  0x00007fffe83e6ba0 in ompl_interface::StateValidityChecker::isValid(ompl::base::State const*, bool) const () from /home/captain-yoshi/ws/ros/catkin_ws/devel/lib/libmoveit_ompl_interface.so.1.1.1
#7  0x00007fffe8403ea5 in ompl_interface::ConstrainedGoalSampler::sampleUsingConstraintSampler(ompl::base::GoalLazySamples const*, ompl::base::State*) ()
   from /home/captain-yoshi/ws/ros/catkin_ws/devel/lib/libmoveit_ompl_interface.so.1.1.1
#8  0x00007fffd22e2824 in ompl::base::GoalLazySamples::goalSamplingThread() () from /home/captain-yoshi/ws/ros/catkin_ws/devel/lib/libompl.so.16
#9  0x00007ffff69b3d84 in ?? () from /lib/x86_64-linux-gnu/libstdc++.so.6
#10 0x00007ffff7f6c609 in start_thread (arg=<optimized out>) at pthread_create.c:477
#11 0x00007ffff67f2293 in clone () at ../sysdeps/unix/sysv/linux/x86_64/clone.S:95

Also tried patching MoveIt with this PR but still got a segfault. Running on Noetic.

[AndyZe]

Yep, that line should be all that's required. Can you please try it with this PR instead: https://github.com/ros-planning/moveit/pull/2481

It's great to have somebody else try this stuff. Thank you!

[AndyZe]

I recommend using this line instead:

void CurrentState::compute() {
    scene_ = std::make_shared<planning_scene::PlanningScene>(robot_model_);
    scene_->setActiveCollisionDetector(collision_detection::CollisionDetectorAllocatorBullet::create(), true /* exclusive */);

That second exclusive arg sets Bullet as the exclusive collision detector, instead of keeping FCL alive in the background. For simplicity's sake, it's best to clear FCL out completely.

[captain-yoshi]

Wow that was a fast reply! With the suggested PR there are no more segfaults. It seems the planning time is much slower then with FCL, even though it should be quicker (~10 times).

This comment has the following table.

What does MTC or MoveIt use as the Collision Environment?

The speed could also be explained by this PR: ros-planning/moveit/pull/1584. One the Con's of a Unified collision environment:

For multi-threaded collision checks, it is not directly clear for me how we can avoid copying the whole collision_env. But as maybe memory got cheaper than it was some years ago, these advantages can be forsaken in turn for the above mentioned advantages. Or, other smart copying techniques could be applied (I am no expert here).

It seems that this could be what is currently happening to me. @AndyZe Are you seeing drop in speed when using Bullet with MTC? I will report back with timing information to compare FCL and bullet.

[AndyZe]

Yeah, we saw a drop in speed too. It was a disappointment. Here are my results (time for a single collision check).

Bullet, checking the endeffector group: period ranges from 9e-6 : 2e-5 s
FCL, checking the endeffector group: period ranges from 7e-6 : 1e-5 s

You could use more crude collision meshes or possibly only check the end-effector group instead of the whole robot.

[AndyZe]

Interesting comments re. the unified collision environment. I don't really know anything about that.

[captain-yoshi]

This ia a comparison between FCL and Bullet in MTC. I have used the same Task for both. The image was taken when the planning did a first pass of each stages (One way trip). Notice the huge difference in time!

There is something weird with Bullet for some IK stages. It seems to detect a collision between the end-effector and the robot from the previous state (Never seen that with FCL).

Another thing, my computer frooze a couple of times and at one time something killed the process!

[ INFO] [1612552680.679012701]: SimpleSetup: Path simplification took 3.119035 seconds and changed from 3 to 2 states
[ INFO] [1612552685.283305421]: Planner configuration 'right_eef' will use planner 'geometric::RRTConnect'. Additional configuration parameters will be set when the planner is constructed.
[ INFO] [1612552685.285116793]: right_eef/right_eef: Starting planning with 1 states already in datastructure
[ INFO] [1612552685.869924170]: right_eef/right_eef: Created 5 states (2 start + 3 goal)
[ INFO] [1612552685.869988899]: Solution found in 0.586518 seconds
[ INFO] [1612552688.132802053]: SimpleSetup: Path simplification took 2.262732 seconds and changed from 4 to 2 states
Killed

Edit: Ubuntu 20.04 killed the process automatically because it was eating all the memory.

[captain-yoshi]

Planning with Bullet with this PR #4281, seems to eat memory. If the planning takes more then 80 seconds, it will eat out all my memory (16 GB minus the other processes). I don't have much experience debugging memory... There is Valgrind but never used it. I can reproduce this after each planning attempt.

[AndyZe]

Wow, that's really awful. My results were nowhere near that bad :/

[captain-yoshi]

I have to use the longuest_valid_segment_fraction = 0.001 because of my environment with FCL. I was hoping to use Bullet with CCD to reduce this parameter which affects planning time.

Maybe there is something with my setup (but with FCL it is really fast...). When you have to plan for a long time before returning a solution, do you have memory issues?

[AndyZe]

My planning scene is very simple compared to yours, so planning never takes a long time. Your meshes look very complicated.

Have you tried using MeshLab to simplify the collision meshes? In the URDF, there is a tag like this where a different, simplified collision mesh can be specified:

      <collision>
        <geometry>
          <mesh filename="package://ur_description/meshes/ur5/collision/shoulder.stl" />
        </geometry>
      </collision>

^ That's an example from the UR URDF. Apologies if you know all of this already.

You could also try planning for a reduced number of bodies, too. Maybe use a joint group which is just the end-effector.

[AndyZe]

For me, Bullet took ~2X as long as FCL in the worst case.

[AndyZe]

It may be that the mutex locks I added, to prevent the segfault with Bullet, are really causing trouble for a large number of threads. Almost a deadlock.

[captain-yoshi]

Good point, I could reduce some of my mesh. Its strange that FCL has no problem with my mesh quality. Thx for your help! Just ping me if you want me to test some PR related to Bullet.

For me, Bullet took ~2X as long as FCL in the worst case.

As my understanding, you could relax the longuest_valid_segment_fraction to say 0.05 with bullet, and compare with FCL againt the same parameter with 0.01 or 0.005. You should see an improvement on time, not sure on the quality of the returning path.

[AndyZe]

Ok, thanks. That's a really good point that might help on my project

[v4hn]

@captain-yoshi We added the lock upstream some days ago. Could you check whether this fixes your segfaults?

[captain-yoshi]

@v4hn The segfaults have disappeared but it is eating a lot of memory in a short amount of time compared to FCL and I can reproduce it everytime. Tested with MoveIt master branch on the latest commit.

I have made 2 videos to demonstrate this behavior: one with FCL and the other with Bullet. With FCL everything seems normal.

EDIT: BUMP the quality of the videos to max settings to see the numbers shown by htop.

[captain-yoshi]

Just to make sure I did the same test with the pick and place demo who has fewer collision objects and the results are the same. It takes 2 seconds to complete the Task with FCL and 9 seconds with Bullet. You should be able to reproduce this behaviour with the MTC pick and place demo.

Memory (GB)	FCL	Bullet
Start	6.22	6.41
End	6.27	10.1
Consumed	0.05	3.83

[YannickRiou]

Good point, I could reduce some of my mesh. Its strange that FCL has no problem with my mesh quality. Thx for your help! Just ping me if you want me to test some PR related to Bullet.

For me, Bullet took ~2X as long as FCL in the worst case.

As my understanding, you could relax the longuest_valid_segment_fraction to say 0.05 with bullet, and compare with FCL againt the same parameter with 0.01 or 0.005. You should see an improvement on time, not sure on the quality of the returning path.

I am currently trying to use Bullet on my project with a PR2 robot. It really confuses me that we talk about the longuest_valid_segment_fraction parameter for bullet. Bullet is described as a continuous collision checker, so why do we have to manually set the fraction when using it ?

[AndyZe]

Bullet only does the continuous collision checks if you use the specialized method for it. By default it's discrete, just like FCL.

[YannickRiou]

By specialized method, do you talk about checking collision for specific situation as in the moveit tutorial about bullet ?

[captain-yoshi]

FYI I also can trigger the memory leak in MoveIt with this CollisionRequest on multiple trials (10,000):

# collision_detection::CollisionRequest
distance: True              # Compute proximity distance (Slow)
cost: False                 # Compute collision cost
contacts: True              # Compute contacts
max_contacts: 1             # Maximum number of contacts
max_contacts_per_pair: 1    # Maximum numbe of contacts per pair of bodies
max_cost_sources: 1         # Defines how many of the top cost sources should be returned
verbose: False              # Report information about collision

This happens only when distance=True. In this mode, the CollisionResult contact_count gives weird results like 1100 contacts for a scene with 4 objects in contact and 1121 contacts with a scene that has no contact. The same scenes with FCL reports the correct values.