search

ros-industrial-consortium / descartes

ROS-Industrial Special Project: Cartesian Path Planner
Apache License 2.0
126 stars 92 forks source link

CartTrajectoryPt::uniform() bug #180

Open davetcoleman opened 8 years ago

davetcoleman commented 8 years ago

I believe I've uncovered a bug in the discretization of poses in Descartes. I'm using AxialSymmetricPt and I find that I get basically the same discretization for both the X_AXIS and Z_AXIS options:

X_AXIS x_axis

Z_AXIS z_axis

Y_AXIS (for reference) y_axis

I've dug into the code for a long time and I believe the underlying method of using roll/pitch/yaw to discretize orientations at arbitrary increments and amounts is incorrect. I believe the 6-way for loop in the uniform() function is subject to errors such as Gimbal Lock... but I'm not exactly sure of the solution. I wanted to report this here as I'm calling it a night, and hopefully someone has some insight into this issue.

I believe the correct way to do this requires a big restructuring of this code - I think we need to call Eigen::AngleAxisd(rx, Eigen::Vector3d::UnitX()) in different ways depending on which axis we are discretizing. Or maybe I just need to figure out how to use the tool discretization instead of just object discretization.

Have you guys run into something like this?

ClayFlannigan commented 8 years ago

Yup, I think this is not the right way to sample orientations. Uniform sampling of Euler angles produces 1) non-uniform distributions and 2) suffers from singularities (gimbal lock). See here. Quaternions are the normal solution with SLERP interpolation.

shaun-edwards commented 8 years ago

There's been quite a few discussion threads here about sampling euler angles. In additional to Clay's suggestions, #43 has some other references. In the case of sampling about a single axis, the current approach should work (AxialSymmetricPoint enforces this). My guess is that the constructor for AxialSymmetricPoint isn't working as expected.

davetcoleman commented 7 years ago

I'm pretty sure I solved this problem in my bolt implementation of the Descartes planner - its essentially a much more moveit-centric way to create a graph of underconstrained redundant Cartesian poses. The function I wrote to turn tolerances on angles into candidate poses is here. It uses a similar data structure to what Descartes uses for tolerances, except I stored the values in a vector so that this rotateOnAxis() recursive function can still work.

I'm wrapping up my Bolt work and need to desperately focus on my dissertation due in a month, so I can't integrate these fixes back into Descartes - especially since I stopped using Descartes a while back. But the references above should help whoever might be motivated to copy the functionality back into Descartes.