Basic Kinematics

MIT 6.4210/2: Robotic Manipulation

Fall 2026, Lecture 4

Basic Pick and Place

Step 1: Kinematic Frames / Spatial Algebra

Step 2: Gripper Frame Plan "Sketch"

Gripper trajectory

  • Given a sequence of transforms (gripper poses)
  • Generate continuous trajectory (function of time) via PiecewisePose
    • piecewise quaternion SLERP (spherical linear interpolation) for orientation
    • piecewise polynomial for position

Kinematics - relevant spaces

  • Configuration space / joint space
    • A vector of the joint "angles" (positions)
    • "minimal coordinates"
  • Workspace / operational space / task space
    • Spatial representation of the poses of the links of the robot - typically the "end-effector" link
    • "maximal coordinates"

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

Forward Kinematics

X^B = f_{kin}^B(q)

Forward Kinematics

Simplify by choosing link frame at the joint

<?xml version="1.0"?>
<sdf version="1.7">
  <model name="iiwa14">
    <link name="iiwa_link_0">
      <inertial>
        <pose>-0.1 0 0.07 0 0 0</pose>
        <mass>5</mass>
        <inertia>
          <ixx>0.05</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.06</iyy>
          <iyz>0</iyz>
          <izz>0.03</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_link_0_fixed_joint_lump__iiwa_link_0_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_0.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <link name="iiwa_link_1">
      <pose relative_to="iiwa_joint_1"/>
      <inertial>
        <pose>0 -0.03 0.12 0 0 0</pose>
        <mass>5.76</mass>
        <inertia>
          <ixx>0.033</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0333</iyy>
          <iyz>0</iyz>
          <izz>0.0123</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_link_1_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_1.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name="iiwa_joint_1" type="revolute">
      <pose relative_to="iiwa_link_0">0 0 0.1575 0 0 0</pose>
      <child>iiwa_link_1</child>
      <parent>iiwa_link_0</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_link_2">
      <pose relative_to="iiwa_joint_2"/>
      <inertial>
        <pose>0.0003 0.059 0.042 0 0 0</pose>
        <mass>6.35</mass>
        <inertia>
          <ixx>0.0305</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0304</iyy>
          <iyz>0</iyz>
          <izz>0.011</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_link_2_visual_grey">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_2_grey.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <visual name="iiwa_link_2_visual_orange">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_2_orange.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name="iiwa_joint_2" type="revolute">
      <pose relative_to="iiwa_link_1">
        0 0 0.2025 1.570796326794897 0 3.141592653589793
      </pose>
      <child>iiwa_link_2</child>
      <parent>iiwa_link_1</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_link_3">
      <pose relative_to="iiwa_joint_3"/>
      <inertial>
        <pose>0 0.03 0.13 0 0 0</pose>
        <mass>3.5</mass>
        <inertia>
          <ixx>0.025</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0238</iyy>
          <iyz>0</iyz>
          <izz>0.0076</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_link_3_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_3.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <visual name="iiwa_link_3_visual_band">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/band.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.6 0.6 0.6 1.0</diffuse>
        </material>
      </visual>
      <visual name="iiwa_link_3_visual_kuka">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/kuka.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0 0 0 1</diffuse>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name="iiwa_joint_3" type="revolute">
      <pose relative_to="iiwa_link_2">0 0.2045 0 1.570796326794897 0 3.141592653589793</pose>
      <child>iiwa_link_3</child>
      <parent>iiwa_link_2</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_link_4">
      <pose relative_to="iiwa_joint_4"/>
      <inertial>
        <pose>0 0.067 0.034 0 0 0</pose>
        <mass>3.5</mass>
        <inertia>
          <ixx>0.017</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0164</iyy>
          <iyz>0</iyz>
          <izz>0.006</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_link_4_visual_grey">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_4_grey.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <visual name="iiwa_link_4_visual_orange">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_4_orange.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name="iiwa_joint_4" type="revolute">
      <pose relative_to="iiwa_link_3">0 0 0.2155 1.570796326794897 0 0</pose>
      <child>iiwa_link_4</child>
      <parent>iiwa_link_3</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_link_5">
      <pose relative_to="iiwa_joint_5"/>
      <inertial>
        <pose>0.0001 0.021 0.076 0 0 0</pose>
        <mass>3.5</mass>
        <inertia>
          <ixx>0.01</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0087</iyy>
          <iyz>0</iyz>
          <izz>0.00449</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_link_5_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_5.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <visual name="iiwa_link_5_visual_band">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/band.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.6 0.6 0.6 1.0</diffuse>
        </material>
      </visual>
      <visual name="iiwa_link_5_visual_kuka">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/kuka.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0 0 0 1</diffuse>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name="iiwa_joint_5" type="revolute">
      <pose relative_to="iiwa_link_4">0 0.1845 0 -1.570796326794897 3.141592653589793 0</pose>
      <child>iiwa_link_5</child>
      <parent>iiwa_link_4</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-2.96706</lower>
          <upper>2.96706</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_link_6">
      <pose relative_to="iiwa_joint_6"/>
      <inertial>
        <pose>0 0.0006 0.0004 0 0 0</pose>
        <mass>1.8</mass>
        <inertia>
          <ixx>0.0049</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.0047</iyy>
          <iyz>0</iyz>
          <izz>0.0036</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_link_6_visual_grey">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_6_grey.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <visual name="iiwa_link_6_visual_orange">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_6_orange.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>1.0 0.423529411765 0.0392156862745 1.0</diffuse>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name="iiwa_joint_6" type="revolute">
      <pose relative_to="iiwa_link_5">0 0 0.2155 1.570796326794897 0 0</pose>
      <child>iiwa_link_6</child>
      <parent>iiwa_link_5</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-2.0944</lower>
          <upper>2.0944</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <link name="iiwa_link_7">
      <pose relative_to="iiwa_joint_7"/>
      <inertial>
        <pose>0 0 0.02 0 0 0</pose>
        <mass>1.2</mass>
        <inertia>
          <ixx>0.001</ixx>
          <ixy>0</ixy>
          <ixz>0</ixz>
          <iyy>0.001</iyy>
          <iyz>0</iyz>
          <izz>0.001</izz>
        </inertia>
      </inertial>
      <visual name="iiwa_link_7_visual">
        <geometry>
          <mesh>
            <scale>1 1 1</scale>
            <uri>../meshes/visual/link_7.obj</uri>
          </mesh>
        </geometry>
        <material>
          <diffuse>0.4 0.4 0.4 1.0</diffuse>
        </material>
      </visual>
      <gravity>1</gravity>
      <velocity_decay/>
      <self_collide>0</self_collide>
    </link>
    <joint name="iiwa_joint_7" type="revolute">
      <pose relative_to="iiwa_link_6">
        0 0.081 0 -1.570796326794897 3.141592653589793 0
      </pose>
      <child>iiwa_link_7</child>
      <parent>iiwa_link_6</parent>
      <axis>
        <xyz>0 0 1</xyz>
        <limit>
          <lower>-3.05433</lower>
          <upper>3.05433</upper>
          <effort>300</effort>
          <velocity>10</velocity>
        </limit>
        <dynamics>
          <damping>0.0</damping>
          <friction>0</friction>
          <spring_reference>0</spring_reference>
          <spring_stiffness>0</spring_stiffness>
        </dynamics>
      </axis>
    </joint>
    <static>0</static>
    <plugin name="gazebo_ros_controller" filename="libgazebo_ros_control.so">
      <robotNamespace>/iiwa</robotNamespace>
    </plugin>
  </model>
