diff --git a/examples/rigid/diffik_controller.py b/examples/rigid/diffik_controller.py
index 2a8f54e..9414d0c 100644
--- a/examples/rigid/diffik_controller.py
+++ b/examples/rigid/diffik_controller.py
@@ -7,16 +7,16 @@ import genesis as gs
 config = {
     "ur5e": {"mjcf_file": "xml/universal_robots_ur5e/ur5e.xml", "end_effector_link": "ee_virtual_link"},
     "panda": {"mjcf_file": "xml/franka_emika_panda/panda.xml", "end_effector_link": "left_finger"},
+    "so_arm_100": {"mjcf_file": "../mujoco_menagerie/trs_so_arm100/so_arm100.xml", "end_effector_link": "Fixed_Jaw"},
 }
 
-
 def main():
 
     parser = argparse.ArgumentParser()
     parser.add_argument("-v", "--vis", action="store_true", default=False)
     parser.add_argument("-c", "--cpu", action="store_true", default=False)
     parser.add_argument(
-        "-r", "--robot", choices=["panda", "ur5e"], default="ur5e", help="Select robot model (panda or ur5e)"
+        "-r", "--robot", default="ur5e", help="Select robot model (panda or ur5e)"
     )
     args = parser.parse_args()
 
@@ -27,8 +27,8 @@ def main():
     ########################## create a scene ##########################
     scene = gs.Scene(
         viewer_options=gs.options.ViewerOptions(
-            camera_pos=(0.0, -2, 1.5),
-            camera_lookat=(0.0, 0.0, 0.5),
+            camera_pos=(0.0, -1, 0.5),
+            camera_lookat=(0.0, 0.0, 0.3),
             camera_fov=40,
             max_FPS=200,
         ),
@@ -66,38 +66,48 @@ def main():
     scene.build()
 
     target_quat = np.array([0, 1, 0, 0])
-    center = np.array([0.5, 0, 0.5])
+    center = np.array([0, -0.1, 0.3])
     r = 0.1
-    damping = 1e-4
+    damping = 6e-4
     diag = damping * np.eye(6)
 
     end_effector_link = robot_config["end_effector_link"]
     ee_link = robot.get_link(end_effector_link)
 
-    for i in range(0, 2000):
+    print("qpos limits=", robot.get_dofs_limit())
+
+    for i in range(0, 100000):
         target_pos = center + np.array([np.cos(i / 360 * np.pi), np.sin(i / 360 * np.pi), 0]) * r
 
         target_entity.set_qpos(np.concatenate([target_pos, target_quat]))
 
-        # Position error.
-        error_pos = target_pos - ee_link.get_pos().cpu().numpy()
+        # control
+        q = goal_qpos(target_pos, ee_link, robot, diag)
+        #q = goal_qpos_ompl(target_pos, ee_link, robot)
+        robot.control_dofs_position(q)
+        scene.step()
 
-        # Orientation error.
-        ee_quat = ee_link.get_quat().cpu().numpy()
-        error_quat = gs.transform_quat_by_quat(gs.inv_quat(ee_quat), target_quat)
-        error_rotvec = gs.quat_to_rotvec(error_quat)
+def goal_qpos(target_pos, ee_link, robot, diag):
+    # Position error.
+    error_pos = target_pos - ee_link.get_pos().cpu().numpy()
 
-        error = np.concatenate([error_pos, error_rotvec])
+    # Orientation error.
+    error_rotvec = [0, 0, 0]
 
-        # jacobian
-        jac = robot.get_jacobian(link=ee_link).cpu().numpy()
-        dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, error)
-        q = robot.get_qpos().cpu().numpy() + dq
+    error = np.concatenate([error_pos, error_rotvec])
 
-        # control
-        robot.control_dofs_position(q)
-        scene.step()
+    # jacobian
+    jac = robot.get_jacobian(link=ee_link).cpu().numpy()
+    dq = jac.T @ np.linalg.solve(jac @ jac.T + diag, error)
+    return robot.get_qpos().cpu().numpy() + dq
 
+def goal_qpos_ompl(target_pos, ee_link, robot):
+    target_quat = None
+    return robot.inverse_kinematics(
+        link     = ee_link,
+        pos      = target_pos,
+        quat     = target_quat,
+    )
 
 if __name__ == "__main__":
     main()