Tomas Lozano-Perez PRO
Roboticist at MIT
MIT 6.4210/2: Robotic Manipulation
Fall 2026, Lecture 5
Kinematic Jacobian: \( {}^WV^G = J^{G}(q) v \)
\( v = [J^{G}(q)]^{+} {}^WV^G \)
spatial velocity
generalized velocity
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)Charles W. Wampler and Andrew J. Sommese. Numerical algebraic geometry and algebraic kinematics. Acta Numerica, 20:469-567, 2011.
subject to:
ik = InverseKinematics(plant, plant_context)
ik.AddPositionConstraint(
gripper_frame,
[0, 0, 0],
plant.world_frame(),
pose.translation(),
pose.translation(),
)
ik.AddOrientationConstraint(
gripper_frame,
RotationMatrix(),
plant.world_frame(),
pose.rotation(),
0.0,
)
prog = ik.get_mutable_prog()
q = ik.q()
prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)
prog.SetInitialGuess(q, q0)
result = Solve(ik.prog())start
goal
fixed number of samples
collision-avoidance
(outside the \(L^1\) ball)
nonconvex
"Using trajectory optimization made scooping up to twice as fast"
credit:
By Tomas Lozano-Perez