Tomas Lozano-Perez PRO
Roboticist at MIT
MIT 6.4210/2: Robotic Manipulation
Fall 2026, Lecture 4
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>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()
What is \(X^B\) ?
X_B = plant.EvalBodyPoseInWorld(brick, context)What is \(q\) ?
q = plant.GetPositions(context)Kinematic "Jacobian"
angular velocity
translational velocity
Q: How do we represent angular velocity, \(\omega^B\)??
A: Unlike 3D rotations, here 3 numbers are sufficient and efficient; so we use the same representation everywhere.
note the typesetting
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()
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):
\( v = [J^{G}(q)]^{+} {}^WV^G \)
\( v = [J^{G}(q)]^{+} {}^WV^G \)
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)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())
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)By Tomas Lozano-Perez