Optimization condition is not correct?

[708yamaguchi]

I'm sorry to have bothered you so many times. @HiroIshida I would appreciate it if you could respond when you have time.

I have found that in two cases the condition of optimization is behaving in an unexpected way.

Case 1: Robot mistakes front and back of polygon.

The robot right hand is reaching for the back side of the polygon.

From the following video, it looks like we can get the polygon direction information from coefficient topic. https://drive.google.com/file/d/1w6r7GluTNfzuu0ACJJcrX-nx3g5MDasa/view

Result of sqp	Robot pose

case1.pickle: https://drive.google.com/file/d/1h_h90w8RnOBILkFC-8pxR1H5N6Sn6St6/view?usp=sharing

Case 2: Robot is reaching for a place with no polygons.

Although sqp result is success: True, there is no polygon in front of the robot's right hand. Maybe constraint function has something wrong(?)

Result of sqp	Robot pose

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

Test code

This program is almost copied from example_multi.py

#!/usr/bin/env python
import argparse
import math
import numpy as np
import pickle
from skrobot.coordinates.math import rotation_matrix
from yamaopt.solver import KinematicSolver, SolverConfig
from yamaopt.polygon_constraint import polygon_to_trans_constraint
from yamaopt.visualizer import VisManager

if __name__=='__main__':
    parser = argparse.ArgumentParser()
    parser.add_argument('-robot', type=str, default='pr2', help='robot name')
    parser.add_argument('-hover', type=float, default='0.05', help='hover distance')
    parser.add_argument('--visualize', action='store_true', help='visualize')
    parser.add_argument('--use_base', action='store_true', help='with base')
    args = parser.parse_args()
    robot_name = args.robot
    visualize = args.visualize
    use_base = args.use_base
    d_hover = args.hover

    if robot_name == 'fetch':
        config_path = "../config/fetch_conf.yaml"
    elif robot_name == 'pr2':
        config_path = "../config/pr2_conf.yaml"
    else:
        raise Exception()

    config = SolverConfig.from_config_path(config_path, use_base=use_base)
    kinsol = KinematicSolver(config)

    with open('/home/leus/Downloads/case1.pickle', 'rb') as f:
    # with open('/home/leus/Downloads/case2.pickle', 'rb') as f:
        polygons = pickle.load(f)
    target_obj_pos = np.array([1.0, 0.3, 1.0])

    q_init = -np.ones(len(kinsol.control_joint_ids)) * 0.4
    sol, target_polygon = kinsol.solve_multiple(q_init, polygons, target_obj_pos, d_hover=d_hover)
    assert sol.success

    if visualize:
        # visualize
        vm = VisManager(config)
        vm.add_polygon_list(polygons)
        vm.add_target(target_obj_pos)
        vm.set_angle_vector(sol.x)
        vm.show_while()

[708yamaguchi]

For a while, I will try to solve this issue by myself.

[HiroIshida]

About case1

Thanks for report, as for case1, it's absolutely just my mistake becase this assumption https://github.com/HiroIshida/yamaopt/blob/3da81e128882a91781c74807b31bbeef3c9b9565/yamaopt/polygon_constraint.py#L72 is wrong. It's nice if your fix the problem by using normal vector date from jsk_ros_pkg's output.

About case2

in my environment, it seems work fine. (visualization is based on https://github.com/HiroIshida/yamaopt/pull/19)

After enabling torso by editting config.yaml file ....

[708yamaguchi]

Thank you so much! :smile:

About case1

It's nice if your fix the problem by using normal vector date from jsk_ros_pkg's output.

OK, I will try. In my idea, I add x_axis option to polygon_to_desired_rpy so that user can use original coefficient information.

def polygon_to_desired_rpy(np_polygon, x_axis=None):
...
    if x_axis is None:
        x_axis = normalize(
            np.cross(points[2] - points[1], points[1] - points[0]))
...

Then, I will pass coefficient rostopic information to this new function in yamaopt_ros.

About case2

Hmmmm....

Something is different between my environment and your environment...

Could you try example_multiple.py in the following branch? https://github.com/708yamaguchi/yamaopt/tree/debugging-polygon

In this branch, I edit example_multiple.py to use Problematic polygons.

The result is here.

$ python example_multi.py -robot pr2 --visualize --use_base

[HiroIshida]

About case1, I think your idea is good!

About case2, the result is still seems to fine on your branch on python3.

Maybe because of python2-3 difference. I will try later, but today maybe I have no time to try it ...

[708yamaguchi]

thank you for your advice! I will check today!

[HiroIshida]

I tried on python 2.7.18 also, but the result was the same...

h-ishida@stonet4:~/python/yamaopt/example$ python --version
Python 2.7.18
h-ishida@stonet4:~/python/yamaopt/example$ python example_multi.py -robot pr2 --visualize --use_base

[708yamaguchi]

I dont think so, but scipy version problem??

could you show result of pip freeze?

I will try tonight.

[HiroIshida]

Result of pip freeze 2.7.18.txt 3.8.10.txt

Also added test case with your real data. https://github.com/HiroIshida/yamaopt/pull/24 In all version test passed. Could you try pytest on this PR on your environment?

[708yamaguchi]

result of pip freeze 2.7.17.txt

Could you try example_multiple.py in the following branch? 708yamaguchi/yamaopt@debugging-polygon

Compared with my python2.7 environment and your python2.7 environment, I found that scipy==0.19.1 and scipy==1.2.3 outputs different result. (To be more precise, the behavior seems to have changed between scipy==1.1.0 and scipy==1.2.0.)

scipy==0.19.1

scipy==1.2.3

[708yamaguchi]

Also added test case with your real data. #24

Thank you so much for adding tests!!! Using scipy==0.19.1, Tests successfully fails https://github.com/HiroIshida/yamaopt/pull/25

My conclusion is that scipy>=1.2.0 solves everything, although I don't know the reason...

If my guess seems ok, please merge https://github.com/HiroIshida/yamaopt/pull/26

[708yamaguchi]

By the way, test fails when the following q_test is used.

Is it ok to loose test condition (e.g. decimal=4 -> decimal=3)?

Or should I fix the program?

q_test = np.array([-0.74996962, 2.0546241, 0.05340954, -0.4791571, 0.35016716,  0.01716473, -0.42914228])

https://github.com/HiroIshida/yamaopt/blob/f5a6c058af57c6a2385d3499d8d8fba0c7bcab37/tests/test_solver.py#L55-L60

I found this failure by

cd yamaopt
pytest tests/test_solver.py

Part of the result.

constraint = <function eq_constraint at 0x7f5a35aed7d0>
q_test = array([-0.74996962,  2.0546241 ,  0.05340954, -0.4791571 ,  0.35016716,
        0.01716473, -0.42914228])

    def _test_constraint_jacobian(constraint, q_test):
        f = lambda q: constraint(q)[0]
        jac_numel = compute_numerical_jacobian(f, q_test)
        jac_anal = constraint(q_test)[1]
>       np.testing.assert_almost_equal(jac_numel, jac_anal, decimal=4)
E       AssertionError: 
E       Arrays are not almost equal to 4 decimals
E       
E       Mismatched elements: 6 / 28 (21.4%)
E       Max absolute difference: 0.00065021
E       Max relative difference: 12487805.87804878
E        x: array([[ 5.4254e-02,  6.2555e-01,  1.6270e-01,  3.4992e-01, -2.8160e-03,
E                7.3876e-02,  2.7756e-11],
E              [ 1.3767e-08, -2.3421e+01,  1.2783e+01, -2.4687e+01,  7.2081e-01,...
E        y: array([[ 5.4254e-02,  6.2555e-01,  1.6270e-01,  3.4992e-01, -2.8160e-03,
E                7.3876e-02, -2.2226e-18],
E              [ 0.0000e+00, -2.3422e+01,  1.2783e+01, -2.4687e+01,  7.2081e-01,...

tests/test_solver.py:54: AssertionError

[HiroIshida]

As for the test failure due to initial angle vector, let me fix the problem. (I'm on my way back to Tokyo, then I will tackle this issue)

By the way, Possible reasons are starting from (a) a singular condition or (b) a angle vector breaking the joint limit

[HiroIshida]

@708yamaguchi Sorry I didn't read the issue well. It's just an issue of discretization accuracy. I fixed that issue here: https://github.com/HiroIshida/yamaopt/pull/27

[708yamaguchi]

Thank you!!

I understand that you solve the problem by calculating numerical jacobian more precisely.