Fix :wait-interpolation-until not to finish immediately

pazeshun commented 5 years ago

Problem

:wait-interpolation-until sometimes finishes immediately after it is called. Once we encountered this problem in @knorth55 's experiment, I suggested adding additional (send *ri* :spin-once). However, although I tried this solution in my experiments, the problem still occurred.

Debugging Result

Long-term observation in my experiments told me that the following case causes the problem:

(send *ri* :interpolatingp) is t just after :wait-interpolation-until is called -> (send *ri* :interpolatingp) becomes nil for a moment (less than 0.1 second) even though motion is not finished -> (send *ri* :interpolatingp) goes back to t

:wait-interpolation-until firstly waits until (send *ri* :interpolatingp) becomes t and secondly waits until it becomes nil. So in the case above, :wait-interpolation-until finishes in the second step.

Solution

With this PR, :wait-interpolation-until measures the duration from t to nil. If that duration is too short, :wait-interpolation-until considers that the problem above occurs and start waiting for t again.

Additional Function

I previously thought the following case causes the problem: (send *ri* :interpolatingp) is nil just after :wait-interpolation-until is called -> (send *ri* :interpolatingp) doesn't become t in dotimes waiting in :wait-interpolation-until

Although this case was not observed, this PR prepares the function to support this. With :inf-wait-interpolatingp argument, :wait-interpolation-until waits infinitely for (send *ri* :interpolatingp) to become t:

(send *ri* :wait-interpolation-until :rarm :grasp :inf-wait-interpolatingp)

However, :inf-wait-interpolatingp causes infinite loop when robot motion finishes before :wait-interpolation-until, so I remain this function optional.

pazeshun commented 5 years ago

Tested with gripper-v6:

$ roseus baxter-interface.l
...
1.irteusgl$ jsk_arc2017_baxter::baxter-init :type :rarm-controller
PQP Error! EndModel() called on model with no triangles
[ WARN] [1557985039.169553197] [/default_robot_interface_1557985035623127840:ros.roseus]: [robot/end_effector/right_gripper/gripper_action] action server is not found
[ WARN] [1557985039.169973584] [/default_robot_interface_1557985035623127840:ros.roseus]:      goal=0, cancel=0, feedback=0, result=0
[ WARN] [1557985039.170062637] [/default_robot_interface_1557985035623127840:ros.roseus]: #<ros::simple-action-client #X8282eb8 robot/end_effector/right_gripper/gripper_action> is not respond
[ INFO] [1557985039.170121644] [/default_robot_interface_1557985035623127840:ros.roseus]: *** if you do not have gripper, you can ignore this message ***
PQP Error! EndModel() called on model with no triangles
#f(19.9731 -36.2109 -65.4346 24.6753 108.259 -20.896 47.6147 143.306 0.699495 -0.178926 4.10889 -69.1699 39.5068 99.3384 -13.2275 57.4146 -10.0635 36.1897 1.43141)
2.irteusgl$ send *ri* :calib-pressure-threshold :rarm
[ INFO] [1557985057.628329796] [/default_robot_interface_1557985035623127840:ros.roseus]: [:calib-pressure-threshold] Threshold r: 826.36 l: nil
t
3.irteusgl$ progn (send *ri* :start-grasp :rarm) (send *ri* :angle-vector (send *baxter* :angle-vector) 10000) (send *ri* :wait-interpolation-until :rarm :grasp) (send *ri* :stop-grasp :rarm)
moveit environment is not correctly set, execute :angle-vector-raw instead
;; #<rotational-joint #X8bd5f30 left_gripper_finger_yaw_joint> :joint-angle(-0.225361) violate min-angle(0.0)
[ INFO] [1557985063.838600329] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Init flex:  r: 701 l: 607
[ INFO] [1557985063.838644683] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Init load: 0.0
[ INFO] [1557985063.838662483] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Prepare for grasp
[ INFO] [1557985063.838716217] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Waiting for interpolatingp
[ INFO] [1557985063.838775951] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] interpolatingp becomes t
[ INFO] [1557985066.384323766] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Detected grasping
[ INFO] [1557985066.384363927] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Cancel angle vector: :rarm
[ INFO] [1557985066.434185221] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Detected grasping
[ INFO] [1557985066.434235333] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Cancel angle vector: :rarm
nil
4.irteusgl$ (send *ri* :wait-interpolation-until :rarm :grasp :inf-wait-interpolatingp)
[ INFO] [1557985179.197122563] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Init flex:  r: 700 l: 603
[ INFO] [1557985179.197181116] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Init load: -0.007812
[ INFO] [1557985179.197206986] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Prepare for grasp
[ INFO] [1557985179.197273297] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Waiting for interpolatingp
[ERROR] [1557985179.305969646] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] interpolatingp is nil in dotimes
[ERROR] [1557985179.306045232] [/default_robot_interface_1557985035623127840:ros.roseus]: [:wait-interpolation-until] Escalate to while
(enters infinite loop)

