jsk-ros-pkg / jsk_pr2eus

PR2 euslisp packages
https://github.com/jsk-ros-pkg/jsk_pr2eus
4 stars 41 forks source link

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

Open pazeshun opened 4 years ago

pazeshun commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

@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: https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/187#issuecomment-409224920.

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:

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 commented 4 years ago

@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 commented 4 years ago

@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 (https://github.com/jsk-ros-pkg/jsk_pr2eus/issues/187#issuecomment-409224920, https://github.com/jsk-ros-pkg/jsk_pr2eus/blob/a228cc7d3bb5b26b92ed25c7d55f32a64d9d7825/pr2eus/robot-interface.l#L626). 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 https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/444#issuecomment-704035383 is wrong. Sorry... )

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

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 https://github.com/jsk-ros-pkg/jsk_pr2eus/pull/444#issuecomment-704035383 is partially wrong...

Affonso-Gui commented 4 years ago

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 commented 4 years ago

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 commented 4 years ago

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

pazeshun commented 3 years ago

@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 commented 3 years ago

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 ...))