Skip to content

tuto using part2

Jump to bottom
GuiHome edited this page Jul 12, 2015 · 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

Objective

  • Executing a pick and a place action
  • Useing 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
arm.detach_object("pen")
  1. Plan and execute a pick action with the two-finger grasp
arm.detach_object("pen")
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.

Clone this wiki locally