Skip to content

tuto using part2

GuiHome edited this page Aug 17, 2017 · 11 revisions

Tutorial 2 : Using the planning environment

Prerequisite

We suppose you have done tutorial 2 part 1 since the same application produced in the first part is continued.

Part 2 : Precise grasping with a dexterous hand

Initial remarks

  • Due to the randomness of planning, there might be time when the planning fails, even if you followed the steps properly.
  • Errors like this :
   Found a contact between 'ground' (type 'Object') and 'ra_upper_arm_link' (type 'Robot link'),

are a typical problem of the planning being stuck with some bad posture (elbow down) for further steps of the motion.

  • This tutorial does not cover the tuning of planner settings, sometimes replanning just works, sometimes a different initial position of the "goal" in rviz helps the arm to be in a "better" posture for the current random seed.

Objective

  • Executing a pick and a place action
  • Using a specific Grasp.msg
  • Understanding reference objects and attached objects

Pick the pen

For this task the pick method will be used but we need to provide a Grasp.msg.

A Grasp.msg contains mainly

  • grasp and pre-grasp postures (joint angles)
  • grasp pose relative to a frame
  • pre-grasp approach direction and distance
  • post-grasp retreat parameters

The idea is to load some grasps from a yaml file to avoid using a grasp database or grasp generator. A grasps.yaml is provided in the resources folder.

  1. Add this method that loads and fills up a grasp dictionary (code inspired by sr_grasp/utils.py, author: Mark Pitchless, licence GPL)
def load_grasps():
    grasp_yaml = yaml.load(open(resourcepath+'grasps/grasps.yaml'))
    grasps={}
    for g in grasp_yaml:
        grasp = Grasp()
        genpy.message.fill_message_args(grasp, g)
          
        if grasp.id is None or grasp.id == "":
            raise Exception("Grasp has no id")
        else:
            grasps[grasp.id] = grasp
    return grasps
  1. load the four-finger-precision-horizontal grasp from the list.
grasps = load_grasps()
four_finger_grasp = grasps['four_finger_precision_horizontal']
  1. The grasp must be referenced to the object to grasp. The message is updated with the pen as a reference
four_finger_grasp.grasp_pose.header.frame_id = "pen"
four_finger_grasp.pre_grasp_approach.direction.header.frame_id = "pen"
four_finger_grasp.post_grasp_retreat.direction.header.frame_id = "pen"
four_finger_grasp.post_place_retreat.direction.header.frame_id = "pen"
  1. Remove any previous attached pen from the arm group. Warning this will produce an non fatal error the first time you run the application, (because the object is not yet attached) [ERROR] [1502292362.272537139]: Attached body 'pen' not found. Ignore the message.
arm.detach_object("pen")
  1. Plan and execute a pick action with the two-finger grasp
ret = arm.pick("pen",[four_finger_grasp])

You should probably see a message of this type in the MoveIt! terminal somewhere

Found a contact between 'cup' (type 'Object') and 'lfmiddle' (type 'Robot link'), which constitutes a collision. Contact information is not stored

To better monitor where the grasp fails, add a MarkerArray plugin in rviz and make it point to the /move_group/display_grasp_markers topic

  1. Restart your app to see how the hand is displayed red near the cup

The grasp fails due to the ring and little finger hitting the cup. Next step is to use a two-finger grasp pulling away the ring and little fingers during the grasp.

  1. Load the two-finger-precision-horizontal grasp from the list.
two_finger_grasp = grasps['two_finger_precision_horizontal']
two_finger_grasp.grasp_pose.header.frame_id = "pen"
two_finger_grasp.pre_grasp_approach.direction.header.frame_id = "pen"
two_finger_grasp.post_grasp_retreat.direction.header.frame_id = "pen"
two_finger_grasp.post_place_retreat.direction.header.frame_id = "pen"
  1. plan and execute a pick action with the two-finger grasp
ret = arm.pick("pen",[two_finger_grasp])

If planning succeeds, you might see the arm executing the reach, approach grasp, retreat sequence.

Note the object changed color to purple as it is now attached to the hand.

Place the pen back in the cup

To understand the importance of the attached object, we will try to place the object in the cup placed upside-down The pen is now part of the hand when checking for collision and the planner should fail with the bottom of the cup not hollow.

  1. First create a place location as a copy of the pen location, a little higher to drop it in the cup
place_loc = copy.deepcopy(p)
place_loc.pose.position.z = 0.07 
  1. Flip the cup
p.pose.position.x = 0.7
p.pose.position.y = 0.7
p.pose.position.z = 0.0
p.pose.orientation.x = 0
p.pose.orientation.y = 1
p.pose.orientation.z = 0
p.pose.orientation.w = 0
scene.remove_world_object("cup")
scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
rospy.sleep(1)
  1. Plan and execute a place motion to the place location
ret = arm.place("pen",place_loc)

This action will fail with a message similar to

Found a contact between 'cup' (type 'Object') and 'pen' (type 'Robot attached'), which constitutes a collision. Contact information is not stored.

  1. Reflip the cup open side up
p.pose.orientation.x = 0
p.pose.orientation.y = 0
p.pose.orientation.z = 0
p.pose.orientation.w = 1
scene.remove_world_object("cup")
scene.add_mesh("cup",p,resourcepath+'objects/cup.dae')
rospy.sleep(1)
  1. Retry to place
ret = arm.place("pen",place_loc)

This action will probably work.

Note, the pen is detached if place task succeeded.

This is on example of result

Regrasping is done in the next tutorial 2 part 3.