Problem Set #5 Part 3: pick-and-place planning with RRT

In problem set 3, you were asked to hand code a series of trajectories which picks up a foam brick and places it on the shelf. In this notebook, we'll see how this process can be (almost) automated with RRT.

Setup

In [ ]:
# Import stuff to be used in this notebook. Run this first!
%reload_ext autoreload
%autoreload 2

import numpy as np
import meshcat

from pydrake.math import RollPitchYaw, RotationMatrix, RigidTransform
from pydrake.trajectories import PiecewisePolynomial
In [ ]:
# First start meshcat for visualization - this only has to be run once.

# If you interrupt the kernel of this notebook, you'll need to run this cell again to 
# restart the meshcat server, and then refresh the visualization window. 

# This will open a mesh-cat server in the background, 
# click on the url to display visualization in a separate window. 
vis = meshcat.Visualizer()
In [ ]:
# Run this if you feel the visualizer is messed up and want to start over.
vis.delete()

1. Joint space trajectory planning with RRT

RRT finds collision-free paths between points in the robot's configuration space. However, pick-and-place tasks are specified in the task space (6-DOF pose of frame L7). Therefore, before planning with RRT, we need to find, with the help of IK, the following robot joint angles (configurations):

  • q_start which picks up the object, and
  • q_goal which places the object on the shelf.
In [ ]:
from ik_iiwa import SolveOneShotIk, q0, q_home, p_L7Q, X_WO

# use the object pose X_WO to find the pick-up configuration.
################# Your code here #########################
p_WQ_0 = None
R_WL7_0 = None

p_WQ_final = None
R_WL7_final = None
##########################################################

q_start = SolveOneShotIk(p_WQ_0, R_WL7_0, p_L7Q, q_home)
q_goal = SolveOneShotIk(p_WQ_final, R_WL7_final, p_L7Q, q_home)

The cell below constructs an RRT problem with the gripper open, the left cabinet door open and the right door closed. The configuration space of the problem is the 7-dimensional joint space of the IIWA arm.

Although drake's IK supports collision-free constraint, it is not used in SolveOneShotIk. It is therefore your responsibility to make sure that both q_start and q_goal are collision-free, which shouldn't be too hard if you pick the end effector poses suitably.

In [ ]:
# Constructs rrt problem. 
from rrt_planner.iiwa_rrt_problem import IiwaProblem

gripper_setpoint = 0.1
door_angle = np.pi/2 - 0.001
left_door_angle = -door_angle
right_door_angle = 0.001

iiwa_problem = IiwaProblem(
    q_start=q_start,
    q_goal=q_goal,
    gripper_setpoint=gripper_setpoint,
    left_door_angle=left_door_angle,
    right_door_angle=right_door_angle,
    is_visualizing=True)

Pick a planning method (RRT or bi-directional RRT) to plan a path from q_start to q_goal. Which one is more likely to succeed?

In [ ]:
path = iiwa_problem.run_planner(method=None)
In [ ]:
# Visualize pick-and-place trajectory in meshcat
iiwa_problem.visualize_path(path)

2. Run pick-and-place trajectories in simulation

The following cell constructs pick-and-place trajectories from the configuration space path (path) planned by RRT and simulates them.

(3 points) We will check off your pick-and-place trajectories on the real robot in lab 2.

In [ ]:
from manip_station_sim.manipulation_station_simulator import ManipulationStationSimulator
from ik_iiwa import X_WO

# joint space trajectory from RRT path
duration = 15
t_knots = np.linspace(0, duration, len(path))
q_knots = np.array(path)
q_traj_rrt = PiecewisePolynomial.Cubic(t_knots, q_knots.T, np.zeros(7), np.zeros(7))

# close gripper
q_knots_close_gripper = np.zeros((2, 7))
q_knots_close_gripper[0] = q_knots[0]
qtraj_close_gripper = PiecewisePolynomial.ZeroOrderHold(
                            [0, 1], q_knots_close_gripper.T)

# open gripper
q_knots_open_gripper = np.zeros((2, 7))
q_knots_open_gripper[0] = q_knots[-1]
qtraj_open_gripper = PiecewisePolynomial.ZeroOrderHold(
                            [0, 1], q_knots_open_gripper.T)

# Generate plan list
plan_list = [JointSpacePlan(qtraj) for qtraj in [qtraj_close_gripper, q_traj_rrt, qtraj_open_gripper]]
gripper_setpoint_list = [0.005, 0.005, 0.1]


# Construct simulator.
sim = ManipulationStationSimulator(X_WO, robot_has_collision=True)

door_angles = np.array([left_door_angle, right_door_angle])
# real_time_rate=0 means running the simulation as fast as possible. 
# Setting real_time_rate 1 to run the simulation close to real time.
(iiwa_position_command_log, iiwa_position_measured_log,
    iiwa_external_torque_log, iiwa_velocity_estimated_log,
    plant_state_log, t_plan) = sim.RunSimulation(
    q_home, door_angles, plan_list, gripper_setpoint_list, extra_time=2.0, real_time_rate=0.0,
    gripper_setpoint_initial=0.1)