Skip polygon whose rpy cannot be calculated

For some polygons, rpy cannot be computable. (value > 1.0 is inputted to asin() function) https://github.com/iory/scikit-robot/blob/7f7427c420f8aeaa91cf380a53f33fac774bb016/skrobot/coordinates/math.py#L848-L849

This problem happens when I use output of jsk_pcl_ros_utils/polygon_array_transformer. https://jsk-docs.readthedocs.io/projects/jsk_recognition/en/latest/jsk_pcl_ros_utils/nodes/polygon_array_transformer.html

So this problem may be caused by this node.

I created this PR for the workaround. If you @HiroIshida have any good idea, please teach me...

result

$ python test.py
Traceback (most recent call last):
  File "test.py", line 6, in <module>
    polygon_to_desired_rpy(np_polygon)
  File "/home/leus/yamaopt_ws/src/yamaopt/yamaopt/polygon_constraint.py", line 82, in polygon_to_desired_rpy
    rpy = quaternion2rpy(matrix2quaternion(M))[0]
  File "/home/leus/scikit-robot/skrobot/coordinates/math.py", line 849, in quaternion2rpy
    2 * q[1] * q[3] - 2 * q[0] * q[2])
ValueError: math domain error

sample code (test.py)

import pickle
from yamaopt.polygon_constraint import polygon_to_desired_rpy

with open('/home/leus/0.pickle', 'rb') as f:
    np_polygon = pickle.load(f)
polygon_to_desired_rpy(np_polygon)

Sample pickles 0.pickle https://drive.google.com/file/d/1FfAUR65KVPSM-MFNZm_U_ZeqCfwms_hK/view?usp=sharing

1.pickle https://drive.google.com/file/d/1A5lBJ3QzI6C_mD1aQT5krOkUpHYqRWFD/view?usp=sharing

HiroIshida / yamaopt

Skip polygon whose rpy cannot be calculated #16