</sdf>

Robot Description Files

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)

Inverse Kinematics

  • Different versions (tasks), e.g. position vs pose of eef
  • Difficult in general - solving trigonometric equations
    • There may be no solutions, multiple solutions or infinite solutions
  • Analytic methods
    • For non-redundant robots - compute all solutions
  • Numerical solution
    • Given initial guess at configuration, do e.g. numerical root finding
    • (More later)

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_z)

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

note the typesetting

Spatial Velocity Algebra

Computing Geometric Jacobian

Analytic vs Geometric Jacobians

  • Analytic Jacobian relates joint velocities to derivatives of pose parameters
  • Geometric Jacobian relates joint velocities to spatial velocity
  • The angular velocity and the derivative of orientation parameters are related by a matrix: N(q)
  • The mapping between analytic and geometric Jacobians involves using this matrix to map the rotational component.

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):

Our first Jacobian pseudo-inverse controller

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

Moore-Penrose pseudo-inverse

Behavior near singularities

  • Pseudo-inverse lets us handle non-square matrices
  • But, near singularities, joint velocities still get very large
  • One approach is damped pseudo-inverse
J^T(JJ^T + \lambda I)^{-1}

Our first Jacobian pseudo-inverse controller

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

Our first Jacobian pseudo-inverse controller

