issue with the pick action

[rubenpar]

Hello,

I don't have too much experience using MoveIt, so maybe I'm missing something obvious but I can't see it. I'm using a shadow hand/PA10 arm configuration with ROS hydro, I'm basically following the tutorial and trying to adapt the code to this particular case.

When I try to "pick" a box ( I used a box because I thought it'd be easier than the cup and the pen) I get the following messages:

[ INFO] [1461959872.576848806]: Loading robot model 'shadow_pa10_7dof'... [ INFO] [1461959872.576927257]: No root joint specified. Assuming fixed joint [ INFO] [1461959873.449899876]: Starting scene monitor [ INFO] [1461959873.453196587]: Listening to '/move_group/monitored_planning_scene' [ INFO] [1461959875.073748769]: Constructing new MoveGroup connection for group 'arm' in namespace '' [ INFO] [1461959875.847877514]: Ready to take MoveGroup commands for group arm. [ INFO] [1461959875.848090866]: Looking around: no [ INFO] [1461959875.848173607]: Replanning: no [ WARN] [1461959899.933188496]: MessageFilter [target=/world ]: Discarding message from [/moveit_grasp_app_18362_1461959898762] due to empty frame_id. This message will only print once. [ERROR] [1461959904.624596470]: Attached body 'cup' not found [ INFO] [1461959904.786446446]: Combined planning and execution request received for MoveGroup action. Forwarding to planning and execution pipeline. [ INFO] [1461959904.786657458]: Goal constraints are already satisfied. No need to plan or execute any motions [ INFO] [1461959904.857003167]: Planning attempt 1 of at most 1 [ WARN] [1461959904.857208366]: Choice of end-effector for group 'hand' is ambiguous. Assuming 'fftip_ee' [ INFO] [1461959904.871353357]: Added plan for pipeline 'pick'. Queue is now of size 1 [ WARN] [1461959904.876513752]: Returning dirty link transforms [ WARN] [1461959905.132044466]: Returning dirty link transforms [ INFO] [1461959905.382692015]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0 [ WARN] [1461959905.383089444]: All supplied grasps failed. Retrying last grasp in verbose mode. [ INFO] [1461959905.383244081]: Re-added last failed plan for pipeline 'pick'. Queue is now of size 1 [ WARN] [1461959905.387101942]: Returning dirty link transforms [ WARN] [1461959905.640601182]: Returning dirty link transforms [ INFO] [1461959905.889029941]: Sampler failed to produce a state [ INFO] [1461959905.889099168]: Manipulation plan 0 failed at stage 'reachable & valid pose filter' on thread 0 [ INFO] [1461959905.889236361]: Pickup planning completed after 0.525824 seconds

I suppose the problem is in grasp.yaml, I've tried to move the robot to the desire position and get the parameters of the robot on that position to write them in the grasp file, but that doesn't work. How should I get the correct parameters?

[guihomework]

Back from holidays, looking into your problem now.

From the info you provide, I don't have many clues on the reason of the problem.

It seems the planning motion you want to do does not require a motion [ INFO] [1461959904.786657458]: Goal constraints are already satisfied. No need to plan or execute any motions

This is also strange, [ WARN] [1461959904.857208366]: Choice of end-effector for group 'hand' is ambiguous. Assuming 'fftip_ee' if moving the arm, to reach the box, why does it need to select fftip_ee. You should double check your srdf to have a single hand end_effector as in here https://github.com/guihomework/dexterous-manipulation-tutorial/blob/master/sr_multi_moveit/shadow_ur10_moveit_config/config/ur10shadow.srdf#L466

The grasp yaml is for the shadow hand, and is used to set the end-effector in different postures, there is no planning involved there, and your problem is apparently on planning. Of course the pick function needs a valid posture, which comes from the grasp.yaml but it should not block the planning unless the posture is in self-collision (but no message says that in your copy-paste)

waiting for you checks to help you further

[rubenpar]

I had to add "forearm" as link in the "arm" group to let me put "forearm" as end effector, otherwise I can't because "forearm" is not part of the group "arm", so now I have this:

`

<end_effector name="fftip_ee" parent_link="fftip" group="first_finger" parent_group="hand" />
<end_effector name="mftip_ee" parent_link="mftip" group="middle_finger" parent_group="hand" />
<end_effector name="rftip_ee" parent_link="rftip" group="ring_finger" parent_group="hand" />
<end_effector name="lftip_ee" parent_link="lftip" group="ring_finger" parent_group="hand" />
<end_effector name="thtip_ee" parent_link="thtip" group="thumb" parent_group="hand" />`

But I get the same warning in the terminal.

I add here the grasp file if that helps, I've tried to move the arm first and then call the pick function. But there is something I do not understand about the grasp file, the grasp pose and the pre_grasp_approach are relative to the position of the box but, where should I move the robot before call the pick function, to the grasp pose or the pre grasp pose?

grasp.txt mod.txt

It looks like the problem is the grasp file, I'll try to find out about this a little more, checking different situations because I don't know exactly what I'm missing.

[guihomework]

usually one puts the fixed joints that attached the hand to the arm as part of the arm, and then automatically the forearm is added as link to the "arm" group as done here https://github.com/guihomework/dexterous-manipulation-tutorial/blob/master/sr_multi_moveit/shadow_ur10_moveit_config/config/ur10shadow.srdf#L20

The idea is that a grasp is object dependent. You don't position your hand the same way for grasping a bottle and a dice and so on. Even per object there might be several grasps suitable for different tasks. In a full pipeline, a object recognition would extract the object info and then a grasp database would contain suitable grasps for this object (stored in advance). Here consider the grasp.yaml as a very small grasp database. You might want to adapt the pose of the palm in the grasp file to match the box you want to grasp.

So you don't need to place the arm anywere, the pick action does it for you. It tries plan from the current position of the end-effector in the world to the pregrasp-pose in the world (and commands the end-effector i.e. the hand to pregrasp posture). Of course there must be a transform (in tf) existing between the object and the world, so that the pregrasp-pose is converted from object frame to world frame and then have the end-effector (normally the palm) know where to go. This transform is automatically available if the object is in the planning scene. the same happens for grasp pose. The name of the frame and of your object must match so something like four_finger_grasp.grasp_pose.header.frame_id = "caja" you hardcoded world in your file grasp.grasp_pose.header.frame_id = "world"

In your grasp file, I think you should adjust the pose of the hand relative to the object. Actually it seems it is not the palm frame that is placed but the forearm frame probably (since this is an arm planning of the last link of the arm). I used y: -0.34 to not collide with the object. you have a -0.1 there.

[rubenpar]

First of all, thank you for your explanation.

I've checked everything you said, and after that I think the problem is the end effector. I've checked the srdf several times and there is only one definition of the hand's end effector, but the warning:

[WARN] [1461959904.857208366]: Choice of end-effector for group 'hand' is ambiguous. Assuming 'fftip_ee'

still appears. So I think that despite of I write the position and orientation of the forearm in the grasp file, as long as moveit chooses 'fftip_ee', those positions and orientations are wrong.

srdf.txt

I used "get_end_effector_link()" to know which link moveit reads as end effector, and the output is an empty string. Also, I've made again the srdf using the setup assistant but that did not solved the problem. Is there other way to make "forearm" the end effector?

EDIT:There is this other warning, that only appears the first time I run the code:

[ WARN] [1463057596.395799492]: MessageFilter [target=/world ]: Discarding message from [/moveit_grasp_app_22747_1463057595214] due to empty frame_id. This message will only print once.

[guihomework]

Maybe moveit in ROS hydro is a little different. We also use a arm + hand in our lab and this was working in hydro. Could you also provide the urdf, then I will check between urdf and srdf what I can see. Something is strange there.

[rubenpar]

Sure, I add here the xacro file I used to generate the srdf.

urdf.txt

[guihomework]

I had a check on your urdf and srdf but could not see a difference with the ur10shadow urdf/srdf. They both have two fixed link in a row (which I don't have in my lab robot). I guess moveit in hydro is more picky. So I had a look at some older revision of my lab robot urdf/srdf when running on hydro and I found that my arm groups were defined as a chain elements with nothing else. You might want to give it a try ? Also try removing all the end-effectors except the arm to see if the ambiguity vanishes.

<group name="right_arm">
        <chain base_link="right_arm_base_link" tip_link="right_arm_hand_mount" />
    </group>

I also just understood that your warning is about the group hand were indeed the end-effector is ambiguous, but you wanted to move the group arm, so this might be the issue. Group arm should not have any problem moving then.

[rubenpar]

After several tries I realised in a previous file when I was trying to find out what I was doing wrong I changed: robot.arm.pick() into robot.hand.pick() and I did not changed it again, so that's the reason I was getting the error with the hand's end effector, as consequence of this, the pick function wasn't working too. After changing that, and adjust the position and orientation in the grasp file everything works perfectly as you said, firstly to pre grasp pose, then pick the object and go to post grasp pose. I did not need to change the arm group description into a chain elements, it works as joint group.

Thanks a lot for helping me with this problem, even though this stupid mistake I made, the tutorial and the info you provided here were very useful to me.

[guihomework]

Glad that you found the mistake. I should have looked more carefully too at the error message.