Fix :end-coords-interpolation problems

[Affonso-Gui]

Fixing some problems in :end-coords-interpolation.

Namely,

1. Does not behave properly when over-360 degree turns are present. This is because pr2-interface splits the angle vector in its midpoints, which is not what we want to do in :end-coords-interpolation.

2. Move only in 50 ms when all end-coords are the same.

3. IK usually fails when the head moves. Dealing with it by having :head and :torso move by midpoint vector, instead of IK.

4. It actually does not send designed angle-vector in the end.

5. Solving IK in order may cause arms IK to change desired torso position. Adding :use-torso nil.

This problems can be reproduced with

(setq vec #f(50.0 47.7222 12.2897 117.176 -106.014 -77.3995 -76.8709 -105.904 -47.7222 12.2897 -117.176 -106.014 77.3995 -76.8709 105.904 0.0 0.0)
      v0 #f(50.0 53.1894 14.6871 123.249 -121.524 -74.0435 -76.2839 266.776 -53.1894 14.6871 -123.249 -121.524 74.0435 -76.2839 93.2238 0.0 0.0)
      v1 #f(50.0 -16.4077 18.0876 93.6936 -54.8829 77.4245 -30.5008 123.25 -50.4711 19.1116 -59.6967 -13.4894 275.94 -109.525 -35.7734 -47.3098 49.3376)
      v2 #f(191.303 50.0682 17.9701 102.195 -10.7369 43.6391 -103.396 221.198 32.3493 45.9097 -15.2106 -55.9981 227.517 -63.9359 38.9722 0.0 0.0))

;;over 360                                                                                         
(send *ri* :angle-vector-sequence (list vec (send *pr2* :reset-pose)) (list 1000 1000) :default-controller 0 :end-coords-interpolation t)

;;same end-coords                                                                                 
 (send *ri* :angle-vector v0 10000 :default-controller 0 :end-coords-interpolation t)

;;with head movement                                                                               
(send *ri* :angle-vector v1)
(send *ri* :angle-vector v2 2000 :default-controller 0 :end-coords-interpolation t)

Captures showing the above code executed on both master branch and after the changes are bellow.
master
with changes

[k-okada]

Great,

1. I have added two test code at add test for end-coords-interpolation #325 #326
2. for head problem, I think it makes sense to remove :head, :torso from limbs. but set use-torso nil is too agressive, if you want we'd better to use option to change by users,
3. For rotation problem, I think the root problem is solving IK in (when end-coords-interpolation ;; set end-coords interpolation block, I still working on this, but for example if we change something like below, we can get better result

                   ;; set midpoint of av as initial pose of IK                     
                   ;;(send robot :angle-vector (midpoint p av-prev av))            
                   (send robot :angle-vector av-prev)

[k-okada]

                   ;; set midpoint of av as initial pose of IK                     
                   ;;(send robot :angle-vector (midpoint p av-prev av))            
                   ;;(send robot :angle-vector av-prev)                            
                   ;;(send robot :angle-vector av)                                 
                   (if (car interpolated-avs)
                       (send robot :angle-vector (car interpolated-avs)))

will pass the tests, but would cause another problem...

[k-okada]

@knorth55 @pazeshun are you using :end-coords-interpolation? if so , please add your use case, we'd like to your case into test code so that we do not have regression on this fix.

[Affonso-Gui]

1. Commented one change on the test code in add test for end-coords-interpolation #325 #326. Still fails normally and passes after changes or with

                   ;; set midpoint of av as initial pose of IK                     
                   ;;(send robot :angle-vector (midpoint p av-prev av))            
                   ;;(send robot :angle-vector av-prev)                            
                   ;;(send robot :angle-vector av)                                 
                   (if (car interpolated-avs)
                       (send robot :angle-vector (car interpolated-avs)))

2. Agree on making :use-torso be an option changeable by users. However, still believe setting it to nil should be better for avoiding unintended and/or fast torso movements.

On this topic, I have also thought about making the time-interval of the IK's (in this case 100) an option and/or passing &rest arguments to the IK call (like :thre). At the end I found it to be too many options for a rarely-used method and did not implement it, but what do you think about it? Is it worth it?

3. As another option, I have also tried to add an :additional-check to find a solution fairly close (or the closest among them) to the previous angle-vector. It looked something like this:

 (let ((min 1000)
        (origin-vec (send *pr2* :angle-vector))
        vec)
...
     (send *pr2* :inverse-kinematics end-lst :stop times :additional-check
           #'(lambda ()
               (let ((diff (norm (v- (send *pr2* :angle-vector) origin-vec))))
                 (if (< diff min) (setq vec (send *pr2* :angle-vector) min diff)) (< diff cthre))))
...

As a result I got a rather smooth path, but calculations took a long time and final angle-vector was pretty different from given one. However, without pr2-interface.l changes it stills draws a weird path, zig-zagging between the generated midpoint vectors.

[knorth55]

@knorth55 @pazeshun are you using :end-coords-interpolation? if so , please add your use case, we'd like to your case into test code so that we do not have regression on this fix.

@pazeshun used this function in ARC2017.

[pazeshun]

@knorth55 @pazeshun are you using :end-coords-interpolation? if so , please add your use case, we'd like to your case into test code so that we do not have regression on this fix.

@pazeshun used this function in ARC2017.

Sorry for late.
https://github.com/start-jsk/jsk_apc/blob/master/jsk_arc2017_baxter/euslisp/lib/arc-interface.l#L778-L779

[k-okada]

ok, I have updated https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/326/files, this will chose smoothest interpolation from different strategy, check if this works for @Affonso-Gui and did not break your current system @pazeshun

Agree on making :use-torso be an option changeable by users. However, still believe setting it to nil should be better for avoiding unintended and/or fast torso movements.

can you add this change to PR?

On this topic, I have also thought about making the time-interval of the IK's (in this case 100) an option and/or passing &rest arguments to the IK call (like :thre). At the end I found it to be too many options for a rarely-used method and did not implement it, but what do you think about it? Is it worth it?

Is 449446b ok?

As another option, I have also tried to add an :additional-check to find a solution fairly close (or the closest among them) to the previous angle-vector. It looked something like this:

Not sure if I understand correctly, does #326 solve your problem? since :inverse-kinematics is gradient based method, so if you start calculation from "previous angle-vector", then you'll get closest solution. If you want to "choose" from multiple solutions, you have to call ik from multiple initial point (We've seen this approach at @ishiguroJSK 's IROS report at lab meeting, which was noted as a not a good idea in Kajita-san's text book)

[Affonso-Gui]

Interesting approach, but still had some problems when testing. Pointed out some of them on #326, some others are the ones already commented above.

1. Does not behave properly when over-360 degree turns are present. This is because pr2-interface splits the angle vector in its midpoints, which is not what we want to do in :end-coords-interpolation.
2. Move only in 50 ms when all end-coords are the same.
3. It actually does not send designed angle-vector in the end.

Also, it seems to be somewhat cost-expensive: it took me around 5 seconds to calculate the path for test-angle-vector-sequence-end-coords-interpolation.

can you add this change to PR?

Added :use-torso and :end-coords-interpolation-pass-time to this PR 6db0e99

Actually I do not believe that the main problem is the path not being smooth enough, but rather the fact that the trajectory gets split up in pr2-interface, before reaching robot-interface's :angle-vector-sequence.

[Affonso-Gui]

Updated for

• use steps (number of divisions) instead of pass-time argument
• theoretically interpolate in given time.
• move accordingly for over 360 deg rotations

[k-okada]

@Affonso-Gui I have added your code to #326, but not sure about the logic of this code.

I thought...
setq av.. update av from avs to av-prev + sub-angle-vector, so if there is no 180 rotation this new av is av. Let set this new av as new-av.
Then, we calculate av - new-av - diff-prev, If new-av is equal to av, then diff is - diff-prev, Where did I go wrong?

[Affonso-Gui]

@k-okada It's exactly as you said, when there is no 180 turns diff is set to - diff-prev, meaning to undo last rotation.

For example, in an av sequence from

0 → 1000 → 0

the new-av sequence becomes:

0 → - 80 → 0

Which is then used to set midpoint for IK.
In this case we have an over 180 deg rotation at first interpolation (0 → 1000) and none at the second (- 80 → 0), making the diff become:

  +1080 , -1080

Which is then slowly added to prev-av and to the IK result (Line 484) in order to generate something like:

0 → 200 → 500 → 800 → 1000 → 800 → 500 → 200 → 0 

[k-okada]

thank you for explanation, merged.