def make_gripper_frames(X_G, X_O):
  """
  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.12, 0]
  R_GgraspO = RotationMatrix.MakeXRotation(np.pi/2.0).multiply(
      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_G["pick"] = X_O["initial"].multiply(X_OGgrasp)
  X_G["prepick"] = X_G["pick"].multiply(X_GgraspGpregrasp)
  X_G["place"] = X_O["goal"].multiply(X_OGgrasp)
  X_G["preplace"] = X_G["place"].multiply(X_GgraspGpregrasp)

  # I'll interpolate a ahlfway orientation by converting to axis angle and halving the angle.
  X_GprepickGpreplace = X_G["prepick"].inverse().multiply(X_G["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_G["clearance"] = X_G["prepick"].multiply(X_GprepickGclearance)

  # Now let's set the timing
  times = {"initial": 0}
  X_GinitialGprepick = X_G["initial"].inverse().multiply(X_G["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
  times["postpick"] = times["pick_end"] + 2.0
  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
  times["postplace"] = times["place_end"] + 2.0

  return X_G, times

X_G = {"initial": RigidTransform(RotationMatrix.MakeXRotation(-np.pi/2.0), [0, -0.25, 0.25])}
X_O = {"initial": RigidTransform(RotationMatrix.MakeZRotation(np.pi/2.0), [-.2, -.75, 0.025]),
       "goal": RigidTransform(RotationMatrix.MakeZRotation(np.pi),[.75, 0, 0.025])}
X_G, times = make_gripper_frames(X_G, X_O)
print(f"Sanity check: The entire maneuver will take {times['postplace']} seconds to execute.")

<?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>