Add pinching with gripper-v6 #2203

pazeshun · 2017-07-03T05:10:30Z

From #2160

pazeshun · 2017-07-03T16:47:08Z

Remaining TODOs

Test grasp stabilization by flex sensor in real robot
Calibrate thresholds for :graspingp (depending on grasp stabilization) -> graspingp for pinching is difficult for small objects #2210
~~- [ ] Test object postion offset in real task (temp fix for point cloud offset)~~ -> Another PR if needed
Test place-object of pick task
Test with moveit (I totally forgot this)
- pick task
- stow task

pazeshun · 2017-07-04T02:32:28Z

My Idea

Try cylindrical pinching after suction grasp fails

Main Changes

In *ti*:

~~Split previous :try-to-pick-object into :try-to-pick-object-v4 and :try-to-suction-object for gripper-v6~~
Split previous :try-to-pick-object into :try-to-suction-object-with-gripper-v4 and :try-to-suction-object-with-gripper-v6
Add :try-to-pinch-object
Select :try-to-suction-object or :try-to-pinch-object depending on grasp-style slot variable
Reset picking-fail-count when target object changes

In state machine:

Add set-grasp-style state after recongnize-object state for deciding grasp-style
Swap check-picking-fail-count and return-from-pick-object
Return to recognize-object if check-picking-fail-count is nil

Stow state machine:

Pick state machine:

Movies

Stow: https://drive.google.com/open?id=0BxxBA3J-CunGeF9HdnE2b05jQmM
Pick:
(pinch motion) https://drive.google.com/open?id=0BxxBA3J-CunGTmJGUHhkSXdNRUU
(suction motion) https://drive.google.com/open?id=0BxxBA3J-CunGeGRMWmRGUlZPQUk

Current Problems

#2209 #2210
I'll fix those after this PR.
#2211 ~~#2212~~ ~~#2213~~

pazeshun · 2017-07-04T10:01:00Z

Done for basic motion.
We can fix #2212 and #2213 whether in this PR or after merging this PR.

pazeshun · 2017-07-04T11:55:31Z

Close #2111

knorth55 · 2017-07-04T11:56:44Z

jsk_arc2017_baxter/euslisp/lib/arc-interface.l

@@ -344,42 +366,21 @@
      (setq obj-box-z-length (z-of-cube (send self :bbox->cube obj-box)))
      (setq obj-pos (v+ obj-pos (float-vector 0 0 (/ obj-box-z-length 2))))
      obj-pos))
-  (:try-to-pick-object
+  (:try-to-pick-object-v4


why did you put -v4?
Your gripper is -v6, right?
It is easier to understand to remain :try-to-pick-object

I prefer :try-to-pick-object

:try-to-pick-object-v4 is for current left hand.
This method is almost the same as :try-to-pick-object before #2155
:try-to-suction-object and :try-to-pinch-object are for gripper-v6 on right hand.

But if you think another name is good, I'll follow you.

I think ::try-to-pick-object is common method for both arm and you should not remove it.
Also it should be meta method for :try-to-pinch-object and try-to-suction-object.
try to be like
pick-object-in-bin or pick-object-in-tote
-> pick-object-with-movable-region
-> :try-to-pick-object
-> try-to-suction-object or try-to-pinch-object

なるほど、

:try-to-pick-object -> :try-to-suction-object-v4 -> :try-to-suction-object -> :try-to-pinch-object

ですかね

こうするメリットを教えていただけると嬉しいです。
こうするとすると、:try-to-pick-objectの中身を見ると、grasp-styleの値によって:try-to-pinch-objectと:try-to-suction-objectが呼び分けられている感じにすればよいでしょうか。

上流が:pick-objectなので:try-to-pick-objectをメタなmethodとして残しておいて欲しいです。
そのほうがわかりやすくかつ:pick-object-with-movable-regionの肥大化を防ぐためです。

:try-to-suction-objectは最初にif (eq arm :larm)を一つ追加するだけだと思います。

もしくは:try-to-suction-objectをメタなmethodとして
(send *ri* :try-to-suction-object :larm)
-> (send *ri* :try-to-suction-object-with-gripper-v2 :larm)

(send *ri* :try-to-suction-object :rarm)
-> (send *ri* :try-to-suction-object-with-gripper-v6 :rarm)
とするのはどうでしょう？

あと左手のgripperは僕の理解ではv2だと思うのですが違うのですか？

もしくは:try-to-suction-objectをメタなmethodとして
(send ri :try-to-suction-object :larm)
-> (send ri :try-to-suction-object-with-gripper-v2 :larm)

(send ri :try-to-suction-object :rarm)
-> (send ri :try-to-suction-object-with-gripper-v6 :rarm)
とするのはどうでしょう？

こっちの方が分かりやすい気がしたので、こっちの方向で修正します。
左手が変わった時には、:try-to-suction-object-with-gripper-v4を消して:try-to-suction-object-with-gripper-v6->:try-to-suction-objectという感じですね。

あと左手のgripperは僕の理解ではv2だと思うのですが違うのですか？

すみません、これはv4でおねがいします。

(:try-to-suction-object (arm) (if (eq arm :larm) (send self :try-to-suction-object-with-gripper-v4 arm) (send self :try-to-suction-object-with-gripper-v6 arm)))

みたいな感じで左手もv6になったらifを消すか置き換えるかでお願いします。
（個人的には置き換えないほうがv7, v8のためになるかなと思います）
gripperを昔のものを使いたいこともあるとおもうので（主にぼく）:try-to-suction-object-with-gripper-v4は消さなくてもいいと思いますが

直しました

pazeshun · 2017-07-04T15:53:07Z

Reflect @knorth55 's comment and back to WIP
I'll test this with #2216 tomorrow

knorth55 · 2017-07-04T16:38:55Z

jsk_arc2017_baxter/euslisp/lib/arc-interface.l

@@ -366,7 +361,17 @@
      (setq obj-box-z-length (z-of-cube (send self :bbox->cube obj-box)))
      (setq obj-pos (v+ obj-pos (float-vector 0 0 (/ obj-box-z-length 2))))
      obj-pos))
