Geometric perception

(part 2)

MIT 6.421:

Robotic Manipulation

Fall 2023, Lecture 7

Follow live at https://slides.com/d/kbcw1PY/live

(or later at https://slides.com/russtedrake/fall23-lec07)

Pick and Place with Perception

from the docs: "Each pixel in the output image from depth_image is a 16bit unsigned short in millimeters."

directives:
- add_model:
    name: mustard
    file: package://drake/manipulation/models/ycb/sdf/006_mustard_bottle.sdf
- add_weld:
    parent: world
    child: mustard::base_link_mustard
    X_PC:
        translation: [0, 0, 0.09515]
        rotation: !Rpy { deg: [-90, 0, -90]}
- add_model:
    name: camera
    file: package://manipulation/camera_box.sdf
- add_weld:
    parent: world
    child: camera::base
    X_PC:
        translation: [0.5, 0.1, 0.2]
        # Point slightly down towards camera
        # RollPitchYaw(0, -0.2, 0.2) @ RollPitchYaw(-np.pi/2, 0, np.pi/2)
        rotation: !Rpy { deg: [-100, 0, 100] }
cameras:
    main_camera:
        name: camera0
        X_PB:
            base_frame: camera::base

Add cameras

MultibodyPlant

SceneGraph

RgbdSensor

DepthImageToPointCloud

MeshcatPointCloudVisualizer

Real point clouds are also messy

figure from Chris Sweeney et al. ICRA 2019.

"Partial views"

two on wrist

H. Yang, J. Shi, and L. Carlone, “TEASER: Fast and Certifiable Point Cloud Registration”, IEEE Transactions on Robotics (T-RO), 2020.

DeepSDF: Learning Continuous Signed Distance Functions for Shape Representation

Jeong Joon Park, Peter Florence, Julian Straub, Richard Newcombe, Steven Lovegrove

prog = MathematicalProgram()
# x is a symbolic variable
x = prog.NewContinuousVariables(2)

# numerical, pass in the coefficients:
# x'Qx + b'x + c
prog.AddQuadraticCost(Q, b, c, x)

# symbolic:
prog.AddCost(x.dot(x))
prog.AddQuadraticCost(x.dot(x))

# autodiff:
def my_cost(x):
	return x.dot(x)
prog.AddCost(my_cost, x)

# Adding autodiff costs/constraints => Solve(prog) will 
# pick a nonlinear optimization solver

    prog = MathematicalProgram()
    p = prog.NewContinuousVariables(2, 'p')
    theta = prog.NewContinuousVariables(1, 'theta')

    def position_model_in_world(vars, i):
        [p, theta] = np.split(vars, [2])
        R = np.array([[np.cos(theta[0]), -np.sin(theta[0])],
                      [np.sin(theta[0]), np.cos(theta[0])]])
        p_Wmci = p + R @ p_Omc[:,i]
        return p_Wmci

    def squared_distance(vars, i):
        p_Wmci = position_model_in_world(vars, i)
        err = p_Wmci - p_s[:,i]
        return err.dot(err)

    for i in range(Ns):
        # forall i, |p + R*p_Omi - p_si|²
        prog.AddCost(partial(squared_distance, i=i),
                     np.concatenate([p[:], theta]))
        # forall i, p + R*p_Omi >= 0.
        prog.AddConstraint(partial(position_model_in_world, i=i), 
                           vars=np.concatenate([p[:], theta]),
                           lb=[0, 0], ub=[np.inf, np.inf])
    
    result = Solve(prog)