russtedrake PRO
Roboticist at MIT and TRI
Follow live at https://slides.com/d/mhvQHfc/live
(or later at https://slides.com/russtedrake/fall23-lec03)
MIT 6.4210/2: Robotic Manipulation
Fall 2023, Lecture 3
Kinematic Jacobian: \( {}^WV^G = J^{G}(q) v \)
\( v = [J^{G}(q)]^{-1} {}^WV^G \)
spatial velocity
generalized velocity
which is a possible value for \({}^Gp^O\)?
a) [.2, 0, -.2]
b) [0, .3, .1]
c) [0, -.3, .1]
Answer: (b)
which is a possible value for \({}^Gp^O_O\)?
a) [.2, 0, -.2]
b) [0, .3, .1]
c) [0, -.3, .1]
Answer: (a)
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...
<?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>
This page will only work during the actual lecture; I'll be updating the url to the live meshcat window so that many people can follow along.
By russtedrake
MIT Robotic Manipulation Fall 2023 http://manipulation.csail.mit.edu