Pick and Place

The following tutorial demonstrates how you can use the moveit task constructor to plan and carry out pick and place movements.

First, lets specify the planning group and the end effector that you want to use.

# Specify robot parameters
arm = "panda_arm"
eef = "hand"

Next, we add the object that we want to displace to the planning scene. To this end, make sure that the planning scene not already contains such object.

# Specify object parameters
object_name = "object"
object_radius = 0.02

# Start with a clear planning scene
psi = PlanningSceneInterface(synchronous=True)
psi.remove_world_object()

# [initCollisionObject]
# Grasp object properties
objectPose = PoseStamped()
objectPose.header.frame_id = "world"
objectPose.pose.orientation.w = 1.0
objectPose.pose.position.x = 0.30702
objectPose.pose.position.y = 0.0
objectPose.pose.position.z = 0.285
# [initCollisionObject]

# Add the grasp object to the planning scene
psi.add_box(object_name, objectPose, size=[0.1, 0.05, 0.03])

At this point, we are ready to create the task hierarchy.

# Create a task
task = core.Task()
task.name = "pick + place"

The pipeline planner encapsulates the moveit interface to sampling-based geometric motion planners.

Tip

Planning does not proceed linearly from top to bottom. Rather, it proceeds from the inside out. Connect stages therefore compute a motion plan between two previously calculated subordinate solutions. For a clear visualization, inspect the rviz mtc panel.

Lets connect the current robot state with the solutions of the following stages.

# Start with the current state
task.add(stages.CurrentState("current"))

# [initAndConfigConnect]
# Create a planner instance that is used to connect
# the current state to the grasp approach pose
pipeline = core.PipelinePlanner()
pipeline.planner = "RRTConnectkConfigDefault"
planners = [(arm, pipeline)]

# Connect the two stages
task.add(stages.Connect("connect", planners))
# [initAndConfigConnect]

To pick the object, we first need to know possible end effector poses with which we can perform a successful grasp. For this, we use a GenerateGraspPoseStage. which essentially spawns poses with a given angle_delta in circular fashion around a center point.

# [initAndConfigGenerateGraspPose]
# The grasp generator spawns a set of possible grasp poses around the object
grasp_generator = stages.GenerateGraspPose("Generate Grasp Pose")
grasp_generator.angle_delta = math.pi / 2
grasp_generator.pregrasp = "open"
grasp_generator.grasp = "close"
grasp_generator.setMonitoredStage(task["current"])  # Generate solutions for all initial states
# [initAndConfigGenerateGraspPose]

Next, we need to compute the inverse kinematics of the robot arm for all previously sampled poses. This way we can rule out solutions that are not feasible due to the robot geometry. The simpleGrasp stage combines ik calculation with motion plan generation for opening and closing the end effector, as well as attaching the object to the robot an disabling collision.

# [initAndConfigSimpleGrasp]
# SimpleGrasp container encapsulates IK calculation of arm pose as well as finger closing
simpleGrasp = stages.SimpleGrasp(grasp_generator, "Grasp")
# Set frame for IK calculation in the center between the fingers
ik_frame = PoseStamped()
ik_frame.header.frame_id = "panda_hand"
ik_frame.pose.position.z = 0.1034  # tcp between fingers
ik_frame.pose.orientation.x = 1.0  # grasp from top
simpleGrasp.setIKFrame(ik_frame)
# [initAndConfigSimpleGrasp]

Lastly, we can insert all the previous steps into the Pick container stage. At this point we might also specify approach and lift twists for the robot relative to the object we want to grasp.

# [initAndConfigPick]
# Pick container comprises approaching, grasping (using SimpleGrasp stage), and lifting of object
pick = stages.Pick(simpleGrasp, "Pick")
pick.eef = eef
pick.object = object_name

# Twist to approach the object
approach = TwistStamped()
approach.header.frame_id = "world"
approach.twist.linear.z = -1.0
pick.setApproachMotion(approach, 0.03, 0.1)

# Twist to lift the object
lift = TwistStamped()
lift.header.frame_id = "panda_hand"
lift.twist.linear.z = -1.0
pick.setLiftMotion(lift, 0.03, 0.1)

Since all the previous stages were chained together via their constructor arguments, we only need to add the top level Pick stage to the task hierarchy.

# Add the pick stage to the task's stage hierarchy
task.add(pick)
# [initAndConfigPick]

Thats everything we need for picking an object! Lets find a motion plan to place the object

# Connect the Pick stage with the following Place stage
con = stages.Connect("connect", planners)
con.path_constraints = constraints
task.add(con)

Similar to the picking procedure, we define the place task. First, start with sampling place poses.

# [initAndConfigGeneratePlacePose]
# Define the pose that the object should have after placing
placePose = objectPose
placePose.pose.position.x = -0.2
placePose.pose.position.y = -0.6
placePose.pose.position.z = 0.0

# Generate Cartesian place poses for the object
place_generator = stages.GeneratePlacePose("Generate Place Pose")
place_generator.setMonitoredStage(task["Pick"])
place_generator.object = object_name
place_generator.pose = placePose
# [initAndConfigGeneratePlacePose]

Next, wrap the inverse kinematics computation and gripper movements.

simpleUnGrasp = stages.SimpleUnGrasp(place_generator, "UnGrasp")

Lastly, add place and retract motions and add the Place stage to the task hierarchy.

# [initAndConfigPlace]
# Place container comprises placing, ungrasping, and retracting
place = stages.Place(simpleUnGrasp, "Place")
place.eef = eef
place.object = object_name
place.eef_frame = "panda_link8"
# [initAndConfigSimpleUnGrasp]

# Twist to retract from the object
retract = TwistStamped()
retract.header.frame_id = "world"
retract.twist.linear.z = 1.0
place.setRetractMotion(retract, 0.03, 0.1)

# Twist to place the object
placeMotion = TwistStamped()
placeMotion.header.frame_id = "panda_hand"
placeMotion.twist.linear.z = 1.0
place.setPlaceMotion(placeMotion, 0.03, 0.1)

# Add the place pipeline to the task's hierarchy
task.add(place)
# [initAndConfigPlace]

Finally, compute solutions for the task hierarchy and delete the planner instances.

if task.plan():
    task.publish(task.solutions[0])

# avoid ClassLoader warning
del pipeline
del planners

At this point, you might inspect the task hierarchy in the mtc rviz plugin.

Tip

Use the mtc rviz plugin to graphically inspect the solutions of individual stages in the task hierarchy.