Gripper always rotated 90 degrees when performing the grasp

[fitter22]

Hi, I stumbled across weird problem when I tried to port my pick&place demo to Tiago robot. I'm using GPD package to generate the grasps positions and PickPlaceInterface to perform them. I'm simulating Tiago in Gazebo. Whole pick and place seems to work, but in the end the gripper position is always rotated 90* - so robot hits the object with its fingers instead of grasping it.

I tried to grasp it using different links, but the result is always the same. Before I ran the demo successfully using the Fetch robot, looks like Tiago interprets my Grasp messages in different way.

I've checked your pick and place solution and I found out you apply -90* roll rotation for every grasp: https://github.com/pal-robotics/tiago_tutorials/blob/235f6efdb5850854cb45b37a4b1d874fa681023a/tiago_pick_demo/config/pick_and_place_params.yaml#L37

Unfortunately I cannot do it like that, thats because my grasps have position and orientation set based on my camera frame, so -90* roll rotation returns me completely wrong grasp poses.

Do you have any ideas how to fix it?

Thank you Lukasz

[AleDF]

Hi, I'm developing a C++ pick&place demo with the TIAGo robot using MoveIt! and moveit_simple_grasps package (https://github.com/davetcoleman/moveit_simple_grasps). I had similar problems, because in MoveIt! the parent_link (in my case arm_tool_link) should have its Z-axis pointing towards the object to grasp, following the the convention laid out in "Robotics" by John Craig in 1955. Additionally, the x-axis should be pointing up along the grasped object, i.e. the circular axis of a bottle if you were holding it. The y-axis should be point towards one of the fingers. However, the TIAGo URDF, as a lot of URDFs, does not follow this convention, so in the moveit_simple_grasps and more in moveit_grasps package there is a transform that allows you to fix it.

Anyway, the arm_tool_link has the X-axis pointing forward, so you need to apply some transforms to the grasp candidates poses. What I'm doing in my demo is to have all the grasp candidates generated in the base_footprint frame and then I apply all the transform that I need to make the arm_tool_link as the convention. Pay attention that if you use the moveit_simple_grasps, it allows a transformation only on the Y-axis, indeed I had to apply the transform manually after the grasp candidates generation, instead of moveit_grasps that I still have to test.

I hope to have been helpful. Best, Alessandro Di Fava (PAL Robotics)

[fitter22]

Hi Alessandro, thank you for your response! It explains a lot why TIAGo interprets my grasp in different way. I managed to fix it in similar way as its done in official TIAGo pick&place demo. So after receiving the grasps from GPD, I'm applying rotation to all of them. In my case fix looks like (maybe someone in future find it helpful):

org_quat = # get your grasp orientation
rot_quat = Quaternion(0.7071, 0.7071, 0, 0)  # 90* around X axis (W, X, Y, Z)
grasp_quat = rot_quat * org_quat

Thats all. I'm generating grasps in robot's camera frame. After that rotation my pick&place demo works correctly.

Regards, Lukasz

[AleDF]

Thanks for your contribution.

Regards, Alessandro

[angelbeibei]

@fitter22 Hi，From the above dialogue I saw that you achieved the grasp with Tiago robot, And Our lab bought a new Tiago robot with Hey5 hand, I want to achieved the demo by using Hey5 , But it has not been modified successfully, and it can’t be caught. Can you give me some advice? Thanks

[fitter22]

Hi, @smarthhy. Yes I managed to perform a full pick&place with Tiago robot after I applied the fix I mentioned above (there was also some minor problem with gripper controller if I remember correctly). Unfortunately for you, I've only made it with simple 2-fingers gripper and I'm no longer working in that project, so I cannot provide you any examples, sorry. I can recommend to you to provide more details about your problem(s). Pal-robotics folks should be able to help you. Good luck!