How to check if the geometric wrap meets expectations?

[zichunxx]

Hi!

I have wrapped my robot links with fcl::Capsule and the code snippet is shown below.

  std::shared_ptr<fcl::CollisionGeometry<double>> capsule_geo (new fcl::Capsule<double> (radius, height));
  fcl::Transform3<double> t = fcl::Transform3d::Identity();
  t.linear() = this_mat.block<3, 3>(0, 0);
  t.translation() = this_mat.block<3, 1>(0, 3);
  auto *b = new fcl::CollisionObject<double> (capsule_geo, t);

When robot links collide with obstacles that are represented by octree, however, cdata.result.isCollision() returns false.

The octomaps of obstacles are obtained with sensor and these poses can be guaranteed.

For fcl::CollisionObject, if its base frame is different from my robot base, then the above process will result in a wrong transformation to capsules.

So, my question is, how to check the base frame of fcl::Capsule. If it is different from my robot base frame, then how can I modify it?

Thanks!

[zichunxx]

Almost the same issue with #344