IK with Mimic Joints

This is a simple test to ensure that mimic joints are handled correctly in the IK solver.

We procedurally generate a “zig-zag” chain of links with mimic joints, where:

  • the first joint is driven directly,

  • and the remaining joints are driven indirectly via mimic joints. The multipliers alternate between -1 and 1, and the offsets are all 0.

All examples can be run by first cloning the PyRoki repository, which includes the pyroki_snippets implementation details.

  1import time
  2
  3import numpy as np
  4import pyroki as pk
  5import viser
  6from viser.extras import ViserUrdf
  7
  8import pyroki_snippets as pks
  9
 10
 11def create_chain_xml(length: float = 0.2, num_chains: int = 5) -> str:
 12    def create_link(idx):
 13        return f"""
 14                <link name="link_{idx}">
 15                    <visual>
 16                    <geometry>
 17                        <cylinder length="{length}" radius="0.02"/>
 18                    </geometry>
 19                    </visual>
 20                    <inertial>
 21                    <mass value="0.1"/>
 22                    <inertia ixx="0.0001" iyy="0.0001" izz="0.0001"/>
 23                    </inertial>
 24                </link>
 25            """
 26
 27    def create_joint(idx, multiplier=1.0, offset=0.0):
 28        mimic = f'<mimic joint="joint_0" multiplier="{multiplier}" offset="{offset}"/>'
 29        return f"""
 30            <joint name="joint_{idx}" type="revolute">
 31                <parent link="link_{idx}"/>
 32                <child link="link_{idx + 1}"/>
 33                <axis xyz="1 0 0"/>
 34                <origin xyz="0 0 {length}" rpy="0 0 0"/>
 35                {mimic if idx != 0 else ""}
 36                <limit lower="-10.0" upper="10.0" velocity="1.0"/>
 37            </joint>
 38            """
 39
 40    world_joint_origin_z = length / 2.0
 41    xml = f"""
 42    <?xml version="1.0"?>
 43    <robot name="chain_with_mimic_joints">
 44    <link name="world"></link>
 45    <joint name="world_joint" type="revolute">
 46        <parent link="world"/>
 47        <child link="link_0"/>
 48        <axis xyz="1 0 0"/>
 49        <origin xyz="0 0 {world_joint_origin_z}" rpy="0 0 0"/>
 50        <limit lower="-10.0" upper="10.0" velocity="1.0"/>
 51    </joint>
 52    """
 53    # Create the definition + first link.
 54    xml += create_link(0)
 55    xml += create_link(1)
 56    xml += create_joint(0)
 57
 58    # Procedurally add more links.
 59    assert num_chains >= 2
 60    for idx in range(2, num_chains):
 61        xml += create_link(idx)
 62        current_offset = 0.0
 63        current_multiplier = 1.0 * ((-1) ** (idx % 2))
 64        xml += create_joint(idx - 1, current_multiplier, current_offset)
 65
 66    xml += """
 67    </robot>
 68    """
 69    return xml
 70
 71
 72def main():
 73    """Main function for basic IK."""
 74
 75    import yourdfpy
 76    import tempfile
 77
 78    xml = create_chain_xml(num_chains=10, length=0.1)
 79    with tempfile.NamedTemporaryFile(mode="w", suffix=".urdf") as f:
 80        f.write(xml)
 81        f.flush()
 82        urdf = yourdfpy.URDF.load(f.name)
 83
 84    # Create robot.
 85    robot = pk.Robot.from_urdf(urdf)
 86
 87    # Set up visualizer.
 88    server = viser.ViserServer()
 89    server.scene.add_grid("/ground", width=2, height=2)
 90    urdf_vis = ViserUrdf(server, urdf, root_node_name="/base")
 91    target_link_name_handle = server.gui.add_dropdown(
 92        "Target Link",
 93        robot.links.names,
 94        initial_value=robot.links.names[-1],
 95    )
 96
 97    # Create interactive controller with initial position.
 98    ik_target = server.scene.add_transform_controls(
 99        "/ik_target", scale=0.2, position=(0.0, 0.1, 0.1), wxyz=(0, 0, 1, 0)
100    )
101    timing_handle = server.gui.add_number("Elapsed (ms)", 0.001, disabled=True)
102
103    while True:
104        # Solve IK.
105        start_time = time.time()
106        solution = pks.solve_ik(
107            robot=robot,
108            target_link_name=target_link_name_handle.value,
109            target_position=np.array(ik_target.position),
110            target_wxyz=np.array(ik_target.wxyz),
111        )
112
113        # Update timing handle.
114        elapsed_time = time.time() - start_time
115        timing_handle.value = 0.99 * timing_handle.value + 0.01 * (elapsed_time * 1000)
116
117        # Update visualizer.
118        urdf_vis.update_cfg(solution)
119
120
121if __name__ == "__main__":
122    main()