Add (failing) test for orientation

[alxy]

The orientation on full referential only works for me on simple orientations. If more complex orientations are used, I get either the translation or the rotation wrong.

Here is the result of the adde test:

========================================================== FAILURES ==========================================================
_____________________________________________ test_orientation_no_eye_full_frame _____________________________________________

baxter_left_arm = Kinematic chain name=baxter_left_arm links=[Link name=Base link bounds=(None, None), Link name=torso_t0 bounds=(-3.01,...ounds=(None, None)] active_links=[False False False  True  True  True  True  True  True  True  True  True
  True False]

    def test_orientation_no_eye_full_frame(baxter_left_arm):
        target_position = [0.1, 0.4, -0.1]
        target_orientation = R.from_euler('xyz', [0, 90, 90], degrees=True).as_matrix()

        # Begin to place the arm in the right position
        ik = baxter_left_arm.inverse_kinematics(target_position)
        ik = baxter_left_arm.inverse_kinematics(target_position, target_orientation, initial_position=ik, orientation_mode='all')

        position = baxter_left_arm.forward_kinematics(ik)[:3, 3]
        orientation = baxter_left_arm.forward_kinematics(ik)[:3, :3]

        # Check
        np.testing.assert_almost_equal(position, target_position, decimal=3)
>       np.testing.assert_almost_equal(orientation, target_orientation, decimal=3)
E       AssertionError: 
E       Arrays are not almost equal to 3 decimals
E       
E       Mismatched elements: 9 / 9 (100%)
E       Max absolute difference: 1.63527758
E       Max relative difference: 1.63527758
E        x: array([[ 0.56 ,  0.426, -0.71 ],
E              [-0.531,  0.843,  0.086],
E              [ 0.635,  0.329,  0.699]])
E        y: array([[ 0., -1.,  0.],
E              [ 0.,  0.,  1.],
E              [-1.,  0.,  0.]])

test_orientation.py:53: AssertionError
================================================== short test summary info ===================================================
FAILED test_orientation.py::test_orientation_no_eye_full_frame - AssertionError:
================================================ 1 failed, 3 passed in 5.00s =================================================

[alxy]

Another test case where the position is off:

target_orientation = R.from_euler('xyz', [0, 0, 90], degrees=True).as_matrix()
target_position = [0.5, -1.1, -0.1]

=========================================================================================================== FAILURES ===========================================================================================================
______________________________________________________________________________________________ test_orientation_no_eye_full_frame ______________________________________________________________________________________________

baxter_left_arm = Kinematic chain name=baxter_left_arm links=[Link name=Base link bounds=(None, None), Link name=torso_t0 bounds=(-3.01,...ounds=(None, None)] active_links=[False False False  True  True  True  True  True  True  True  True  True
  True False]

    def test_orientation_no_eye_full_frame(baxter_left_arm):
        target_position = [0.1, 0.4, -0.1]
        target_orientation = R.from_euler('xyz', [0, 90, 90], degrees=True).as_matrix()

        target_orientation = R.from_euler('xyz', [0, 0, 90], degrees=True).as_matrix()
        target_position = [0.5, -1.1, -0.1]

        # Begin to place the arm in the right position
        ik = baxter_left_arm.inverse_kinematics(target_position)
        ik = baxter_left_arm.inverse_kinematics(target_position, target_orientation, initial_position=ik, orientation_mode='all')

        position = baxter_left_arm.forward_kinematics(ik)[:3, 3]
        orientation = baxter_left_arm.forward_kinematics(ik)[:3, :3]

        # Check
>       np.testing.assert_almost_equal(position, target_position, decimal=3)
E       AssertionError: 
E       Arrays are not almost equal to 3 decimals
E       
E       Mismatched elements: 3 / 3 (100%)
E       Max absolute difference: 0.76661575
E       Max relative difference: 2.73259607
E        x: array([ 0.05 , -0.333,  0.173])
E        y: array([ 0.5, -1.1, -0.1])

test_orientation.py:54: AssertionError
=================================================================================================== short test summary info ====================================================================================================
FAILED test_orientation.py::test_orientation_no_eye_full_frame - AssertionError:
================================================================================================= 1 failed, 3 passed in 4.42s ==================================================================================================

[Phylliade]

Hello @alxy,

First, thanks for your contribution!

Yep, sometimes IKPy doesn't find a solution to some orientations (whether it's because it's not reachable, or can be very hard to reach).

Regarding this test, we cannot add a failing test, so maybe we should just fix this test to make it pass? (by changing the orientation matrix where it works?)

[alxy]

The problem is, I had the same issue for positions which are actually easy to reach for the robot, but where position or orientations was extremely odd in the end. It works far better if I use the method with orientations based on a single axis (basically aligning two vectors), however, that has the downside of obviously not working for rotations around that specific axis.

I think we should just leave this open until maybe somebody can come up with a solution for the problem...

[Phylliade]

Well, the problem is that asking IKPy to do the optimization on both axes at the same time can be challenging for the solver.

The advantage of using IKPy on the two axes instead on the matrix itself is that it forces the solver to work sequentially, one axis at a time.

So, if you split your orientation matrix into two orientation matrices that correspond each to one axis, and do two calls to the FK, this should work. However, it would defeat the purpose of having this ...

[Phylliade]

BTW, what happens when you comment out the first fk, in position (e.g. the line:

        ik = baxter_left_arm.inverse_kinematics(target_position)
```)

[stale[bot]]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.