pazeshun commented 5 years ago

Tested with gripper-v7/gripper-v8

pazeshun commented 5 years ago

Infinite loop occurs even without :inf-wait-interpolatingp. Current code is wrong. Back to WIP:

63.irteusgl$ send *ri* :wait-interpolation-until :larm :grasp
[ INFO] [1559366793.054740746] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] Init flex:  r: 3204 l: 3131
[ INFO] [1559366793.054791065] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] Init load: 0.0
[ INFO] [1559366793.054809589] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] Prepare for grasp
[ INFO] [1559366793.054841829] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] Waiting for interpolatingp
[ERROR] [1559366793.380538645] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] interpolatingp becomes nil too early
[ERROR] [1559366793.380621392] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] strange behavior, waiting again
[ INFO] [1559366793.380657302] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] Waiting for interpolatingp
[ERROR] [1559366793.709716457] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] interpolatingp becomes nil too early
[ERROR] [1559366793.709792681] [/default_robot_interface_1559358929908176640:ros.roseus]: [:wait-interpolation-until] strange behavior, waiting again
...

pazeshun commented 5 years ago

Infinite loop occurs even without :inf-wait-interpolatingp. Current code is wrong.

This is fixed in gripper-v6:

$ roseus baxter-interface.l
...
1.irteusgl$ jsk_arc2017_baxter::baxter-init :type :rarm-controller
...
2.irteusgl$ send *ri* :calib-pressure-threshold :rarm
[ INFO] [1560918002.268901551] [/default_robot_interface_1560917975750995522:ros.roseus]: [:calib-pressure-threshold] Threshold r: 815.81 l: nil
t
3.irteusgl$ send *ri* :wait-interpolation-until :larm :grasp
[ INFO] [1560918013.421021299] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Init flex:  r: 645 l: 631
[ INFO] [1560918013.421092419] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Init load: 0.0
[ INFO] [1560918013.421139926] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Prepare for grasp
[ INFO] [1560918013.421280333] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Waiting for interpolatingp
[ INFO] [1560918013.529456700] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] interpolatingp remains nil
nil

Also, other test cases were checked in gripper-v6:

4.irteusgl$ progn (send *ri* :start-grasp :rarm) (send *ri* :angle-vector (send *baxter* :angle-vector) 10000) (send *ri* :wait-interpolation-until :rarm :grasp) (send *ri* :stop-grasp :rarm)
moveit environment is not correctly set, execute :angle-vector-raw instead
[ INFO] [1560918063.397248245] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Init flex:  r: 685 l: 600
[ INFO] [1560918063.397333122] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Init load: -0.007812
[ INFO] [1560918063.397354142] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Prepare for grasp
[ INFO] [1560918063.397380188] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Waiting for interpolatingp
[ INFO] [1560918063.397421866] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] interpolatingp becomes t
[ INFO] [1560918066.089455642] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Detected grasping
[ INFO] [1560918066.089509074] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Cancel angle vector: :rarm
[ INFO] [1560918066.139349672] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Detected grasping
[ INFO] [1560918066.139389655] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Cancel angle vector: :rarm
nil
5.irteusgl$ (send *ri* :wait-interpolation-until :rarm :grasp :inf-wait-interpolatingp)
[ INFO] [1560918086.028408362] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Init flex:  r: 691 l: 604
[ INFO] [1560918086.028475440] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Init load: -0.007812
[ INFO] [1560918086.028509865] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Prepare for grasp
[ INFO] [1560918086.028596066] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Waiting for interpolatingp
[ERROR] [1560918086.136591968] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] interpolatingp is nil in dotimes
[ERROR] [1560918086.136673744] [/default_robot_interface_1560917975750995522:ros.roseus]: [:wait-interpolation-until] Escalate to while
(enters infinite loop)

pazeshun commented 5 years ago

Tested with gripper-v7/gripper-v8. No more WIP.

knorth55 commented 5 years ago

@pazeshun can i merge this PR?

pazeshun commented 5 years ago

Yes, please. If you find new problem in :wait-interpolation-until, please tell me.

start-jsk / jsk_apc