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