MIT 6.4210/2: Robotic Manipulation
Fall 2026, Lecture 3
Kinematic Jacobian: \( {}^WV^G = J^{G}(q) v \)
\( v = [J^{G}(q)]^{+} {}^WV^G \)
spatial velocity
generalized velocity
which is a possible value for \({}^Gp^O\)?
a) [.3, 0, -.1]
b) [0, .3, .1]
c) [0, -.3, .1]
Answer: (b)
which is a possible value for \({}^Gp^O_O\)?
a) [.3, 0, -.1]
b) [0, .3, .1]
c) [0, -.3, .1]
Answer: (a)
Example: Gimbal Lock
def MakeGripperFrames(X_WG, X_WO):
"""
Takes a partial specification with X_G["initial"] and X_O["initial"] and
X_0["goal"], and returns a X_G and times with all of the pick and place
frames populated.
"""
# Define (again) the gripper pose relative to the object when in grasp.
p_GgraspO = [0, 0.11, 0]
R_GgraspO = RotationMatrix.MakeXRotation(
np.pi / 2.0
) @ RotationMatrix.MakeZRotation(np.pi / 2.0)
X_GgraspO = RigidTransform(R_GgraspO, p_GgraspO)
X_OGgrasp = X_GgraspO.inverse()
# pregrasp is negative y in the gripper frame (see the figure!).
X_GgraspGpregrasp = RigidTransform([0, -0.08, 0])
X_WG["pick"] = X_WO["initial"] @ X_OGgrasp
X_WG["prepick"] = X_WG["pick"] @ X_GgraspGpregrasp
X_WG["place"] = X_WO["goal"] @ X_OGgrasp
X_WG["preplace"] = X_WG["place"] @ X_GgraspGpregrasp
# I'll interpolate a halfway orientation by converting to axis angle and halving the angle.
X_GprepickGpreplace = X_WG["prepick"].inverse() @ X_WG["preplace"]
angle_axis = X_GprepickGpreplace.rotation().ToAngleAxis()
X_GprepickGclearance = RigidTransform(
AngleAxis(angle=angle_axis.angle() / 2.0, axis=angle_axis.axis()),
X_GprepickGpreplace.translation() / 2.0 + np.array([0, -0.3, 0]),
)
X_WG["clearance"] = X_WG["prepick"] @ X_GprepickGclearance
# Now let's set the timing
times = {"initial": 0}
X_GinitialGprepick = X_G["initial"].inverse() @ X_WG["prepick"]
times["prepick"] = times["initial"] + 10.0 * np.linalg.norm(
X_GinitialGprepick.translation()
)
# Allow some time for the gripper to close.
times["pick_start"] = times["prepick"] + 2.0
times["pick_end"] = times["pick_start"] + 2.0
X_WG["pick_start"] = X_WG["pick"]
X_WG["pick_end"] = X_WG["pick"]
times["postpick"] = times["pick_end"] + 2.0
X_WG["postpick"] = X_WG["prepick"]
time_to_from_clearance = 10.0 * np.linalg.norm(
X_GprepickGclearance.translation()
)
times["clearance"] = times["postpick"] + time_to_from_clearance
times["preplace"] = times["clearance"] + time_to_from_clearance
times["place_start"] = times["preplace"] + 2.0
times["place_end"] = times["place_start"] + 2.0
X_WG["place_start"] = X_WG["place"]
X_WG["place_end"] = X_WG["place"]
times["postplace"] = times["place_end"] + 2.0
X_WG["postplace"] = X_WG["preplace"]
return X_WG, times
Make sure your trajectory safely clears the bins...