[WIP][pr2eus/robot-interface.l] Add method waiting until last sent motion is finished or given function returns t

[pazeshun]

We sometimes want to do :wait-interpolation just until some condition is satisfied (e.g., a sensor value becomes under a threshold).
In that case, we usually also want to stop robot motion just after that condition is satisfied.
This PR adds :wait-interpolation-until-func method able to be commonly used in that case.
Example usage with Gazebo:

(pr2-init)
(send *ri* :angle-vector (send *pr2* :reset-pose) 2000)
(send *ri* :wait-interpolation)
(send *ri* :angle-vector (send *pr2* :init-pose) 2000)
(send *ri* :wait-interpolation-until-func #'(lambda () (> (abs (aref (send *ri* :state :torque-vector) 1)) 5)))

This PR also includes test code of :wait-interpolation-until-func.

@Kanazawanaoaki Does this PR meet your requirement? If you have time, please test this and give your feedback comment.

[Affonso-Gui]

Neat feature!

However, I don't really like the method-when-func; it is confusing and a bit of a hassle to extend it (at least it could be a callable function instead of a method).

Wouldn't it be better to have different return values for when it finishes by :wait-interpolation and for when it finishes by func?
This way it should be easier for the user to control what to do when the call either fails or succeeds in a more simple, transparent and extendable form.
Maybe t and nil resembling unix sleep calls, or maybe the :interpolatingp list and the given function return value to be closer to the original :wait-interpolation method?

[pazeshun]

@Affonso-Gui Thank you for your comment.

However, I don't really like the method-when-func; it is confusing and a bit of a hassle to extend it (at least it could be a callable function instead of a method).

I changed to a callable function and changed name method-when-func -> post-process.

Wouldn't it be better to have different return values for when it finishes by :wait-interpolation and for when it finishes by func?
This way it should be easier for the user to control what to do when the call either fails or succeeds in a more simple, transparent and extendable form.
Maybe t and nil resembling unix sleep calls, or maybe the :interpolatingp list and the given function return value to be closer to the original :wait-interpolation method?

I think the default return value should be a list of :interpolatingp for all controllers as with :wait-interpolation.
This is easy to understand (good for beginners) and has a compatibility with the guideline of the other :wait-interpolation* methods: #187 (comment).

On the other hand, I understand your request, so I enabled to return another value by passing an additional argument :return-post-process-ret t.
I think there are three possible functions which we may want to get their return values in :wait-interpolation-until-func:

• func
• (send self :interpolatingp ctype)
• post-process (e.g., (send self :cancel-angle-vector :controller-type ctype))

As for func, we can know whether func returned t or nil by checking the default return value (the list of :interpolatingp) because this list includes t when func returned t and :wait-interpolation-until-func was aborted.
As for (send self :interpolatingp ctype), we can get its value easily by calling it just after :wait-interpolation-until-func.
So I think I only have to enable to return post-process return value.

[Affonso-Gui]

@pazeshun

Are you sure that it isn't better to just return the interpolatingp list and leave the post processing to the caller code?
:wait-interpolation is a waiting function, so I believe it shouldn't do anything other than waiting...

The :post-process and :return-post-process-ret also seem a bit too much of a trouble just to assign :cancel-angle-vector as a default post behavior, which is ultimately case dependent and by the way I also do not recommend to set it as default (since it is rather abrupt for some robots, can lead to behavior not predicted by the user and, in worst case scenario, damage the robot and/or its surroundings).

Just to be clear, my recommendation would be something like this:

(if (some #'identity (send *ri* :wait-interpolation-until-fn fn))
    (do-smt-when-interrupted)
  (do-smt-when-not-interrupted))

@knorth55 @Naoki-Hiraoka Any comments are appreciated.

[pazeshun]

@Affonso-Gui Sorry for late.
I found that

(if (some #'identity (send *ri* :wait-interpolation-until-fn fn))
    (do-smt-when-interrupted)
  (do-smt-when-not-interrupted))

sometimes doesn't work when we pass specific ctype because the return value of :wait-interpolation* includes all controllers according to the current guideline (#187 (comment),

jsk_pr2eus/pr2eus/robot-interface.l

Line 626 in a228cc7

(send-all controller-actions :interpolatingp))

).
For instance,

(progn (send *ri* :angle-vector (send *pr2* :init-pose) 3000 :rarm-controller) (send *ri* :angle-vector (send *pr2* :init-pose) 1000 :larm-controller) (send *ri* :wait-interpolation :larm-controller))

returns (nil t nil nil) (including t) even though (send *ri* :wait-interpolation :larm-controller) wasn't interrupted.
(So

As for func, we can know whether func returned t or nil by checking the default return value (the list of :interpolatingp) because this list includes t when func returned t and :wait-interpolation-until-func was aborted.

in #444 (comment) is wrong. Sorry...
)

Therefore, in order to execute the stopping motion without :post-process, we have to:

• extract interesting :interpolatingp values from the returned list; or
• change the guideline.

I think both are troublesome.
How about just setting default :post-process as nil?

As for :return-post-process-ret, I have to re-consider because #444 (comment) is partially wrong...

[Affonso-Gui]

Extract interesting :interpolatingp values from the returned list in the case a controller is given, or even better just setting the wait-interpolation-until-func return value to t or nil as I mentioned before do appear to me as a better alternative. But in case we are keeping the :post-process keyarg, defaulting it to nil seems nice.

Anyways, would still like to hear opinions from other people on this topic.
@708yamaguchi @Naoki-Hiraoka

[Naoki-Hiraoka]

Considering the use case of this function,

(send *ri* :angle-vector-until-finc #F(. . .) 5000 :rarm-controller 0 :func func)
;; Send joint angle to robot.
;; Then, wait until sent motion is finished or given function (func) returns t.
;; If func returns t, the motion stops.
;; If func returns t, return value is t. Otherwise return value is nil. 

may be sufficient.

Users who want to perform advanced processing will write their own control function, so they will not use this function. This function should be simple, because one of the roles of this function is to be referenced by such users as sample code.

[Naoki-Hiraoka]

Generalization of advanced processing is difficult.
:set-object-turnaround-ref-force of rtm-ros-robot-interface is one of the examples of advanced processing.
https://github.com/start-jsk/rtmros_common/blob/646c5b7e31f1cf1656733a950f4a7755ddd288b3/hrpsys_ros_bridge/euslisp/rtm-ros-robot-interface.l#L888-L975

[Affonso-Gui]

I agree with @Naoki-Hiraoka point of making this method as simple as possible for standard use cases.

To sum up:

• Remove post-process and return-post-process-ret key arguments
• Instead, use t/nil return values which allow for the user to define custom post processes outside the function body
• Always stop the motion after func returns t (would personally prefer using the :stop-motion method, which give us a smoother stop and can probably be overwritten by :cancel-angle-vector (?) if speed is needed)

Regarding the return value, if necessary the standard :interpolatingp value can be easily accessed by another method call, but whether the function was interrupted or not can only be accessed from within this method.

[pazeshun]

@Affonso-Gui @Naoki-Hiraoka
Sorry for super late...

I started to think that the function introduced in this PR should not be :wait-interpolation* nor :angle-vector*.
The former makes users think this function does nothing other than waiting, but I want to stop the robot motion in the function for usability.
The latter makes them think this function just sends something to the robot and returns immediately, but waiting is the essence of the function.

So I started to think about a new name now.
I came up with :stop-when-func-while-interpolation but it may be long.
What do you think about my idea?

[Affonso-Gui]

You have a point.
I personally don't think it would be too awful to write it as an exclusively waiting function and commonly use it as

(if (send *ri* :wait-interpolation-until fn)
    (send *ri* :stop-motion))

But if we want to include the stop motion into the function then maybe something like :interpolate-while or :stop-motion-if?
In this case it would also be nice to define the :wait-interpolation-until method and then add the other one as a wrapper.

(I think you can abbreviate the -fn and -func because it enters as an argument. Kinda like it is (find-if pred ...) instead of (find-if-pred pred ...))