Sapien to Planning World

[Lexseal]

It would be nice to have a function take a sapien world object and populate the planning world accordingly.

[Lexseal]

fcl seems to have a bug that causes segfault when colliding anything with a plane geometry.

Collide with ground
[1] 223712 segmentation fault (core dumped)

According to this post, the issue should have been fixed back in 2016 but the 2020 version of fcl still segfaults.

[KolinGuo]

Will be completed by #73