class PseudoInverseController(LeafSystem):
    def __init__(self, plant):
        LeafSystem.__init__(self)
        self._plant = plant
        self._plant_context = plant.CreateDefaultContext()
        self._iiwa = plant.GetModelInstanceByName("iiwa")
        self._G = plant.GetBodyByName("body").body_frame()
        self._W = plant.world_frame()

        self.V_G_port = self.DeclareVectorInputPort("V_WG", 6)
        self.q_port = self.DeclareVectorInputPort("iiwa.position", 7)
        self.DeclareVectorOutputPort("iiwa.velocity", 7, self.CalcOutput)
        self.iiwa_start = plant.GetJointByName("iiwa_joint_1").velocity_start()
        self.iiwa_end = plant.GetJointByName("iiwa_joint_7").velocity_start()

    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)

Pick and Place

X_O = {
    "initial": RigidTransform(
        RotationMatrix.MakeZRotation(np.pi / 2.0), [0, -0.6, 0.0]
    ),
    "goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi), [0.5, 0, 0.0]),
}

builder = DiagramBuilder()

# Initialize
scenario_data = """ ... """
scenario = LoadScenario(data=scenario_data)
station = builder.AddSystem(MakeHardwareStation(scenario, meshcat=meshcat))
plant = station.GetSubsystemByName("plant")
plant.SetDefaultFreeBodyPose(plant.GetBodyByName("base_link"), X_O["initial"])

# Find the initial pose of the gripper and object (as set in the default Context)
temp_context = station.CreateDefaultContext()
temp_plant_context = plant.GetMyContextFromRoot(temp_context)
X_G = {
    "initial": plant.EvalBodyPoseInWorld(
        temp_plant_context, plant.GetBodyByName("body")
    )
}
X_O["initial"] = plant.EvalBodyPoseInWorld(
    temp_plant_context, plant.GetBodyByName("base_link")
)
X_G, times = MakeGripperFrames(X_G, X_O)

# Make the trajectories
traj = MakeGripperPoseTrajectory(X_G, times)
traj_V_G = traj.MakeDerivative()

V_G_source = builder.AddSystem(TrajectorySource(traj_V_G))
controller = builder.AddSystem(PseudoInverseController(plant))
builder.Connect(V_G_source.get_output_port(), controller.GetInputPort("V_WG"))

integrator = builder.AddSystem(Integrator(7))
builder.Connect(controller.get_output_port(), integrator.get_input_port())
builder.Connect(integrator.get_output_port(), station.GetInputPort("iiwa.position"))
builder.Connect(
    station.GetOutputPort("iiwa.position_measured"),
    controller.GetInputPort("iiwa.position"),
)

traj_wsg_command = MakeGripperCommandTrajectory(times)
wsg_source = builder.AddSystem(TrajectorySource(traj_wsg_command))
builder.Connect(wsg_source.get_output_port(), station.GetInputPort("wsg.position"))

diagram = builder.Build()

simulator = Simulator(diagram)
context = simulator.get_mutable_context()
station_context = station.GetMyContextFromRoot(context)
integrator.set_integral_value(
    integrator.GetMyContextFromRoot(context),
    plant.GetPositions(
        plant.GetMyContextFromRoot(context),
        plant.GetModelInstanceByName("iiwa"),
    ),
)

simulator.AdvanceTo(traj_p_G.end_time())

Basic Pick and Place

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)

Fall 26 - Lec 4

By Tomas Lozano-Perez

Fall 26 - Lec 4

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

  • 653