Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add pinching with gripper-v6 #2203

Merged
merged 34 commits into from
Jul 5, 2017
Merged

Conversation

pazeshun
Copy link
Collaborator

@pazeshun pazeshun commented Jul 3, 2017

From #2160

@pazeshun pazeshun added this to the 3.2.0 (07/04) milestone Jul 3, 2017
@pazeshun pazeshun force-pushed the add-pinching-update branch from a9b04bc to 0fe6f04 Compare July 3, 2017 07:18
@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 3, 2017

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 pazeshun force-pushed the add-pinching-update branch 3 times, most recently from 74cffe0 to d269c53 Compare July 4, 2017 01:27
@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 4, 2017

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:
stow_statemachine
Pick state machine:
pick_statemachine

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 pazeshun force-pushed the add-pinching-update branch from d269c53 to 665f480 Compare July 4, 2017 05:04
@pazeshun pazeshun force-pushed the add-pinching-update branch 2 times, most recently from 5156489 to 0a4c4ed Compare July 4, 2017 09:45
@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 4, 2017

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

@pazeshun pazeshun changed the title [WIP] Add pinching with gripper-v6 Add pinching with gripper-v6 Jul 4, 2017
@pazeshun pazeshun requested a review from knorth55 July 4, 2017 10:02
@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 4, 2017

Close #2111

@@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I prefer :try-to-pick-object

Copy link
Collaborator Author

@pazeshun pazeshun Jul 4, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

@pazeshun pazeshun Jul 4, 2017

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

なるほど、

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

ですかね

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

こうするメリットを教えていただけると嬉しいです。
こうするとすると、: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だと思うのですが違うのですか?

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

もしくは: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でおねがいします。

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

  (: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は消さなくてもいいと思いますが

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

直しました

@pazeshun pazeshun changed the title Add pinching with gripper-v6 [WIP] Add pinching with gripper-v6 Jul 4, 2017
@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 4, 2017

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

@@ -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))
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

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

Copy link
Collaborator Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Fixed

@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 5, 2017

Tested in real robot with #2216

@pazeshun pazeshun changed the title [WIP] Add pinching with gripper-v6 Add pinching with gripper-v6 Jul 5, 2017
@pazeshun pazeshun force-pushed the add-pinching-update branch from fd2d27d to 11b4df9 Compare July 5, 2017 01:36
@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 5, 2017

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
Copy link
Collaborator Author

pazeshun commented Jul 5, 2017

Includes #2216

@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 5, 2017

Close #2213

@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 5, 2017

Movie was updated

@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 5, 2017

Close #2212

@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 5, 2017

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

@pazeshun
Copy link
Collaborator Author

pazeshun commented Jul 5, 2017

@wkentaro Please merge this

@wkentaro wkentaro merged commit d26410b into start-jsk:master Jul 5, 2017
@pazeshun pazeshun deleted the add-pinching-update branch July 5, 2017 23:06
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants