add test for end-coords-interpolation #325

[k-okada]

add test for end-coords-interpolation problem #325

[Affonso-Gui]

In test-angle-vector-sequence-end-coords-interpolation, adding

(send *ri* :angle-vector (send *pr2* :reset-pose))

before

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

should recreate more accurately the conditions I tried.

Sorry about forgetting this in the original comment.

[Affonso-Gui]

:end-coords-interpolation-pass-time related:

• also needs to be added on :angle-vector
• should also be used when pushing to interpolated-tms (L494, L498)

ik-method related:

• ec-prev goes forward every time ik-method loop (L453) is repeated, causing some problems and inaccuracies.
• For instance, when executing test-angle-vector-sequence-end-coords-interpolation-head, on the second loop ec-prev becomes equal to ec-current, making target-limbs go to nil and the interpolation to be done as if :end-coords-interpolation were nil (and with time set to 50, since that wasn't fixed).

[k-okada]

@Affonso-Gui sorry for late and thank you for your in depth investigation , check out our latest commit 949b526, 60d8b26

[Affonso-Gui]

Here you can see the result of executing the two test functions with the code. The first call makes the robot move weirdly as always, because pr2-interface divided the angle-vector sequence into four, instead of the original two. The second call ends up in error.

The error in the second test function is caused because av-prev is not reseted to av-prev-orig during ik-method loop, making the robot try to move back to the original position by ik, and failing because it cannot use the torso. (Actually it appears that the torso cannot be used at all for calls like (send robot limb :inverse-kinematics coord), which makes the conversation we had about setting :use-torso to nil or not kind of pointless).

Here is the result after fixing the problem. end-coords-interpolation-pass-time also seems to be working fine, although high values can show some unpredictable behavior. Please note how the head and torso doesn't move at second test, since the ik-method nil got chosen.

That being said,

• Requested changes:

1. Reset av-prev in ik-method loop. Solved by, for example, moving the following lines inside the ik-method loop

;; restore states
(send robot :move-to c-orig :world)
(send robot :angle-vector av-orig)
(setq av-prev av-prev-orig)

This would also probably make L454 unnecessary (needs verification).

2. On test-angle-vector-sequence-end-coords-interpolation-head, should be better to send reset-pose before beginning the test. Sorry for forgetting about this in the original post.

• Remaining problems

1. Weird movement on test-angle-vector-sequence-end-coords-interpolation. Actually not :end-coords-interpolation's problem, but rather pr2-interface's lack of compatibility with this method.

2. Calculation and execution takes quite some time. Can be amended by raising pass-time value, but that leads to the next problem:

3. High pass-time interval seems to be unstable. Do not know if there is something we can do about this, though.

4. Final angle-vector passed to robot is (sometimes completely) different from designated one.

Honestly speaking, I do not really know if choosing ik-method like that is a good strategy or not.

[Affonso-Gui]

Updated #325 to behave properly when over-360 degree turns are present. Splitting the angle-vector by midpoint and/or midcoords did not went well, so I opt for interpolating with :sub-angle-vector and adding the difference in the end.

Can be verified with:

(send *ri* :angle-vector (send *pr2* :reset-pose))
(send *ri* :wait-interpolation)
(send *ri* :angle-vector #f(50.0 33.3265 55.5323 45.1982 -89.514 4.85469 -40.518 1000.0 -33.3265 55.5323 -45.1982 -89.514 -4.85469 -40.518 1000.0 0.0 0.0) 2000 :default-controller 0 :end-coords-interpolation t)

or

(send *ri* :angle-vector (send *pr2* :reset-pose))
(send *ri* :wait-interpolation)
(send *ri* :angle-vector-sequence
      (list #f(50.0 33.3265 55.5323 45.1982 -89.514 4.85469 -40.518 1000.0 -33.3265 55.5323 -45.1982 -89.514 -4.85469 -40.518 1000.0 0.0 0.0) (send *pr2* :reset-pose))
      (list 2000 2000)
      :default-controller 0 :end-coords-interpolation t)

Interpolation interval and time can be controlled with :end-coords-interpolation-steps and :min-time .