Basic Pick and Place

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

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

MIT 6.4210/2: Robotic Manipulation

Fall 2023, Lecture 4

(part 2)

Basic Pick and Place

Step 1: Kinematic Frames / Spatial Algebra

Step 2: Gripper Frame Plan "Sketch"

Step 3: Forward kinematics of the iiwa + WSG

Step 4: Differential Inverse Kinematics

Kinematic Jacobian:  \( {}^WV^G = J^{G}(q) v \)

\( v = [J^{G}(q)]^{-1} {}^WV^G \)

spatial velocity

generalized velocity

Representations for 3D Rotation

Example: Gimbal Lock

Check yourself: A single "free" body

plant = MultibodyPlant(time_step = 0.0)
Parser(plant).AddModelFromFile(FindResourceOrThrow(
	"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
brick = plant.GetBodyByName("base_link")

context = plant.CreateDefaultContext()
X^B = f_{kin}^B(q)

What is \(X^B\) ? 

X_B = plant.EvalBodyPoseInWorld(brick, context)

What is \(q\) ? 

q = plant.GetPositions(context)

Step 4: Differential Kinematics

dX^B = \frac{\partial f_{kin}^B(q)}{\partial q} dq
= \underbrace{J^B(q)}dq
X^B = f_{kin}^B(q)

Kinematic "Jacobian"

Spatial Velocity

\frac{d}{dt} {}^AX^B_C \equiv \, ^AV^B_C = \begin{bmatrix} {}^A\omega^B_C \\ {}^A\text{v}^B_C \end{bmatrix}

angular velocity

translational velocity

Q: How do we represent angular velocity, \(\omega^B\)??

 

 

(w_x, w_y, w_x)

A: Unlike 3D rotations, here 3 numbers are sufficient and efficient; so we use the same representation everywhere.

note the typesetting

Spatial Velocity Algebra

Check yourself: A single "free" body

plant = MultibodyPlant(time_step = 0.0)
Parser(plant).AddModelFromFile(FindResourceOrThrow(
	"drake/examples/manipulation_station/models/061_foam_brick.sdf"))
plant.Finalize()
brick = plant.GetBodyByName("base_link")

context = plant.CreateDefaultContext()
\dot{q} = N(q) v

What size is \(v\) ? 

v = plant.GetVelocities(context)

What size is \(q\) ? 

q = plant.GetPositions(context)
v = plant.MapQDotToVelocity(context, qdot)
qdot = plant.MapVelocityToQDot(context, v)

For any MultibodyPlant (not just a single body):

Kinematics

  • Forward kinematics:    joint positions \(\Rightarrow\) pose
  • Inverse kinematics*:      pose \(\Rightarrow\) joint positions

 

  • Differential kinematics:
          joint positions, velocities \(\Rightarrow\) spatial velocity
     
  • Differential inverse kinematics:
          spatial velocity, joint positions \(\Rightarrow\) joint velocities

 

q \Rightarrow X^B
X^B \Rightarrow q
q, v \Rightarrow V^B
V^B, q \Rightarrow v

Our first Jacobian pseudo-inverse controller

\( v = [J^{G}(q)]^{-1} {}^WV^G \)

Our first Jacobian pseudo-inverse controller

class PseudoInverseController(LeafSystem):
    def __init__(self, plant):
        LeafSystem.__init__(self)
        self.V_G_port = self.DeclareVectorInputPort("V_WG", 6)
        self.q_port = self.DeclareVectorInputPort("iiwa.position", 7)
        self.DeclareVectorOutputPort("iiwa.velocity", 7, self.CalcOutput)

    def CalcOutput(self, context, output):
        V_G = self.V_G_port.Eval(context)
        q = self.q_port.Eval(context)
        self._plant.SetPositions(self._plant_context, self._iiwa, q)
        J_G = self._plant.CalcJacobianSpatialVelocity(
            self._plant_context,
            JacobianWrtVariable.kV,
            self._G,
            [0, 0, 0],
            self._W,
            self._W,
        )
        J_G = J_G[:, self.iiwa_start : self.iiwa_end + 1]  # Only iiwa terms.
        v = np.linalg.pinv(J_G).dot(V_G)
        output.SetFromVector(v)
prog = MathematicalProgram()
x = prog.NewContinuousVariables(2)
prog.AddConstraint(x[0] + x[1] == 1)
prog.AddConstraint(x[0] <= x[1])
prog.AddCost(x[0] ** 2 + x[1] ** 2)
result = Solve(prog)

Lecture 4: Basic pick and place (part 2)

By russtedrake

Lecture 4: Basic pick and place (part 2)

MIT Robotic Manipulation Fall 2023 http://manipulation.csail.mit.edu

  • 478

More from russtedrake