Add :rotate-wrist-ik

[pazeshun]

What is problem

I feel IK failures in picking task are sometimes caused by wrist angle limit.

I'll show you one example:
Baxter's initial pose

got by

36.irteusgl$ send *ri* :ik->bin-entrance :larm :b :offset #f(-30 0 0) :rotation-axis t

I want to solve IK to the coordinate in the picture above:

37.irteusgl$ ec
#<coordinates #Xa1ab078  965.957 13.486 844.095 / 0.034 -0.056 1.566>

Normal IK fails:

38.irteusgl$ send *baxter* :larm :inverse-kinematics ec
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for slow ik, try to start from prepared poses
; starting from prepared pose ':untuck-pose'
nil

This is caused because wrist angle is too near joint limit:

39.irteusgl$ send *baxter* :larm :wrist-r :joint-angle
115.831
40.irteusgl$ send *baxter* :larm :wrist-r :max-angle
175.268

But wrist has wide range to opposite direction, too.

42.irteusgl$ send *baxter* :larm :wrist-r :min-angle
-175.268

So rotating wrist to opposite direction is the solution.

45.irteusgl$ send *baxter* :larm :wrist-r :joint-angle -180 :relative t
-64.1695
46.irteusgl$ send *baxter* :larm :inverse-kinematics ec
#f(0.0 -34.2761 -66.8794 -76.907 69.884 -70.546 -61.3844 -166.702 90.0 -20.0 -25.0 40.0 60.0 20.0 80.0 0.0 0.0)

(send *ri* :send-av 1000) will move robot like below:

What I did

In :rotate-wrist-ik method, I set 6 patterns of angle vector whose wrist angle is different each other to pose-candidates of :ik->nearest-pose.
With this, you can ignore wrist angle limits:

48.irteusgl$ send *ri* :ik->bin-entrance :larm :b :offset #f(-30 0 0) :rotation-axis t
; failed for normal ik, starting from relaxed position
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for normal ik, starting from relaxed position
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for normal ik, starting from relaxed position
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for normal ik, starting from relaxed position
[:ik->nearest-pose] arm::larm midpose: #f(0.0 37.4634 -29.3774 -142.449 117.883 -82.0679 -18.8306 123.882 90.0 -20.0 -25.0 40.0 60.0 20.0 80.0 0.0 0.0)
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
[ INFO] [1483202189.378042794]: gripper-angle: 90
[:ik->nearest-pose] arm::larm midpose: #f(0.0 48.8672 -10.437 -80.6616 148.645 80.9033 1.38428 18.8745 90.0 -20.0 -25.0 40.0 60.0 20.0 80.0 0.0 0.0)
#f(0.0 -24.9233 -79.766 -70.0252 64.2905 -94.6367 -49.8544 115.831 90.0 -20.0 -25.0 40.0 60.0 20.0 80.0 0.0 0.0)
49.irteusgl$ send *ri* :rotate-wrist-ik :larm ec
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for slow ik, try to start from prepared poses
; starting from prepared pose ':untuck-pose'
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for slow ik, try to start from prepared poses
; starting from prepared pose ':untuck-pose'
; failed for normal ik, starting from relaxed position
; failed for normal ik, try to move arms very slowly
; failed for slow ik, try to start from prepared poses
; starting from prepared pose ':untuck-pose'
[:ik->nearest-pose] arm::larm midpose: #f(0.0 -24.9233 -79.766 -70.0252 64.2905 -94.6367 -49.8544 -87.6339 90.0 -20.0 -25.0 40.0 60.0 20.0 80.0 0.0 0.0)
#f(0.0 -34.9018 -68.4358 -74.4777 70.0551 -72.4282 -60.2473 -167.163 90.0 -20.0 -25.0 40.0 60.0 20.0 80.0 0.0 0.0)

[wkentaro]

Please rebase

[pazeshun]

rebased

[pazeshun]

@wkentaro Please merge this