-  (:try-to-pick-object-v4
+  (:try-to-pick-object
+    (arm obj-pos pinch-yaw &key (offset #f(0 0 0)) (grasp-style :suction))


can you modify to pinch-yaw as a key arg?

IMO, pinch-yaw is necessary for pinching as obj-pos, so we should remain this.

Sorry, my comment above is for try-to-pinch-object.
I'll fix pinch-yaw of try-to-pick-object

pazeshun · 2017-07-05T01:17:53Z

Tested in real robot with #2216

pazeshun · 2017-07-05T01:37:48Z

But I found that moveit motion plan failed while pinching:

[ INFO] [1499217109.536797435]: [:try-to-pinch-object] arm::rarm pinch-yaw: 1.5708
~~~
[ INFO] [1499217116.422882344]: [:try-to-pinch-object] arm::rarm graspingp: nil
[ INFO] [1499217116.422938943]: [:try-to-pinch-object] arm::rarm lift the object
;; #<rotational-joint #X15bcc7f0 head_pan> :joint-angle(81.8701) violate max-angle(80.0021)
;; #<rotational-joint #X15bcc7f0 head_pan> :joint-angle(81.9141) violate max-angle(80.0021)
[ INFO] [1499217120.792329434]: [:try-to-pinch-object] arm::rarm graspingp: nil
[ERROR] [1499217121.039785890]: attempted to call service  /plan_kinematic_path, but failed 
;; motion plan error at nil
;; #<rotational-joint #X15bcc7f0 head_pan> :joint-angle(81.936) violate max-angle(80.0021)
[ INFO] [1499217121.092368014]: [main] arm: :rarm graspingp: nil
[ INFO] [1499217121.604114825]: [main] arm: :rarm, picking fail count: 2

I think this is because of collision between gripper base and fingers.
@knorth55 Please help me while fixing this.

pazeshun · 2017-07-05T03:54:16Z

Includes #2216

pazeshun · 2017-07-05T04:26:50Z

Close #2213

pazeshun · 2017-07-05T05:16:28Z

Movie was updated

pazeshun · 2017-07-05T05:43:54Z

Close #2212

pazeshun · 2017-07-05T09:40:05Z

Fixed failure of moveit motion plan.
All required in this PR is done.
@knorth55 Please fix #2211 in another PR

pazeshun · 2017-07-05T13:05:53Z

@wkentaro Please merge this

pazeshun added this to the 3.2.0 (07/04) milestone Jul 3, 2017

pazeshun mentioned this pull request Jul 3, 2017

[WIP] Add pinching with gripper-v6 #2160

Closed

pazeshun force-pushed the add-pinching-update branch from a9b04bc to 0fe6f04 Compare July 3, 2017 07:18

pazeshun force-pushed the add-pinching-update branch 3 times, most recently from 74cffe0 to d269c53 Compare July 4, 2017 01:27

pazeshun force-pushed the add-pinching-update branch from d269c53 to 665f480 Compare July 4, 2017 05:04

pazeshun mentioned this pull request Jul 4, 2017

MoveIt path constraints for pinching #2211

Closed

pazeshun force-pushed the add-pinching-update branch 2 times, most recently from 5156489 to 0a4c4ed Compare July 4, 2017 09:45

pazeshun changed the title ~~[WIP] Add pinching with gripper-v6~~ Add pinching with gripper-v6 Jul 4, 2017

pazeshun requested a review from knorth55 July 4, 2017 10:02

pazeshun added the feature label Jul 4, 2017

knorth55 requested changes Jul 4, 2017

View reviewed changes

wkentaro assigned pazeshun Jul 4, 2017

pazeshun mentioned this pull request Jul 4, 2017

Don't turn gripper over in ik->cardboard-center #2216

Closed

pazeshun changed the title ~~Add pinching with gripper-v6~~ [WIP] Add pinching with gripper-v6 Jul 4, 2017

knorth55 requested changes Jul 4, 2017

View reviewed changes

pazeshun changed the title ~~[WIP] Add pinching with gripper-v6~~ Add pinching with gripper-v6 Jul 5, 2017

pazeshun added 5 commits July 5, 2017 10:36

Enable to get gripper sensor states

0707b25

Add get func of gripper sensor states

06b647b

Enable :start-grasp and :stop-grasp to move hand

95481f2

Erase one-shot-subscribe and consider pinching in :graspingp

c67fe05

Erase one-shot-subscribe in pressure calib

ffef86b

pazeshun added 10 commits July 5, 2017 10:36

Re-calibrate finger tendon winder

f2600cd

Calib finger init state of try-to-pick-object

5969a9e

Don't use prismatic load for graspingp and calib thresholds

eb18c88

Ignore unstable flex value and calib flex offset

bb755b0

Reset picking-fail-count for new target obj

9ba6b28

Rewrite waiting for :interpolatingp

e15845e

Add meta method :try-to-pick-object and :try-to-suction-object

d192d14

Move pinch-yaw to key in try-to-pick-object

057e20c

Fold fingers more tightly before suction-object

e56f868

Fix logging of wait-interpolation-until

11b4df9

pazeshun force-pushed the add-pinching-update branch from fd2d27d to 11b4df9 Compare July 5, 2017 01:36

Don't turn gripper over in ik->cardboard-center

67e17a2

pazeshun mentioned this pull request Jul 5, 2017

place-object and recognize-object almost collide in pick task #2212

Closed

Wait for opposite return-object in pick task

356a958

pazeshun added 3 commits July 5, 2017 18:27

Ignore collision between fingers and other gripper parts

dd85519

Fix offset of place-object in pick for moveit

f2e1ba9

Avoid collision to shelf or tote in pick-object

682f82b

knorth55 approved these changes Jul 5, 2017

View reviewed changes

wkentaro merged commit d26410b into start-jsk:master Jul 5, 2017

wkentaro mentioned this pull request Jul 5, 2017

Introduce gripper-v6 with pinching motion #2111

Closed

pazeshun deleted the add-pinching-update branch July 5, 2017 23:06

pazeshun mentioned this pull request Jul 5, 2017

Gripper turns over in place object #2213

Closed

knorth55 mentioned this pull request Jul 16, 2017

Add trial count and redesign state machine #2302

Merged

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Add pinching with gripper-v6 #2203

Add pinching with gripper-v6 #2203

pazeshun commented Jul 3, 2017

pazeshun commented Jul 3, 2017 •

edited

Loading

pazeshun commented Jul 4, 2017 •

edited

Loading

pazeshun commented Jul 4, 2017 •

edited

Loading

pazeshun commented Jul 4, 2017

knorth55 Jul 4, 2017

knorth55 Jul 4, 2017

pazeshun Jul 4, 2017 •

edited

Loading

pazeshun Jul 4, 2017

knorth55 Jul 4, 2017

pazeshun Jul 4, 2017 •

edited

Loading

knorth55 Jul 4, 2017

pazeshun Jul 4, 2017

knorth55 Jul 4, 2017

pazeshun Jul 4, 2017

pazeshun commented Jul 4, 2017

knorth55 Jul 4, 2017

pazeshun Jul 4, 2017

pazeshun Jul 4, 2017

pazeshun Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017 •

edited

Loading

pazeshun commented Jul 5, 2017

Add pinching with gripper-v6 #2203

Add pinching with gripper-v6 #2203

Conversation

pazeshun commented Jul 3, 2017

pazeshun commented Jul 3, 2017 • edited Loading

Remaining TODOs

pazeshun commented Jul 4, 2017 • edited Loading

My Idea

Main Changes

Movies

Current Problems

pazeshun commented Jul 4, 2017 • edited Loading

pazeshun commented Jul 4, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pazeshun Jul 4, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pazeshun Jul 4, 2017 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pazeshun commented Jul 4, 2017

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017

pazeshun commented Jul 5, 2017 • edited Loading

pazeshun commented Jul 5, 2017

pazeshun commented Jul 3, 2017 •

edited

Loading

pazeshun commented Jul 4, 2017 •

edited

Loading

pazeshun commented Jul 4, 2017 •

edited

Loading

pazeshun Jul 4, 2017 •

edited

Loading

pazeshun Jul 4, 2017 •

edited

Loading

pazeshun commented Jul 5, 2017 •

edited

Loading