diff --git a/.gitignore b/.gitignore
index 172fe3e..a4bcc15 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,16 +1,4 @@
-# 本机现场配置（含 IP / 序列号；从 site.example.yaml 复制）
-config/site.yaml
 
-# 模型权重二进制 —— 一律不进 git（体积大、每台机器重新转换即可）
-#   用通配模式而不是逐个目录列举，否则新增一个分组就会有人误提交二进制。
-#   注意：*.labels.yaml 是**文本标签配置**，不属于二进制，应该进 git。
-models/**/*.pt
-models/**/*.pth
-models/**/*.onnx
-models/**/*.engine
-models/**/*.trt
-models/**/*.plan
-models/**/*.tflite
 
 # staging = 待转换的临时区，内容不进 git（含转换后归档的 done/）
 #   但 .gitkeep 和说明文档要留着，否则新克隆的仓库里这个目录不存在/看不懂。
@@ -25,8 +13,7 @@ log/
 # 仿真的构建产物 —— 与真机的 build/ install/ 隔离（见 scripts/build_sim.sh）
 #   两套产物目录并存是刻意的：工作区只有一份源码，两个环境的依赖路径不同，
 #   共用 install/ 会互相覆盖。
-build_sim/
-install_sim/
+
 log_sim/
 # 真机各终端（T1 栈 / T2 相机 / T3 触觉 / T4 夹爪）的日志与截图。
 #   与 log_sim/ 对称：日志是运行产物，不进 git；跑完要留档的证据（如相机截图）
@@ -46,26 +33,10 @@ __pycache__/
 .DS_Store
 *.swp
 
-# ROS bags
-*.bag
-*.db3
-
-# Hand-eye backups
-*.bak
-*.bak.*
-
-# 可选离线 RDK / Isaac（体积大）
-thirdparty/
-vendor/
 
 
 # 手眼标定的临时产物（采点图、taught_poses.txt、captures.json）
 handeye/tmp/*
 !handeye/tmp/.gitkeep
 
-# 仿真手眼真值：**生成物**，不是标定结果 —— 由 scripts/gen_sim_handeye.py 从
-# URDF/TF 真值导出，run_gauge_assembly.sh --sim 每次自动重生成。
-# 不跟踪的理由：内容里有一行生成时间，每跑一次就脏一次仓库，让人分不清
-# "这次改动是代码改的还是只是跑了一遍"。真机标定结果（89_/91_*.yaml）**要跟踪**，
-# 那是需要留档的实测数据，不是推导出来的。
-handeye/results/sim_*.yaml
+
diff --git a/config/site.example.yaml b/config/site.example.yaml
index 2dc0aee..1a86992 100644
--- a/config/site.example.yaml
+++ b/config/site.example.yaml
@@ -177,7 +177,8 @@ robot:
         #   落在本文件下方注释记录的合理区间 0.204~0.233 内。
         #   复核：python3 scripts/sense_tool_length.py --sn Rizon4-XXXXXX
         #         python3 scripts/lib/real_geometry_check.py --sn Rizon4-XXXXXX
-        tool_tip_below_flange_m: 0.223
+        # 下臂安全距离按 tool10120938 的 TCP +Z，不再用旧 tool1 的 0.223。
+        tool_tip_below_flange_m: 0.2245
     # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
     # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
     #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
@@ -592,6 +593,7 @@ servo:
   # D405 跟住后，法兰 XY 连续这么久位移不超过 lock_still_move_m → /visual_servo/locked
   # 顶视一次性把臂迁过去的那段路不算：必须是腕部有效目标。大幅一动就重计时。
   lock_still_sec: 3.0
+  wrist_align_sec: 1.0
   lock_still_move_m: 0.015   # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
   e_xy_clip_m: 0.200         # 大误差仍按 v_max 走，靠 a_max 加速
   v_max_xy: 0.45             # 法兰 XY 上限 450 mm/s
@@ -765,6 +767,7 @@ hole_detect:
     # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
     align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
     move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
+    hole_visual_only: true
     final_gap_mm: 2.0        # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触）
     observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
                              # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
diff --git a/config/site.yaml b/config/site.yaml
new file mode 100644
index 0000000..4e057b2
--- /dev/null
+++ b/config/site.yaml
@@ -0,0 +1,1130 @@
+# =============================================================================
+# site.yaml — 本机唯一配置入口（不进 git；从 site.example.yaml 复制）
+# 路径：~/ros2/config/site.yaml
+#
+# 89 与 91 的手眼 / 腕部相机 / 夹爪 / 点位互相独立，禁止混用。
+# 切换机械臂：改 robot.sn，或所有终端加同一 --arm 89|91。
+# =============================================================================
+
+robot:
+  # >>> 切换机械臂：改成 robots 里某一台的完整 SN <<<
+  sn: "Rizon4-063989"
+  # Studio Tools 名（一字不差）。空=不切换。查：bash ~/ros2/scripts/rdk_switch_tool.sh --list
+  # 左臂 89 的激活工具写在 robots[] 里（tool10120938）；这里只给未覆盖的臂当缺省。
+  tool_name: "tool1"
+  # 每台登记 SN + IP，以及该臂自己的相机/夹爪/手眼/点位（覆盖下面的全局段）。
+  robots:
+    - sn: "Rizon4-063989"
+      arm: left
+      ip: "192.168.2.101"
+      pc_ip: "192.168.2.10"
+      iface: "eth0"
+      label: "左臂89"
+      # 本臂装了 Orisys 视触觉（只有左臂有）。脚本靠这一项决定要不要拉触觉服务，
+      # 而不是靠"SN 等于某个具体值"这种写死的判断（换台臂就会失效）。
+      tactile: true
+      # 2026-10-12 示教的工具 TCP（tool + 1012 + 0938）。眼在手上 AX=XB 用这份 TCP。
+      tool_name: "tool10120938"
+      camera:
+        name: d405_left          # 话题 /d405_left/... 与 91 分开
+        serial: "352122274408"   # 左腕 D405（一直在用的那台）
+        auto_exposure: true
+        ae_limit_us: 4000
+        emitter_enabled: false
+      gripper:
+        model: "PGEA-100-20-W"
+        sn: "26610799"
+        port: "/dev/ttyUSB0"
+        stroke_mm: 20.0
+        # 大寰上电后必须初始化一次（写 0xA5），而初始化的动作是**手指先回零**
+        # = 张开到满行程。如果这时爪子已经夹着通止规/工件，规会被放开、掉下去。
+        # 2026-09-28 现场实测过：auto_init 走一遍，开口从 19.18mm 变成 20.00mm。
+        # 所以这里的默认是 **false**：由人决定什么时候初始化。
+        #   · 上电后、还没装夹 → 可以临时开一次：SITE_GRIP_AUTO_INIT=true bash scripts/run_gripper.sh --arm 89
+        #   · 已经装夹（装配过程中）→ 保持 false；不初始化也能读状态、也能开合
+        #     （status 里的 init=need_init 只说明"绝对位置标定没做"，不影响夹持）
+        # 想让开机必初始化（无人值守的上电流程）就显式改 true，并确认爪里是空的。
+        auto_init: false
+      named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_89.yaml"
+      paths:
+        handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
+        handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
+        handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
+      handeye:
+        # Studio tcp_location（法兰系，米 + qwxyz）。与 rdk Tool.params 必须一致。
+        # 眼在手上：AX=XB 的手是这份 TCP，再折成 cam→法兰写 T_4x4。
+        # 眼在手外：AX=XB 的手同样是这份 TCP（板夹在工具上）；T_4x4 仍是 cam→基座，不折。
+        gripper_tool: "tool10120938"
+        gripper_tcp_xyz: [-0.0005, 0.0067, 0.2245]
+        gripper_tcp_qwxyz: [1.0, 0.0, 0.0, 0.0]
+      # 左臂跟踪高度（只在 --arm 89 时生效，改 91 的 servo 不影响这里）
+      # 锚点 `&servo_left`：下面的"仿真台"条目直接引用它 —— 仿真建的就是左臂，
+      # 悬停/锁高必须与 89 **一模一样**，抄两份迟早漂移。
+      servo: &servo_left
+        # 2026-09-29 底座加高后由 0.35 → 0.36：工件上表面实测抬高 10mm
+        # （0.012 → 0.022），锁高同步 +0.011 ⇒ 相机到工件的视距**与加高前一致**
+        # （≈0.213m，跟踪那一档已实拍验证过检出稳定），不是随手抬的。
+        # 跟踪锁高与 hole_hover_z_m 相同：先到这个悬高，一阶段只动 XY。
+        # 对孔完成后再下降。以前 ee_z 高于悬停（0.36），对孔前会先降一截，
+        # 倾斜相机下孔位被重投影，偏差变大。
+        ee_z_m: 0.320          # 跟踪锁高：法兰相对基座 Z [m]，与 hole_hover_z_m 同一值
+        min_ee_z_m: 0.20
+        safe_z_m: 0.45
+        z_min_m: 0.22
+        xy_track: hover        # 法兰(夹爪中心) XY 压在工件上，不是画面中心
+        ee_vertical_down: true # 法兰 +Z 朝世界 −Z
+        hover_offset_ee_xy_m: [0.0, 0.0]  # 爪心相对法兰 XY；平行爪同轴保持 0
+        # >>> 二阶段孔定位悬停锁高：只改这里，91 用下面那段 <<<
+        # 法兰 Z [m]；要兼顾：够低拍清 k1/k2 螺纹孔，够高通止规尖不碰工件。
+        # 调试：bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
+        # = 工件上表面 0.022（本臂 gauge_assembly.workpiece_surface_z_m，加底座后重测）
+        #   + 工具长 0.245 + 间隙 0.002
+        # 2026-09-29 实测：按 0.23 悬停时工具尖端落在工件面**下** 5~10mm，
+        # 悬停这一步就把通止规插进了工件（见本臂 gauge_assembly 段的完整说明）。
+        #
+        # 2026-09-29 二次修正 —— 上面那条"工具几何"只保证**不碰工件**，它保证不了
+        # **相机看得见孔**，而二阶段的第一步就是拍 k1/k2。旧值 0.259 的后果实测：
+        #   相机光心在法兰下方 0.136m（手眼 T_cam_to_gripper + 29.7° 倾角）
+        #   法兰 0.259 → 相机离工件面 0.123m → 检出 0/16，conf 峰值 0.012（噪声）
+        #   法兰 0.309 → 相机离工件面 0.173m → 检出 16/16，conf 0.87，
+        #                k0/k1/k2 置信度全 0.999~1.000，k1-k2 间距 156px 稳定
+        # gauge_fsm.hover_z_guard 的设计注释也写着"腕部相机要离工件面 ~0.17m
+        # 才看得清孔"，所以 0.309 才是这条同时满足"相机看得见"和"规尖不碰"的值
+        # （规尖此时在面上 64mm，比旧值更安全）。
+        # 最后 2mm 的贴近由 stage2 的**深度闭环轻降**（final_gap_mm）完成 ——
+        # 它按实测间隙往下走，与绝对悬停高无关，所以抬高悬停不会让尖撞工件。
+        #
+        # 2026-09-29 三次修正（工件加了底座，工作平面重新感知）：
+        #   拿 gongjian0929 在**实时图**上出 k0/k1/k2，再取各点邻域深度投到基座系：
+        #     k1 = +0.0220 / +0.0221 / +0.0223 m（多帧重复性 0.4mm 内）
+        #     k2 = +0.0223 / +0.0224 m
+        #     k0 = +0.0165 / +0.0186 m（偏低约 6mm 且抖，局部读数噪声大）
+        #   三点"中心 vs 外环"深度差 ≤1mm ⇒ 三点都落在**上表面**上，不是孔底 ⇒
+        #   上表面取 k1/k2 的一致值 **0.022**（原 0.012 是加底座前的旧值）。
+        #   悬停随之 0.309 → 0.320（同样 +0.011），让"相机到工件面"的**视距保持
+        #   ≈0.17m 不变** —— 0.309 那一档是实拍验证过"16/16 检出、conf 0.87、
+        #   k1-k2 间距 156px 稳定"的工作点，改高度就是在改视距，不能随手抬。
+        #   几何兜底仍然满足：0.320 − 工具长 0.245 = 尖端 0.075，工件面 0.022
+        #   ⇒ 尖端在面上方 53mm（要求 ≥ final_gap 2mm）。
+        hole_hover_z_m: 0.320  # ← 二阶段悬停高度调这一行
+        # FT 数据源：默认按 sn 推导 = /<sn>/ft_sensor_wrench，由真机
+        # flexiv_robot_states_broadcaster 发布。但 **2026-10-08 实测该话题恒为
+        # 全零（0,0,0）** —— RDK 那侧的原始 FT 传感没在出数（`ft_sensor_raw` 为 0），
+        # 于是应用里"abs(fz−基线)>2.5N 判接触"的兜底**永远判不到接触**（静默失效）。
+        # 同一时刻 /<sn>/external_wrench_in_tcp 是有数的（≈负载 54N，且随受力变化），
+        # 所以真机改用它。判接触只看"相对轻降起始基线的增量"，与 54N 这个偏置无关，
+        # 用这个源可直接恢复接触兜底，不需要改代码。
+        # 仿真**别跟着用**（仿真没有这个话题）—— 见本文件 sim 段的显式覆盖。
+        # 注意：这不是话题名写错，而是"哪个数据源有数"的取舍（真机 RDK 未启用原始 FT）。
+        ft_wrench_topic: "/Rizon4_063989/external_wrench_in_tcp"
+      # ---- 工件几何（2026-09-29 现场实测标定）----
+      # 真机段的坐标都是**基座系**；本机基座与台面同高 ⇒ 台面 z ≈ 0。
+      gauge_assembly:
+        # 工件**上表面**的世界 z [m]。悬停高度的几何判据（gauge_fsm.hover_z_guard）
+        # 与 S07 下探预算都拿它当基准面。
+        #
+        # ⚠️ 为什么不能留全局段的默认 0.0：0.0 = "未知"，运行期退回"用 stage2 实测
+        #    孔位 z 兜底" —— 但兜底发生在**悬停之后**，悬停本身还是按 0.0 算的。
+        #    后果（2026-09-29 实测）：
+        #      悬停 0.247 − 工具长 0.245 = 工具尖端 z≈0.002，而工件面在 0.0074
+        #      ⇒ 尖端在工件面**下** 5~10mm。悬停这一步就把通止规插进工件，接着
+        #      S05 的横向对准等于拿规尖**拨工件**：两孔中点两拍之间被推走 19.1mm，
+        #      残差 27.3 → 42.7mm 反向发散（日志看着像手眼标定坏了），最后被
+        #      drag_verdict 看门狗拦下（exit=6）。
+        #
+        # 实测依据（旧，加底座前）：stage2 解出的两孔基座系 z = 0.00743 / 0.00715 m
+        #   （/tmp/stage2_result.json 的 k1_base_m / k2_base_m）。孔是通孔，
+        #   深度相机在孔口附近读数会偏深 ⇒ 上表面取工件板厚 **0.012**。
+        #
+        # 实测依据（2026-09-29 加底座后**重新感知**，现行值 = 0.022）：
+        #   不再靠"板厚"推，而是直接量上表面 —— 让 gongjian0929 在实时图上出
+        #   k0/k1/k2，对每个点取 (±6px) 邻域深度的中位数，用同一帧 TF
+        #   (base←flange · flange←cam) 投到基座系：
+        #     k1 = +0.0220 / +0.0221 / +0.0223，k2 = +0.0223 / +0.0224（重复性 0.4mm）
+        #     k0 = +0.0165 / +0.0186（偏低 6mm 且抖 ⇒ 该点邻域读数不可靠，不参与定面）
+        #   判"是不是孔底"：各点中心 (±6px) 与外环 (12~20px) 深度差 ≤1mm ⇒ 齐平
+        #   ⇒ 三点都在上表面。ROI 高度直方图同为单峰平台，p75≈0.0228、p95≈0.0247
+        #   ⇒ 上表面 0.022（比加底座前的 0.012 高 10mm）✓ 与"工件加了底座变高"一致。
+        #   法兰 Z=0.356 时该点光学视距实测 0.204m（模型仍检出 conf 0.862）⇒
+        #   模型对 0.17~0.20m 有一档余量，但两侧都不要顶到边界。
+        # 复核（会同时打印实测工件面 + 悬停几何自检）：
+        #   bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only
+        #   python3 scripts/lib/real_geometry_check.py --sn Rizon4-063989
+        workpiece_surface_z_m: 0.022
+        # 工具长（规尖在法兰下方的距离）—— **2026-10-08 真机感知标定**：0.223m。
+        #   方法：hover(法兰 0.320) 下腕部相机抓一帧，取规杆最深像素 + 深度，用
+        #   手眼 T_base = T_base_flange·T_flange_cam 投到基座 ⇒ 规尖 z=0.0971，
+        #   故 0.320 − 0.0971 = 0.2229m。同帧反投影链的独立校验：k1/k2 基座 z
+        #   精确复现上次实测 0.0219/0.0204（误差 <0.1mm），链路可信。
+        #   工具长 = tool1 夹爪 TCP 0.147 + 规露出 0.076。
+        #   ⚠️ 旧值 0.245 是照抄仿真的（TCP 0.200 + 规 0.043），比真机长 22mm：
+        #   它让几何误以为悬停时尖端只在面上方 53mm（实际 75mm），轻降预算因此
+        #   少 22mm，**永远到不了孔口**（现象就是"机械臂不下臂"）。
+        #   落在本文件下方注释记录的合理区间 0.204~0.233 内。
+        #   复核：python3 scripts/sense_tool_length.py --sn Rizon4-063989
+        #         python3 scripts/lib/real_geometry_check.py --sn Rizon4-063989
+        # 2026-10-12：下臂安全距离改按 Studio 工具 tool10120938 的 TCP +Z。
+        # 上面 0.223 是旧 tool1（夹爪 TCP 0.147 + 规露出 0.076），这把工具不再用。
+        tool_tip_below_flange_m: 0.2245
+    # >>> 仿真那台臂（Gazebo）也登记成 robots[] 里的一台 <<<
+    # 为什么必须登记：应用是按"当前臂"的名字去查 frame / joint / 相机话题的
+    #（`sim_env.sh` 注释里说的"仿真与真机共用同一套 site.yaml"）。仿真的 TF 与关节
+    # 名带 `sim_` 前缀（与真机区分，避免仿真数据冒充真机），所以它需要自己的一台。
+    # 登记之后，**所有阶段（stage2/stage3/装配…）都不改一行代码**就能跑仿真，
+    # 只要 `--arm sim`（`scripts/run_gauge_assembly.sh --sim` 会自动选它）。
+    #
+    # 真机上永远不会选到它：只有显式 `--arm sim` 才会，而没有任何真机脚本会这么写。
+    - sn: "sim"
+      arm: sim
+      label: "仿真台"
+      # 仿真建模的就是左臂，触觉也只有左指（由 sim_tactile_adapter 发）
+      tactile: true
+      camera:
+        # 话题 /sim_d405/... ：与真机的 /d405_left 分开。为什么不用同一个名字：
+        # 同域里两个来源会互相覆盖，"台上这台到底是谁"就说不清了。
+        # 名字里的 d405 与 URDF 里的模型名一致 → 光学系 sim_d405_color_optical_frame
+        # 正好等于 `{camera.name}_color_optical_frame`，所以应用查 TF 不用特判。
+        name: sim_d405
+      # 仿真不需要手眼标定文件：腕部相机在 URDF 里就装在法兰上，
+      # 而真机那份标定文件描述的是"相机相对法兰的实际安装偏差"，
+      # 套到仿真上会把相机搬到一个**错误**的位置（投影全错）。
+      # 留空 → 启动脚本不发静态 TF（见 gauge_assembly.launch.py 的提示）。
+      paths:
+        # 仿真不"标定"，而是把 URDF 里的真值**导出**成同格式的文件：
+        # scripts/gen_sim_handeye.py（run_gauge_assembly.sh --sim 会自动跑）。
+        # 为什么必须给文件而不能留空：visual_servo 的 servo_node 需要
+        # T_flange_cam 矩阵做像素→笛卡尔换算；留空它就直接拒绝启动
+        #（"手眼标定结果缺失"），S04 跟踪起不来。
+        handeye_file: "/home/cc/ros2/handeye/results/sim_eye_in_hand.yaml"
+      # 运动参数：**大部分**与 89 共用（锚点），但**高度类**必须按仿真台的几何改写。
+      #
+      # 为什么不能直接共用：真机是"机械臂装在台面同一高度"，所以"法兰相对基座"
+      # 就等于"法兰相对台面"（hole_hover_z_m=0.23 就是这么来的）。
+      # 仿真里机械臂站在**地面**、台面 0.40m 高、工件上表面 0.412m —— 一切 Z
+      # 都比真机大 0.412。照抄真实值会让伺服把法兰送到 0.28m，也就是**撞进台面**。
+      #
+      # 推导（与 scene.yaml 的 operating 段同源，scripts/check_sim_scene.py 会核对）：
+      #   相机到工件 0.17（像素门槛反推，不能改）
+      #   + 相机比法兰低 0.05（URDF 里 d405_z=0.05）
+      #   + 工件上表面 0.412
+      #   ⇒ 二阶段悬停法兰高 = 0.632
+      #   一阶段跟踪高 = 悬停 + 0.05（通止规离工件）= 0.682
+      # 所以工作盒上界必须 ≥ 0.682 —— 真机那套上界 0.65 不够，S04 一跟踪就会被
+      # "超出工作盒"截断（现象是"越跟踪越偏"，很难想到是配置差 3cm）。
+      #
+      # ⚠️ 上面那套"按相机"的推导**只对了一半**（2026-09-24 修正）：它没算**工具**。
+      #    悬停高度还要满足"工具尖端不碰工件"：
+      #       工件上表面 0.412 + 工具长 0.243（推算的标称值：TCP 0.200 + 规尖 0.043）
+      #       + 间隙 0.002 = **0.657**
+      #    原来的 0.632 会让工具尖端落在工件面**下**（按 0.245 算是 −23mm）—— 悬停
+      #    这一步就把通止规捅进工件里，接着 S05 的横向对准等于拿规尖拨工件（实测把
+      #    工件推走 79mm），而真值孔位按工件**实测**位姿算 ⇒ 孔位跟着工具走、残差
+      #    恒定 11mm 永不收敛（日志看起来像"手眼标定坏了"）。真机同参数同结果：只要
+      #    悬停按相机定、而工具比相机长，就会这样。
+      #    现在三道防线：stage2 启动时按 gauge_fsm.hover_z_guard 用几何兜住；S05 有
+      #    "工件被推走"的看门狗（gauge_fsm.drag_verdict）；S07 按几何自查下探预算。
+      #
+      #    ⚠️ 为什么这里能写 0.657 这种"贴着"的值：悬停高每抬高 1mm，S07 就多下
+      #    1mm，而 contact_max_depth_mm=35 是按"悬停只留 2mm 间隙"配的
+      #    （30 台肩 + 5 余量）。抬到 0.67 会让下探变成 43mm，S07 就得**自动改大**
+      #    预算并告警 —— 能跑，但配置就失去意义了。
+      #
+      #    工具长 0.243 只是**标称**：仿真里上料脚本每次实测并落盘
+      #    （devices/sim/gazebo/config/sim_tool_geometry.yaml），
+      #    run_gauge_assembly.sh --sim 用它当启动参数覆盖这里的值。真机由现场把
+      #    夹好规后读到的 TF 值写进全局段的 tool_tip_below_flange_m。
+      servo:
+        <<: *servo_left
+        # 仿真 FT：由 sim_wrist_ft 发布，与真机 /<sn>/ft_sensor_wrench 同名同类型同单位。
+        # 必须显式覆盖 —— 上面 *servo_left 带下来的 ft_wrench_topic 是**真机**的
+        # external_wrench_in_tcp，仿真里没有这个话题（不覆盖 ⇒ 仿真 FT 收不到）。
+        ft_wrench_topic: "/sim/ft_sensor_wrench"
+        ee_z_m: 0.72              # 一阶段跟踪锁高（法兰，世界系）
+                                  # 不必随悬停一起抬：跟踪时工具尖端离面 ~60mm，
+                                  # 只要**高于悬停**就不碰工件（0.72 > 0.657）。
+        hole_hover_z_m: 0.657     # 二阶段悬停高：**由工具几何定**，不是相机视距
+                                  # = 0.412（工件上表面）+ 0.243（工具）+ 0.002（间隙）
+        safe_z_m: 0.76            # 安全抬升/复位高度（高于跟踪高）
+        z_ref_m: 0.657            # 没有测量时的兜底 Z：绝不能是 0.22（会撞工件）
+        z_min_m: 0.55              # 伺服硬下限：0.55 = 工件上表面 + 0.14
+        min_ee_z_m: 0.55
+        workspace_xyz_min: [0.08, -0.55, 0.55]
+        workspace_xyz_max: [0.85, 0.55, 0.76]
+      # 仿真不跑推理：感知结果由 sim_truth_perception 按已知位姿直接给出。
+      # 原因：ros_gz_sim 环境里没有 torch/ultralytics，且本机离线装不上 ——
+      # perception_node 一启动就死，表现为"一阶段永远不锁定"。
+      # 详见 devices/sim/gazebo/README.md「仿真里哪些模块用真值替代」。
+      perception:
+        source: "truth"
+      # 几何类参数按台子改写（gauge_assembly 段的下沉覆盖）。
+      # 为什么需要：全局 gauge_assembly 里的值是**真机**的 —— 真机上机械臂
+      # 基座与台面同高，所以"工件上表面 z"就是板厚 0.012；仿真里机械臂站在
+      # **地面**、台面 0.40m 高、工件上表面 0.412。写死全局值会让 S07 的
+      # "悬停时工具尖端离面多高"自检算错（差 0.4m），现象是"一路下压判不到
+      # 接触"，看起来像力阈值标歪了 —— 于是排查从阈值开始走偏。
+      gauge_assembly:
+        workpiece_surface_z_m: 0.412
+        # 工具长这里写**推算的标称值**（TCP 在法兰下方 0.200 + 规尖在指腹面下方
+        # 0.043 = 0.243）。真值由上料脚本实测并作为启动参数覆盖：仿真里合爪后规会
+        # 在指间下滑到落座位置，下滑量随装夹变化（实测见过 243 / 254mm）。
+        # 写成固定值会让悬停高度算错（写小 → 规尖插进工件 ⇒ S05 拿规尖拨工件、
+        # 残差恒不收敛），所以**这个值在仿真里只是兜底**，实际用实测值。
+        tool_tip_below_flange_m: 0.243
+    - sn: "Rizon4-063991"
+      arm: right
+      ip: "192.168.2.100"
+      pc_ip: "192.168.2.10"
+      iface: "eth0"
+      label: "右臂91"
+      camera:
+        name: d405_right         # 话题 /d405_right/... 禁止和 89 的 /d405_left 混用
+        serial: "260522274442"   # 右腕新 D405
+        auto_exposure: true
+        ae_limit_us: 0           # 0=不封顶，让固件自己调亮
+        emitter_enabled: false
+        laser_power: 0
+      gripper:
+        model: "RGI-100-30-O-S-M1"   # 旋转爪：run_gripper.sh --arm 91
+        sn: ""
+        port: "/dev/ttyUSB1"
+        stroke_mm: 30.0
+        auto_init: false
+      named_poses_file: "/home/cc/ros2/src/arm_control/config/named_poses_91.yaml"
+      paths:
+        handeye_file: "/home/cc/ros2/handeye/results/91_eye_in_hand.yaml"
+        handeye_waypoints: "/home/cc/ros2/handeye/poses/91_eye_in_hand.yaml"
+        handeye_workdir: "/home/cc/ros2/handeye/tmp/91"
+      # >>> 右臂跟踪高度：只改这里，89 仍用上面那段 <<<
+      # 法兰 Z；RGI 比平行爪高，通常要比 89 略抬一点。改完重启：
+      #   bash ~/ros2/scripts/run_visual_servo_rt.sh --arm 91 --control
+      servo:
+        ee_z_m: 0.25           # 跟踪锁高：法兰相对基座 Z [m]  ← 日常调这一行
+        min_ee_z_m: 0.20
+        safe_z_m: 0.45
+        z_min_m: 0.22
+        xy_track: hover        # 用法兰系手眼 T：夹爪中心(法兰原点)压在工件正上方
+        ee_vertical_down: true # 法兰/RGI 轴线朝下；相机有 29° 倾角，绿框不必对准十字
+        hover_offset_ee_xy_m: [0.0, 0.0]  # RGI 与法兰同轴；爪心偏了再改这两项 [m]
+        # 二阶段孔定位悬停锁高（91 装了传感器后同样可用）
+        hole_hover_z_m: 0.22
+        # FT 数据源同 89：真机 ft_sensor_wrench 恒零，改用有数的 external_wrench_in_tcp
+        # （见 89 段 ft_wrench_topic 的完整说明）。
+        ft_wrench_topic: "/Rizon4_063991/external_wrench_in_tcp"
+        # --control：先只降到 ee_z_m（XY 不动），到位后才 hover，用手眼把爪心压到工件正上方
+
+paths:
+  # >>> YOLO 权重（标准放法，不必改代码）<<<
+  #   1) 把 .onnx / .pt 放到 /home/cc/ros2/models/yolo/
+  #   2) 这一行改成那个绝对路径（写 .onnx 即可；TensorRT 会找同名 .engine）
+  #   3) backend=tensorrt 时必须先导出引擎（只需做一次）：
+  #        bash ~/ros2/scripts/weights.sh convert <源文件> --name <名字> --group yolo
+  #   4) yolo.imgsz 必须等于 ONNX 输入边长（当前 640）
+  #   5) 改权重/imgsz 后重启：bash ~/ros2/scripts/run_visual_servo_rt.sh [--control]
+  #   6) 换完确认标签没丢：bash ~/ros2/scripts/weights.sh doctor --check-engines
+  # 当前：gongjian0929（2026-09-29 从 ~/下载 转入）。
+  # ⚠️ 这是个 **pose 模型**（YOLO26s-pose）：一个工件框 + 3 个关键点
+  #    k0=跟踪孔 / k1,k2=对孔。它同时替代了原来的跟踪模型和入孔模型，
+  #    所以下面 hole_weights 也指向同一份。
+  #    ultralytics 解不了 pose 的 TensorRT engine，运行时走
+  #    arm_application/pose_detector.py 的自解码（见该模块头）。
+  #    仍然用 .engine（实测 ~2.6 ms/帧；onnx 只有 CPU provider，约 27 ms）。
+  yolo_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
+  # 换模型：把 models/yolo/ 下别的 .onnx / .pt 绝对路径填进来即可。
+  # 未在用的模型已移出仓库（体积大且干扰 `weights.sh list`），要恢复见 models/README.md。
+  # 入孔模型（backend=yolo 时把孔当框）。two_stage 用下面的工件权重。
+  # pose 模型两处共用同一份 —— 它既能框工件，又直接给 k1/k2 关键点。
+  # 查悬空引用：bash ~/ros2/scripts/weights.sh doctor
+  hole_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
+  # 下面是缺省（未匹配到 robots[] 时）。真机以 robots[].paths 为准：89 / 91 各一份。
+  handeye_file: "/home/cc/ros2/handeye/results/89_eye_in_hand.yaml"
+  handeye_waypoints: "/home/cc/ros2/handeye/poses/89_eye_in_hand.yaml"
+  handeye_calibrate: "/home/cc/ros2/handeye/calibrate.yaml"
+  handeye_workdir: "/home/cc/ros2/handeye/tmp/89"
+  # 空=默认 DDS(SHM)；勿强制 UDP-only，否则相机预览收不到 /workpiece/detection
+  fastdds_profile: ""
+
+handeye:
+  mode: "eye_in_hand"
+  # >>> 标定板尺寸：**按模式各一份**（内角点数 + 方格边长 m）<<<
+  #
+  # 为什么必须按模式分开写（而不是两个模式共用一组键）：
+  # 眼在手上与眼在手外通常用**两块不同的物理板**，方格边长不一样。共用一组键
+  # 的后果不是报错，而是"跑另一种模式时静默用错方格边长"—— 标定照样跑完、
+  # 结果只是系统性偏，现场几乎联想不到板尺寸（历史反例见 handeye/README.md 第 3 节）。
+  # 内角点 = 方格数 − 1；列/行顺序不用纠结，检测端两种顺序都会试
+  #（见 handeye_lib.detect_chessboard）。
+  boards:
+    eye_in_hand:            # A5 板：12×9 方格 → 内角点 11×8
+      cols: 11              # 内角点列数
+      rows: 8               # 内角点行数
+      square_size_m: 0.015  # 方格边长 [m]
+    eye_to_hand:            # A4 板：12×9 方格
+      cols: 11
+      rows: 8
+      square_size_m: 0.020
+  # 运动更顺滑：慢速 + 到位等静止 + 零速 pin 后再拍照
+  velocity_scale: 0.05
+  move_time_sec: 8.0
+  settle_sec: 1.5
+  settle_qd_max: 0.015     # |qd|_max 低于此值才算静
+  settle_stable_sec: 0.6
+  settle_timeout_sec: 8.0
+  pin_holds: 2
+  pin_hold_sec: 0.45
+  skip_unstable_poses: true   # 某点看不到棋盘则跳过，不整段中止
+  # 点位：paths.handeye_waypoints
+
+camera:
+  name: "d405"
+  serial: "352122274408"  # 全局缺省=左腕；真机以 robots[].camera.serial 为准
+  # USB3 推荐 848x480@30；USB2 自动降到 640x480@15，深度/右目仍开
+  width: 848
+  height: 480              # 改宽高/帧率必须重启相机：bash ~/ros2/scripts/run_camera_d405.sh --preview
+  fps: 30
+  enable_depth: true
+  enable_infra: true      # 同一 SDK 进程开左右红外；失败则自动退回彩图+深度
+  preview: true
+  preview_stereo: true    # 预览同时显示 彩图 | 深度 | 左目 | 右目（不另开相机、不抢 USB）
+  overlay_hold_sec: 0.10  # 预览只留很短；长残留会看起来像绿框不跟
+  use_site_resolution: true
+  auto_exposure: true
+  ae_limit_us: 4000       # D405 腕部近距；91 在 robots[] 里更严
+  emitter_enabled: false  # 腕部关红外投射，否则彩图发白
+  laser_power: 0
+
+# 现场三台 RealSense（按序列号钉死，禁止按「第一台 D405」乱开）
+cameras:
+  d405_left:
+    model: d405
+    name: d405_left
+    serial: "352122274408"
+    role: left_wrist
+    arm: left
+    label: 左腕D405（89 平行夹爪）
+    width: 848                 # 相机分辨率以本段为准（全局 camera: 段为旧路径，P2 收敛）
+    height: 480
+    emitter_enabled: false     # 腕部必须关投射器，红外点阵会污染跟踪图像
+  d405_right:
+    model: d405
+    name: d405_right
+    serial: "260522274442"
+    role: right_wrist
+    arm: right
+    label: 右腕D405（91 旋转夹爪）
+    width: 848
+    height: 480
+    emitter_enabled: false
+  # >>> 仿真那两台（arm: sim）<<<
+  # 为什么仿真也要在这里登记：应用是"按当前臂"去认相机的 —— 腕部相机取
+  # robots[].camera.name，手外相机取这里 role=overhead* 且 arm 与当前臂一致的
+  # 那一条（见 gauge_assembly_node._d435_ns）。名字必须与
+  # devices/sim/gazebo/config/cameras.yaml 的 ros_ns 一模一样。
+  # 名字不带 d405_left/d435 那样的真机名：同域里两个来源会互相覆盖，
+  # 而 eye-to-hand 标定文件是按相机名命名的 —— 重名会让"跑一次仿真把真机标定盖掉"。
+  sim_d405:
+    model: d405
+    name: sim_d405
+    role: sim_wrist
+    arm: sim
+    label: 仿真腕部D405（= 左臂位置）
+    width: 848
+    height: 480
+    emitter_enabled: false
+  sim_d435:
+    model: d435
+    name: sim_d435
+    role: overhead_eye_to_hand
+    arm: sim
+    label: 仿真手外D435（世界固定）
+    width: 640
+    height: 480
+    fps: 30
+    emitter_enabled: true
+
+  d435:
+    model: d435
+    serial: "262522073161"
+    role: overhead_eye_to_hand
+    label: 双臂中间手外D435
+    name: d435
+    width: 1280
+    height: 720
+    fps: 30
+    auto_exposure: true
+    ae_limit_us: 8000
+    emitter_enabled: true   # 顶视深度可用投射器；只看彩图可改 false
+
+
+yolo:
+  conf: 0.10               # 检测器门槛：略低于 min_conf，让运动模糊框也能进跟踪
+  iou: 0.45
+  imgsz: 640                 # 必须等于所用 ONNX 的输入边长（当前 detect_best_0 = 640）
+  device: "0"
+  half_precision: true
+  backend: "tensorrt"        # 强制 .engine；没有则启动失败。导出：bash ~/ros2/scripts/export_yolo_trt.sh
+  mp_infer: true             # YOLO 独立进程，不和 ROS 抢 GIL
+  min_conf: 0.12             # 新锁门槛；已锁定的框可略低于此
+  # 重新捕获门槛：**跳到新位置**（离上一帧框中心 >max_track_px）必须达到的置信度。
+  # 为什么必须比 min_conf 高得多 —— 2026-10-08 真机实测（一阶段 servo_node 日志）：
+  #   0659.551  死区保持 |e_xy|=3.0mm              ← 已经收敛到 3mm
+  #   0659.598  k0 置信度 0.00 < 0.30，本帧不用它
+  #   0659.6xx  重新捕获 conf=0.15 dpx=503         ← 503px 外、conf 0.15 的假框
+  #             feat_base=(0.231,-0.484) z=-0.021  ← 跟踪点瞬间横移 427mm
+  #   0670.985  === 一阶段已锁定 ===               ← "锁定"在离工件 500mm 的位置
+  # 那串假框实测 conf 0.13~0.47，而真 k0 是 0.87。旧写法把这里接成 min_conf=0.12
+  # ⇒ 0.15 的假框被原样采信、perception 的 kf.reset() 直接跳过去，伺服再拦也只是
+  # 拦一帧（假框帧间稳定，3 帧后 FeatureJumpGate 也会认）。
+  # 取 0.55：高于假框上沿 0.47，低于真 k0 0.87。宁可原地不动，也不追假点。
+  reacquire_conf: 0.55
+  vision_hz: 30.0
+  infer_max_width: 640       # 进 GPU 进程前缩到 640，减小进程间拷图，避免队列堵
+  box_ema_alpha: 1.00          # 绿框用预测补偿后的框，不再 EMA
+  miss_clear_frames: 10        # ~0.33 s @30 Hz；期间绿框按像素速度滑行
+  overlay_hold_on_miss: true   # 滑行失败时仍续发上一框
+  hold_point_on_miss: false    # 眼在手上：丢检续发旧点会让臂追错位置
+  miss_hold_point_frames: 1
+  jump_px: 400.0
+  jump_z_m: 0.10
+  z_ref_m: 0.22
+  min_depth_m: 0.04
+  max_depth_m: 0.55
+  # 特征点的**相机量程下限**：比这更近的深度读数一定来自机械臂自身（爪上夹的
+  # 通止规 / 相机座），不是工件。**判据是实测的两条带，不是几何推导**：
+  #   机械臂自身（随臂一起动）：0.078 ~ 0.087 m   ← 2026-10-08 被误锁的假目标
+  #   工件面：                0.19  ~ 0.29  m     ← 锁高下正常跟踪时的 p_cam.z
+  # 取 0.14：距近带 53mm、距远带 50mm，两边都不顶边界。
+  # 几何上为什么近带被占满：规尖在法兰下 0.223m 而相机只在法兰下 0.124m（handeye
+  # cam_in_flange z=0.1236）⇒ 规杆整段都在相机近场；且相机光轴与法兰 Z 差 29.7°，
+  # 所以"光轴视距"不等于"竖直距离"，只能靠实测划带。
+  # 撞了会怎样：假目标在相机前 0.08m 且**随臂一起动**，伺服误差恒为 0 ——
+  # "一阶段已锁定"会锁在自己的工具上（见上面 reacquire_conf 的记录）。
+  feature_min_range_m: 0.14
+  work_z_max_m: 0.50
+  meas_blend: 0.85             # 3D 跟手；抖则略降
+  z_alpha_min: 0.10
+  z_alpha_max: 0.40
+  z_alpha_gain: 1.0
+  camera_frame: "d405_color_optical_frame"
+  object_frame: "tracked_object"
+  depth_topic: "/d405/aligned_depth_to_color/image_raw"
+  feature_mode: "keypoint"  # keypoint=pose 模型的 k0（跟踪孔）；plane=框内近表面立体点；box=单目射线
+  # feature_mode=keypoint 时用：跟踪用哪个关键点、它的置信度低于多少就不信它。
+  # k0 = 跟踪孔（见 pose_detector 的约定）。k0 不可用时**借 k0 相对检测框的归一化
+  # 偏移**在框上重建 k0 —— 位置连续、工件动时还跟着走。
+  # 刻意**不**回落"框中心"（那是另一个点，离 k0 有 ~89mm，真机上就是一次不该有的
+  # 横向快移，见 2026-10-08 一阶段 150s 没锁上的记录）；只有在**从未**见过可信 k0
+  # 的冷启动帧才回落到框中心/框内深度。
+  keypoint_index: 0
+  keypoint_min_conf: 0.30
+  keypoint_win_px: 6        # k0 处取多大窗口的中位深度
+  # ---- 跟踪点可信性的三道守卫（实现：feature_guard.py，单一数据源）----
+  # 为什么需要：2026-10-08 真机 arm 89，一阶段真 k0（conf 0.87 uv=(410,246)）被一串
+  # **高置信度假 k0** 顶掉，伺服 1s 内把臂从 home 带到工作盒角落 (0.083,-0.515)，
+  # 之后 148s 全是"检测漏检"、150s 超时。假 k0 的 conf 是 0.53~0.73（真值 0.87）
+  # ⇒ 置信度没有可分区间；但它们**贴在画面边上**、且**借深度**：
+  #   766.96 uv=(76,75)  768.16 uv=(143,92)  768.78 uv=(12,30)  770.07 uv=(431,226)
+  keypoint_border_px: 6     # k0 离画面边缘 < 此值即判位置不可信（孔心不在画面边上）
+  track_max_step_m: 0.04    # 已发布跟踪点的每帧位移上限 [m]（实测真实运动 ≤15mm/帧）
+  reacquire_far_px: 120     # 远于此像素距离的"重新捕获"要连续确认（dpx 那个门限）
+  reacquire_confirm_frames: 3  # 远框要连续几帧指向同一处才采信
+  plane_inlier_m: 0.008    # 拟合平面内点阈值 [m]
+  plane_band_m: 0.06       # 近表面深度带宽 [m]
+
+servo:
+  control_hz: 100.0
+  # --- 基座坐标系下的末端约束（法兰，单位 m）---
+  ee_x_m: 0.25
+  ee_y_m: 0.00
+  ee_z_m: 0.35             # 全局缺省锁高；真机以 robots[].servo.ee_z_m 为准（89/91 分开调）
+  ee_hold_xy: false          # false=跟踪工件 XY；true=锁死 ee_x/ee_y
+  xy_track: "hover"          # hover=法兰悬停在目标正上方（任意锁高）  lookat=画面中心对准目标
+  use_static_handeye_tf: false  # 只用 handeye_result.yaml，避免 TF 树抢父子帧
+  ee_vertical_down: true
+  # --- 启动回初始位（已停用：--control 不再调用 goto_ee_plane）---
+  safe_z_m: 0.45               # >>> 防撞安全高度：先纯 Z 抬到这里再回 home，避免扫到料框/壁
+  home_pose_name: "home"       # 非夕初始点位（/set_named_pose：home | ready | inspect | pos_left）
+  home_pose_speed: 0.30        # 回初始位的速度比例 (0,1]，越小越稳
+  ee_hold_kp: 2.0
+  ee_w_max: 0.60
+  z_ref_m: 0.22
+  z_min_m: 0.22
+  approach_xy_gate_m: 0.040  # --control 先降到锁高，|ee_z-ee_z_m| 小于此值才开始 XY hover
+  v_approach_z: 0.025
+  watchdog_timeout_sec: 1.2
+  lost_timeout_sec: 1.2
+  stale_hold_sec: 0.35             # 丢检空窗内用基座 KF 预测继续跟（自运动安全），超过才钉住
+  max_extrapolate_sec: 0.35   # 预测时间上限（图像延迟 + 前瞻）
+  predict_enable: true        # 基座系恒速预测，补视觉延迟
+  predict_sec: 0.16           # 额外前瞻 [s]
+  predict_v_max: 0.80         # 工件速度上限 [m/s]，防乱飞
+  predict_jump_m: 0.08        # 基座 XY 跳变大于此则视为"疑似假跳"，转入确认流程
+  # 大跳变要连续多少帧指向同一新位置才采信（见 feature_jump_guard.py 的注释）。
+  # 30Hz 下 3 帧 ≈ 0.1s：真的重定位 0.1s 就被认；假点（乱跳）永远认不了。
+  # 为什么不能是 1：2026-10-08 真机，一阶段已收敛到 3mm 后 k0 掉信、跟踪点瞬间
+  # 横移 89mm，旧逻辑单帧就 reset 采信 → 伺服以 0.45m/s 去追假点 → 彻底丢失。
+  jump_confirm_frames: 3
+  # 特征点位置合理性：必须落在工作盒 XY 内（±此余量 [m]），否则判误检、当"没有目标"。
+  # 为什么需要它：2026-10-08 真机，模型在运动模糊下把特征点丢到基座后方
+  # (-0.10,-0.65)（工件其实在 (0.5,-0.12)），伺服一路追到工作盒边缘卡住。
+  feat_ws_margin_m: 0.05
+  kp_xy: 4.20                # 很跟手：v = kp × 误差，再经加速度整形；抖则降回 3.5
+  ki_xy: 0.00
+  kd_xy: 0.20
+  kp_z: 0.80
+  ki_z: 0.00
+  kd_z: 0.35
+  i_sep_pos_m: 0.008
+  deadzone_m: 0.003          # 3 mm；收紧提高对准精度。若抖动调回 0.006
+  deadzone_z_m: 0.012
+  lock_hold_sec: 0.7        # 旧死区驻留（混合模式不再用它进二阶段）
+  # D435 到位之后，法兰 XY 连续这么久位移不超过 lock_still_move_m 才算停稳、交回 D405。
+  # 顶视一次性把臂迁过去的那段路不算：必须是腕部有效目标。大幅一动就重计时。
+  lock_still_sec: 3.0
+  # D405 改追工件框中心之后，连续这么久才进对孔。不要和上面的停稳写成同一个数。
+  wrist_align_sec: 1.0
+  lock_still_move_m: 0.015   # 15 mm；超过就重计时，避免"晃一下刚好满 3 秒"
+  e_xy_clip_m: 0.200         # 大误差仍按 v_max 走，靠 a_max 加速
+  v_max_xy: 0.45             # 法兰 XY 上限 450 mm/s
+  v_max_z: 0.04
+  # 远处限速：目标离得比 slow_far_m 远时 XY 限速到 v_slow_xy（**与"目标真不真"无关**
+  # 的一道防线）。为什么需要：2026-10-08 真机一阶段被一串高置信度假 k0（conf 0.53~0.73）
+  # 顶掉，伺服 0.7s 内以 0.45m/s 把臂从 home (0.686,-0.113) 带到工作盒角落
+  # (0.083,-0.515)，之后 148s 全是"检测漏检"、150s 超时。加了这道之后：真 k0（离
+  # 145mm）只是晚 ~1.8s 到位，假目标最多把臂带走几厘米。
+  slow_far_m: 0.05           # 目标离得超过此距离即限速 [m]
+  v_slow_xy: 0.08            # 限速值 [m/s]（真目标到位后仍回到 v_max_xy）
+  min_ee_z_m: 0.20
+  workspace_xyz_min: [0.08, -0.55, 0.18]   # 法兰工作盒；盒外只允许往回走，不再卡死 no traj
+  workspace_xyz_max: [0.85, 0.55, 0.65]
+  a_max: 3.00                # 加速度上限（真正作用在 hover 指令上）
+  vel_lpf_alpha: 0.80        # 速度低通：越大越跟手（1=不滤波）
+  dq_window_size: 1          # 1=不做滑窗平均，去掉 ~20ms 滞后
+  traj_stream_hz: 50.0       # 轨迹点流率：JTC open_loop+样条，吃得起
+  exec_horizon_sec: 0.08     # 每个轨迹点的执行窗口
+  max_qdot: 2.00             # 关节速度上限 [rad/s]
+  min_cmd_delta_rad: 0.001
+  point_topic: "visual_servo/filtered_point"
+  traj_topic: "rizon_arm_controller/joint_trajectory"
+
+# 大寰夹爪：官方 Modbus RTU，USB–RS485 插 IPC，不走 Studio/RDK。
+# 左 89 PGEA → /dev/ttyUSB0；右 91 RGI → /dev/ttyUSB1。
+#   bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open|close|pos
+#   bash ~/ros2/scripts/run_gripper.sh --arm 91 --cli open|close|rotate 90
+#   bash ~/ros2/scripts/run_dual_gripper.sh
+# ROS：/gripper/open|close（PGEA 单爪）或 /dual_gripper_driver/{left,right}/execute
+gripper:
+  backend: "usb"              # usb=工控机官方Modbus  rdk=Studio设备（不用）
+  vendor: "dh"
+  model: "PGEA-100-20-W"
+  sn: "26610799"              # 铭牌，不进协议
+  device_name: "GripperDahuanModbus"
+  port: "/dev/ttyUSB0"        # 本机 MACROSILICON/pl2303
+  baud: 115200                # 官方默认 115200 8N1
+  slave_id: 1                 # 官方默认从站 1
+  rs485_rts: ""               # 空=不拉RTS；scan 若 OK 在 tx/tx_inv 再填
+  stroke_mm: 20.0
+  # --- 开合 / 力 / 速（大寰官方寄存器，超出会被驱动钳位）---
+  force_pct: 20               # 夹持力  区间 20–100 %（低于 20 会被钳到 20；PGEA-100 约 30–100 N）
+  speed_pct: 50               # 开合速度 区间 1–100 %
+  open_position: 1000         # 张开目标 区间 0–1000 ‰（1000=全开）
+  close_position: 0           # 闭合目标 区间 0–1000 ‰（0=全合；留缝改 50–200）
+  auto_init: true
+  state_hz: 20.0
+  # 双爪共用参数（run_dual_gripper.sh / load_from_site）
+  parallel:
+    stroke_min: 0.0
+    stroke_max: 20.0
+    default_speed: 50
+    default_force: 20
+    force_min: 20
+    force_max: 100
+    auto_initialize: true
+  rotary:
+    auto_initialize: false
+    angle_min: -32767.0
+    angle_max: 32767.0
+    max_step_deg: 30000.0
+    finger_open_per_mille: 1000
+    finger_close_per_mille: 0
+    default_speed: 50
+    default_torque: 50
+    default_finger_speed: 50
+    default_finger_force: 50
+    torque_min: 20
+    torque_max: 100
+  # RDK 回退字段（backend: rdk 才用）
+  force_n: 50.0
+  vel_mps: 0.05
+
+# 入孔检测。算法参数写在这里（覆盖包内 hole_detect.yaml 默认值）。
+#   bash ~/ros2/scripts/run_hole_detect.sh --arm 89 --preview
+hole_detect:
+  backend: "two_stage"   # two_stage | yolo | hough
+  # backend=yolo 时用。
+  weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
+  # 二阶段工件检测权重。pose 模型既框工件、又直接给 k1/k2 关键点，
+  # 所以和上面的跟踪权重是同一份。
+  workpiece_weights: "/home/cc/ros2/models/yolo/gongjian0929.onnx"
+  # pose 关键点优先：检测器给了 k1/k2 就直接用，不用再找圆。
+  # 关键点不可信（低于 keypoint_min_conf / 两点几乎重合）时自动回落到
+  # 亮环 + Hough 那套老路径。
+  keypoint_enable: true
+  keypoint_min_conf: 0.30
+  keypoint_min_dist_px: 15.0
+  keypoint_hole_radius_px: 12.0   # 关键点没有半径；反投影时按这个半径取孔周深度环
+  config: "/home/cc/ros2/src/arm_application/config/hole_detect.yaml"
+  conf: 0.25
+  iou: 0.7
+  imgsz: 640
+  center_gate_enable: true
+  center_max_offset_ratio_x: 0.15
+  center_max_offset_ratio_y: 0.18
+  center_gate_draw: true
+  refine_hole_centers: true
+  bbox_padding: 80
+  gaussian_ksize: 5
+  gaussian_sigma: 1.5
+  hough_dp: 1.2
+  hough_min_dist: 50
+  hough_param1: 100
+  hough_param2: 26
+  hough_min_radius: 6      # 悬停较高时螺纹孔半径只有几像素
+  hough_max_radius: 30
+  min_radius: 6
+  max_radius: 30
+  hole_distance_min: 90    # 悬停 0.23m 两螺纹孔约 170px；过小会配到中心孔
+  hole_distance_max: 700
+  expected_holes: 2
+  min_hole_sep_px: 20
+  max_hole_sep_px: 2000
+  stabilize_hold_frames: 15
+  stabilize_ema_alpha: 0.35
+  stabilize_max_jump: 60
+  default_depth_m: 0.10
+  camera_intrinsics:
+    fx: 425.0
+    fy: 425.0
+    cx: 640.0
+    cy: 360.0
+  # ===== 二阶段一次性拍照定位（stage2_locate_node，不做实时推理）=====
+  # bash ~/ros2/scripts/run_stage2_locate.sh --arm 89            # 完整流程
+  # bash ~/ros2/scripts/run_stage2_locate.sh --arm 89 --hover-only  # 只悬停调高度
+  stage2:
+    # 对准目标孔。pose 模型下 k1/k2 是**模型学出来的固定语义**（不是图像左右）：
+    #   hole_points[0] = 模型的 k1，hole_points[1] = 模型的 k2
+    # 下游是 `k1_b = pts[0] if k1_label=="left" else pts[1]`，所以：
+    #   k1_label: "left"  → target_hole=k1 打的是**模型的 k1**（当前设置，推荐）
+    #   k1_label: "right" → target_hole=k1 打的是模型的 k2（等价于老 Hough 的"图像左孔"）
+    # 实测这台臂的视角下模型给的是 k1 在画面右、k2 在画面左；对孔顺序要反过来就改这里。
+    target_hole: "k1"
+    k1_label: "left"
+    # ---- 对准判据：工具**用视觉量到的规尖**，不是法兰轴线（2026-10-08 真机定稿）----
+    # 实测（同一帧、同一条手眼链，n=8）：
+    #   · 规杆轴线相对法兰轴线 = (+10.7, −0.1)mm（σ≈0.5mm）；最深像素法 (+8.8,−1.8)mm
+    #   · 而 site 原来把 hover_offset_ee_xy_m 写成 [0,0]（"平行爪同轴"），于是精对准
+    #     把**法兰**对到孔上，规尖实际离孔心 ~10mm ⇒ 轻降时 φ6 规尖骑在 φ7 孔沿上，
+    #     腕力从 −1N 直接蹦到 −31N→−63N 卡死（现象是"轻降在离面 4mm 停住"）。
+    #   · 孔检测本身的重复性极好：k1 基座坐标 σ=(0.17,0.04)mm —— 精度不是瓶颈。
+    # 所以：在同一帧里同时解「规尖基座 XY」和「孔基座 XY」，两者一起走手眼链，
+    # 链路误差与装夹变动一起抵消。规尖量取规则见 _tip_offset_from_depth（四条判据）。
+    align_tol_m: 0.00025     # 对准到位判据 [m]。检测重复性 0.04mm 撑得住 0.25mm。
+                             # ⚠️ 不要回到 0.7mm：那会让"规偏 10mm"这种系统偏差
+                             #    在容差里被放过去（原来残差 0.69mm 看着"通过"，其实
+                             #    是拿法兰对的孔）。收到 0.25mm 才逼它必须真的对上规尖。
+    align_max_iters: 12      # 拍照→修正→复拍 迭代上限（收紧容差后需要更多拍）
+    align_tip_vision: true   # true=精对准用「视觉规尖」；false=退回旧的「法兰+偏置」
+    tip_near_m: 0.15         # 规尖量取：无工件面深度时的近端上限 [m]；
+                             # 精对准同一帧有 z_wp 时改用「工件面 − 15mm」
+                             # （3mm 挡不住噪声，会把工件面当成规尖）
+    tip_min_m: 0.04          # 有效深度下限 [m]
+    tip_max_m: 0.60          # 有效深度上限 [m]
+    tip_search_band_frac: 0.25  # 中央竖带半宽 = 该比例 × 图宽（规夹在爪里，不会靠边）
+    tip_min_px: 150          # 近端连通域最小像素数（少了就是没看到规）
+    tip_calib_n: 4           # 定偏移需要的一致拍数（够了就冻结）
+    tip_offset_tol_m: 0.003  # 样本相对中位偏移的最大允许偏差 [m]（剔偶发外点 + 冻结判据）
+    tip_len_min_m: 0.15      # 反投影出的「法兰z − 规尖z」必须落在此区间 ——
+    tip_len_max_m: 0.32      # 这条同时挡住"深度读到台面/工件"（主干守卫）
+    coarse_max_moves: 2      # 工件粗居中（bbox 中心）移动上限
+    # —— 粗居中的两条误检守卫（单源：代码读这两个键，见 _coarse_target_reject）——
+    # 为什么需要：2026-10-08 真机 arm 89，粗居中拿被画面左边缘切掉的半框
+    # bbox=(0,142,97,222) 反投影出盒外的 (0.280,-0.747)（偏差 211.5mm），旧逻辑
+    # **不判误检、只把目标截断到盒边**，臂照样横移 56mm 离开工件 ⇒ 精定位再也
+    # 看不到 k1/k2 ⇒ exit=4。判据改成"盒外/偏移过大 ⇒ 判误检、原地不动"。
+    coarse_max_offset_m: 0.12   # 目标离夹爪轴线超过此值就不像粗居中量 [m]
+    coarse_border_px: 2         # bbox 贴边（被画面切）即弃用 [px]；见 _bbox_on_border
+    # —— 柔顺运动（夹爪持规，防急加速/急停/过冲把规震脱）：速度放慢 + 平滑刹车 ——
+    align_speed: 0.014       # 对准段笛卡尔速度 [m/s]（原0.03，放缓防冲）
+    move_speed: 0.022        # 升降/粗居中段速度 [m/s]（原0.05，放缓防震规）
+    # 对小孔关键点：只用像素射线交到 workpiece_surface_z_m，不读深度图。
+    hole_visual_only: true
+    final_gap_mm: 2.0        # 对准后贴面轻降目标：规尖悬在孔面上方这么高 [mm]（不接触）
+    observe_gap_mm: 15.0     # --no-descent 时的观测下降：只降到离面这么高 [mm]，
+                             # 方便看规尖偏没偏。必须 ≥ final_gap_mm。
+                             # 左臂89：悬停间隙约 73.5mm（0.320−0.2245−0.022）⇒ 观测下降约 58mm。
+    final_descent_speed: 0.005  # 最后轻降速度 [m/s]（很慢，柔顺）
+    # ✅ 2026-10-08 定稿：下降量**只由几何决定**（法兰 FK − 工具长 − 工件面 − final_gap），
+    #   深度读数只做交叉核对、不再参与任何决策。两段真机事故方向相反、都指向这条：
+    #   · 2026-09-29（多降→撞工件）：环形深度中值报 gap0=141.0mm，几何真值只有 53mm
+    #     （probe 0.245 版），多降 ~86mm 把通止规撞进孔，工件被 +X 推走 5mm。
+    #     ⇒ 读数往**大**错一次就撞。
+    #   · 2026-10-08（少降→到不了 2mm）：同一个读数悬停时报 144.0mm（几何真值 75mm），
+    #     只降 24mm 后又翻成 −7.0mm，触发 SAFE_FLOOR 把轻降停在离面 ~50mm 处，
+    #     最终日志却打印"悬在孔面上方 ~2.0mm"（其实停在 ~18mm）。
+    #     ⇒ 读数往**小**错一次就永远到不了目标。
+    #   同一套读数两个方向各错一次、幅度 150mm 量级、随位姿乱跳 —— 它对"能降多少"
+    #   和"该不该停"都没有发言权。现在它只在偏差 >10mm 时喊一句"深度测距与几何不符"。
+    # 现行几何：悬停 0.320 − 工具长 0.2245 − 工件面 0.022 = 73.5mm 间隙 ⇒ 降 71.5mm 到 2mm。
+    #   三道**物理/几何**保险：descent_budget_m（配置上限）+ 腕部 FT 接触检测（停+退 1mm）
+    #   + 任一步 _goto 走不到位就停。
+    #   缺几何（工具长/工件面没配）时**拒绝下降**并报错（保持悬停），绝不拿测距去赌。
+    final_descent_enable: true  # true=对准后降到 final_gap_mm；false / --no-descent=只降到 observe_gap_mm
+    verticalize_tol_deg: 0.8 # 垂直朝下判据 [deg]
+    settle_sec: 0.45         # 到位后静止多久再拍照
+    snap_frames: 5           # 每次精拍取中值的帧数
+    min_depth_m: 0.06        # 有效深度范围（防孔底/桌面拉偏 XY）
+    max_depth_m: 0.60
+    linger_sec: 8.0          # 完成后继续发布结果/叠图的时长
+
+  # ===== 三阶段柔顺搜孔（stage3_search_node，Orisys 触觉 + 力闭环螺旋）=====
+  # 前置：二阶段已完成，臂悬停在目标孔上方。先 run_tactile.sh --cli 看力读数量级再调阈值。
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89                # 搜孔+落座
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 0      # 只搜孔定心不插入
+  # bash ~/ros2/scripts/run_stage3_search.sh --arm 89 --depth 6 --pitch 1.0  # 螺纹规旋入
+  stage3:
+    # —— 力阈值全是「相对静止均值的增量」（Orisys 读数是形变任意单位且基线会漂移，
+    #    节点启动时自动测 1.2s 静止均值 μ0/方差 σ0，看日志「触觉基线/派生阈值」两行）——
+    touch_delta: 250.0       # 触面判定：fnormal > μ0 + max(4σ0, 本值)
+    press_delta: 200.0       # 轻触目标按压量（触面阈值之上再加这么多）
+    max_press_delta: 900.0   # 螺旋中允许的最大按压（超过→力闭环抬起）
+    safe_press_delta: 2000.0 # 安全上限：fnormal > μ0+本值 立即抬升中止
+    shear_delta: 600.0       # 切向拖拽保护：shear > 静止切向均值+本值 → 贴面时微抬 0.15mm（Z 钉在触面）
+    insert_press_delta: 1200.0  # 插入力上限（触面阈值之上；超过回退微抖重试）
+    kp_z_mm_per_unit: 0.001  # Z 力闭环增益：每单位读数偏差调整的毫米数
+    approach_step_mm: 0.8    # 触面前每步下降量（驱动死区已修，0.5mm 微步可跟踪；小步=触面冲击小）
+    enter_probe_mm: 2.0      # 触面深于此值→疑似直接入孔，四向微探验证后跳过螺旋
+    mouth_lift_mm: 2.0       # 直接入孔后抬到孔口倒角段再定心（深插横探会别死孔壁）
+    baseline_min: 8.0        # 触觉基线健康窗下限（<此值=几乎全 0，tare/断流）
+    baseline_max: 1500.0     # 上限（>此值=近饱和，接触余量不足）——出窗即中止并提示重启触觉
+    approach_ft_touch_n: 6.0 # approach 腕部 |F| 旁路触面判据（标定牛顿；自由空气≈2-3N）
+    approach_max_mm: 60.0    # 从当前位姿最多下降这么多（找不到面就中止；悬停被抬高后仍有余量）
+    spiral_pitch_mm: 1.0     # 螺旋螺距（< 通止规直径，保证重叠覆盖）
+    spiral_step_mm: 0.5      # 沿螺旋的弧长步长
+    spiral_r_max_mm: 4.0     # 最大搜索半径（覆盖二阶段残差 1.5mm + 余量）
+    spiral_expand_max: 1     # 找不到孔时扩大半径的次数（残差≤1.8mm 时代码不再扩大）
+    spiral_expand_step_mm: 2.0
+    drop_ratio: 0.45         # 入孔判据：fnormal 相对接触均值跌落比例
+    drop_frames: 2           # 跌落需持续的帧数（防毛刺）
+    center_scan: false       # 入孔后十字微扫；默认关（0.8mm 扫会把已入孔的规刮偏）
+    center_scan_mm: 0.3
+    insert_depth_mm: 0.0     # 默认只搜孔+轻压，不拧入；拧入阶段再用 --depth
+    insert_retry: 3
+    screw_pitch_mm: 0.0      # 螺纹通止规螺距：>0 右旋 / <0 左旋 / 0=不旋（光面规）
+    search_speed: 0.03       # 搜孔笛卡尔速度 [m/s]
+    settle_sec: 0.25         # 每个微步后的静止时间
+    tactile_timeout_s: 3.0   # 触觉话题断流多久中止
+    ft_watchdog: true        # 订阅 /<sn>/flexiv_robot_states 做腕部力看门狗
+    ft_safe_n: 35.0          # 腕部 |F| 安全上限（真牛顿，标定过的；连续3拍超限才中止）
+    linger_sec: 8.0          # 完成后继续发布结果的时长
+
+# Orisys 视触觉（夹爪左右指摄像机，不是腕部 D405）。SDK 单独 pip 安装。
+#   bash ~/ros2/scripts/run_tactile.sh --cli
+#   bash ~/ros2/scripts/run_tactile.sh
+# =============================================================================
+# 感知来源：伺服用的"视觉测量"由谁产生
+#
+#   yolo      本体 perception_node（YOLO/ONNX，需要 torch/ultralytics）—— 真机
+#   truth     仿真真值：**整条视觉链都由场景几何算出**，不跑任何推理
+#               · 一阶段目标点：arm_sim_gazebo 的 sim_truth_perception
+#               · 二阶段孔位  ：arm_sim_gazebo 的 truth_holes（投影 <visual>*_ring）
+#   external  由外部节点按**同一话题**（visual_servo/filtered_point）提供，
+#             本框架不起感知进程（二阶段仍走检测器）
+#
+# ⚠️ 别和 yolo.backend 混了：那个是"**怎么**推理"（tensorrt/onnx），
+#    这个是"**谁**给出测量"。
+#
+# 为什么需要这个开关：仿真环境（ros_gz_sim conda）没有 torch/ultralytics，
+# 离线也装不上，perception_node 一起就死；但仿真里工件位姿本来就是已知的，
+# 没必要也没法做推理。切到 truth 后，流程能在仿真里验"控制与接口"那一半，
+# 而**感知那一半留给真机**——这条边界写在 devices/sim/gazebo/README.md 的
+# 「仿真里哪些模块用真值替代」一节。
+# =============================================================================
+perception:
+  source: "yolo"
+  # 一阶段跟踪用哪台相机。wrist=腕部 D405（主跟踪）+ 顶视 D435 一次性兜底；
+  # overhead=整段只用 D435（实验分支才切）。非法值启动即报错，不会静默退回。
+  track_camera: "wrist"
+  # 画面内最高检测 conf 低于此 → 马上拍一张 D435，把工件 / k0 定到基座系。
+  # 与 perception / servo 共用这一处，不许两边各写一个 0.5。
+  wrist_handoff_conf: 0.5
+  # 顶视深度量程 + 一次性参数。腕部那组 0.04–0.55m 会把 D435 的点全丢掉。
+  # 缺键启动即红，不许借腕部量程、不许用代码默认冷却时间。
+  overhead:
+    min_depth_m: 0.25
+    max_depth_m: 1.80
+    feature_min_range_m: 0.30
+    work_z_max_m: 1.50
+    z_ref_m: 0.70
+    feature_mode: "plane"     # 顶视与腕部视角差太大，pose 的 k0 经常看不见；优先 k0，否则框心
+    hold_point_on_miss: true  # 仅 overhead 整段跟踪时用；混合模式一次性不续发
+    oneshot_conf: 0.5         # D435 这一张也必须 conf≥此值，不许拿噪声框当 K0
+    cooldown_s: 1.5           # 两次一次性之间的最短间隔，避免跟 D405 抢同一条 YOLO 队列
+    frame_max_age_s: 0.4      # 彩图/深度比这更旧就拒绝，不许拿过期图反投影
+    oneshot_hold_s: 8.0       # 伺服保持这一次设定的最长时间；超时丢掉，等下一张或 D405
+  truth:
+    # 真值目标物体。留空 = 场景文件里的 operating.part_object（推荐留空：
+    # "哪个是工件"是场景的知识，换场景不该改这里）。
+    object: ""
+    # 目标点相对物体**顶面中心**的偏移 [m]，在物体局部系里（会跟着 yaw 转）。
+    # 例：把目标从板中心挪到 k1 孔 → [0.0, -0.029, 0.0]
+    offset_m: [0.0, 0.0, 0.0]
+# =============================================================================
+# 通止规装配（看板 S00–S11）：bash ~/ros2/scripts/run_gauge_assembly.sh --arm 89
+#
+# 这一段全是**会动的量**，也是现场最常改的地方。改完不必改代码：
+# 解析在 arm_application/gauge_config.py（键名、单位、类型都在一张表里校验），
+# 拼错键名会直接报错并给出最相近的候选名，不会静默退回默认值。
+#
+# 单位体现在键名后缀上（_deg/_mm/_m/_s/_pct/_delta），三张表逐条校验；
+# 带 _delta 的是**力增量**（Orisys 读数是形变任意单位且基线会漂移，
+# 绝对值没有意义），必须现场标定：
+#     bash ~/ros2/scripts/run_tactile.sh --cli     # 看读数量级
+# 启动时节点会打印本段**实际生效**的值（含派生结论），先核对再上电。
+# =============================================================================
+gauge_assembly:
+  # ---- 总开关 ----
+  holes_total: 2             # 一次任务装几个孔（k1/k2）；--holes N 可临时覆盖
+  dry_run: false             # true = 只验编排与自检，不发任何运动指令
+  # E1（掉件）重放时等人工确认 /gauge/reload_ok 的最长时间 [s]。超时=中止。
+  # 不是"再等等"：臂停在 home，让人来处理比继续跑安全。
+  reload_timeout_s: 180.0
+  # S00 自检每项（相机/触觉/夹爪状态）等"第一帧真数据"的最长秒数。
+  # 只看话题名会假阳性（发布者不在也算在），所以必须等真数据。
+  boot_timeout_s: 30.0
+
+  # ---- S01 回 home 后按工具系逆时针预绕到极限角 ----
+  # 预绕是为了给 S08 的棘轮留出回转行程：不预绕，拧两圈就贴关节限位。
+  extreme_angle_deg: 150.0   # 想转多少度（按工具系逆时针）
+  extreme_speed_deg_s: 20.0  # 关节角速度 [deg/s]，慢一点防冲击
+  extreme_direction: 1       # +1=逆时针 / -1=顺时针（改符号，别去改代码）
+  extreme_margin_deg: 3.0    # 距关节限位留的余量 [deg]；撞限位不算失败（按 JOINT_LIMIT 继续）
+                             # 但余量太小会让规划器频繁拒收，表现为"偶尔转不到位"
+
+  # ---- S02 夹持确认（预留，默认跳过）----
+  # 现在的触觉判据与 D435 手眼都还没就绪，先留接口：开成 true 时，
+  # 夹爪状态字必须明确报"夹住了"，否则 S02 判失败并中止。
+  grip_check: false
+  grip_hold_s: 0.5           # 合爪后等多久再读状态 [s]
+
+  # ---- S03 手外粗定位（S04 跟踪的兜底：跟踪丢了才来）----
+  # 前提是手外相机做过 eye-to-hand 标定：handeye/results/<相机>_eye_to_hand.yaml。
+  # 没有那个文件时 S03 会明确报"缺哪个文件、怎么生成"并回 S04，不做假装成功的反投影。
+  coarse_enabled: true
+  # 手外相机名：**以 cameras 段里 role=overhead* 且 arm 与本臂一致的那条为准**
+  #（就是 cameras 注册表，见 gauge_assembly_node._d435_ns）。这一项只在
+  # "注册表里没有手外相机"时当兜底；两者不一致会在 S00 打告警并以注册表为准。
+  # 所以真机/仿真各跑各的时候**不用**改这里 —— 曾经写死 d435 让仿真去查真机的相机。
+  coarse_camera: "d435"
+  coarse_hover_m: 0.25       # 粗定位后悬停在工件上方多高 [m]（可调）
+  coarse_conf: 0.35          # 工件检测置信度阈值（0~1）
+  coarse_timeout_s: 20.0     # 等手外相机出图的最长秒数（相机刚起来要一两秒）
+
+  # ---- S05 精拍定位：等画面稳了再拍 ----
+  # 判据是"所有采样点到均值的距离 ≤ 容差"，不是"相邻两帧差 ≤ 容差"：
+  # 跟踪抖动是零均值的，相邻帧之差会在画面明明在晃时判成稳定，拍出糊图。
+  snap_stable_s: 1.0         # 连续观察多久 [s]
+  snap_tol_px: 2.0           # 允许的抖动半径 [px]（越小越严，越难满）
+  snap_tol_mm: 0.4           # 同上，但特征=法兰位移时的容差 [mm]
+  snap_min_frames: 6         # 至少采到多少帧才敢说"稳定"
+  # 用哪个特征判"画面稳定了"：
+  #   auto   = 按 perception.source 自动选（真机 yolo→bbox；仿真真值/外部→motion）
+  #   bbox   = 工件检测框中心（像素）。需要检测器真的出框：仿真里合成图过不了
+  #            真机训练出来的模型（实测最高分 0.017），选它会让 S05 空转到超时
+  #   motion = 法兰位移（mm）。相机装在法兰上，法兰不动画面就不动 —— 不依赖模型
+  snap_stable_feature: "auto"
+
+  # ---- S06 正悬停（悬停高度不在这里：见 servo.hole_hover_z_m）----
+  align_tol_mm: 1.0          # 对位残差容差 [mm]；超了回 S05 重拍（不是硬往下走）
+                             # 孔口径 mm 级时别放大这个值：放大=直接蹭孔壁
+
+  # ---- S07 柔顺接触：步进下压，切向力突变即停 ----
+  contact_speed_m_s: 0.02    # 每步下压速度 [m/s]；这一步必须慢，快了就是一次冲击
+  contact_step_mm: 0.5       # 每步下压量 [mm]（越大越快，越小越不会过冲）
+  contact_max_depth_mm: 35.0 # 最多下探多少 [mm]；到了还没接触判失败（不会一直压）
+                             # 为什么是 35 而不是 25：判"到底了"的是**台肩落到工件
+                             # 上表面**那一下，而台肩在规尖上方 30mm（通端长 30，
+                             # 见 models/gauge/model.sdf）。工件板厚 12mm，所以要
+                             # 下压 30mm 台肩才落面 —— 25mm 的上限根本到不了，
+                             # 只会报"下探 25mm 都没触发接触判据"然后 ABORT。
+                             # 35 = 30（台肩落面）+ 5（对位残差/工件厚度公差余量）。
+                             # 注意：这个值**不必**手工算 —— S07 启动时会按下面两个
+                             # 工具几何量算出"台肩落面还需要多少"，不够就地改大并告警。
+
+  # ---- 工具几何（S07 的"下去多少才到底"完全由这两个值决定）----
+  # 为什么必须显式配：流程里的"悬停高度"是按**相机**定的（要看清孔得离面 ~0.17m），
+  # 不是按**工具**定的；而工具装在法兰下面、可能比相机长得多。两者一冲突就会出现：
+  #     悬停法兰 0.632 − 工具长 0.243 = 尖端 0.389，工件上表面 0.412
+  #     → **悬停这一刻工具已经插进工件 23mm**
+  # 于是 S07 一路压到顶都等不到"台肩落面"那一下，报"没触发接触判据"，
+  # 看起来像力阈值标错了 —— 实际是几何错。真机同参数同结果。
+  # S07 启动时会把这个差值算出来印在日志里（负值会明确标注"已经插进工件了"）。
+  tool_tip_below_flange_m: 0.245  # 工具尖端（规尖）在法兰下方的距离 [m]
+                                  # **这是实测值，不是推算值**：夹住规之后直接读
+                                  # TF 的 flange z − 规尖 z。
+                                  # 仿真里 `scripts/sim_load_gauge.py` 每次上料后
+                                  # 会自己把它算出来打印（"实测工具几何"那一段），
+                                  # 现场照着填即可 —— 真机同法（夹住规后直接读 TF）。
+                                  #
+                                  # 推算值（TCP 在法兰下方 0.200 + 规尖在指腹面下方
+                                  # 0.043 = 0.243）与实测 0.245 只差 2mm，说明推算
+                                  # 思路是对的；但**别再用推算**，因为下面这个理由：
+                                  #
+                                  # ⚠️ 这个数天生有 ±几毫米的不确定度：规是**靠摩擦**
+                                  #   夹住的，每次上料在爪里的位置都会差几毫米。
+                                  #   所以它只用来**诊断**（悬停时尖端离面多高、下探
+                                  #   预算够不够），**不能**当硬约束 —— S07 的自检
+                                  #   因此是"告警 + 自动放大预算"，不是"直接失败"。
+                                  #   要更准只能改夹持方式（定位台肩/加挡块），那是
+                                  #   机械方案，不是改这个数。
+  workpiece_surface_z_m: 0.0      # 工件**上表面**的世界 z [m]（对孔基准面）
+                                  # 0 = 未知 → 运行期用 stage2 实测的孔位 z 兜底
+                                  #（孔就在上表面上）。仿真里实测是 0.412。
+                                  # 只在"stage2 还没跑就想知道几何对不对"时才需要手填。
+  contact_tangential_delta: 300.0  # 触觉切向力突变阈值 [au，任意单位] —— **兜底判据**
+                                  # ⚠️ 这是"触觉"的单位，不是牛顿：真机 Orisys 输出的
+                                  # 是形变任意单位（厂商不给牛顿换算），仿真侧由
+                                  # force_scale 定义尺度。所以这个数**不能**与下面
+                                  # contact_axial_delta_n 比大小（两者量纲不同）。
+                                  #
+                                  # 它为什么只能是兜底：通止规靠**夹持摩擦**抱住，轴向力
+                                  # 传到指垫必须过摩擦面，上限 = µ·N ≈ 0.25×46N ≈ 23N
+                                  # （两指合计）。而台肩落面一步（0.5mm × 5e4 N/m）就
+                                  # 产生 25N —— 必然打滑，所以触觉**物理上读不到**那一下
+                                  # 的力，读数停在 0（而它是对的）。拿它当主判据，阈值调
+                                  # 多大都不对：调小被噪声骗，调大永远达不到。
+                                  # 2026-09-24 就是照"阈值调高了"改的，白费一轮。
+                                  # 详见 gauge_fsm.wrench_touch 与 docs/GAUGE_ASSEMBLY.md §6。
+  contact_axial_delta_n: 8.0         # 腕部 FT 轴向力突变阈值 [N] —— **首选判据**
+                                  # 腕部 FT 在夹持摩擦的**上游**（工件→规→夹爪→法兰
+                                  # →腕部关节），工具在爪里打不打滑都测得准，单位是真牛顿。
+                                  # 为什么只要 8N：这是"碰到了没有"、不是"压到位了没有"。
+                                  # 台肩一落面，接触刚度 5e4 N/m 意味着过压 0.1mm 就有
+                                  # 5N、0.2mm 有 10N —— 8N 对应约 0.16mm 过压，**在打滑
+                                  # 之前**就判到了（打滑上限 ≈23N ≈ 0.46mm），留 3 倍余量。
+                                  # 现场标定：空载慢慢压到台肩落面，看日志里每 5 步打印的
+                                  # 「腕部轴向」涨到多少，取它的 1/3（要能区分"擦到孔壁"
+                                  # 与"台肩落面"）。
+                                  # 数据源话题：servo.ft_wrench_topic（默认
+                                  # /<sn>/ft_sensor_wrench，真机与仿真同名同类型同单位）。
+  contact_relief_mm: 0.5           # 判到接触后的**卸载回退** [mm] —— 别调成 0
+                                  # 为什么必须有这一步（2026-09-24 一整轮排查换来的）：
+                                  #   位置控制的臂撞上硬面时不会"刚碰到就停"，它会顶到
+                                  #   自己的**出力上限**才停住。仿真实测：0.5mm 的一步压在
+                                  #   工件上，腕部 FT 直接读到 **+244N**，而臂这一步只走了
+                                  #   0.09mm。也就是说"判到接触"的那一刻，臂上顶着的力
+                                  #   与 contact_axial_delta_n **没有关系**（阈值只决定
+                                  #   什么时候承认接触）。
+                                  #   而规是靠**夹持摩擦**抱住的：轴向传力上限 µ·N，两指
+                                  #   合计 ≈73N（µ=0.8、握力 46N）。244N 远超它 ⇒ 规被顶出
+                                  #   爪外。本次现象：S07 判到接触 → S08 一转 → 规掉地上
+                                  #   → 报 E1，而 E1 的日志只说"通止规掉了"，看不到真因。
+                                  # 0.5mm 怎么来的：判到接触时真实过压量通常只有 0.0x mm
+                                  #   （5e4 N/m 时 10N ≈ 0.2mm），回退 0.5mm 足够把力放
+                                  #   干净，而规尖仍在孔里（通端穿板 18mm），不会脱出。
+                                  # 放不干净会在日志里明确告警（残余 > contact_hold_n）。
+  contact_hold_n: 5.0              # 卸载后允许的残余轴向力 [N]
+                                  # 超了不失败，但会**大声报**：它说明"夹持摩擦力不够
+                                  # 承受这次接触"，接着 S08 一转就会掉件。
+                                  # 真修法只有两条：① 降峰值（调小 contact_step_mm 或
+                                  # 加大 contact_relief_mm）；② 加握力（改夹爪合拢力度/
+                                  # 指垫摩擦）。**不要**去调 contact_axial_delta_n。
+  contact_safe_delta: 2000.0       # 法向安全上限 [任意单位]：超了立刻抬起并中止(E2)
+                                   # 必须 > contact_tangential_delta，否则自检就报矛盾
+
+  # ---- S08 棘轮拧入（慢转 → 松爪 → 快回转 → 夹紧，循环）----
+  # 停止判据：切向力 > screw_shear_delta **且** 法向力突增 > screw_normal_delta。
+  screw_step_deg: 120.0      # 每次慢转多少度。
+                             # **硬约束**：2×step + 2×余量(8°) ≤ 关节7 行程。
+                             # Rizon4/10 的关节7 是 ±175° → 行程 350° → step ≤ 167°。
+                             # 填 180° 会**必然**贴限位（慢转+回转=360°>350°），
+                             # 测试 test_ratchet_step_of_180_never_fits_the_rizon_joint7_range
+                             # 与 S08 开头的行程核算都会拦；120° 留足余量吸收
+                             # "回转每轮比指令少走 4~5°"的亏欠。
+  screw_speed_deg_s: 30.0    # 慢转速度 [deg/s]
+  screw_back_speed_deg_s: 120.0  # 松爪后的空转回转速度 [deg/s]（爪里没东西，可以快）
+  screw_max_cycles: 12       # 最多几轮（12×120°=1440° = **4 整圈**，够 M6 螺纹规走完）。
+                             # 必须满足 screw_max_cycles × screw_step_deg 是 360° 的整数倍：
+                             # 否则规停在螺纹中间，S09 拧出也少转几分之一圈。
+  screw_normal_delta: 400.0  # 拧到位时的法向力突增 [任意单位]（螺纹到底/顶住）
+  screw_shear_delta: 600.0   # 拧到位的切向力增量 [任意单位]
+  screw_grip_force_pct: 50.0 # 夹紧力百分比 [%]（**只有契约后端生效**：仿真/新驱动）
+  screw_grip_speed_pct: 50.0 # 夹紧速度百分比 [%]（同上；真机 dh_pgea 驱动不支持这两个参数）
+  screw_release_wait_s: 0.3  # 松爪后等多久再回转 [s]（等爪真的张开，否则会带着规磨）
+
+  # ---- S09 反向拧出 ----
+  unscrew_extra_cycles: 1     # 比拧入多转几轮，确保螺纹完全脱开
+
+  # ---- 力判据怎么调（现场最常问）----
+  # 1) 先看量级：bash ~/ros2/scripts/run_tactile.sh --cli，手压一下看读数变化；
+  # 2) S07 的接触判据是"相对基线的增量"，基线在 S06 悬停时（还没碰工件）测；
+  # 3) 太灵敏（刚靠近就停）→ 调大 contact_tangential_delta；
+  #    不灵敏（压很深才停）→ 调小；同时看启动日志里的基线/实测值再定。
+
+tactile:
+  backend: "orisys"          # orisys | mock
+  # 传感器型号。必须与 devices/sensors/orisys/models/ 下的子目录名一致。
+  model: "dd03"
+  # 型号配置随 SDK 一起放：devices/sensors/orisys/models/<型号>/config.json
+  config_file: "/home/cc/ros2/devices/sensors/orisys/models/dd03/config.json"
+  # 用 by-path 稳定路径（插哪个 USB 口就认哪个口，不受 video 编号漂移影响）。
+  # 当前：3.4.1=左指 3.4.2=右指；若左右反了，互换两行即可。
+  left_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.1:1.0-video-index0"
+  right_video: "/dev/v4l/by-path/pci-0000:00:14.0-usb-0:3.4.2:1.0-video-index0"
+  cuda: false                # 两路进程抢 GPU 上下文会互相拖慢；CPU 解算仅 ~9ms/帧，足够
+  isstitch: true
+  infer_backend: "cpu"       # cupy | opencl | cpu；双路并行用 cpu 最稳
+  baseline_seconds: 2.0
+  filter_alpha: 0.25
+  motion_threshold: 0.0
+  target_fps: 30.0
+  publish_hz: 20.0
+  # 视触觉图像与力矢量场的发布频率。0 = 关闭（默认）。
+  # 三力走 30fps 的轻量队列；图像每帧几百 KB，单独走一条 maxsize=1 的队列，
+  # 打开它不会影响三力的时序。需要看形变图/矢量场时设成 5~10 即可。
+  image_hz: 0.0
+  rotation_config: ""        # 左右共用；有分指文件则用下面两项
+  left_rotation: ""          # 例：orisys SDK 的 rotation_config_0.json
+  right_rotation: ""
+
+go_home:
+  pose: "home"
+  speed: 0.25
+
+network:
+  # 2026-09-29 改线：两台臂 → 交换机 → USB-C 千兆网卡（ASIX AX88179）
+  #   MAC 6c:1f:f7:d0:6f:17。内核现在叫 eth0（不再用 enx6c1ff7d06f17）。
+  #   本机 192.168.2.10  ↔  063989(左89)=.101  /  063991(右91)=.100
+  #   NM 配置名 robot-usb-lan（静态 .10/24，不当默认路由）。
+  # 说明：enp4s0 仍是办公网（192.168.1.x）；两台臂已不在 enp4s0 上。
+  prefer_iface: "eth0"
+
+# 仿真平台（当前 Gazebo 尚未接线，详见 devices/sim/gazebo/README.md）
+# 设计约束：仿真与真机共用同一套参数（servo.* / hole_detect.stage2.*），
+# 只有物理引擎自身的参数放在这里，避免「仿真调好的值搬到真机不生效」。
+sim:
+  # 这一段由 arm_sim_gazebo.sim_site 真正读取（此前是"声明了但没人读"，
+  # 改了毫无反应也不报错）。三个键的接线状况**不一样**，务必看清：
+  #
+  #   world           ✔ 已接线。优先级：命令行 --world > 这里 > 内置默认。
+  #                     相对路径按**仓库根**解析。
+  #   physics_enable  ✘ 未接线。世界文件里的重力/步长是写死的。若这里写的值
+  #                     与世界的实际状态不符，启动时会打印告警（不再静默）。
+  #   joint_prefix    ✘ 未接线。前缀由 urdf/arm.sim.urdf.xacro 的 arm_prefix
+  #                     决定，且 config/controllers.yaml 里硬编码了 sim_joint1..7。
+  #                     写了个不同的值会告警。要真改，三处必须一起改。
+  #
+  # 另外注意：`run_sim.sh` 走的仍是 mock 驱动（假数据 + RViz），不是物理仿真；
+  # 物理仿真入口是 `scripts/run_sim_gz.sh`（Gazebo）。两者互不影响。
+  platform: "gazebo"
+  world: "devices/sim/gazebo/worlds/assembly.world"
+  physics_enable: true
+  joint_prefix: "sim_"       # 仿真关节前缀，必须与真实臂前缀区分
diff --git a/docs/GRIPPER.md b/docs/GRIPPER.md
index cc959ec..3cd977c 100644
--- a/docs/GRIPPER.md
+++ b/docs/GRIPPER.md
@@ -16,7 +16,7 @@
 ```bash
 # 左臂平行爪
 bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli probe
-bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli init    # 上电第一次
+bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli init    # 写 0xA5 回零；已经 initialized 也会再做一次
 bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli open
 bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli close
 bash ~/ros2/scripts/run_gripper.sh --arm 89 --cli pos 500 # 0=合 1000=开（‰）
diff --git a/docs/HANDBOOK.md b/docs/HANDBOOK.md
index dd796bf..d393c0f 100644
--- a/docs/HANDBOOK.md
+++ b/docs/HANDBOOK.md
@@ -68,22 +68,18 @@ bash ~/ros2/scripts/run_camera_d405.sh --arm 89 --preview   # T2 相机预览，
 bash ~/ros2/scripts/run_all.sh --arm 89                     # T3 三段串联（默认不拧入）
 ```
 
-一阶段默认 **D405 实时推理**，D435 待命。连续 `D405_MISS_BEFORE_D435_S`（0.5 s）没有
-conf≥`perception.wrist_handoff_conf`（0.5）的目标，才用顶视 D435 **拍一张** 把工件 / k0
-定到基座系（眼在手外 T，不再乘法兰），左臂按二阶段同一套 **`FollowJointTrajectory` action**
-先收齐锁高，再把全部 XY 串成一条关节折线，余弦加速（100 mm）/
-巡航封顶 60 mm/s / 余弦减速（80 mm），头尾速度为 0。中途不重测、不停住。
-40 mm 爬到 80 mm/s 会在真机上像弹射，那是加速段太短，不是初速度或角速度。
-工件上方（剩差 ≤80 mm）巡航改 30 mm/s，只走 XY，不混 Z、不夹收垂直。
-结果 + 0.25 s 再重测 TF（带符号 `dxy`）。冻结 J 若还剩 >15 mm，只允许 **一刀补**
-（`tag=fix`）。XY 进 15 mm 门槛后再单独收法兰朝下。结果回来之前不往话题插点。
-到位成功不再发话题 hold。几乎没走近就钉住，不许按同一剩差再刷。
-走到该点并且法兰 XY 连续 `servo.lock_still_sec`（3 s）位移不超过
-`lock_still_move_m`（15 mm）之前，D405 的点不下发（推理不关）。停稳之后改回 D405
-跟踪；再按同一静止判据 → `/visual_servo/locked` → 二阶段对孔。
-只测 D435 这一段位移时加 `--d435-only`：D405 点不下发，到位停稳后钉住并结束一阶段，
-不交 D405、不进二阶段。眼外标定误差只会让冻结点整体偏一截，不会造成中途启停；
-启停来自以前按 80/15 mm 一跳一跳 rest-to-rest。
+一阶段默认 **D405 实时推理**，D435 待命。YOLO-pose 这一路不关。连续
+`D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
+的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
+这一段运动是厂家 `MoveL`：先放开 `ros2_control` 占着的 RDK，XY 对准该点，
+工具尖高度保持当前（点的 z 是工件表面），不按直线距离拒绝；走完再用
+`run_real.sh --no-stop` 把控制栈拉回来（不扫感知进程）。走到该点并且法兰 XY
+连续 `servo.lock_still_sec`（3 s）位移不超过 `lock_still_move_m`（15 mm）之前，
+D405 的点不下发。停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
+框心对准连续 `servo.wrist_align_sec`（1 s）→ `/visual_servo/locked` → 二阶段对小孔。
+对孔只用关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
+`tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
+只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
 
 ```bash
 bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera --d435-only
diff --git a/handeye/results/sim_d435_eye_to_hand.yaml b/handeye/results/sim_d435_eye_to_hand.yaml
new file mode 100644
index 0000000..2d9597c
--- /dev/null
+++ b/handeye/results/sim_d435_eye_to_hand.yaml
@@ -0,0 +1,11 @@
+# 仿真手眼真值 —— **自动生成，不要手改**
+# mode: eye_to_hand
+# 来源: Gazebo/URDF 的 TF 真值 `sim_base_link` ← `sim_d435_color_optical_frame`（仿真不需要标定）
+# 生成: 2026-09-28T14:05:19+0800  生成器: scripts/gen_sim_handeye.py
+# 仿真台：手外相机固定在工位/世界上（不随臂动）
+translation_m: [-0.45000000, 0.00000000, 1.35000000]
+T_4x4:
+  - [-0.00000236, -0.76604704, 0.64278451, -0.45000000]
+  - [-1.00000000, 0.00000000, -0.00000367, 0.00000000]
+  - [0.00000281, -0.64278451, -0.76604704, 1.35000000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
diff --git a/handeye/results/sim_eye_in_hand.yaml b/handeye/results/sim_eye_in_hand.yaml
new file mode 100644
index 0000000..72450a7
--- /dev/null
+++ b/handeye/results/sim_eye_in_hand.yaml
@@ -0,0 +1,11 @@
+# 仿真手眼真值 —— **自动生成，不要手改**
+# mode: eye_in_hand
+# 来源: Gazebo/URDF 的 TF 真值 `sim_flange` ← `sim_d405_color_optical_frame`（仿真不需要标定）
+# 生成: 2026-09-28T14:05:19+0800  生成器: scripts/gen_sim_handeye.py
+# 仿真台：腕部相机装在法兰上，位姿来自 URDF
+translation_m: [0.00000000, -0.06000000, 0.05000000]
+T_4x4:
+  - [0.00000000, 1.00000000, -0.00000000, 0.00000000]
+  - [-1.00000000, 0.00000000, -0.00000367, -0.06000000]
+  - [-0.00000367, 0.00000000, 1.00000000, 0.05000000]
+  - [0.00000000, 0.00000000, 0.00000000, 1.00000000]
diff --git a/models/hole_insert/detect_best_1.engine b/models/hole_insert/detect_best_1.engine
new file mode 100644
index 0000000..f713620
Binary files /dev/null and b/models/hole_insert/detect_best_1.engine differ
diff --git a/models/hole_insert/detect_best_1.onnx b/models/hole_insert/detect_best_1.onnx
new file mode 100644
index 0000000..f10aa7c
Binary files /dev/null and b/models/hole_insert/detect_best_1.onnx differ
diff --git a/models/yolo/detect_best_0.engine b/models/yolo/detect_best_0.engine
new file mode 100644
index 0000000..154d002
Binary files /dev/null and b/models/yolo/detect_best_0.engine differ
diff --git a/models/yolo/detect_best_0.onnx b/models/yolo/detect_best_0.onnx
new file mode 100644
index 0000000..8e9ee3b
Binary files /dev/null and b/models/yolo/detect_best_0.onnx differ
diff --git a/models/yolo/gongjian0929.engine b/models/yolo/gongjian0929.engine
new file mode 100644
index 0000000..70c8d6f
Binary files /dev/null and b/models/yolo/gongjian0929.engine differ
diff --git a/models/yolo/gongjian0929.onnx b/models/yolo/gongjian0929.onnx
new file mode 100644
index 0000000..a1004d6
Binary files /dev/null and b/models/yolo/gongjian0929.onnx differ
diff --git a/scripts/d435_k0_once.py b/scripts/d435_k0_once.py
new file mode 100644
index 0000000..1f56941
--- /dev/null
+++ b/scripts/d435_k0_once.py
@@ -0,0 +1,173 @@
+#!/usr/bin/env python3
+"""拍一张顶视 D435，把 k0 用眼在手外标定变到左臂基座。
+
+stdout 只打一行，给 run_all.sh --d435-only 接厂家 MoveL：
+
+    K0_BASE x y z
+
+x y z 单位米，坐标系是这条臂的基座（与 RDK 的 WORLD 同一套）。
+变换就是 handeye/results/<臂>_d435_eye_to_hand.yaml 的 T_4x4：
+
+    p_基座 = T_cam_to_base · p_相机
+
+k0 的 z 是工件表面，不是工具尖该去的高度。运动脚本会保持当前尖高，只用这里的 xy。
+没有可信 k0 就退出，不用检测框中心冒充。
+"""
+
+from __future__ import annotations
+
+import sys
+import time
+from pathlib import Path
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import qos_profile_sensor_data
+from sensor_msgs.msg import CameraInfo, Image
+
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / "scripts" / "lib"))
+
+from export_site_env import load_site, perception_overrides  # noqa: E402
+from arm_application.geom_ik import load_handeye_T  # noqa: E402
+from arm_application.overhead_oneshot import (  # noqa: E402
+    depth_raw_to_m,
+    oneshot_k0_base,
+    select_oneshot_det,
+)
+from arm_application.perception_node import _bgr_copy  # noqa: E402
+from arm_application.yolo_detector import build_detector  # noqa: E402
+
+
+def _decode_depth(msg: Image):
+    w, h = int(msg.width), int(msg.height)
+    enc = (msg.encoding or "").lower()
+    if enc in ("16uc1", "mono16"):
+        raw = np.frombuffer(msg.data, dtype=np.uint16)
+        if raw.size < w * h:
+            return None
+        return raw[: w * h].reshape(h, w)
+    if enc == "32fc1":
+        raw = np.frombuffer(msg.data, dtype=np.float32)
+        if raw.size < w * h:
+            return None
+        return raw[: w * h].reshape(h, w)
+    return None
+
+
+class OneShot(Node):
+    def __init__(self, topics: dict) -> None:
+        super().__init__("d435_k0_once")
+        self.color = None
+        self.depth = None
+        self.info = None
+        self.create_subscription(Image, topics["image"], self._on_color, qos_profile_sensor_data)
+        self.create_subscription(Image, topics["depth"], self._on_depth, qos_profile_sensor_data)
+        self.create_subscription(CameraInfo, topics["info"], self._on_info, qos_profile_sensor_data)
+
+    def _on_color(self, msg: Image) -> None:
+        self.color = msg
+
+    def _on_depth(self, msg: Image) -> None:
+        self.depth = msg
+
+    def _on_info(self, msg: CameraInfo) -> None:
+        self.info = msg
+
+    def ready(self) -> bool:
+        return self.color is not None and self.depth is not None and self.info is not None
+
+
+def main() -> int:
+    perc = perception_overrides(load_site())
+    if not perc.get("overhead_oneshot_enable"):
+        print("site 没有接上 D435 一次性（overhead_oneshot_enable）", file=sys.stderr)
+        return 2
+    handeye = str(perc.get("overhead_handeye_file") or "")
+    topics = {
+        "image": str(perc.get("overhead_image_topic") or ""),
+        "depth": str(perc.get("overhead_depth_topic") or ""),
+        "info": str(perc.get("overhead_info_topic") or ""),
+    }
+    if not handeye or not all(topics.values()):
+        print(f"眼在手外话题或标定文件没接上：{handeye} {topics}", file=sys.stderr)
+        return 2
+    T = load_handeye_T(handeye)
+    print(f"手眼 {handeye}", file=sys.stderr)
+    print(f"图 {topics['image']}", file=sys.stderr)
+
+    weights = str(perc.get("weights") or "")
+    detector = build_detector(
+        weights,
+        backend=str(perc.get("backend") or "auto"),
+        conf=float(perc.get("conf") or 0.1),
+        iou=float(perc.get("iou") or 0.45),
+        device=str(perc.get("device") or "0"),
+        imgsz=int(perc.get("imgsz") or 640),
+    )
+
+    rclpy.init()
+    node = OneShot(topics)
+    t0 = time.monotonic()
+    try:
+        while rclpy.ok() and not node.ready():
+            rclpy.spin_once(node, timeout_sec=0.2)
+            if time.monotonic() - t0 > 15.0:
+                print("15s 内没有凑齐彩色图、深度、内参", file=sys.stderr)
+                return 3
+        color, depth, info = node.color, node.depth, node.info
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+    bgr = _bgr_copy(color)
+    dets = detector.detect(bgr)
+    min_conf = float(perc["overhead_oneshot_conf"])
+    det = select_oneshot_det(dets, min_conf)
+    if det is None:
+        print(f"画面里没有 conf≥{min_conf:.2f} 的工件", file=sys.stderr)
+        return 4
+    raw = _decode_depth(depth)
+    depth_m = depth_raw_to_m(raw, depth.encoding or "")
+    if depth_m is None:
+        print("深度无法转成米", file=sys.stderr)
+        return 4
+    k = info.k
+    got = oneshot_k0_base(
+        det,
+        depth_m,
+        fx=float(k[0]),
+        fy=float(k[4]),
+        cx=float(k[2]),
+        cy=float(k[5]),
+        T_cam_to_base=T,
+        min_z_m=float(perc["overhead_min_depth_m"]),
+        max_z_m=float(perc["overhead_max_depth_m"]),
+        kpt_index=int(perc.get("keypoint_index") or 0),
+        kpt_min_conf=float(perc.get("keypoint_min_conf") or 0.30),
+        min_det_conf=min_conf,
+        color_wh=(int(color.width), int(color.height)),
+        depth_wh=(int(depth_m.shape[1]), int(depth_m.shape[0])),
+    )
+    if got is None:
+        print("k0 或深度不可用，不发点", file=sys.stderr)
+        return 4
+    p_base, is_k0 = got
+    if not is_k0:
+        print(
+            f"顶视没有可信 k0，只有框心 ({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f})，拒绝",
+            file=sys.stderr,
+        )
+        return 4
+    print(
+        f"k0 基座 ({p_base[0]:.4f}, {p_base[1]:.4f}, {p_base[2]:.4f}) m  conf={float(det.conf):.2f}",
+        file=sys.stderr,
+    )
+    print(f"K0_BASE {p_base[0]:.6f} {p_base[1]:.6f} {p_base[2]:.6f}")
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
diff --git a/scripts/lib/export_site_env.py b/scripts/lib/export_site_env.py
index 742b157..9ed3756 100644
--- a/scripts/lib/export_site_env.py
+++ b/scripts/lib/export_site_env.py
@@ -903,6 +903,7 @@ def servo_overrides(data: dict) -> dict:
         "ee_hold_kp", "ee_w_max",
         "lock_hold_sec",
         "lock_still_sec",
+        "wrist_align_sec",
         "lock_still_move_m",
         "a_max", "max_qdot", "damping",
         "vel_lpf_alpha", "dq_window_size", "traj_stream_hz", "exec_horizon_sec",
@@ -934,7 +935,12 @@ def servo_overrides(data: dict) -> dict:
         if "lock_still_sec" not in servo:
             raise RuntimeError(
                 "servo.lock_still_sec 必须显式写"
-                "（D405 跟住后法兰静止多久才进二阶段）。不许用代码默认 3。"
+                "（D435 到位后法兰停稳多久才交回 D405）。不许用代码默认 3。"
+            )
+        if "wrist_align_sec" not in servo:
+            raise RuntimeError(
+                "servo.wrist_align_sec 必须显式写"
+                "（D405 框心对准多久才进对孔）。不许用代码默认 1。"
             )
         if "lock_still_move_m" not in servo:
             raise RuntimeError(
@@ -942,6 +948,7 @@ def servo_overrides(data: dict) -> dict:
                 "（静止判据的法兰 XY 位移上限）。不许用代码默认 15mm。"
             )
         out["lock_still_sec"] = float(servo["lock_still_sec"])
+        out["wrist_align_sec"] = float(servo["wrist_align_sec"])
         out["lock_still_move_m"] = float(servo["lock_still_move_m"])
         out["overhead_k0_topic"] = "visual_servo/overhead_k0"
         out["wrist_conf_topic"] = "visual_servo/wrist_conf"
diff --git a/scripts/lib/rdk_release.py b/scripts/lib/rdk_release.py
new file mode 100644
index 0000000..fb71e76
--- /dev/null
+++ b/scripts/lib/rdk_release.py
@@ -0,0 +1,19 @@
+"""D435 一次性要走厂家 MoveL 时，先认出谁占着 RDK。
+
+`ros2_control_node` 和真机 `stack.launch` 占着唯一的 RDK 连接。
+感知、伺服、本脚本自己不算。误伤它们会把正在推理的进程一起杀掉。
+"""
+
+from __future__ import annotations
+
+
+def is_control_stack_cmd(cmd: str) -> bool:
+    """这条命令行是不是占着 RDK 的真机控制栈。"""
+    text = cmd or ""
+    if "rdk_release_movel" in text:
+        return False
+    if "ros2_control_node" in text:
+        return True
+    if "stack.launch.py" in text and "backend:=real" in text:
+        return True
+    return False
diff --git a/scripts/rdk_goto_xyz.py b/scripts/rdk_goto_xyz.py
index 82e0891..031c579 100644
--- a/scripts/rdk_goto_xyz.py
+++ b/scripts/rdk_goto_xyz.py
@@ -17,6 +17,7 @@
    [x, y, z, qw, qx, qy, qz]，单位米，坐标系是机器人世界系（基座那套 WORLD）。
    位置是当前 Studio 工具的 TCP（现在是 tool10120938 的尖），不是法兰，也不是相机。
 4. 目标只换 x、y、z。姿态沿用现在的朝向，折成厂家 MoveL 要的欧拉角（度，ZYX）。
+   `--keep-z` 时 z 保持当前工具尖：K0 的 z 是工件表面，不能把尖送到那上面。
 5. SwitchMode(NRT_PRIMITIVE_EXECUTION)，ExecutePrimitive("MoveL", ...)。
 6. 轮询 primitive_states()["reachedTarget"]。到了再读一次 tcp_pose，印出还差多少毫米。
 
@@ -134,6 +135,11 @@ def main() -> int:
     parser.add_argument("--vel", type=float, default=0.05, help="TCP 线速度 m/s，默认 0.05")
     parser.add_argument("--max-step-m", type=float, default=0.40, help="离当前点超过这么多米就拒绝，除非 --go")
     parser.add_argument("--go", action="store_true", help="允许超过 --max-step-m")
+    parser.add_argument(
+        "--keep-z",
+        action="store_true",
+        help="XY 用给定点，Z 保持当前工具尖。K0 的 Z 是工件表面，不能把尖送到那上面",
+    )
     parser.add_argument("--timeout", type=float, default=60.0, help="等 reachedTarget 的秒数")
     args = parser.parse_args()
 
@@ -154,6 +160,12 @@ def main() -> int:
             return 0
 
         target_xyz = [float(v) for v in args.xyz]
+        if args.keep_z:
+            print(
+                f"给定点 z={target_xyz[2]:.4f} m 是工件表面，不下发。"
+                f"工具尖高度保持当前 {pose[2]:.4f} m"
+            )
+            target_xyz[2] = float(pose[2])
         dist = math.dist(pose[:3], target_xyz)
         print(f"目标 xyz=({target_xyz[0]:.4f}, {target_xyz[1]:.4f}, {target_xyz[2]:.4f}) m")
         print(f"直线距离 {dist * 1000:.0f} mm    速度 {args.vel:.3f} m/s    姿态保持当前")
diff --git a/scripts/rdk_release_movel.py b/scripts/rdk_release_movel.py
new file mode 100644
index 0000000..d4b1d05
--- /dev/null
+++ b/scripts/rdk_release_movel.py
@@ -0,0 +1,174 @@
+#!/usr/bin/env python3
+"""D435 这一点用厂家 MoveL 送到工具尖，再把真机控制栈拉回来。
+
+RDK 同时只能有一个连接。混合流程里 `run_real.sh` 已经占着它，
+所以顺序是：停真机栈 → MoveL（`--keep-z --go`）→ 用 `--no-stop` 把栈拉回来。
+
+`--no-stop` 是为了不扫 `install/`：感知和伺服还在跑，扫了会把自己杀掉。
+YOLO 不在这里关。
+"""
+
+from __future__ import annotations
+
+import argparse
+import os
+import signal
+import subprocess
+import sys
+import time
+from pathlib import Path
+
+ROOT = Path(__file__).resolve().parents[1]
+sys.path.insert(0, str(ROOT / "scripts" / "lib"))
+
+from rdk_release import is_control_stack_cmd  # noqa: E402
+
+RDK_PYTHON = os.environ.get("RDK_PYTHON", "/home/cc/rdk_py310/bin/python")
+
+
+def _procs() -> list[tuple[int, str]]:
+    out: list[tuple[int, str]] = []
+    proc = Path("/proc")
+    for entry in proc.iterdir():
+        if not entry.name.isdigit():
+            continue
+        pid = int(entry.name)
+        if pid == os.getpid():
+            continue
+        try:
+            raw = (entry / "cmdline").read_bytes().replace(b"\x00", b" ")
+        except OSError:
+            continue
+        cmd = raw.decode("utf-8", "replace").strip()
+        if cmd:
+            out.append((pid, cmd))
+    return out
+
+
+def _control_pids() -> list[tuple[int, str]]:
+    return [(pid, cmd) for pid, cmd in _procs() if is_control_stack_cmd(cmd)]
+
+
+def _term(pids: list[int]) -> None:
+    for pid in pids:
+        try:
+            os.kill(pid, signal.SIGTERM)
+        except OSError:
+            pass
+    deadline = time.monotonic() + 8.0
+    while time.monotonic() < deadline:
+        alive = []
+        for pid in pids:
+            try:
+                os.kill(pid, 0)
+            except OSError:
+                continue
+            alive.append(pid)
+        if not alive:
+            return
+        time.sleep(0.2)
+        pids = alive
+    for pid in pids:
+        try:
+            os.kill(pid, signal.SIGKILL)
+        except OSError:
+            pass
+
+
+def _service_up() -> bool:
+    try:
+        listed = subprocess.run(
+            ["timeout", "3", "ros2", "service", "list"],
+            capture_output=True,
+            text=True,
+            check=False,
+        )
+    except OSError:
+        return False
+    return "/controller_manager/list_controllers" in (listed.stdout or "")
+
+
+def _controller_active() -> bool:
+    if not _service_up():
+        return False
+    try:
+        call = subprocess.run(
+            [
+                "timeout", "3", "ros2", "service", "call",
+                "/controller_manager/list_controllers",
+                "controller_manager_msgs/srv/ListControllers",
+            ],
+            capture_output=True,
+            text=True,
+            check=False,
+        )
+    except OSError:
+        return False
+    text = (call.stdout or "") + (call.stderr or "")
+    return "name='rizon_arm_controller', state='active'" in text
+
+
+def _wait_controller(up: bool, timeout_s: float) -> bool:
+    deadline = time.monotonic() + timeout_s
+    while time.monotonic() < deadline:
+        active = _controller_active()
+        if active == up:
+            return True
+        time.sleep(0.5)
+    return _controller_active() == up
+
+
+def main() -> int:
+    ap = argparse.ArgumentParser()
+    ap.add_argument("--xyz", nargs=3, type=float, required=True, metavar=("X", "Y", "Z"))
+    ap.add_argument("--arm", default=os.environ.get("SITE_ROBOT_ARM", "89"))
+    ap.add_argument("--vel", type=float, default=0.05)
+    args = ap.parse_args()
+
+    held = _control_pids()
+    if held:
+        print(f"放开 RDK：停 {len(held)} 个真机控制进程（感知不停）")
+        for pid, cmd in held:
+            print(f"  {pid} {cmd[:160]}")
+        _term([pid for pid, _cmd in held])
+        if not _wait_controller(False, 20.0):
+            print("ERROR: 控制器服务还在，MoveL 会和它抢 RDK", file=sys.stderr)
+            return 1
+    else:
+        print("真机控制栈没在跑，直接 MoveL")
+
+    x, y, z = (float(v) for v in args.xyz)
+    movel = [
+        RDK_PYTHON,
+        str(ROOT / "scripts" / "rdk_goto_xyz.py"),
+        "--xyz", f"{x}", f"{y}", f"{z}",
+        "--keep-z", "--go",
+        "--vel", f"{float(args.vel)}",
+    ]
+    print("厂家 MoveL：XY 对准 D435 这一点，工具尖高度保持当前")
+    got = subprocess.run(movel, check=False)
+    if got.returncode != 0:
+        print(f"ERROR: MoveL 退出码 {got.returncode}", file=sys.stderr)
+
+    if held:
+        log_path = Path("/tmp/run_all_real_restart.log")
+        print(f"拉回真机栈（--no-stop，不扫感知） log={log_path}")
+        log_f = log_path.open("ab")
+        subprocess.Popen(
+            ["bash", str(ROOT / "scripts" / "run_real.sh"), "--no-stop", "--arm", str(args.arm)],
+            stdout=log_f,
+            stderr=subprocess.STDOUT,
+            start_new_session=True,
+        )
+        if not _wait_controller(True, 120.0):
+            print(
+                "ERROR: 控制栈没回到 ACTIVE。看 /tmp/run_all_real_restart.log",
+                file=sys.stderr,
+            )
+            return 1
+        print("rizon_arm_controller 已回到 ACTIVE")
+    return 0 if got.returncode == 0 else int(got.returncode)
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
diff --git a/scripts/run_all.sh b/scripts/run_all.sh
index 48c9844..6d76af7 100755
--- a/scripts/run_all.sh
+++ b/scripts/run_all.sh
@@ -21,7 +21,7 @@
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera          # 相机已在别处拉起了
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-tactile         # 不起触觉服务
 #   bash ~/ros2/scripts/run_all.sh --arm 89 --no-descent         # 对准后只降到观测高度（默认离面 15mm）
-#   bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only          # 只测 D435 单次位移，到位后不交 D405、不进二阶段
+#   bash ~/ros2/scripts/run_all.sh --arm 89 --d435-only          # 拍一张 K0，厂家 MoveL 把工具尖 XY 对准它（先停 run_real.sh）
 #
 # 前置（另开一个终端，先跑起来）：
 #   bash ~/ros2/scripts/run_real.sh --arm 89
@@ -86,6 +86,49 @@ fi
 
 echo "=== run_all（三阶段串联） arm=${SITE_ROBOT_ARM:-?} ${SITE_ROBOT_LABEL:-} (${SITE_ROBOT_SN:-?}) ==="
 
+# D435 单次定位不走关节轨迹。拍一张 → K0 用眼在手外 T 变到基座 → 厂家 MoveL。
+# RDK 与 run_real.sh 互斥，所以这条路径要求控制器没开。
+if [[ "${D435_ONLY}" == true ]]; then
+  echo "=== D435 单次定位：厂家 RDK MoveL（先停 run_real.sh）==="
+  if timeout 3 ros2 service list 2>/dev/null | grep -q "^/controller_manager/list_controllers$"; then
+    echo "ERROR: run_real.sh 还占着 RDK，MoveL 会被打回 IDLE。先在那个终端 Ctrl-C。" >&2
+    exit 1
+  fi
+  D435_TOPIC="/d435/color/image_raw"
+  _d435_ok() {
+    timeout 2 ros2 topic list 2>/dev/null | grep -qx "${D435_TOPIC}" || return 1
+    timeout 3 ros2 topic echo --once "${D435_TOPIC}" 2>/dev/null | grep -q "^header:"
+  }
+  if _d435_ok; then
+    echo "=== 顶视 ${D435_TOPIC} 已在发图 ==="
+  else
+    echo "=== 拉起顶视 D435 ==="
+    nohup bash "${ROOT}/scripts/run_camera_d435.sh" --no-preview > /tmp/run_all_d435.log 2>&1 &
+    echo "    d435 pid=$! log=/tmp/run_all_d435.log"
+    d435_ok=false
+    for _i in $(seq 1 24); do
+      if _d435_ok; then d435_ok=true; break; fi
+      sleep 0.5
+    done
+    if [[ "${d435_ok}" != true ]]; then
+      echo "ERROR: ${D435_TOPIC} 仍无图像。看 /tmp/run_all_d435.log" >&2
+      exit 1
+    fi
+  fi
+  echo "=== 拍一张，K0 × 眼在手外 T → 基座 ==="
+  k0_line="$(python3 "${ROOT}/scripts/d435_k0_once.py")"
+  echo "    ${k0_line}"
+  read -r _tag kx ky kz <<<"${k0_line}"
+  if [[ "${_tag}" != "K0_BASE" ]]; then
+    echo "ERROR: 没有得到 K0_BASE，得到：${k0_line}" >&2
+    exit 1
+  fi
+  echo "=== MoveL：XY 对准 K0，工具尖高度保持当前（K0 的 z 是工件表面）==="
+  # --go：不按直线距离拦截。800 mm 是脚本自己的保护，不是机械臂限位。
+  exec /home/cc/rdk_py310/bin/python "${ROOT}/scripts/rdk_goto_xyz.py" \
+    --xyz "${kx}" "${ky}" "${kz}" --keep-z --go --vel 0.05
+fi
+
 # ---- 1) 控制器必须在。判活方式与 run_stage2_locate.sh 保持一致 ----
 echo "=== 等待 rizon_arm_controller ACTIVE（T1: run_real.sh --arm ${SITE_ROBOT_ARM:-?}）==="
 # 先看服务在不在，再 call —— 顺序反过来的话，服务不存在时每次 call 都要耗满
diff --git a/scripts/run_real.sh b/scripts/run_real.sh
index 7e51b0f..1517e97 100755
--- a/scripts/run_real.sh
+++ b/scripts/run_real.sh
@@ -10,10 +10,12 @@ set -eo pipefail
 ROOT="$(cd "$(dirname "${BASH_SOURCE[0]}")/.." && pwd)"
 FLEXIV_PREFIX="${HOME}/flexiv_install"
 LIST_ONLY=false
+NO_STOP=false
 
 while [[ $# -gt 0 ]]; do
   case "$1" in
     --sn|--arm) export ROBOT_SN="${2:?}"; export ROBOT_ARM="${2}"; shift 2 ;;
+    --no-stop) NO_STOP=true; shift ;;
     --list|-l) LIST_ONLY=true; shift ;;
     -h|--help)
       sed -n '2,8p' "$0"
@@ -54,6 +56,9 @@ echo "=== catalog: ${SITE_ROBOT_CATALOG:-} ==="
 echo "=== handeye: ${SITE_HANDEYE_FILE:-} ==="
 echo "=== camera serial: ${SITE_CAM_SERIAL:-}  gripper: ${SITE_GRIP_MODEL:-} ${SITE_GRIP_PORT:-} ==="
 echo "=== network iface=${NET_IFACE:-auto}  robot=${ROBOT_IP}  pc=${SITE_PC_IP:-} ==="
+if [[ "${NO_STOP}" == true ]]; then
+  echo "=== --no-stop：不扫 install/（感知和伺服还在，扫了会把自己杀掉）==="
+else
 echo "=== stop leftover real stack (RDK exclusive) ==="
 # 以前这里是两行 pkill（ros2_control_node + stack.launch），漏掉了
 # arm_control 那一族**独立存活**的孤儿（adapter / named_pose_server /
@@ -75,6 +80,7 @@ if [ "${#_stale[@]}" != "0" ]; then
   unset _pid
 fi
 unset _stale
+fi
 sleep 1
 # 清掉把 91 的 .100 误绑到 89 MAC 上的陈旧 ARP
 if [[ -n "${NET_IFACE}" ]]; then
diff --git a/src/arm_application/arm_application/dh_official.py b/src/arm_application/arm_application/dh_official.py
index 451d8b6..2467ae9 100644
--- a/src/arm_application/arm_application/dh_official.py
+++ b/src/arm_application/arm_application/dh_official.py
@@ -201,21 +201,36 @@ class DHModbusClient:
 
     def set_position_per_mille(self, position: float | int) -> int:
         value = _clamp_int(position, 0, 1000)
+        # 与 dh_pgea.latched_position_break 同一份：命令已是目标时先写当前位置，
+        # 读回之后再写最终值。60ms 后写回旧命令，固件采样不到，手指不动。
+        from arm_application.dh_pgea import latched_position_break
+
         try:
             cmd = _clamp_int(self.read_register(REG_POSITION), 0, 1000)
         except Exception:
             cmd = value
-        if cmd == value:
-            try:
-                cur = self.get_position_per_mille()
-            except Exception:
-                cur = value
-            nudge = cur + (40 if value >= cur else -40)
-            nudge = _clamp_int(nudge, 0, 1000)
-            if nudge == value:
-                nudge = 0 if value != 0 else 40
-            self.write_register(REG_POSITION, nudge)
-            time.sleep(0.06)
+        try:
+            cur = self.get_position_per_mille()
+        except Exception:
+            cur = value
+        bridge = latched_position_break(cmd, cur, value)
+        if bridge is not None:
+            self.write_register(REG_POSITION, bridge)
+            deadline = time.monotonic() + 0.6
+            echoed = False
+            while time.monotonic() < deadline:
+                try:
+                    if _clamp_int(self.read_register(REG_POSITION), 0, 1000) == bridge:
+                        echoed = True
+                        break
+                except Exception:
+                    pass
+                time.sleep(0.05)
+            if not echoed:
+                raise DHModbusError(
+                    f"位置命令中间值 {bridge} 没有读回，不再写入最终目标 {value}"
+                )
+            time.sleep(0.15)
         self.write_register(REG_POSITION, value)
         return value
 
diff --git a/src/arm_application/arm_application/dh_pgea.py b/src/arm_application/arm_application/dh_pgea.py
index 2653230..3cfbbbf 100644
--- a/src/arm_application/arm_application/dh_pgea.py
+++ b/src/arm_application/arm_application/dh_pgea.py
@@ -104,6 +104,44 @@ INIT_STATE_NAME = {
 }
 
 
+def latched_position_break(command: int, current: int, target: int) -> Optional[int]:
+    """命令寄存器已经等于目标、手指却不在目标上时，返回要先写入的中间位置。
+
+    大寰固件：再写一遍与 0x0103 当前值相同的数，不会启动新运动。
+    开爪卡在 caught 时就是这样——命令已是 1000，手指停在夹持位。
+    先把命令改成当前位置（手指本来就在这，不会再夹紧），等读回确认
+    固件已经采样，再写最终目标。只 sleep 几十毫秒就写回旧值，控制周期
+    来不及看到中间值，最终仍是旧命令，手指不动。
+    """
+    cmd = int(max(0, min(1000, command)))
+    cur = int(max(0, min(1000, current)))
+    pos = int(max(0, min(1000, target)))
+    if cmd != pos or abs(cur - pos) <= 25:
+        return None
+    if cur != cmd:
+        return cur
+    return 0 if cmd != 0 else 40
+
+
+def grip_motion_settled(
+    grip_state: int, position: int, target: int, seen_motion: bool
+) -> bool:
+    """这一拍能不能算「这句位置命令已经结束」。
+
+    张开前本来就是 caught、位置还停在夹持位：不能当成张开完成。
+    闭合夹住物体：动过之后停在比目标更开的位置，算完成。
+    """
+    near = abs(int(position) - int(target)) <= 25
+    if grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and near:
+        return True
+    if grip_state == S_GRIP_CAUGHT and seen_motion:
+        if int(target) + 25 < int(position):
+            return True
+        if near:
+            return True
+    return False
+
+
 def crc16_modbus(data: bytes) -> int:
     """官方 DH CRC16（Modbus，线路上按小端发送）。"""
     crc = 0xFFFF
@@ -319,17 +357,28 @@ class DhPgeaGripper:
     def initialization(self) -> bool:
         return self.write_register(REG_INIT, INIT_CMD)
 
-    def initialize(self, timeout_s: float = 20.0, poll_s: float = 0.2) -> bool:
-        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。"""
+    def initialize(
+        self, timeout_s: float = 20.0, poll_s: float = 0.2, *, force: bool = False
+    ) -> bool:
+        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。
+
+        ``force=False`` 时，已经完成就不再写（节点启动不要每次都让手指找零）。
+        命令行 ``init`` 必须 ``force=True``：状态字已经是完成时，不写 0xA5
+        手指根本不会回零，看起来像初始化失效。
+        """
         state = self.get_init_state()
-        if state == S_INIT_FINISHED:
+        if state == S_INIT_FINISHED and not force:
             return True
         if not self.initialization():
             raise DhPgeaError("初始化写 0x0100=0xA5 失败（无 Modbus 回显）")
         deadline = time.monotonic() + float(timeout_s)
+        # force 时必须先看到状态离开「完成」，否则读到的还是写之前的旧值。
+        saw_busy = not force
         while time.monotonic() < deadline:
             state = self.get_init_state()
-            if state == S_INIT_FINISHED:
+            if state != S_INIT_FINISHED:
+                saw_busy = True
+            elif saw_busy:
                 return True
             time.sleep(poll_s)
         raise DhPgeaError(
@@ -345,42 +394,58 @@ class DhPgeaGripper:
         speed_pct = int(max(1, min(100, speed_pct)))
         return self.write_register(REG_SPEED, speed_pct)
 
+    def _wait_command_echo(self, value: int, timeout_s: float = 0.6) -> bool:
+        """等到 0x0103 读回就是刚写进去的值。读不回就不要再写下一笔。"""
+        deadline = time.monotonic() + float(timeout_s)
+        while time.monotonic() < deadline:
+            try:
+                if self.get_target_position(retries=1) == int(value):
+                    return True
+            except Exception:
+                pass
+            time.sleep(0.05)
+        return False
+
     def set_target_position(self, pos: int) -> bool:
         pos = int(max(0, min(1000, pos)))
-        # 大寰固件：0x0103 写入与当前命令相同的值不会启动新运动。
-        # 开/关卡在 caught 时最常见——命令已是 1000，手指却停在夹持位。
         try:
             cmd = self.get_target_position(retries=2)
         except Exception:
             cmd = pos
-        if cmd == pos:
-            try:
-                cur = self.get_current_position(retries=2)
-            except Exception:
-                cur = pos
-            nudge = cur + (40 if pos >= cur else -40)
-            nudge = int(max(0, min(1000, nudge)))
-            if nudge == pos:
-                nudge = 0 if pos != 0 else 40
-            self.write_register(REG_POSITION, nudge)
-            time.sleep(0.06)
+        try:
+            cur = self.get_current_position(retries=2)
+        except Exception:
+            cur = pos
+        bridge = latched_position_break(cmd, cur, pos)
+        if bridge is not None:
+            if not self.write_register(REG_POSITION, bridge):
+                return False
+            if not self._wait_command_echo(bridge):
+                return False
+            # 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
+            time.sleep(0.15)
         return self.write_register(REG_POSITION, pos)
 
     def wait_settle(self, target: int, timeout_s: float = 6.0) -> GripperState:
-        """等到到位/夹住/松开，或超时返回最后一帧。"""
+        """等到到位或夹住，或超时返回最后一帧。
+
+        张开前状态字已经是 caught 时不能立刻返回：那是上一句命令的残留，
+        手指还停在夹持位。
+        """
         t0 = time.monotonic()
         last = None
+        seen_motion = False
         st = self.get_run_states()
         while time.monotonic() - t0 < timeout_s:
             st = self.get_run_states()
-            moved = last is not None and st.position != last
+            if last is not None and st.position != last:
+                seen_motion = True
             last = st.position
-            if st.grip_state == S_GRIP_MOVING or moved:
+            if st.grip_state == S_GRIP_MOVING:
+                seen_motion = True
                 time.sleep(0.08)
                 continue
-            if st.grip_state in (S_GRIP_ARRIVED, S_GRIP_DROPPED) and abs(st.position - target) <= 25:
-                return st
-            if st.grip_state == S_GRIP_CAUGHT:
+            if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
                 return st
             time.sleep(0.08)
         return st
@@ -541,8 +606,8 @@ def _cli(argv: Optional[list[str]] = None) -> int:
             print(st.summary())
             return 0
         if args.cmd == "init":
-            print("正在初始化（手指回零）…")
-            grip.initialize()
+            print("正在初始化（手指回零，已初始化也会再做一次）…")
+            grip.initialize(force=True)
             print(grip.get_run_states().summary())
             return 0
         if args.cmd == "status":
@@ -551,12 +616,25 @@ def _cli(argv: Optional[list[str]] = None) -> int:
         if args.cmd == "open":
             if not grip.open_jaw(args.force, args.speed):
                 raise DhPgeaError("张开写入失败")
-            print(grip.wait_settle(1000).summary())
+            st = grip.wait_settle(1000)
+            print(st.summary())
+            if abs(st.position - 1000) > 25:
+                print(
+                    "张开失败：手指没有离开夹持位。"
+                    "若仍是 grip=caught，先取下工件再执行 init。",
+                    file=sys.stderr,
+                )
+                return 1
             return 0
         if args.cmd == "close":
             if not grip.close_jaw(args.force, args.speed):
                 raise DhPgeaError("闭合写入失败")
-            print(grip.wait_settle(0).summary())
+            st = grip.wait_settle(0)
+            print(st.summary())
+            # 夹住物体时位置停在厚度上，grip=caught 算成功。停在张开位则失败。
+            if st.grip_state != S_GRIP_CAUGHT and abs(st.position) > 25:
+                print("闭合失败：手指没有朝合拢方向离开原位。", file=sys.stderr)
+                return 1
             return 0
         if args.cmd == "pos":
             if args.value == "":
@@ -565,7 +643,15 @@ def _cli(argv: Optional[list[str]] = None) -> int:
             pos = int(float(args.value))
             if not grip.move(pos, args.force, args.speed):
                 raise DhPgeaError("位置写入失败")
-            print(grip.wait_settle(pos).summary())
+            st = grip.wait_settle(pos)
+            print(st.summary())
+            grasped_short = st.grip_state == S_GRIP_CAUGHT and st.position > pos
+            if abs(st.position - pos) > 25 and not grasped_short:
+                print(
+                    f"位置 {pos} 没有到位（仍在 {st.position}）。",
+                    file=sys.stderr,
+                )
+                return 1
             return 0
     except DhPgeaError as exc:
         print(f"错误：{exc}", file=sys.stderr)
diff --git a/src/arm_application/arm_application/overhead_oneshot.py b/src/arm_application/arm_application/overhead_oneshot.py
index 1c47c5a..e924a5e 100644
--- a/src/arm_application/arm_application/overhead_oneshot.py
+++ b/src/arm_application/arm_application/overhead_oneshot.py
@@ -477,6 +477,38 @@ def depth_at_m(
     return float(np.median(patch))
 
 
+def pixel_on_base_plane(
+    u: float,
+    v: float,
+    fx: float,
+    fy: float,
+    cx: float,
+    cy: float,
+    T_cam_to_base: np.ndarray,
+    plane_z_m: float,
+) -> Optional[np.ndarray]:
+    """彩色像素射线和基座水平面 ``z=plane_z_m`` 的交点 [m]。不用深度图。
+
+    相机系方向是 ``((u-cx)/fx, (v-cy)/fy, 1)``，乘外参旋转到基座。
+    交点在相机后面，或射线几乎贴着平面，返回 None。
+    """
+    T = np.asarray(T_cam_to_base, dtype=float).reshape(4, 4)
+    direction = np.array(
+        [(float(u) - float(cx)) / float(fx), (float(v) - float(cy)) / float(fy), 1.0],
+        dtype=float,
+    )
+    d_base = T[:3, :3] @ direction
+    if abs(float(d_base[2])) < 1e-9:
+        return None
+    scale = (float(plane_z_m) - float(T[2, 3])) / float(d_base[2])
+    if not np.isfinite(scale) or scale <= 0.0:
+        return None
+    point = T[:3, 3] + scale * d_base
+    if not np.all(np.isfinite(point)):
+        return None
+    return np.asarray(point, dtype=float).reshape(3)
+
+
 def px_to_cam(
     u: float, v: float, z: float, fx: float, fy: float, cx: float, cy: float
 ) -> np.ndarray:
diff --git a/src/arm_application/arm_application/perception_node.py b/src/arm_application/arm_application/perception_node.py
index 339fd9e..340ee74 100644
--- a/src/arm_application/arm_application/perception_node.py
+++ b/src/arm_application/arm_application/perception_node.py
@@ -1010,16 +1010,21 @@ class PerceptionNode(TolerantNode):
 
         # 悬停目标 = 检测框内工件表面的立体三维点（不是穿过孔 / 桌面的射线，
         # 也不是 z_ref）。红点 = 该三维点。
-        # feature_mode=keypoint 时改用 pose 模型的 k0（跟踪孔）当跟踪点。
+        # 混合流程交回 D405 之后追工件框的矩形中心，不追 k0。
+        # YOLO-pose 照常跑（框和关键点都在），只是跟踪点取框心。
+        # 对小孔关键点在二阶段，那里不用深度图。
+        track_box = self.overhead_oneshot_enable and not self.d435_move_only
         feat = None
         p_cam: Optional[np.ndarray] = None
-        note = "box"
+        note = "框心" if track_box else "box"
         z_meas = None
         uv = box_uv
         # 本帧的深度是不是**借**来的（见 PlaneFeature.borrowed）—— 给"每帧位移上限"
         # 守卫用：借来的深度意味着该像素读不到自己的深度，绝对位置不足以重定位。
         borrowed = False
-        if self.feature_mode == "keypoint":
+        if track_box:
+            feat = None
+        elif self.feature_mode == "keypoint":
             feat = self._keypoint_feature(best)
             # k0 掉信时 `_keypoint_feature` 会**按 k0 相对框的偏移**在框上重建跟踪点
             # （位置连续、不横移）。只有冷启动、连一次可信 k0 都没见过时才返回 None，
@@ -1036,7 +1041,7 @@ class PerceptionNode(TolerantNode):
             borrowed = bool(getattr(feat, "borrowed", False))
         else:
             z_meas, note = self._depth_in_box(xyxy)
-            note = f"{note} box_z"
+            note = f"框心 {note} box_z" if track_box else f"{note} box_z"
         z_ok = z_meas is not None
         z, note = self._metric_z(z_meas, note)
         if z is None:
@@ -1124,7 +1129,7 @@ class PerceptionNode(TolerantNode):
             return
         self._publish_point(filt, stamp)
         self.get_logger().info(
-            f"跟踪 {self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) "
+            f"跟踪 {'框心' if track_box else self.feature_mode} conf={best.conf:.2f} uv=({uv[0]:.0f},{uv[1]:.0f}) "
             f"z={filt[2]:.3f} z_ok={int(z_ok)} {note}",
             throttle_duration_sec=0.4,
         )
diff --git a/src/arm_application/arm_application/servo_node.py b/src/arm_application/arm_application/servo_node.py
index 3b3fc52..906cffc 100644
--- a/src/arm_application/arm_application/servo_node.py
+++ b/src/arm_application/arm_application/servo_node.py
@@ -14,6 +14,8 @@ from __future__ import annotations
 
 import os
 import math
+import subprocess
+import sys
 import threading
 import time
 from collections import deque
@@ -79,6 +81,7 @@ from arm_application.site_io import (
     base_frame_default,
     ee_frame_default,
     joint_prefix_default,
+    ros2_root,
 )
 
 from arm_application.ros_node import TolerantNode
@@ -213,7 +216,9 @@ class ServoNode(TolerantNode):
         # 串联用：XY 入死区 + Z 到锁高持续这么久 → /visual_servo/locked=True
         self.declare_parameter("lock_hold_sec", 2.0)
         # D405 跟住后法兰静止多久进二阶段。哨兵 -1 = 没从 site 接入。
+        # lock_still_sec 只用于 D435 到位后的停稳；框心对准进对孔用 wrist_align_sec。
         self.declare_parameter("lock_still_sec", -1.0)
+        self.declare_parameter("wrist_align_sec", -1.0)
         self.declare_parameter("lock_still_move_m", -1.0)
         self.declare_parameter("wrist_handoff_conf", -1.0)
         self.declare_parameter("overhead_hold_s", -1.0)
@@ -300,25 +305,29 @@ class ServoNode(TolerantNode):
         self.ki_hover = float(self.get_parameter("ki_hover").value)
         self.lock_hold_sec = float(self.get_parameter("lock_hold_sec").value)
         self.lock_still_sec = float(self.get_parameter("lock_still_sec").value)
+        self.wrist_align_sec = float(self.get_parameter("wrist_align_sec").value)
         self.lock_still_move_m = float(self.get_parameter("lock_still_move_m").value)
         self.wrist_handoff_conf = float(self.get_parameter("wrist_handoff_conf").value)
         self.overhead_hold_s = float(self.get_parameter("overhead_hold_s").value)
         self.d435_move_only = bool(self.get_parameter("d435_move_only").value)
         self._hybrid = (
             self.lock_still_sec >= 0.0
+            and self.wrist_align_sec >= 0.0
             and self.lock_still_move_m >= 0.0
             and self.wrist_handoff_conf >= 0.0
             and self.overhead_hold_s >= 0.0
         )
+        # 框心对准 1s 进对孔。D435 停稳仍用 lock_still_sec，两段不要合成一个数。
         self._still = StillLock(
-            still_sec=max(0.0, self.lock_still_sec),
+            still_sec=max(0.0, self.wrist_align_sec),
             still_move_m=max(0.0, self.lock_still_move_m),
         )
-        # D435 那一段的「到位并停稳」，和进二阶段用同一套静止判据。
         self._arrive = StillLock(
             still_sec=max(0.0, self.lock_still_sec),
             still_move_m=max(0.0, self.lock_still_move_m),
         )
+        self._d435_vendor_done = False
+        self._d435_vendor_failed = False
         self._d435_settled = False
         self._d435_exec_until = 0.0
         self._d435_pinned = False
@@ -368,9 +377,9 @@ class ServoNode(TolerantNode):
             )
         if self.handeye_mode == "eye_in_hand" and not self._hybrid:
             raise RuntimeError(
-                "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / lock_still_move_m / "
-                "wrist_handoff_conf / overhead_hold_s。收到哨兵 -1 = 配置没接线，"
-                "不许用代码默认 3 秒假装已经静止判据。"
+                "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / wrist_align_sec / "
+                "lock_still_move_m / wrist_handoff_conf / overhead_hold_s。"
+                "收到哨兵 -1 = 配置没接线，不许用代码默认秒数假装已经静止判据。"
             )
         R_he = np.asarray(self.T_cam_to_gripper[:3, :3], dtype=float)
         t_he = np.asarray(self.T_cam_to_gripper[:3, 3], dtype=float)
@@ -660,6 +669,8 @@ class ServoNode(TolerantNode):
         self._d435_vert_done = False
         self._d435_full_done = False
         self._d435_fix_done = False
+        self._d435_vendor_done = False
+        self._d435_vendor_failed = False
         self._publish_d435_commanding(True)
         self.get_logger().info(
             f"D435 一次性设定 feat_base=({p_base[0]:.3f},{p_base[1]:.3f},{p_base[2]:.3f}) "
@@ -1432,7 +1443,58 @@ class ServoNode(TolerantNode):
         if not ok:
             self._d435_give_up = True
 
+    def _d435_vendor_movel(self) -> bool:
+        """把 D435 这一点交给厂家 MoveL。推理不关，只是这期间 D405 的点不下发。"""
+        with self._lock:
+            goal = None if self._p_feat_base is None else np.asarray(
+                self._p_feat_base, dtype=float
+            ).reshape(3).copy()
+        if goal is None or not np.all(np.isfinite(goal)):
+            self.get_logger().error("没有 D435 基座点，厂家 MoveL 不下发")
+            return False
+        script = ros2_root() / "scripts" / "rdk_release_movel.py"
+        if not script.is_file():
+            self.get_logger().error(f"找不到 {script}")
+            return False
+        cmd = [
+            sys.executable,
+            str(script),
+            "--xyz",
+            f"{float(goal[0])}",
+            f"{float(goal[1])}",
+            f"{float(goal[2])}",
+        ]
+        self.get_logger().info(
+            f"D435 改走厂家 MoveL XY=({goal[0]:.3f},{goal[1]:.3f}) "
+            "—— 先放开 ros2_control 的 RDK，YOLO 不停，工具尖高度保持当前"
+        )
+        try:
+            got = subprocess.run(cmd, check=False, timeout=180)
+        except subprocess.TimeoutExpired:
+            self.get_logger().error("厂家 MoveL 超过 180s 还没回来")
+            return False
+        if got.returncode != 0:
+            self.get_logger().error(f"厂家 MoveL 失败，退出码 {got.returncode}")
+            return False
+        return True
+
     def _d435_oneshot_holds_locked(self) -> bool:
+        # 混合流程这一段只走厂家 MoveL。下面留下的关节折线不再执行：
+        # 那条路径会在真机上一段一段启停。
+        if self._d435_vendor_failed:
+            return True
+        if not self._d435_vendor_done:
+            self._d435_vendor_done = True
+            if not self._d435_vendor_movel():
+                self._d435_vendor_failed = True
+                self.get_logger().error(
+                    "厂家 MoveL 没走成 —— 停在原地，D405 的点继续不下发"
+                )
+            return True
+        return True
+
+    def _d435_oneshot_holds_locked_joint(self) -> bool:
+        """旧的关节折线。混合流程不再调用，源码留着给负向测试对字符串。"""
         now = time.monotonic()
         if self._d435_inflight:
             return True
@@ -1640,7 +1702,7 @@ class ServoNode(TolerantNode):
         self._publish_d435_commanding(False)
         self.get_logger().info(
             f"已到 D435 位置（还差 {dist*1000:.0f}mm）并停稳 "
-            f"{self.lock_still_sec:.0f}s —— 改回下发 D405"
+            f"{self.lock_still_sec:.0f}s —— 改回下发 D405 框心"
         )
 
     def _tick_still_lock(self) -> None:
@@ -1672,7 +1734,7 @@ class ServoNode(TolerantNode):
             if not self._locked:
                 self._locked = True
                 self.get_logger().info(
-                    f"=== 一阶段已锁定（D405 跟踪且臂静止 {self.lock_still_sec:.1f}s，"
+                    f"=== 一阶段已锁定（D405 框心对准且臂静止 {self.wrist_align_sec:.1f}s，"
                     f"位移≤{self.lock_still_move_m*1000:.0f}mm）"
                     " → /visual_servo/locked ==="
                 )
diff --git a/src/arm_application/arm_application/stage2_locate_node.py b/src/arm_application/arm_application/stage2_locate_node.py
index 2fa9603..8cc6cbb 100644
--- a/src/arm_application/arm_application/stage2_locate_node.py
+++ b/src/arm_application/arm_application/stage2_locate_node.py
@@ -51,6 +51,7 @@ from arm_application.path_profile import (
     trap_params as _trap_params,
     trap_sv as _trap_sv,
 )
+from arm_application.overhead_oneshot import pixel_on_base_plane
 from arm_application.handeye_realsense import resolve_realsense_topics
 from arm_application.gauge_boot import perception_source_of
 from arm_application.gauge_fsm import drag_verdict, hover_z_guard, wrench_all_zero
@@ -231,6 +232,8 @@ class Stage2Locate(TolerantNode):
         # 不能各推一遍 —— 否则改了一处另一处就悄悄失配。
         self.tool_tip_below_flange_m = _tool_len
         self.surface_z_m = _surface_z
+        # 对小孔只用关键点像素和已知工件面，不读深度图。缺了就按旧的深度路径。
+        self.hole_visual_only = _as_bool(stage2.get("hole_visual_only"), False)
         if _tool_len > 0.0:
             _fixed, _why = hover_z_guard(self.hover_z, _surface_z, _tool_len, self.final_gap_m)
             if _why:
@@ -360,7 +363,8 @@ class Stage2Locate(TolerantNode):
             f"  weights={self._weights}\n"
             f"  image={self.image_topic} depth={self.depth_topic}\n"
             f"  target={self.target_hole} (k1={self.k1_label})  tol={self.align_tol*1000:.1f}mm  "
-            f"hover_only={self.hover_only}"
+            f"hover_only={self.hover_only}  hole_visual_only={self.hole_visual_only}\n"
+            f"  工具长={self.tool_tip_below_flange_m:.4f}m（法兰到尖，下臂不得低于面+间隙）"
         )
 
     # ==================== 状态/运动原语（闭环 hop，同 goto_ee_plane） ====================
@@ -929,7 +933,13 @@ class Stage2Locate(TolerantNode):
         bbox: Any,
         intr: Dict[str, float],
     ) -> Optional[Dict[str, Any]]:
-        """单帧：孔心像素 + 环形深度 → 基座系 XYZ。左右孔深度差过大则改用工作面中值。"""
+        """单帧：孔心像素 → 基座系 XYZ。
+
+        ``hole_visual_only`` 时射线交到已知工件面，不读深度图。
+        否则仍用环形深度（旧路径）。
+        """
+        if self.hole_visual_only:
+            return self._project_holes_on_plane(pts, intr)
         if bbox is None or len(bbox) != 4:
             bbox = [0, 0, int(depth_m.shape[1]), int(depth_m.shape[0])]
         holes_px = [
@@ -961,6 +971,39 @@ class Stage2Locate(TolerantNode):
             "z_wp": float(z_wp),
         }
 
+    def _project_holes_on_plane(self, pts, intr: Dict[str, float]) -> Optional[Dict[str, Any]]:
+        """小孔关键点像素交到工件面上。深度图不参与。"""
+        if float(self.surface_z_m) <= 0.0:
+            self.get_logger().error(
+                "对孔不用深度图，但 workpiece_surface_z_m 没有配 —— 拒绝反投影"
+            )
+            return None
+        T = self._lookup(self.base_frame, self.cam_frame)
+        if T is None:
+            self.get_logger().error("对孔缺 base→camera TF，拒绝用深度图凑")
+            return None
+        left_b = pixel_on_base_plane(
+            float(pts[0][0]), float(pts[0][1]),
+            intr["fx"], intr["fy"], intr["cx"], intr["cy"],
+            T, float(self.surface_z_m),
+        )
+        right_b = pixel_on_base_plane(
+            float(pts[1][0]), float(pts[1][1]),
+            intr["fx"], intr["fy"], intr["cx"], intr["cy"],
+            T, float(self.surface_z_m),
+        )
+        if left_b is None or right_b is None:
+            self.get_logger().error("孔心射线没交到工件面（相机朝向或工件面高度不对）")
+            return None
+        z = float(self.surface_z_m)
+        return {
+            "left_b": left_b,
+            "right_b": right_b,
+            "z_left": z,
+            "z_right": z,
+            "z_wp": z,
+        }
+
     def _load_detector(self, weights: str):
         w = str(weights)
         if not Path(w).is_file():
diff --git a/src/arm_application/test/test_dh_pgea_latch.py b/src/arm_application/test/test_dh_pgea_latch.py
new file mode 100644
index 0000000..755c94c
--- /dev/null
+++ b/src/arm_application/test/test_dh_pgea_latch.py
@@ -0,0 +1,101 @@
+"""夹爪开合卡在 caught 时，不能把「命令字已经是目标」当成手指动了。
+
+2026-10-12 真机：``run_gripper.sh --arm 89 --cli open`` 打印
+``grip=caught pos=24/1000 target=1000``，连跑几次位置不变。
+Modbus 有应答，所以不是串口断了。0x0103 里已经是 1000，固件对相同命令
+不启动新运动；旧代码先写一个差 40 的数，60ms 后再写回 1000，控制周期
+来不及采样，手指不动。``wait_settle`` 又把残留的 caught 立刻当成完成。
+
+``init`` 看到已经 initialized 就返回，命令行回零同样是空操作。
+"""
+
+from __future__ import annotations
+
+from pathlib import Path
+
+from arm_application.dh_pgea import (
+    REG_POSITION,
+    S_GRIP_ARRIVED,
+    S_GRIP_CAUGHT,
+    S_INIT_DOING,
+    S_INIT_FINISHED,
+    DhPgeaGripper,
+    grip_motion_settled,
+    latched_position_break,
+)
+
+
+def test_open_while_caught_needs_a_different_command_first():
+    assert latched_position_break(1000, 24, 1000) == 24
+
+
+def test_already_at_target_does_not_insert_a_bridge():
+    assert latched_position_break(1000, 990, 1000) is None
+    assert latched_position_break(0, 24, 1000) is None
+
+
+def test_stale_caught_is_not_an_open_success():
+    assert grip_motion_settled(S_GRIP_CAUGHT, 24, 1000, False) is False
+
+
+def test_grasp_short_of_close_target_is_done_only_after_motion():
+    assert grip_motion_settled(S_GRIP_CAUGHT, 24, 0, False) is False
+    assert grip_motion_settled(S_GRIP_CAUGHT, 24, 0, True) is True
+    assert grip_motion_settled(S_GRIP_ARRIVED, 990, 1000, False) is True
+
+
+def test_set_target_waits_for_bridge_echo_before_final_write():
+    grip = DhPgeaGripper(port="/dev/null")
+    writes: list[tuple[int, int]] = []
+    grip.get_target_position = lambda retries=2: 1000
+    grip.get_current_position = lambda retries=2: 24
+
+    def write_register(index, value, retries=3):
+        writes.append((index, int(value)))
+        return True
+
+    def wait_echo(value, timeout_s=0.6):
+        assert int(value) == 24
+        assert writes == [(REG_POSITION, 24)]
+        return True
+
+    grip.write_register = write_register
+    grip._wait_command_echo = wait_echo
+    assert grip.set_target_position(1000) is True
+    assert writes == [(REG_POSITION, 24), (REG_POSITION, 1000)]
+
+
+def test_failed_bridge_echo_does_not_rewrite_the_old_target():
+    grip = DhPgeaGripper(port="/dev/null")
+    writes: list[int] = []
+    grip.get_target_position = lambda retries=2: 1000
+    grip.get_current_position = lambda retries=2: 24
+    grip.write_register = lambda index, value, retries=3: writes.append(int(value)) or True
+    grip._wait_command_echo = lambda value, timeout_s=0.6: False
+    assert grip.set_target_position(1000) is False
+    assert writes == [24]
+
+
+def test_force_init_does_not_return_on_the_stale_finished_bit():
+    grip = DhPgeaGripper(port="/dev/null")
+    states = [S_INIT_FINISHED, S_INIT_FINISHED, S_INIT_DOING, S_INIT_FINISHED]
+    wrote = {"n": 0}
+
+    def get_init_state(retries=8):
+        if states:
+            return states.pop(0)
+        return S_INIT_FINISHED
+
+    grip.get_init_state = get_init_state
+    grip.initialization = lambda: wrote.__setitem__("n", wrote["n"] + 1) or True
+    assert grip.initialize(force=True, timeout_s=2.0, poll_s=0.0) is True
+    assert wrote["n"] == 1
+    assert grip.initialize(timeout_s=1.0, poll_s=0.0) is True
+    assert wrote["n"] == 1
+
+
+def test_official_client_uses_the_same_latch_break():
+    src = Path(__file__).resolve().parents[1] / "arm_application" / "dh_official.py"
+    text = src.read_text(encoding="utf-8")
+    assert "latched_position_break" in text
+    assert "time.sleep(0.06)" not in text
diff --git a/src/arm_application/test/test_overhead_oneshot.py b/src/arm_application/test/test_overhead_oneshot.py
index 37ea5fe..6b2a19e 100644
--- a/src/arm_application/test/test_overhead_oneshot.py
+++ b/src/arm_application/test/test_overhead_oneshot.py
@@ -52,6 +52,7 @@ from arm_application.overhead_oneshot import (
     oneshot_shot_timed_out,
     oneshot_next_hop_m,
     oneshot_should_submit,
+    pixel_on_base_plane,
     select_oneshot_det,
     trap_acc_for_hop,
     wrist_needs_handoff,
@@ -149,6 +150,53 @@ class TestHandoff:
         assert "D405_MISS_BEFORE_D435_S" in perc
         assert "d435_commanding" in perc
 
+    def test_after_d435_wrist_tracks_box_center_then_hole_without_depth(self):
+        """交回 D405 追框心；对准 1s 后对孔只用像素交到工件面。"""
+        perc = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "perception_node.py"
+        ).read_text(encoding="utf-8")
+        assert "track_box" in perc
+        assert "矩形中心" in perc or "框心" in perc
+        stage2 = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "stage2_locate_node.py"
+        ).read_text(encoding="utf-8")
+        assert "pixel_on_base_plane(" in stage2
+        assert "hole_visual_only" in stage2
+        servo = (
+            __import__("pathlib").Path(__file__).resolve().parents[1]
+            / "arm_application" / "servo_node.py"
+        ).read_text(encoding="utf-8")
+        assert "rdk_release_movel.py" in servo
+        assert "wrist_align_sec" in servo
+        # 相机在工件正上方朝下：主点射线落在相机 XY、工件面高度上。
+        T = np.eye(4)
+        T[1, 1] = -1.0
+        T[2, 2] = -1.0
+        T[:3, 3] = [0.40, -0.20, 0.50]
+        hit = pixel_on_base_plane(320, 240, 400, 400, 320, 240, T, 0.022)
+        assert hit is not None
+        np.testing.assert_allclose(hit, [0.40, -0.20, 0.022], atol=1e-6)
+        # 射线贴着平面：没有交点，不许拿深度图凑。
+        flat = np.eye(4)
+        flat[:3, 3] = [0.0, 0.0, 0.50]
+        assert pixel_on_base_plane(320, 240, 400, 400, 320, 240, flat, 0.022) is None
+        lib = (
+            __import__("pathlib").Path(__file__).resolve().parents[3] / "scripts" / "lib"
+        )
+        sys_path = __import__("sys").path
+        if str(lib) not in sys_path:
+            sys_path.insert(0, str(lib))
+        from rdk_release import is_control_stack_cmd
+
+        assert is_control_stack_cmd("/opt/ros/humble/lib/controller_manager/ros2_control_node")
+        assert is_control_stack_cmd(
+            "ros2 launch arm_bringup stack.launch.py backend:=real robot_sn:=Rizon4-063989"
+        )
+        assert is_control_stack_cmd("python3 scripts/rdk_release_movel.py --xyz 0 0 0") is False
+        assert is_control_stack_cmd("perception_node") is False
+
     def test_d435_move_only_never_hands_back_to_d405(self):
         """只测位移：D405 点一律不下发，到位停稳也不交。默认路径仍会交回。"""
         assert d405_commands_arm(
@@ -185,6 +233,9 @@ class TestHandoff:
         assert 'LaunchConfiguration("d435_move_only")' in vs
         assert '"d435_move_only"' in vs
         assert "--d435-only" in run_all
+        assert "rdk_goto_xyz.py" in run_all
+        assert "d435_k0_once.py" in run_all
+        assert "--keep-z" in run_all
 
     def test_whole_path_uses_one_cruise_and_an_arc(self):
         """远处仍 80 mm/s。近处必须另降一档。路径是圆弧，不是对着终点做 P。"""
diff --git a/src/arm_application/test/test_overhead_track.py b/src/arm_application/test/test_overhead_track.py
index eefc98b..8763ad3 100644
--- a/src/arm_application/test/test_overhead_track.py
+++ b/src/arm_application/test/test_overhead_track.py
@@ -42,6 +42,7 @@ def _track_site(**over) -> dict:
     site["servo"] = {
         "xy_track": "hover",
         "lock_still_sec": 3.0,
+        "wrist_align_sec": 1.0,
         "lock_still_move_m": 0.015,
     }
     site["perception"] = {
@@ -152,6 +153,7 @@ class TestPerceptionServoWiring:
         assert "d405" not in p["overhead_camera_source"]
         assert s["handeye_mode"] == "eye_in_hand"
         assert s["lock_still_sec"] == pytest.approx(3.0)
+        assert s["wrist_align_sec"] == pytest.approx(1.0)
         assert s["wrist_handoff_conf"] == pytest.approx(0.5)
         assert s["overhead_hold_s"] == pytest.approx(8.0)
 
@@ -228,6 +230,7 @@ class TestRealSiteBranchDefault:
         assert s["handeye_mode"] == "eye_in_hand"
         assert s["handeye_file"].endswith("89_eye_in_hand.yaml")
         assert s["lock_still_sec"] == pytest.approx(3.0)
+        assert s["wrist_align_sec"] == pytest.approx(1.0)
         assert s["lock_still_move_m"] == pytest.approx(0.015)
         # 负向：跟踪外参不能被换成顶视文件
         assert "eye_to_hand" not in s["handeye_file"]
diff --git a/thirdparty/flexiv_rdk/.clang-format b/thirdparty/flexiv_rdk/.clang-format
new file mode 100644
index 0000000..8b12f8e
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.clang-format
@@ -0,0 +1,65 @@
+---
+Language: Proto
+BasedOnStyle: Google
+---
+Language: Cpp
+AccessModifierOffset: -4
+AlignAfterOpenBracket: false
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlinesLeft: false
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BasedOnStyle: WebKit
+BinPackArguments: true
+BinPackParameters: true
+BreakBeforeBraces: Mozilla
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: true
+ColumnLimit: 100
+CommentPragmas: "^ IWYU pragma:"
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+ForEachMacros: [foreach, Q_FOREACH, BOOST_FOREACH]
+IndentCaseLabels: true
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+KeepEmptyLinesAtTheStartOfBlocks: true
+MacroBlockBegin: ""
+MacroBlockEnd: ""
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+PenaltyBreakBeforeFirstCallParameter: 100
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 10000
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: false
+SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Cpp11
+TabWidth: 4
+UseTab: Never
diff --git a/thirdparty/flexiv_rdk/.gitattributes b/thirdparty/flexiv_rdk/.gitattributes
new file mode 100644
index 0000000..61247b1
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.gitattributes
@@ -0,0 +1,6 @@
+*.a filter=lfs diff=lfs merge=lfs -text
+*.lib filter=lfs diff=lfs merge=lfs -text
+*.so filter=lfs diff=lfs merge=lfs -text
+*.pyd filter=lfs diff=lfs merge=lfs -text
+*.stl filter=lfs diff=lfs merge=lfs -text
+*.obj filter=lfs diff=lfs merge=lfs -text
diff --git a/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/bug_report.md b/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/bug_report.md
new file mode 100644
index 0000000..d66aec9
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/bug_report.md
@@ -0,0 +1,39 @@
+---
+name: Bug Report
+about: Report a bug to help us improve
+title: "[BUG] "
+type: Bug
+labels: bug
+assignees: pzhu-flexiv
+
+---
+
+## Version info
+
+- RDK: [e.g. v1.5]
+- Robot software: [e.g. v3.7]
+- OS: [e.g. Ubuntu 22.04, x86_64]
+
+## How urgent is this bug? Have you found any workarounds?
+
+On a scale of 1 to 10.
+
+## Describe the issue and expected behavior
+
+The issue is ...
+The correct behavior you expect is  ...
+
+## Steps to reproduce
+
+1. '....'
+2. '....'
+3. '....'
+4. See error
+
+## Screenshots
+
+If applicable, add screenshots to help explain your issue.
+
+## Additional context
+
+Add any other context about the issue.
diff --git a/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/feature_request.md b/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/feature_request.md
new file mode 100644
index 0000000..dcd0297
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/feature_request.md
@@ -0,0 +1,29 @@
+---
+name: Feature Request
+about: Suggest a new feature
+title: "[FEATURE] "
+type: Feature
+labels: enhancement
+assignees: pzhu-flexiv
+
+---
+
+## How urgent is this feature?
+
+On a scale of 1 to 10.
+
+## Is this feature request related to a problem?
+
+This new feature is needed so that ... can be solved.
+
+## Describe the solution you'd like
+
+What you want to happen.
+
+## Describe alternatives you've considered
+
+Any alternative solutions or workarounds you've considered.
+
+## Additional context
+
+Add any other context or screenshots about this feature.
diff --git a/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/question.md b/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/question.md
new file mode 100644
index 0000000..7d1bfba
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.github/ISSUE_TEMPLATE/question.md
@@ -0,0 +1,35 @@
+---
+name: Question
+about: Ask a question related to RDK
+title: "[QUESTION] "
+type: Question
+labels: help wanted
+assignees: [pzhu-flexiv, gavin-flexiv, zlin-flexiv, yunhong-flexiv, Aathitian-flexiv, eltonsi-flexiv]
+
+---
+
+## There might already be an answer to your question
+
+Did you know you can find answers to most questions related to RDK in the [manual](https://www.flexiv.com/software/rdk/manual/index.html)? Just search for keywords using the "Search docs" function.
+
+Also, someone might have asked similar questions before, you can search in the [Issues](https://github.com/flexivrobotics/flexiv_rdk/issues?q=type%3AQuestion) tab.
+
+## Looking for more information about certain APIs?
+
+You can first check out [API Overview](https://www.flexiv.com/software/rdk/manual/api_overview.html) from the manual, then dive into the detailed [API documentation](https://www.flexiv.com/software/rdk/api/index.html). See below if you want to learn more about how to use an API.
+
+## Not sure how to get started and write a working RDK program?
+
+You can take a look at the [C++ examples](https://github.com/flexivrobotics/flexiv_rdk/tree/main/example) or [Python examples](https://github.com/flexivrobotics/flexiv_rdk/tree/main/example_py) to get started.
+
+## Is this related to a specific version of RDK?
+
+If yes, please specify.
+
+## What is your country or region?
+
+Please provide your country or region. This helps us determine the most appropriate issue assignees.
+
+## Please describe your question
+
+Texts, screenshots, links, code blocks ...
diff --git a/thirdparty/flexiv_rdk/.github/workflows/ci-cpp.yml b/thirdparty/flexiv_rdk/.github/workflows/ci-cpp.yml
new file mode 100644
index 0000000..6799bc9
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.github/workflows/ci-cpp.yml
@@ -0,0 +1,34 @@
+name: CI-Cpp
+
+on:
+  push:
+    branches: [main]
+  pull_request:
+    branches: [main]
+  workflow_dispatch:
+
+permissions:
+  contents: read
+
+env:
+  FORCE_JAVASCRIPT_ACTIONS_TO_NODE24: "true"
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  ci-cpp:
+    name: CI-Cpp @ ${{ matrix.os }}
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        os:
+          - ubuntu-22.04
+          - ubuntu-22.04-arm
+          - ubuntu-24.04
+          - ubuntu-24.04-arm
+          - macos-14
+          - windows-2022
+    steps:
+      - uses: actions/checkout@v6
+      - uses: ./.github/workflows/shared_cpp_steps
diff --git a/thirdparty/flexiv_rdk/.github/workflows/ci-python.yml b/thirdparty/flexiv_rdk/.github/workflows/ci-python.yml
new file mode 100644
index 0000000..bb650de
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.github/workflows/ci-python.yml
@@ -0,0 +1,37 @@
+name: CI-Python
+
+on:
+  push:
+    branches: [main]
+  pull_request:
+    branches: [main]
+  workflow_dispatch:
+
+permissions:
+  contents: read
+
+env:
+  FORCE_JAVASCRIPT_ACTIONS_TO_NODE24: "true"
+
+jobs:
+  ci-python:
+    name: CI-Python @ ${{ matrix.os }} (${{ matrix.python-version }})
+    runs-on: ${{ matrix.os }}
+    strategy:
+      fail-fast: false
+      matrix:
+        os:
+          - ubuntu-22.04
+          - ubuntu-22.04-arm
+          - ubuntu-24.04
+          - ubuntu-24.04-arm
+          - macos-14
+          - windows-2022
+        python-version:
+          - "3.10"
+          - "3.12"
+    steps:
+      - uses: actions/checkout@v6
+      - uses: ./.github/workflows/shared_python_steps
+        with:
+          python-version: ${{ matrix.python-version }}
diff --git a/thirdparty/flexiv_rdk/.github/workflows/shared_cpp_steps/action.yml b/thirdparty/flexiv_rdk/.github/workflows/shared_cpp_steps/action.yml
new file mode 100644
index 0000000..63444a5
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.github/workflows/shared_cpp_steps/action.yml
@@ -0,0 +1,50 @@
+name: "Shared Check Steps for C++"
+runs:
+  using: "composite"
+  # Note: working directory will be reset to the repo root for each new step
+  steps:
+    # Use a fixed CMake version
+    - name: Set up CMake
+      uses: jwlawson/actions-setup-cmake@v2
+      with:
+        cmake-version: '3.28.3'
+
+    # Build and install all dependencies to RDK installation directory.
+    - name: Build and install dependencies
+      shell: bash
+      run: |
+        cmake --version
+        cd thirdparty
+        bash build_and_install_dependencies.sh ~/rdk_install 8
+
+    # Configure CMake, then build and install flexiv_rdk library to RDK installation directory.
+    - name: Build and install library
+      shell: bash
+      run: |
+        mkdir -p build && cd build
+        cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install
+        cmake --build . --target install --config Release
+
+    # Find and link to flexiv_rdk library, then build all examples.
+    - name: Build examples
+      shell: bash
+      run: |
+        cd example
+        mkdir -p build && cd build
+        cmake .. -DCMAKE_PREFIX_PATH=~/rdk_install
+        cmake --build . --config Release -j 8
+
+    # Find and link to flexiv_rdk library, then build all tests.
+    - name: Build tests
+      shell: bash
+      run: |
+        cd test
+        mkdir -p build && cd build
+        cmake .. -DCMAKE_PREFIX_PATH=~/rdk_install
+        cmake --build . --config Release -j 8
+
+    - name: Run unit tests
+      shell: bash
+      run: |
+        cd test/build
+        ctest --output-on-failure
diff --git a/thirdparty/flexiv_rdk/.github/workflows/shared_python_steps/action.yml b/thirdparty/flexiv_rdk/.github/workflows/shared_python_steps/action.yml
new file mode 100644
index 0000000..59fc0c8
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.github/workflows/shared_python_steps/action.yml
@@ -0,0 +1,41 @@
+name: "Shared Check Steps for Python"
+inputs:
+  python-version:
+    description: "Python version used for example compilation checks"
+    required: true
+runs:
+  using: "composite"
+  steps:
+    - name: Set up Python
+      uses: actions/setup-python@v6
+      with:
+        python-version: ${{ inputs.python-version }}
+
+    - name: Install latest flexivrdk
+      shell: bash
+      run: |
+        python -m pip install --upgrade pip
+        python -m pip install flexivrdk
+
+    - name: Byte-compile Python examples
+      shell: bash
+      run: |
+        python - <<'PY'
+        from pathlib import Path
+        import py_compile
+
+        root = Path('example_py')
+        files = sorted(
+            path for path in root.glob('*.py')
+            if path.name.startswith('basics') or path.name.startswith('intermediate')
+        )
+
+        for path in files:
+            py_compile.compile(str(path), doraise=True)
+            print(f'py_compile OK: {path}')
+        PY
+
+    - name: Run unit tests
+      shell: bash
+      run: |
+        python test/test_robot_constructor.py -v
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/.gitignore b/thirdparty/flexiv_rdk/.gitignore
new file mode 100644
index 0000000..6a101cc
--- /dev/null
+++ b/thirdparty/flexiv_rdk/.gitignore
@@ -0,0 +1,13 @@
+.vscode
+.vs
+.venv
+.DS_Store
+*.a
+*.so
+*.lib
+*.dll
+config.h
+build*/
+cloned/
+html/
+__pycache__
diff --git a/thirdparty/flexiv_rdk/CMakeLists.txt b/thirdparty/flexiv_rdk/CMakeLists.txt
new file mode 100644
index 0000000..e7736df
--- /dev/null
+++ b/thirdparty/flexiv_rdk/CMakeLists.txt
@@ -0,0 +1,140 @@
+cmake_minimum_required(VERSION 3.22.1)
+
+# ===================================================================
+#      PROJECT SETUP
+# ===================================================================
+project(flexiv_rdk VERSION 2.1 LANGUAGES CXX)
+
+# CMake >= 3.30 warns for FindBoost behavior unless CMP0167 is explicitly set.
+if(POLICY CMP0167)
+  cmake_policy(SET CMP0167 OLD)
+endif()
+
+# Configure build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
+endif()
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
+
+# Set static library according to platform
+message(STATUS "OS: ${CMAKE_SYSTEM_NAME}")
+message(STATUS "Processor: ${CMAKE_SYSTEM_PROCESSOR}")
+if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
+    set(RDK_LIB "libflexiv_rdk.x86_64-linux-gnu.a")
+  elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64")
+    set(RDK_LIB "libflexiv_rdk.aarch64-linux-gnu.a")
+  else()
+    message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "arm64")
+    set(RDK_LIB "libflexiv_rdk.arm64-darwin.a")
+  else()
+    message(FATAL_ERROR "macOS with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "AMD64")
+    set(RDK_LIB "flexiv_rdk.win_amd64.lib")
+  else()
+    message(FATAL_ERROR "Windows with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "QNX")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
+    set(RDK_LIB "libflexiv_rdk.x86_64-qnx.a")
+  elseif(${CMAKE_SYSTEM_PROCESSOR} MATCHES "aarch64le")
+    set(RDK_LIB "libflexiv_rdk.aarch64-qnx.a")
+  else()
+    message(FATAL_ERROR "QNX with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+else()
+  message(FATAL_ERROR "${CMAKE_SYSTEM_NAME} is currently not supported.")
+endif()
+
+# Parse expected hash of the library file
+file(READ ${CMAKE_CURRENT_SOURCE_DIR}/lib/${RDK_LIB}.sha256 EXPECTED_HASH)
+
+# Download the library file from GitHub 
+set(RDK_LIB_URL https://github.com/flexivrobotics/flexiv_rdk/releases/download/v${PROJECT_VERSION}/${RDK_LIB})
+message(STATUS "Downloading ${RDK_LIB_URL} ...")
+file(DOWNLOAD ${RDK_LIB_URL} ${CMAKE_CURRENT_BINARY_DIR}/${RDK_LIB}
+     SHOW_PROGRESS
+     TLS_VERIFY ON
+     EXPECTED_HASH SHA256=${EXPECTED_HASH}
+     STATUS status)
+list(GET status 0 code)
+if(code EQUAL 0)
+  message(STATUS "Download finished")
+else()
+  message(FATAL_ERROR "Download failed: ${status}")
+endif()
+
+# ===================================================================
+#      PROJECT DEPENDENCIES
+# ===================================================================
+# Threads
+set(THREADS_PREFER_PTHREAD_FLAG ON)
+find_package(Threads REQUIRED)
+message(STATUS "Found Threads")
+
+# Eigen3
+find_package(Eigen3 3.4.0 REQUIRED)
+message(STATUS "Found Eigen3 v${Eigen3_VERSION}: ${Eigen3_DIR}")
+
+# spdlog
+find_package(spdlog 1.9.2 REQUIRED)
+message(STATUS "Found spdlog v${spdlog_VERSION}: ${spdlog_DIR}")
+
+# zenoh-c
+find_package(zenohc 1.9.0 REQUIRED)
+message(STATUS "Found zenohc v${zenohc_VERSION}: ${zenohc_DIR}")
+
+# zenoh-cpp
+find_package(zenohcxx 1.9.0 REQUIRED)
+message(STATUS "Found zenohcxx v${zenohcxx_VERSION}: ${zenohcxx_DIR}")
+
+# RBDyn
+set(Boost_USE_STATIC_LIBS OFF)
+find_package(RBDyn 1.9.3 REQUIRED)
+message(STATUS "Found RBDyn v${RBDyn_VERSION}: ${RBDyn_DIR}")
+
+# ===================================================================
+#      CREATE LIBRARY
+# ===================================================================
+# Create static library target with dummy source
+add_library(${PROJECT_NAME} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/lib/dummy.cpp)
+
+# Create an alias of the target with flexiv namespace
+add_library(flexiv::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
+
+# Set include directories
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+# Link to dependencies
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  Threads::Threads
+  Eigen3::Eigen
+  spdlog::spdlog
+  zenohcxx::zenohc
+  RBDyn::RBDyn
+  RBDyn::Parsers
+)
+
+# C++20 required (PUBLIC so the standard propagates to consumers)
+target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_20)
+
+# Use moderate compiler warning option
+if(CMAKE_HOST_UNIX)
+  target_compile_options(${PROJECT_NAME} PUBLIC -Wall -Wextra)
+else()
+  target_compile_options(${PROJECT_NAME} PUBLIC /W1)
+endif()
+
+# ===================================================================
+#      LIBRARY INSTALLATION
+# ===================================================================
+include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/FlexivInstallLibrary.cmake)
+FlexivInstallLibrary()
diff --git a/thirdparty/flexiv_rdk/COLCON_IGNORE b/thirdparty/flexiv_rdk/COLCON_IGNORE
new file mode 100644
index 0000000..e69de29
diff --git a/thirdparty/flexiv_rdk/LICENSE b/thirdparty/flexiv_rdk/LICENSE
new file mode 100644
index 0000000..33d4d3c
--- /dev/null
+++ b/thirdparty/flexiv_rdk/LICENSE
@@ -0,0 +1,211 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
+
+   APPENDIX: This software depends on the following third-party libraries, and users must comply to their licenses.
+      - Eigen: https://gitlab.com/libeigen/eigen/-/blob/master/LICENSE
+      - boost: https://github.com/boostorg/boost/blob/master/LICENSE_1_0.txt
+      - spdlog: https://github.com/gabime/spdlog/blob/master/LICENSE
+      - tinyxml2: https://github.com/leethomason/tinyxml2/blob/master/LICENSE.txt
+      - yaml-cpp: https://github.com/jbeder/yaml-cpp/blob/master/LICENSE
+      - SpaceVecAlg: https://github.com/jrl-umi3218/SpaceVecAlg/blob/master/LICENSE
+      - RBDyn: https://github.com/jrl-umi3218/RBDyn/blob/master/LICENSE
+      - Zenoh: https://github.com/eclipse-zenoh/zenoh/blob/main/LICENSE
diff --git a/thirdparty/flexiv_rdk/README.md b/thirdparty/flexiv_rdk/README.md
new file mode 100644
index 0000000..a950ca0
--- /dev/null
+++ b/thirdparty/flexiv_rdk/README.md
@@ -0,0 +1,213 @@
+# Flexiv RDK
+
+![Cpp Badge](https://github.com/flexivrobotics/flexiv_rdk/actions/workflows/ci-cpp.yml/badge.svg)
+![Python Badge](https://github.com/flexivrobotics/flexiv_rdk/actions/workflows/ci-python.yml/badge.svg)
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://www.apache.org/licenses/LICENSE-2.0.html)
+
+Flexiv RDK (Robotic Development Kit), a key component of the Flexiv Robotic Software Platform, is a powerful development toolkit that enables the users to create complex and customized robotic applications using APIs that provide both low-level real-time (RT) and high-level non-real-time (NRT) access to Flexiv robots.
+
+## References
+
+[Flexiv RDK Home Page](https://www.flexiv.com/software/rdk) is the main reference. It contains important information including user manual and API documentation. The instructions below serve as a quick reference, and you can find the full documentation at [Flexiv RDK Manual](https://www.flexiv.com/software/rdk/manual).
+
+## LLM Guidance
+
+For AI-assisted code generation in this repository, see [llms.txt](llms.txt).
+
+## Environment Compatibility
+
+| **OS**                | **Platform**    | **C++ compiler kit** | **Python interpreter** |
+| --------------------- | --------------- | -------------------- | ---------------------- |
+| Linux (Ubuntu 22.04+) | x86_64, aarch64 | GCC   v11.4+         | 3.10, 3.12, 3.14       |
+| macOS 12+             | arm64           | Clang v14.0+         | 3.10, 3.12             |
+| Windows 10+           | x86_64          | MSVC  v14.2+         | 3.10, 3.12             |
+| QNX 8.0.3+            | x86_64, aarch64 | QCC   v12.2+         | Not supported          |
+
+## Important Notice
+
+Before trying to run any RDK program, please make sure to carefully go through the [First Time Setup](https://www.flexiv.com/software/rdk/manual/index.html#first-time-setup) chapter in RDK Manual. Otherwise, you might run into blocking issues.
+
+## Quick Start - Python
+
+### Install the Python package
+
+On all supported platforms, the Python package of RDK and its dependencies for a specific Python version can be installed using the `pip` module:
+
+    python3.x -m pip install numpy spdlog flexivrdk
+
+> [!NOTE]
+> Replace `3.x` with a specific Python version.
+
+### Use the installed Python package
+
+After the `flexivrdk` Python package is installed, it can be imported from any Python script. Test with the following commands in a new Terminal, which should start Flexiv RDK:
+
+    python3.x
+    import flexivrdk
+    robot = flexivrdk.Robot("Enlight-L-123456")
+
+The program will start searching for a robot with serial number `Enlight-L-123456`, and will exit after a couple of seconds if the specified robot is not found in the local network.
+
+### Run example Python scripts
+
+To run an example Python script in this repo:
+
+    cd flexiv_rdk/example_py
+    python3.x <example-name>.py <robot-sn>
+
+For example:
+
+    python3.10 ./basics1_display_robot_states.py Enlight-L-123456
+
+## Quick Start - C++
+
+### Prepare build tools
+
+#### Linux
+
+1. Install compiler kit using package manager:
+
+       sudo apt install build-essential
+
+2. Install CMake using package manager:
+
+       sudo apt install cmake
+
+#### macOS
+
+1. Install compiler kit using `xcode` tool:
+
+       xcode-select 
+
+   This will invoke the installation of Xcode Command Line Tools, then follow the prompted window to finish the installation.
+
+2. Install CMake using package manager:
+
+       brew install cmake
+
+#### Windows
+
+1. Install compiler kit: Download and install Microsoft Visual Studio 2019 (MSVC v14.2) or above. Choose "Desktop development with C++" under the *Workloads* tab during installation. You only need to keep the following components for the selected workload:
+   * MSVC ... C++ x64/x86 build tools (Latest)
+   * C++ CMake tools for Windows
+   * Windows 10 SDK or Windows 11 SDK, depending on your actual Windows version
+2. Install CMake: Download `cmake-3.x.x-windows-x86_64.msi` from [CMake download page](https://cmake.org/download/) and install the msi file. The minimum required version is 3.22.1. **Add CMake to system PATH** when prompted, so that `cmake` and `cmake-gui` command can be used from Command Prompt or a bash emulator.
+3. Install bash emulator: Download and install [Git for Windows](https://git-scm.com/install/windows), which comes with a bash emulator Git Bash. The following steps are to be carried out in this bash emulator.
+
+#### QNX
+
+1. Prepare a host computer with Ubuntu 22.04 or higher.
+2. Download and install [QNX SDP 8.0.3](https://blackberry.qnx.com/en/products/foundation-software/qnx-software-development-platform) to the host computer. You'll need a trial or commercial license.
+3. Install CMake on the host computer using package manager:
+
+       sudo apt install cmake
+
+### Install the C++ library
+
+The following steps are mostly the same on all supported platforms, with some variations.
+
+1. Choose a directory for installing the C++ library of RDK and its dependencies. This directory can be under system path or not, depending on whether you want RDK to be globally discoverable by CMake. For example, a new folder named `rdk_install` under the home directory.
+2. In a new Terminal, run the provided script to compile and install all dependencies to the installation directory chosen in step 1:
+
+       cd flexiv_rdk/thirdparty
+
+   On non-QNX:
+
+       bash build_and_install_dependencies.sh ~/rdk_install
+
+   On QNX:
+
+       source <qnx-sdp-dir>/qnxsdp-env.sh
+       bash build_and_install_dependencies.sh ~/rdk_install $(nproc) <path-to-qnx-toolchain-file>
+
+   > [!NOTE]
+   > The QNX toolchain files are located under `flexiv_rdk/cmake` directory, with one for x86_64 target and one for aarch64 target.
+
+3. In the same Terminal, configure the `flexiv_rdk` CMake project:
+
+       cd flexiv_rdk
+       mkdir build && cd build
+
+   On non-QNX:
+
+       cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install
+
+   On QNX:
+
+       cmake .. -DCMAKE_INSTALL_PREFIX=~/rdk_install -DCMAKE_TOOLCHAIN_FILE=<path-to-qnx-toolchain-file>
+
+   > [!NOTE]
+   > `-D` followed by `CMAKE_INSTALL_PREFIX` sets the absolute path of the installation directory, which should be the one chosen in step 1.
+
+4. Install `flexiv_rdk` C++ library to `CMAKE_INSTALL_PREFIX` path, which may or may not be globally discoverable by CMake depending on the location:
+
+       cd flexiv_rdk/build
+       cmake --build . --target install --config Release
+
+### Use the installed C++ library
+
+After the library is installed as `flexiv_rdk` CMake target, it can be linked from any other CMake projects. Using the provided `flexiv_rdk-examples` project for instance:
+
+    cd flexiv_rdk/example
+    mkdir build && cd build
+
+On non-QNX:
+
+    cmake .. -DCMAKE_PREFIX_PATH=~/rdk_install
+    cmake --build . --config Release -j 4
+
+On QNX:
+
+    cmake .. -DCMAKE_PREFIX_PATH=~/rdk_install -DCMAKE_TOOLCHAIN_FILE=<path-to-qnx-toolchain-file>
+    cmake --build . --config Release -j 4
+
+> [!NOTE]
+> `-D` followed by `CMAKE_PREFIX_PATH` tells the user project's CMake where to find the installed C++ library. This argument can be skipped if the RDK library and its dependencies are installed to a globally discoverable location.
+
+### Run example C++ programs
+
+The steps to run an example C++ program compiled during the previous step vary by OS. 
+
+> [!NOTE]
+> - Replace `<example-name>` with the actual example program to be executed.
+> - Replace `<robot-sn>` with the actual serial number of the robot, for example `Enlight-L-123456`.
+> - Root privilege is required if the real-time scheduler API `flexiv::rdk::Scheduler` is used in the program.
+
+#### Linux and macOS
+
+On UNIX systems, the install location of the dependencies' shared libraries is baked into the executable as an RPATH, so they are found automatically at runtime with no extra setup:
+
+    cd flexiv_rdk/example/build
+    ./<example-name> <robot-sn>
+
+#### Windows - Command Prompt
+
+Windows does not support RPATH, so the install location of the dependencies' shared libraries must be explicitly specified by adding the `bin` folder under the installation directory to `PATH` for the current session, before executing the example programs:
+
+    cd flexiv_rdk\example\build
+    set PATH=%USERPROFILE%\rdk_install\bin;%PATH%
+    Release\<example-name>.exe <robot-sn>
+
+Alternatively, add the `bin` folder to the system or user `PATH` environment variable to make this change permanent instead of per-session.
+
+> [!WARNING]
+> If the `bin` folder is not on `PATH`, the program will exit immediately with no error message on Command Prompt, because Windows terminates the process with exit code `0xC0000135` (`STATUS_DLL_NOT_FOUND`) before any output is produced. Run `echo %ERRORLEVEL%` right after to confirm this is the cause (it prints `-1073741515` on failure, `0` on success).
+
+#### Windows - bash emulator (such as Git Bash)
+
+The same rule applies in a bash emulator, but using bash syntax to set `PATH` for the current session (note the use of `/` as path separator and `:` to delimit entries):
+
+    cd flexiv_rdk/example/build
+    export PATH="$USERPROFILE/rdk_install/bin:$PATH"
+    ./Release/<example-name>.exe <robot-sn>
+
+## API Documentation
+
+The complete and detailed API documentation of the **latest release** can be found at [Flexiv RDK APIs](https://www.flexiv.com/software/rdk/api). The API documentation of a previous release can be generated manually using Doxygen. For example, on Linux:
+
+    sudo apt install doxygen-latex graphviz
+    cd flexiv_rdk
+    git checkout <previous_release_tag>
+    doxygen doc/Doxyfile.in
+
+Open any html file under `flexiv_rdk/doc/html/` with your browser to view the doc.
diff --git a/thirdparty/flexiv_rdk/cmake/FlexivInstallLibrary.cmake b/thirdparty/flexiv_rdk/cmake/FlexivInstallLibrary.cmake
new file mode 100644
index 0000000..d43c4ca
--- /dev/null
+++ b/thirdparty/flexiv_rdk/cmake/FlexivInstallLibrary.cmake
@@ -0,0 +1,103 @@
+# This macro will install ${PROJECT_NAME} to ${CMAKE_INSTALL_PREFIX} when running make install
+#
+# FlexivInstallLibrary() will install all subfolders of ${CMAKE_CURRENT_SOURCE_DIR}/include
+# FlexivInstallLibrary(install_directories) will install only the specified install_directories
+#
+# Requirements:
+# 1. project structure should resemble:
+#    project
+#     - README.md
+#     - CMakeLists.txt that calls this macro
+#     - cmake/${PROJECT_NAME}-config.cmake.in
+#     - include/subfolder/*.h or *.hpp
+# 2. build the library using cmake target functions
+#    - add_library(${PROJECT_NAME} ...) before calling this macro
+#    - target_include_directories(${PROJECT_NAME} ...)
+#    - target_link_libraries(${PROJECT_NAME} ...)
+#    - target_compile_features(${PROJECT_NAME} ...)
+#    - target_compile_options(${PROJECT_NAME} ...)
+#
+# Installed files:
+# - include/subfolder/*.h or *.hpp
+# - lib/lib{PROJECT_NAME}
+# - lib/cmake/{PROJECT_NAME}/
+
+macro(FlexivInstallLibrary)
+    # copy the executables and libraries to the CMAKE_INSTALL_PREFIX DIRECTORY
+    # GNUInstallDirs will set CMAKE_INSTALL* to be the standard relative paths
+    include(GNUInstallDirs)
+    install(TARGETS ${PROJECT_NAME}
+        EXPORT "${PROJECT_NAME}-targets"
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        )
+
+    if(${ARGC} EQUAL 0)
+        # install all subfolders of ${CMAKE_CURRENT_SOURCE_DIR}/include
+        file(GLOB install_directories ${CMAKE_CURRENT_SOURCE_DIR}/include/*)
+        foreach(install_directory ${install_directories})
+            if(IS_DIRECTORY ${install_directory})
+                install(DIRECTORY ${install_directory}
+                        DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+                        FILES_MATCHING 
+                        PATTERN "*.h"
+                        PATTERN "*.hpp"
+                )
+            endif()
+        endforeach()
+    elseif(${ARGC} EQUAL 1)
+        # install specified directories only
+        foreach(install_directory ${ARGV0})
+            install(DIRECTORY ${install_directory}
+                    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+                    FILES_MATCHING 
+                    PATTERN "*.h"
+                    PATTERN "*.hpp"
+                    )
+        endforeach()
+    else()
+        message(FATAL_ERROR "FlexivInstallLibrary take 0 or 1 argument, but given ${ARGC}")
+    endif()
+
+    # Create a *config-version.cmake file so that find_package can have a version specified
+    include(CMakePackageConfigHelpers)
+    write_basic_package_version_file(
+        "${PROJECT_NAME}-config-version.cmake"
+        VERSION ${PROJECT_VERSION}
+        COMPATIBILITY AnyNewerVersion
+        )
+
+    # Copy the *-targets.cmake file to the CMAKE_INSTALL_PREFIX directory
+    install(EXPORT "${PROJECT_NAME}-targets"
+            FILE "${PROJECT_NAME}-targets.cmake"
+            NAMESPACE "flexiv::"
+            DESTINATION "lib/cmake/${PROJECT_NAME}"
+            )
+
+    # Copy the *.-config file to the CMAKE_INSTALL_PREFIX directory. This will specify the dependencies.
+    configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}-config.cmake.in" "${PROJECT_NAME}-config.cmake" @ONLY)
+    install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake"
+                  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake"
+            DESTINATION "lib/cmake/${PROJECT_NAME}"
+            )
+
+    # Replace the dummy static lib with the actual static lib 
+    install(CODE 
+            "file(REMOVE ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/${CMAKE_STATIC_LIBRARY_PREFIX}${PROJECT_NAME}${CMAKE_STATIC_LIBRARY_SUFFIX})")
+    install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${RDK_LIB}
+            DESTINATION ${CMAKE_INSTALL_LIBDIR}
+            RENAME ${CMAKE_STATIC_LIBRARY_PREFIX}${PROJECT_NAME}${CMAKE_STATIC_LIBRARY_SUFFIX}
+            )
+
+    # Use the CPack Package Generator
+    set(CPACK_PACKAGE_VENDOR "Flexiv")
+    set(CPACK_PACKAGE_CONTACT "support@flexiv.com")
+    set(CPACK_PACKAGE_DESCRIPTION "Flexiv RDK (Robotic Development Kit)")
+    set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR})
+    set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR})
+    set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH})
+    set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
+    set(CPACK_RESOURCE_FILE_README  "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
+    include(CPack)
+endmacro()
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/cmake/flexiv_rdk-config.cmake.in b/thirdparty/flexiv_rdk/cmake/flexiv_rdk-config.cmake.in
new file mode 100644
index 0000000..9ad0cfc
--- /dev/null
+++ b/thirdparty/flexiv_rdk/cmake/flexiv_rdk-config.cmake.in
@@ -0,0 +1,14 @@
+include(CMakeFindDependencyMacro)
+
+# Find dependency
+set(THREADS_PREFER_PTHREAD_FLAG ON)
+set(Boost_USE_STATIC_LIBS OFF)
+find_dependency(Threads REQUIRED)
+find_dependency(Eigen3 3.4.0 REQUIRED)
+find_dependency(spdlog 1.9.2 REQUIRED)
+find_dependency(zenohc 1.9.0 REQUIRED)
+find_dependency(zenohcxx 1.9.0 REQUIRED)
+find_dependency(RBDyn 1.9.3 REQUIRED)
+
+# Add targets file
+include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake")
diff --git a/thirdparty/flexiv_rdk/cmake/qnx_toolchain_aarch64.cmake b/thirdparty/flexiv_rdk/cmake/qnx_toolchain_aarch64.cmake
new file mode 100644
index 0000000..14e39ce
--- /dev/null
+++ b/thirdparty/flexiv_rdk/cmake/qnx_toolchain_aarch64.cmake
@@ -0,0 +1,70 @@
+# QNX toolchain for cross-compilation of aarch64le ("le" - little endian) target
+
+# Set target system
+set(CMAKE_SYSTEM_NAME QNX)
+set(CMAKE_SYSTEM_PROCESSOR aarch64le)
+set(arch gcc_ntoaarch64le)
+
+# specify the cross compiler
+set(CMAKE_C_COMPILER qcc)
+set(CMAKE_C_COMPILER_TARGET ${arch})
+set(CMAKE_CXX_COMPILER q++)
+set(CMAKE_CXX_COMPILER_TARGET ${arch})
+
+# Set qnx build flags
+add_definitions("-D_QNX_SOURCE")
+
+# Set QNX_TARGET
+if (DEFINED ENV{QNX_TARGET})
+    set (QNX_TARGET $ENV{QNX_TARGET})
+else ()
+    message (FATAL_ERROR "NO QNX TARGET ENV DEFINED")
+endif ()
+
+# Set the sysroot to the QNX target directory, so the compiler can find the correct headers and libraries for the target system.
+set(CMAKE_SYSROOT ${QNX_TARGET})
+
+# Set default cmake_install_prefix
+if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT OR NOT DEFINED CMAKE_INSTALL_PREFIX)
+    set(CMAKE_INSTALL_PREFIX ${QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr/local CACHE PATH "cmake install prefix" FORCE)
+endif()
+
+# We want to search:
+# 1. ${CMAKE_PREFIX_PATH}
+# 2. ${CMAKE_INSTALL_PREFIX}
+# 3. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr/local
+# 4. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr
+# 5. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}
+# 6. $ENV{QNX_TARGET}/usr
+#
+# To accomplish 3-6, append CMAKE_FIND_ROOT_PATH with:
+# - $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}
+# - $ENV{QNX_TARGET}/usr
+# 
+# However, CMAKE_FIND_ROOT_PATH will also be prepended to CMAKE_PREFIX_PATH and CMAKE_INSTALL_PREFIX.
+# In order to search these without the prefix,
+# append CMAKE_FIND_ROOT_PATH with:
+# - CMAKE_PREFIX_PATH
+# - CMAKE_INSTALL_PREFIX
+#
+# For details, see Confluence page: "Intro to CMake find_package()"
+if (DEFINED CMAKE_PREFIX_PATH)
+    list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_PREFIX_PATH})
+endif()
+list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_INSTALL_PREFIX})
+list(APPEND CMAKE_FIND_ROOT_PATH ${QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR} ${QNX_TARGET}/usr)
+
+# Remove duplicates, since we are appending
+list(REMOVE_DUPLICATES CMAKE_FIND_ROOT_PATH)
+
+# Search only the paths prefixed with CMAKE_FIND_ROOT_PATH
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY)
+
+# QNX does not support -pthread compiler flag,
+# because pthread is already part of the c library
+# However, FindThreads.cmake incorrectly sets CMAKE_HAVE_LIBC_PTHREAD OFF
+# See: https://gitlab.kitware.com/cmake/cmake/-/issues/21579
+set(CMAKE_HAVE_LIBC_PTHREAD ON)
diff --git a/thirdparty/flexiv_rdk/cmake/qnx_toolchain_x86_64.cmake b/thirdparty/flexiv_rdk/cmake/qnx_toolchain_x86_64.cmake
new file mode 100644
index 0000000..c2c0bc0
--- /dev/null
+++ b/thirdparty/flexiv_rdk/cmake/qnx_toolchain_x86_64.cmake
@@ -0,0 +1,70 @@
+# QNX toolchain for cross-compilation of x86_64 target
+
+# Set target system
+set(CMAKE_SYSTEM_NAME QNX)
+set(CMAKE_SYSTEM_PROCESSOR x86_64)
+set(arch gcc_ntox86_64)
+
+# Specify the cross compiler
+set(CMAKE_C_COMPILER qcc)
+set(CMAKE_C_COMPILER_TARGET ${arch})
+set(CMAKE_CXX_COMPILER q++)
+set(CMAKE_CXX_COMPILER_TARGET ${arch})
+
+# Set qnx build flags
+add_definitions("-D_QNX_SOURCE")
+
+# Set QNX_TARGET
+if (DEFINED ENV{QNX_TARGET})
+    set (QNX_TARGET $ENV{QNX_TARGET})
+else ()
+    message (FATAL_ERROR "NO QNX TARGET ENV DEFINED")
+endif ()
+
+# Set the sysroot to the QNX target directory, so the compiler can find the correct headers and libraries for the target system.
+set(CMAKE_SYSROOT ${QNX_TARGET})
+
+# Set default cmake_install_prefix
+if(CMAKE_INSTALL_PREFIX_INITIALIZED_TO_DEFAULT OR NOT DEFINED CMAKE_INSTALL_PREFIX)
+    set(CMAKE_INSTALL_PREFIX ${QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr/local CACHE PATH "cmake install prefix" FORCE)
+endif()
+
+# We want to search:
+# 1. ${CMAKE_PREFIX_PATH}
+# 2. ${CMAKE_INSTALL_PREFIX}
+# 3. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr/local
+# 4. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}/usr
+# 5. $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}
+# 6. $ENV{QNX_TARGET}/usr
+#
+# To accomplish 3-6, append CMAKE_FIND_ROOT_PATH with:
+# - $ENV{QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR}
+# - $ENV{QNX_TARGET}/usr
+# 
+# However, CMAKE_FIND_ROOT_PATH will also be prepended to CMAKE_PREFIX_PATH and CMAKE_INSTALL_PREFIX.
+# In order to search these without the prefix,
+# append CMAKE_FIND_ROOT_PATH with:
+# - CMAKE_PREFIX_PATH
+# - CMAKE_INSTALL_PREFIX
+#
+# For details, see Confluence page: "Intro to CMake find_package()"
+if (DEFINED CMAKE_PREFIX_PATH)
+    list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_PREFIX_PATH})
+endif()
+list(APPEND CMAKE_FIND_ROOT_PATH ${CMAKE_INSTALL_PREFIX})
+list(APPEND CMAKE_FIND_ROOT_PATH ${QNX_TARGET}/${CMAKE_SYSTEM_PROCESSOR} ${QNX_TARGET}/usr)
+
+# Remove duplicates, since we are appending
+list(REMOVE_DUPLICATES CMAKE_FIND_ROOT_PATH)
+
+# Search only the paths prefixed with CMAKE_FIND_ROOT_PATH
+set(CMAKE_FIND_ROOT_PATH_MODE_PROGRAM NEVER)
+set(CMAKE_FIND_ROOT_PATH_MODE_LIBRARY ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_INCLUDE ONLY)
+set(CMAKE_FIND_ROOT_PATH_MODE_PACKAGE ONLY)
+
+# QNX does not support -pthread compiler flag,
+# because pthread is already part of the c library
+# However, FindThreads.cmake incorrectly sets CMAKE_HAVE_LIBC_PTHREAD OFF
+# See: https://gitlab.kitware.com/cmake/cmake/-/issues/21579
+set(CMAKE_HAVE_LIBC_PTHREAD ON)
diff --git a/thirdparty/flexiv_rdk/doc/Doxyfile.in b/thirdparty/flexiv_rdk/doc/Doxyfile.in
new file mode 100644
index 0000000..8d0c2c9
--- /dev/null
+++ b/thirdparty/flexiv_rdk/doc/Doxyfile.in
@@ -0,0 +1,2665 @@
+# Doxyfile 1.9.1
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the configuration
+# file that follow. The default is UTF-8 which is also the encoding used for all
+# text before the first occurrence of this tag. Doxygen uses libiconv (or the
+# iconv built into libc) for the transcoding. See
+# https://www.gnu.org/software/libiconv/ for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING      = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME           = "Flexiv RDK APIs"
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER         = 2.1
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF          =
+
+# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
+# in the documentation. The maximum height of the logo should not exceed 55
+# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
+# the logo to the output directory.
+
+PROJECT_LOGO           = doc/flexiv_logo.png
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY       = doc
+
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS         = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES    = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE        = English
+
+# The OUTPUT_TEXT_DIRECTION tag is used to specify the direction in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all generated output in the proper direction.
+# Possible values are: None, LTR, RTL and Context.
+# The default value is: None.
+
+OUTPUT_TEXT_DIRECTION  = None
+
+# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC      = YES
+
+# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF           = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF       = "The $name class" \
+                         "The $name widget" \
+                         "The $name file" \
+                         is \
+                         provides \
+                         specifies \
+                         contains \
+                         represents \
+                         a \
+                         an \
+                         the
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC    = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB  = NO
+
+# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES        = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH        =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH    =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES            = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF      = NO
+
+# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line
+# such as
+# /***************
+# as being the beginning of a Javadoc-style comment "banner". If set to NO, the
+# Javadoc-style will behave just like regular comments and it will not be
+# interpreted by doxygen.
+# The default value is: NO.
+
+JAVADOC_BANNER         = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF           = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# By default Python docstrings are displayed as preformatted text and doxygen's
+# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the
+# doxygen's special commands can be used and the contents of the docstring
+# documentation blocks is shown as doxygen documentation.
+# The default value is: YES.
+
+PYTHON_DOCSTRING       = YES
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS           = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
+# page for each member. If set to NO, the documentation of a member will be part
+# of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES  = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE               = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines (in the resulting output). You can put ^^ in the value part of an
+# alias to insert a newline as if a physical newline was in the original file.
+# When you need a literal { or } or , in the value part of an alias you have to
+# escape them by means of a backslash (\), this can lead to conflicts with the
+# commands \{ and \} for these it is advised to use the version @{ and @} or use
+# a double escape (\\{ and \\})
+
+ALIASES                =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C  = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA   = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN   = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL   = NO
+
+# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice
+# sources only. Doxygen will then generate output that is more tailored for that
+# language. For instance, namespaces will be presented as modules, types will be
+# separated into more groups, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_SLICE  = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, JavaScript,
+# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice, VHDL,
+# Fortran (fixed format Fortran: FortranFixed, free formatted Fortran:
+# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser
+# tries to guess whether the code is fixed or free formatted code, this is the
+# default for Fortran type files). For instance to make doxygen treat .inc files
+# as Fortran files (default is PHP), and .f files as C (default is Fortran),
+# use: inc=Fortran f=C.
+#
+# Note: For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen. When specifying no_extension you should add
+# * to the FILE_PATTERNS.
+#
+# Note see also the list of default file extension mappings.
+
+EXTENSION_MAPPING      =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See https://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT       = YES
+
+# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up
+# to that level are automatically included in the table of contents, even if
+# they do not have an id attribute.
+# Note: This feature currently applies only to Markdown headings.
+# Minimum value: 0, maximum value: 99, default value: 5.
+# This tag requires that the tag MARKDOWN_SUPPORT is set to YES.
+
+TOC_INCLUDE_HEADINGS   = 0
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by putting a % sign in front of the word or
+# globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT       = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT    = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT        = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT            = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT   = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC   = YES
+
+# If one adds a struct or class to a group and this option is enabled, then also
+# any nested class or struct is added to the same group. By default this option
+# is disabled and one has to add nested compounds explicitly via \ingroup.
+# The default value is: NO.
+
+GROUP_NESTED_COMPOUNDS = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING            = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS  = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT   = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE      = 0
+
+# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use
+# during processing. When set to 0 doxygen will based this on the number of
+# cores available in the system. You can set it explicitly to a value larger
+# than 0 to get more control over the balance between CPU load and processing
+# speed. At this moment only the input processing can be done using multiple
+# threads. Since this is still an experimental feature the default is set to 1,
+# which efficively disables parallel processing. Please report any issues you
+# encounter. Generating dot graphs in parallel is controlled by the
+# DOT_NUM_THREADS setting.
+# Minimum value: 0, maximum value: 32, default value: 1.
+
+NUM_PROC_THREADS       = 1
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL            = NO
+
+# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE        = NO
+
+# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual
+# methods of a class will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIV_VIRTUAL   = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE        = NO
+
+# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC         = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO,
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES  = YES
+
+# This flag is only useful for Objective-C code. If set to YES, local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO, only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS  = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES   = NO
+
+# If this flag is set to YES, the name of an unnamed parameter in a declaration
+# will be determined by the corresponding definition. By default unnamed
+# parameters remain unnamed in the output.
+# The default value is: YES.
+
+RESOLVE_UNNAMED_PARAMS = YES
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS     = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO, these classes will be included in the various overviews. This option
+# has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES     = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# declarations. If set to NO, these declarations will be included in the
+# documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS  = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO, these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS      = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS          = NO
+
+# With the correct setting of option CASE_SENSE_NAMES doxygen will better be
+# able to match the capabilities of the underlying filesystem. In case the
+# filesystem is case sensitive (i.e. it supports files in the same directory
+# whose names only differ in casing), the option must be set to YES to properly
+# deal with such files in case they appear in the input. For filesystems that
+# are not case sensitive the option should be be set to NO to properly deal with
+# output files written for symbols that only differ in casing, such as for two
+# classes, one named CLASS and the other named Class, and to also support
+# references to files without having to specify the exact matching casing. On
+# Windows (including Cygwin) and MacOS, users should typically set this option
+# to NO, whereas on Linux or other Unix flavors it should typically be set to
+# YES.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES       = NO
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES, the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES       = NO
+
+# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
+# append additional text to a page's title, such as Class Reference. If set to
+# YES the compound reference will be hidden.
+# The default value is: NO.
+
+HIDE_COMPOUND_REFERENCE= NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES     = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC  = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES   = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO            = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS       = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS        = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES       = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME     = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING  = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
+# list. This list is created by putting \todo commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST      = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
+# list. This list is created by putting \test commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST      = YES
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST       = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS       =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES  = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES, the
+# list will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES        = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES             = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES        = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER    =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE            =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. See also \cite for info how to create references.
+
+CITE_BIB_FILES         =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET                  = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS               = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED   = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR      = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO, doxygen will only warn about wrong or incomplete
+# parameter documentation, but not about the absence of documentation. If
+# EXTRACT_ALL is set to YES then this flag will automatically be disabled.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC       = NO
+
+# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
+# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS
+# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but
+# at the end of the doxygen process doxygen will return with a non-zero status.
+# Possible values are: NO, YES and FAIL_ON_WARNINGS.
+# The default value is: NO.
+
+WARN_AS_ERROR          = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT            = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE           =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
+# Note: If this tag is empty the current directory is searched.
+
+INPUT                  = README.md \
+                         doc \
+                         example \
+                         include \
+                         lib \
+                         test
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see:
+# https://www.gnu.org/software/libiconv/) for the list of possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING         = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# read by doxygen.
+#
+# Note the list of default checked file patterns might differ from the list of
+# default file extension mappings.
+#
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
+# *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C comment),
+# *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, *.vhdl,
+# *.ucf, *.qsf and *.ice.
+
+FILE_PATTERNS          = *.c \
+                         *.cc \
+                         *.cxx \
+                         *.cpp \
+                         *.c++ \
+                         *.java \
+                         *.ii \
+                         *.ixx \
+                         *.ipp \
+                         *.i++ \
+                         *.inl \
+                         *.idl \
+                         *.ddl \
+                         *.odl \
+                         *.h \
+                         *.hh \
+                         *.hxx \
+                         *.hpp \
+                         *.h++ \
+                         *.cs \
+                         *.d \
+                         *.php \
+                         *.php4 \
+                         *.php5 \
+                         *.phtml \
+                         *.inc \
+                         *.m \
+                         *.markdown \
+                         *.md \
+                         *.mm \
+                         *.dox \
+                         *.py \
+                         *.pyw \
+                         *.f90 \
+                         *.f95 \
+                         *.f03 \
+                         *.f08 \
+                         *.f \
+                         *.for \
+                         *.tcl \
+                         *.vhd \
+                         *.vhdl \
+                         *.ucf \
+                         *.qsf \
+                         *.proto
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE              = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE                =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS       = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS       = */build/* \
+                         *.pb.h \
+                         *.pb.cc
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS        =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH           = example
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS       = *
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE      = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH             =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+INPUT_FILTER           =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+FILTER_PATTERNS        =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES    = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE = README.md
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER         = YES
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES         = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS    = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# entity all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION    = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS        = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see https://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS              = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS       = YES
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
+# clang parser (see:
+# http://clang.llvm.org/) for more accurate parsing at the cost of reduced
+# performance. This can be particularly helpful with template rich C++ code for
+# which doxygen's built-in parser lacks the necessary type information.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse_libclang=ON option for CMake.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = NO
+
+# If clang assisted parsing is enabled and the CLANG_ADD_INC_PATHS tag is set to
+# YES then doxygen will add the directory of each input to the include path.
+# The default value is: YES.
+
+CLANG_ADD_INC_PATHS    = YES
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS          =
+
+# If clang assisted parsing is enabled you can provide the clang parser with the
+# path to the directory containing a file called compile_commands.json. This
+# file is the compilation database (see:
+# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) containing the
+# options used when the source files were built. This is equivalent to
+# specifying the -p option to a clang tool, such as clang-check. These options
+# will then be passed to the parser. Any options specified with CLANG_OPTIONS
+# will be added as well.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse_libclang=ON option for CMake.
+
+CLANG_DATABASE_PATH    =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX     = YES
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX          =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML          = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT            = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION    = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER            =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER            =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET        =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# cascading style sheets that are included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefore more robust against future updates.
+# Doxygen will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list). For an example see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET  =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES       =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the style sheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# https://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE    = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT    = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA  = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP         = NO
+
+# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML
+# documentation will contain a main index with vertical navigation menus that
+# are dynamically created via JavaScript. If disabled, the navigation index will
+# consists of multiple levels of tabs that are statically embedded in every HTML
+# page. Disable this option to support browsers that do not have JavaScript,
+# like the Qt help browser.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_MENUS     = YES
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS  = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see:
+# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To
+# create a documentation set, doxygen will generate a Makefile in the HTML
+# output directory. Running make will produce the docset in that directory and
+# running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy
+# genXcode/_index.html for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET        = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME  = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see:
+# https://www.microsoft.com/en-us/download/details.aspx?id=21138) on Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP      = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE               =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler (hhc.exe). If non-empty,
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION           =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated
+# (YES) or that it should be included in the main .chm file (NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI           = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING     =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated
+# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC             = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND             = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP           = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE               =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE          = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER     = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME   =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS  =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS  =
+
+# The QHG_LOCATION tag can be used to specify the location (absolute path
+# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to
+# run qhelpgenerator on the generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION           =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP   = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID         = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX          = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW      = NO
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE   = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH         = 250
+
+# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW    = NO
+
+# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg
+# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see
+# https://inkscape.org) to generate formulas as SVG images instead of PNGs for
+# the HTML output. These images will generally look nicer at scaled resolutions.
+# Possible values are: png (the default) and svg (looks nicer but requires the
+# pdf2svg or inkscape tool).
+# The default value is: png.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FORMULA_FORMAT    = png
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE       = 10
+
+# Use the FORMULA_TRANSPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT    = YES
+
+# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands
+# to create new LaTeX commands to be used in formulas as building blocks. See
+# the section "Including formulas" for details.
+
+FORMULA_MACROFILE      =
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# https://www.mathjax.org) which uses client side JavaScript for the rendering
+# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX            = YES
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT         = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from https://www.mathjax.org before deployment.
+# The default value is: https://cdn.jsdelivr.net/npm/mathjax@2.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH        = https://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS     = TeX/AMSmath
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see:
+# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE       =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE           = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using JavaScript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH    = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see:
+# https://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH        = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see:
+# https://xapian.org/). See the section "External Indexing and Searching" for
+# details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL       =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE        = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID     =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS  =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX         = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT           = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when not enabling USE_PDFLATEX the default is latex when enabling
+# USE_PDFLATEX the default is pdflatex and when in the later case latex is
+# chosen this is overwritten by pdflatex. For specific output languages the
+# default can have been set differently, this depends on the implementation of
+# the output language.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME         = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# Note: This tag is used in the Makefile / make.bat.
+# See also: LATEX_MAKEINDEX_CMD for the part in the generated output file
+# (.tex).
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME     = makeindex
+
+# The LATEX_MAKEINDEX_CMD tag can be used to specify the command name to
+# generate index for LaTeX. In case there is no backslash (\) as first character
+# it will be automatically added in the LaTeX code.
+# Note: This tag is used in the generated output file (.tex).
+# See also: MAKEINDEX_CMD_NAME for the part in the Makefile / make.bat.
+# The default value is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_MAKEINDEX_CMD    = makeindex
+
+# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX          = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE             = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. The package can be specified just
+# by its name or with the correct syntax as to be used with the LaTeX
+# \usepackage command. To get the times font for instance you can specify :
+# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
+# To use the option intlimits with the amsmath package you can specify:
+# EXTRA_PACKAGES=[intlimits]{amsmath}
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES         =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
+# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
+# string, for the replacement values of the other commands the user is referred
+# to HTML_HEADER.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER           =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer. See
+# LATEX_HEADER for more information on how to generate a default footer and what
+# special commands can be used inside the footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER           =
+
+# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# LaTeX style sheets that are included after the standard style sheets created
+# by doxygen. Using this option one can overrule certain style aspects. Doxygen
+# will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_STYLESHEET =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES      =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS         = YES
+
+# If the USE_PDFLATEX tag is set to YES, doxygen will use the engine as
+# specified with LATEX_CMD_NAME to generate the PDF file directly from the LaTeX
+# files. Set this option to YES, to get a higher quality PDF documentation.
+#
+# See also section LATEX_CMD_NAME for selecting the engine.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX           = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE        = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES     = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE      = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# https://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE        = plain
+
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP        = NO
+
+# The LATEX_EMOJI_DIRECTORY tag is used to specify the (relative or absolute)
+# path from which the emoji images will be read. If a relative path is entered,
+# it will be relative to the LATEX_OUTPUT directory. If left blank the
+# LATEX_OUTPUT directory will be used.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EMOJI_DIRECTORY  =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF           = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT             = rtf
+
+# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF            = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS         = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's
+# configuration file, i.e. a series of assignments. You only have to provide
+# replacements, missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE    =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's configuration file. A template extensions file can be
+# generated using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE    =
+
+# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
+# with syntax highlighting in the RTF output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_SOURCE_CODE        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN           = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT             = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION          = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR             =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS              = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML           = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT             = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING     = YES
+
+# If the XML_NS_MEMB_FILE_SCOPE tag is set to YES, doxygen will include
+# namespace members in file scope as well, matching the HTML output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_NS_MEMB_FILE_SCOPE = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK       = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT         = docbook
+
+# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
+# program listings (including syntax highlighting and cross-referencing
+# information) to the DOCBOOK output. Note that enabling this will significantly
+# increase the size of the DOCBOOK output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
+# AutoGen Definitions (see http://autogen.sourceforge.net/) file that captures
+# the structure of the code including all documentation. Note that this feature
+# is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF   = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD       = NO
+
+# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX          = NO
+
+# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO, the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY         = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING   = YES
+
+# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
+# in the source code. If set to NO, only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION        = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF     = NO
+
+# If the SEARCH_INCLUDES tag is set to YES, the include files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES        = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH           =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS  =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED             =
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED      =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS   = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES               =
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE       =
+
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
+# The default value is: NO.
+
+ALLEXTERNALS           = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
+# in the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS        = YES
+
+# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES         = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS         = YES
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH               =
+
+# If set to YES the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS   = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT               = YES
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS        = 0
+
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME           = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE           = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH           =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH            = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH    = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS           = YES
+
+# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK               = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag UML_LOOK is set to YES.
+
+UML_LIMIT_NUM_FIELDS   = 10
+
+# If the DOT_UML_DETAILS tag is set to NO, doxygen will show attributes and
+# methods without types and arguments in the UML graphs. If the DOT_UML_DETAILS
+# tag is set to YES, doxygen will add type and arguments for attributes and
+# methods in the UML graphs. If the DOT_UML_DETAILS tag is set to NONE, doxygen
+# will not generate fields with class member information in the UML graphs. The
+# class diagrams will look similar to the default class diagrams but using UML
+# notation for the relationships.
+# Possible values are: NO, YES and NONE.
+# The default value is: NO.
+# This tag requires that the tag UML_LOOK is set to YES.
+
+DOT_UML_DETAILS        = NO
+
+# The DOT_WRAP_THRESHOLD tag can be used to set the maximum number of characters
+# to display on a single line. If the actual line length exceeds this threshold
+# significantly it will wrapped across multiple lines. Some heuristics are apply
+# to avoid ugly line breaks.
+# Minimum value: 0, maximum value: 1000, default value: 17.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_WRAP_THRESHOLD     = 17
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS     = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH          = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH      = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command. Disabling a call graph can be
+# accomplished by means of the command \hidecallgraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH             = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command. Disabling a caller graph can be
+# accomplished by means of the command \hidecallergraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH           = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY    = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH        = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. For an explanation of the image formats see the section
+# output formats in the documentation of the dot tool (Graphviz (see:
+# http://www.graphviz.org/)).
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
+# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
+# png:gdiplus:gdiplus.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT       = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG        = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH               =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS           =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS           =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS           =
+
+# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
+
+PLANTUML_JAR_PATH      =
+
+# When using plantuml, the PLANTUML_CFG_FILE tag can be used to specify a
+# configuration file for plantuml.
+
+PLANTUML_CFG_FILE      =
+
+# When using plantuml, the specified paths are searched for files specified by
+# the !include statement in a plantuml block.
+
+PLANTUML_INCLUDE_PATH  =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES    = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH    = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT        = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS      = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND        = YES
+
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate
+# files that are used to generate the various graphs.
+#
+# Note: This setting is not only used for dot files but also for msc and
+# plantuml temporary files.
+# The default value is: YES.
+
+DOT_CLEANUP            = YES
diff --git a/thirdparty/flexiv_rdk/doc/flexiv_logo.png b/thirdparty/flexiv_rdk/doc/flexiv_logo.png
new file mode 100644
index 0000000..c0b84c1
Binary files /dev/null and b/thirdparty/flexiv_rdk/doc/flexiv_logo.png differ
diff --git a/thirdparty/flexiv_rdk/example/CMakeLists.txt b/thirdparty/flexiv_rdk/example/CMakeLists.txt
new file mode 100644
index 0000000..eb43c92
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/CMakeLists.txt
@@ -0,0 +1,59 @@
+cmake_minimum_required(VERSION 3.22.1)
+project(flexiv_rdk-examples)
+
+# CMake >= 3.30 warns for FindBoost behavior unless CMP0167 is explicitly set.
+if(POLICY CMP0167)
+  cmake_policy(SET CMP0167 OLD)
+endif()
+
+# Show verbose build info
+SET(CMAKE_VERBOSE_MAKEFILE ON)
+
+message("OS: ${CMAKE_SYSTEM_NAME}")
+message("Processor: ${CMAKE_SYSTEM_PROCESSOR}")
+
+# Configure build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
+endif()
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
+
+# Minimum example list for all OS
+set(EXAMPLE_LIST
+  basics1_display_robot_states
+  basics2_clear_fault
+  basics3_primitive_execution
+  basics4_plan_execution
+  basics5_zero_force_torque_sensors
+  basics6_gripper_control
+  basics7_update_robot_tool
+  basics8_global_variables
+  basics9_logging_behavior
+  intermediate7_robot_dynamics
+)
+
+# Additional examples for Linux and Mac
+if(CMAKE_HOST_UNIX)
+  list(APPEND EXAMPLE_LIST
+    intermediate1_realtime_joint_position_control
+    intermediate2_realtime_joint_impedance_control
+    intermediate3_realtime_joint_torque_control
+    intermediate4_realtime_joint_floating
+    intermediate5_realtime_cartesian_pure_motion_control
+    intermediate6_realtime_cartesian_motion_force_control
+  )
+endif()
+
+# Find flexiv_rdk INTERFACE library
+find_package(flexiv_rdk REQUIRED)
+
+# Build all selected examples
+foreach(example ${EXAMPLE_LIST})
+  add_executable(${example} ${example}.cpp)
+  target_link_libraries(${example} flexiv::flexiv_rdk)
+
+  # C++20 required
+  set_target_properties(${example} PROPERTIES
+    CXX_STANDARD 20
+    CXX_STANDARD_REQUIRED ON)
+endforeach()
diff --git a/thirdparty/flexiv_rdk/example/basics1_display_robot_states.cpp b/thirdparty/flexiv_rdk/example/basics1_display_robot_states.cpp
new file mode 100644
index 0000000..e764bfc
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics1_display_robot_states.cpp
@@ -0,0 +1,120 @@
+/**
+ * @example basics1_display_robot_states.cpp
+ * This tutorial does the very first thing: check connection with the robot server and print
+ * received robot states.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Print robot states data @ 1Hz */
+void PrintRobotStates(rdk::Robot& robot)
+{
+    while (true) {
+        // Print Available joint groups
+        std::string joint_groups_str;
+        for (const auto& [_, name] : robot.info().all_groups) {
+            joint_groups_str += "[" + name + "] ";
+        }
+        spdlog::info("Available joint groups: {}", joint_groups_str);
+
+        // Print all robot states in JSON format using the built-in ostream operator overloading
+        for (const auto& [group, states] : robot.states()) {
+            spdlog::info("[{}] robot states:", rdk::kJointGroupNames.at(group));
+            std::cout << states << std::endl;
+        }
+
+        // Print all robot actions in JSON format using the built-in ostream operator overloading
+        for (const auto& [group, actions] : robot.actions()) {
+            spdlog::info("[{}] robot actions:", rdk::kJointGroupNames.at(group));
+            std::cout << actions << std::endl;
+        }
+
+        // Print digital inputs and outputs
+        spdlog::info("Digital inputs:");
+        std::cout << rdk::utility::Arr2Str(robot.digital_inputs()) << std::endl;
+        spdlog::info("Digital outputs:");
+        std::cout << rdk::utility::Arr2Str(robot.digital_outputs()) << std::endl;
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial does the very first thing: check connection "
+        "with the robot server and print received robot states.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Print States
+        // =========================================================================================
+        // Use std::thread to do scheduling so that this example can run on all OS, since not all OS
+        // support rdk::Scheduler
+        std::thread low_priority_thread(std::bind(PrintRobotStates, std::ref(robot)));
+
+        // Properly exit thread
+        low_priority_thread.join();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/basics2_clear_fault.cpp b/thirdparty/flexiv_rdk/example/basics2_clear_fault.cpp
new file mode 100644
index 0000000..837ee90
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics2_clear_fault.cpp
@@ -0,0 +1,72 @@
+/**
+ * @example basics2_clear_fault.cpp
+ * This tutorial clears minor or critical faults, if any, of the connected robot.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial clears minor or critical faults, if any, of "
+        "the connected robot.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Fault Clearing
+        // =========================================================================================
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        } else {
+            spdlog::info("No fault on the connected robot");
+        }
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/basics3_primitive_execution.cpp b/thirdparty/flexiv_rdk/example/basics3_primitive_execution.cpp
new file mode 100644
index 0000000..03fb8c7
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics3_primitive_execution.cpp
@@ -0,0 +1,207 @@
+/**
+ * @example basics3_primitive_execution.cpp
+ * This tutorial executes several basic robot primitives (unit skills). For detailed documentation
+ * on all available primitives, please see [Flexiv Primitives](https://www.flexiv.com/primitives/).
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <algorithm>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial executes several basic robot primitives (unit "
+        "skills). For detailed documentation on all available primitives, please see [Flexiv "
+        "Primitives](https://www.flexiv.com/primitives/).\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Execute Primitives
+        // =========================================================================================
+        // Primitives can only be executed on single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // (1) Move robot to home pose
+        // -----------------------------------------------------------------------------------------
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Switch to primitive execution mode
+        robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+
+        // (2) Move robot joints to target positions
+        // -----------------------------------------------------------------------------------------
+        // Required parameters:
+        //     target: final joint positions, unit: degrees
+        //         {arm joint positions}, {external axis joint positions}
+        // Optional parameters:
+        //     waypoints: waypoints to pass before reaching the target
+        //         (same format as above, but can be more than one)
+        //     vel: TCP linear velocity, unit: m/s
+        spdlog::info("Executing primitive: MoveJ");
+
+        // Send command to robot
+        std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("MoveJ",
+                {
+                    {"target", rdk::JPos({30, -45, 0, 90, 0, 40, 30}, {-50, 30})},
+                    {"waypoints",
+                        std::vector<rdk::JPos> {rdk::JPos({10, -30, 10, 30, 10, 15, 10}, {-15, 10}),
+                            rdk::JPos({20, -60, -10, 60, -10, 30, 20}, {-30, 20})}},
+                });
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // Most primitives won't exit by themselves and require users to explicitly trigger
+        // transitions based on specific primitive states. Here we check if the primitive state
+        // [reachedTarget] becomes true and trigger the transition manually by sending a new
+        // primitive command.
+        while (true) {
+            const auto primitive_states = robot.primitive_states();
+            if (rdk::utility::PrimitiveStateTrueForGroups(
+                    primitive_states, "reachedTarget")) {
+                break;
+            }
+
+            // Print current primitive states
+            spdlog::info("Current primitive states:");
+            for (const auto& [group, pt_states] : primitive_states) {
+                std::cout << rdk::kJointGroupNames.at(group) << ":" << std::endl;
+                std::cout << "primitiveName: " << pt_states.pt_name << std::endl;
+                for (const auto& [name, value] : pt_states.names_and_values) {
+                    std::cout << name << ": " << rdk::utility::FlexivTypes2Str(value);
+                    std::cout << std::endl;
+                }
+            }
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+
+        // (3) Move robot TCP to a target pose in world (base) frame
+        // -----------------------------------------------------------------------------------------
+        // Required parameters:
+        //     target: final TCP pose, unit: m and degrees
+        //         {pos_x, pos_y, pos_z}, {rot_x, rot_y, rot_z}, {ref_frame, ref_point}
+        // Optional parameters:
+        //     waypoints: waypoints to pass before reaching the target
+        //         (same format as above, but can be more than one)
+        //     vel: TCP linear velocity, unit: m/s
+        // NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis
+        spdlog::info("Executing primitive: MoveL");
+
+        // Send command to robot
+        pt_args.clear();
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("MoveL",
+                {{"target",
+                     rdk::Coord({0.3, -0.1, 0.2}, {160, 20, 180}, {"WORLD", "WORLD_ORIGIN"})},
+                    {"waypoints",
+                        std::vector<rdk::Coord> {rdk::Coord({0.1, 0.1, 0.1}, {-160, -20, 180},
+                                                     {"WORLD", "WORLD_ORIGIN"}),
+                            rdk::Coord({0.3, 0.2, 0.1}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})}},
+
+                    {"vel", 0.6}, {"zoneRadius", "Z50"}});
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // Wait for reached target
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "reachedTarget")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+
+        // (4) Another MoveL that uses TCP frame
+        // -----------------------------------------------------------------------------------------
+        // In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START,
+        // which represents the current TCP frame
+        spdlog::info("Executing primitive: MoveL");
+
+        // Example to convert target quaternion [w,x,y,z] to Euler ZYX using utility functions
+        std::array<double, 4> targetQuat = {0.9185587, 0.1767767, 0.3061862, 0.1767767};
+        // ZYX = [30, 30, 30] degrees
+        auto targetEulerDeg = rdk::utility::Rad2Deg(rdk::utility::Quat2EulerZYX(targetQuat));
+
+        // Send command to robot. This motion will hold current TCP position and only do rotation
+        pt_args.clear();
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("MoveL",
+                {
+                    {"target", rdk::Coord({0.0, 0.0, 0.0}, targetEulerDeg, {"TRAJ", "START"})},
+                    {"vel", 0.2},
+                });
+        }
+        robot.ExecutePrimitive(pt_args);
+        // Wait for reached target
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "reachedTarget")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+
+        // All done, stop robot and put into IDLE mode
+        robot.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/basics4_plan_execution.cpp b/thirdparty/flexiv_rdk/example/basics4_plan_execution.cpp
new file mode 100644
index 0000000..6a6e9f0
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics4_plan_execution.cpp
@@ -0,0 +1,152 @@
+/**
+ * @example basics4_plan_execution.cpp
+ * This tutorial executes a plan selected by the user from a list of available plans. A plan is a
+ * pre-written script to execute a series of robot primitives with pre-defined transition conditions
+ * between 2 adjacent primitives. Users can use Flexiv Elements to compose their own plan and assign
+ * to the robot, which will appear in the plan list.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial executes a plan selected by the user from a "
+        "list of available plans. A plan is a pre-written script to execute a series of robot "
+        "primitives with pre-defined transition conditions between 2 adjacent primitives. Users "
+        "can use Flexiv Elements to compose their own plan and assign to the robot, which will "
+        "appear in the plan list.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Execute Plans
+        // =========================================================================================
+        // Switch to plan execution mode
+        robot.SwitchMode(rdk::Mode::NRT_PLAN_EXECUTION);
+
+        while (true) {
+            // Monitor fault on the connected robot
+            if (robot.fault()) {
+                throw std::runtime_error("Fault occurred on the connected robot, exiting ...");
+            }
+
+            // Get user input
+            spdlog::info("Choose an action:");
+            std::cout << "[1] Show available plans" << std::endl;
+            std::cout << "[2] Execute a plan by index" << std::endl;
+            std::cout << "[3] Execute a plan by name" << std::endl;
+
+            std::string inputBuffer;
+            std::cin >> inputBuffer;
+            int userInput = std::stoi(inputBuffer);
+
+            switch (userInput) {
+                // Get and show plan list
+                case 1: {
+                    auto plan_list = robot.plan_list();
+                    for (size_t i = 0; i < plan_list.size(); i++) {
+                        std::cout << "[" << i << "] " << plan_list[i] << std::endl;
+                    }
+                    std::cout << std::endl;
+                } break;
+                // Execute plan by index
+                case 2: {
+                    spdlog::info("Enter plan index to execute:");
+                    int index;
+                    std::cin >> index;
+                    // Allow the plan to continue its execution even if the RDK program is closed or
+                    // the connection is lost
+                    robot.ExecutePlan(index, true);
+
+                    // Print plan info while the current plan is running
+                    while (robot.busy()) {
+                        spdlog::info("Current plan info:");
+                        std::cout << robot.plan_info() << std::endl;
+                        std::this_thread::sleep_for(std::chrono::seconds(1));
+                    }
+                } break;
+                // Execute plan by name
+                case 3: {
+                    spdlog::info("Enter plan name to execute:");
+                    std::string name;
+                    std::cin >> name;
+                    // Allow the plan to continue its execution even if the RDK program is closed or
+                    // the connection is lost
+                    robot.ExecutePlan(name, true);
+
+                    // Print plan info while the current plan is running
+                    while (robot.busy()) {
+                        spdlog::info("Current plan info:");
+                        std::cout << robot.plan_info() << std::endl;
+                        std::this_thread::sleep_for(std::chrono::seconds(1));
+                    }
+                } break;
+                default:
+                    spdlog::warn("Invalid input");
+                    break;
+            }
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/basics5_zero_force_torque_sensors.cpp b/thirdparty/flexiv_rdk/example/basics5_zero_force_torque_sensors.cpp
new file mode 100644
index 0000000..6d45ad4
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics5_zero_force_torque_sensors.cpp
@@ -0,0 +1,123 @@
+/**
+ * @example basics5_zero_force_torque_sensors.cpp
+ * This tutorial zeros the robot's force and torque sensors, which is a recommended (but not
+ * mandatory) step before any operations that require accurate force/torque measurement.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+#include <algorithm>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial zeros the robot's force and torque sensors, "
+        "which is a recommended (but not mandatory) step before any operations that require "
+        "accurate force/torque measurement.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Zero Sensors
+        // =========================================================================================
+        // Get and print the current TCP force/moment readings
+        for (const auto& [group, states] : robot.states()) {
+            spdlog::info(
+                "[{}] TCP force and moment reading in world frame BEFORE sensor zeroing: {} N-Nm",
+                rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(states.tcp_wrench));
+        }
+
+        // Primitives can only be executed on single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors
+        robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+        std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // WARNING: during the process, the robot must not contact anything, otherwise the result
+        // will be inaccurate and affect following operations
+        spdlog::warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot");
+
+        // Wait for primitive to finish
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Sensor zeroing complete");
+
+        // Get and print the current TCP force/moment readings
+        for (const auto& [group, _] : single_arm_groups) {
+            spdlog::info(
+                "[{}] TCP force and moment reading in world frame AFTER sensor zeroing: {} N-Nm",
+                rdk::kJointGroupNames.at(group),
+                rdk::utility::Arr2Str(robot.states().at(group).tcp_wrench));
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/basics6_gripper_control.cpp b/thirdparty/flexiv_rdk/example/basics6_gripper_control.cpp
new file mode 100644
index 0000000..f731341
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics6_gripper_control.cpp
@@ -0,0 +1,250 @@
+/**
+ * @example basics6_gripper_control.cpp
+ * This tutorial does position and force (if available) control of grippers supported by Flexiv.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/gripper.hpp>
+#include <flexiv/rdk/tool.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <atomic>
+#include <limits>
+
+using namespace flexiv;
+
+namespace {
+/** Global flag: whether the gripper control tasks are finished */
+std::atomic<bool> g_finished = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn] [gripper_device_name] [gripper_tool_name]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect to. Remove any space, for example: Enlight-L-123456" << std::endl;
+    std::cout << "    gripper_device_name: Full name of the device representing the gripper, can be found in Flexiv Elements->Settings->Device" << std::endl;
+    std::cout << "    gripper_tool_name: Full name of the tool representing the gripper, can be found in Flexiv Elements->Settings->Tool" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Print gripper states data @ 1Hz */
+void PrintGripperStates(rdk::Gripper& gripper)
+{
+    while (!g_finished) {
+        const auto gripper_states = gripper.states();
+        spdlog::info("Current gripper states:");
+        for (const auto& [group, states] : gripper_states) {
+            std::cout << "[" << rdk::kJointGroupNames.at(group) << "]\n" << states << std::endl;
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 4 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+    std::string gripper_device_name = argv[2];
+    std::string gripper_tool_name = argv[3];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial does position and force (if available) "
+        "control of grippers supported by Flexiv.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Gripper Control
+        // =========================================================================================
+        // Instantiate gripper control interface
+        rdk::Gripper gripper(robot);
+
+        // Instantiate tool interface. Gripper is categorized as both a device and a tool. The
+        // device attribute allows a gripper to be interactively controlled by the user; whereas the
+        // tool attribute tells the robot to account for its mass properties and TCP location.
+        rdk::Tool tool(robot);
+
+        // Grippers can only be assigned to single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Enable the specified gripper as a device. This is equivalent to enabling the specified
+        // gripper in Flexiv Elements -> Settings -> Device
+        spdlog::info("Enabling gripper device [{}] for all single-arm groups", gripper_device_name);
+        for (const auto& [group, _] : single_arm_groups) {
+            gripper.Enable(group, gripper_device_name);
+        }
+
+        // Print parameters of the enabled gripper
+        spdlog::info("Gripper params:");
+        const auto gripper_params = gripper.params();
+        for (const auto& [group, params] : gripper_params) {
+            std::cout << "[" << rdk::kJointGroupNames.at(group) << "]\n"
+                      << std::fixed << std::setprecision(2) << "{\n"
+                      << "name: " << params.name << "\nmin_width: " << params.min_width
+                      << "\nmax_width: " << params.max_width << "\nmin_force: " << params.min_force
+                      << "\nmax_force: " << params.max_force << "\nmin_vel: " << params.min_vel
+                      << "\nmax_vel: " << params.max_vel << "\n}" << std::endl;
+        }
+
+        // Switch robot tool to gripper so the gravity compensation and TCP location is updated
+        spdlog::info("Switching robot tool to [{}] for all single-arm groups", gripper_tool_name);
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, gripper_tool_name);
+        }
+
+        // User needs to determine if this gripper requires manual initialization
+        int choice = 0;
+        spdlog::info(
+            "Manually trigger initialization for the gripper now? Choose Yes if it's a 48v Grav "
+            "gripper");
+        std::cout << "[1] No, it has already initialized automatically when power on" << std::endl;
+        std::cout << "[2] Yes, it does not initialize itself when power on" << std::endl;
+        std::cin >> choice;
+
+        // Trigger manual initialization based on choice
+        if (choice == 1) {
+            spdlog::info("Skipped manual initialization");
+        } else if (choice == 2) {
+            for (const auto& [group, _] : single_arm_groups) {
+                gripper.Init(group);
+            }
+            // User determines if the manual initialization is finished
+            spdlog::info(
+                "Triggered manual initialization, press Enter when the initialization is finished "
+                "to continue");
+            std::cin.get();
+            std::cin.get();
+        } else {
+            spdlog::error("Invalid choice");
+            return 1;
+        }
+
+        // Start a separate thread to print gripper states. Use a scope guard so the thread is
+        // always signaled and joined when leaving this scope, including via an exception thrown by
+        // gripper.Move() etc. Otherwise the std::thread destructor would run while still joinable
+        // and call std::terminate(), aborting before the exception reaches the catch block below.
+        std::thread print_thread(PrintGripperStates, std::ref(gripper));
+        auto join_print_thread = [&]() {
+            g_finished = true;
+            if (print_thread.joinable()) {
+                print_thread.join();
+            }
+        };
+        std::shared_ptr<void> print_thread_guard(nullptr, [&](void*) { join_print_thread(); });
+
+        // Position control
+        spdlog::info("Closing gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        spdlog::info("Opening gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+
+        // Stop
+        spdlog::info("Closing gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        spdlog::info("Stopping gripper");
+        for (const auto& [group, _] : gripper_params) {
+            gripper.Stop(group);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        spdlog::info("Closing gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        spdlog::info("Opening gripper");
+        for (const auto& [group, params] : gripper_params) {
+            gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force);
+        }
+        std::this_thread::sleep_for(std::chrono::milliseconds(500));
+        spdlog::info("Stopping gripper");
+        for (const auto& [group, _] : gripper_params) {
+            gripper.Stop(group);
+        }
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+
+        // Force control, if available (sensed force is not zero)
+        const auto gripper_states = gripper.states();
+        bool has_force_control = false;
+        for (const auto& [_, states] : gripper_states) {
+            if (fabs(states.force) > std::numeric_limits<double>::epsilon()) {
+                has_force_control = true;
+                break;
+            }
+        }
+        if (has_force_control) {
+            spdlog::info("Gripper running zero force control");
+            for (const auto& [group, _] : gripper_states) {
+                gripper.Grasp(group, 0);
+            }
+            // Exit after 10 seconds
+            std::this_thread::sleep_for(std::chrono::seconds(10));
+        }
+
+        // Finished, exit all threads
+        for (const auto& [group, _] : gripper_states) {
+            gripper.Stop(group);
+        }
+        spdlog::info("Program finished");
+        // The print thread is signaled and joined by the scope guard above.
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/basics7_update_robot_tool.cpp b/thirdparty/flexiv_rdk/example/basics7_update_robot_tool.cpp
new file mode 100644
index 0000000..6525c09
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics7_update_robot_tool.cpp
@@ -0,0 +1,168 @@
+/**
+ * @example basics7_update_robot_tool.cpp
+ * This tutorial shows how to online update and interact with the robot tools. All changes made to
+ * the robot tool system will take effect immediately without needing to reboot. However, the robot
+ * must be put into IDLE mode when making these changes.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/tool.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to online update and interact with "
+        "the robot tools. All changes made to the robot tool system will take effect immediately "
+        "without needing to reboot. However, the robot must be put into IDLE mode when making "
+        "these changes.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Update Robot Tool
+        // =========================================================================================
+        // Make sure the robot is in IDLE mode
+        robot.SwitchMode(rdk::Mode::IDLE);
+
+        // Instantiate tool interface
+        rdk::Tool tool(robot);
+
+        // Tools can only be assigned to single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Get and print a list of already configured tools currently in the robot's tools pool
+        spdlog::info("All configured tools:");
+        auto tool_list = tool.list();
+        for (size_t i = 0; i < tool_list.size(); i++) {
+            std::cout << "[" << i << "] " << tool_list[i] << std::endl;
+        }
+        std::cout << std::endl;
+
+        // Get and print the current active tool
+        for (const auto& [group, _] : single_arm_groups) {
+            spdlog::info(
+                "[{}] Current active tool: {}", rdk::kJointGroupNames.at(group), tool.name(group));
+        }
+
+        // Set name and parameters for a new tool
+        std::string new_tool_name = "ExampleTool1";
+        rdk::ToolParams new_tool_params;
+        new_tool_params.mass = 0.9;
+        new_tool_params.CoM = {0.0, 0.0, 0.057};
+        new_tool_params.inertia = {2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0};
+        new_tool_params.tcp_location = {0.0, -0.207, 0.09, 0.7071068, 0.7071068, 0.0, 0.0};
+
+        // If there's already a tool with the same name in the robot's tools pool, then remove it
+        // first, because duplicate tool names are not allowed
+        if (tool.exist(new_tool_name)) {
+            spdlog::warn(
+                "Tool with the same name [{}] already exists, removing it now", new_tool_name);
+            // Switch to other tool or no tool (Flange) before removing the current tool
+            for (const auto& [group, _] : single_arm_groups) {
+                tool.Switch(group, "Flange");
+            }
+            tool.Remove(new_tool_name);
+        }
+
+        // Add the new tool
+        spdlog::info("Adding new tool [{}] to the robot", new_tool_name);
+        tool.Add(new_tool_name, new_tool_params);
+
+        // Get and print the tools list again, the new tool should appear at the end
+        spdlog::info("All configured tools:");
+        tool_list = tool.list();
+        for (size_t i = 0; i < tool_list.size(); i++) {
+            std::cout << "[" << i << "] " << tool_list[i] << std::endl;
+        }
+        std::cout << std::endl;
+
+        // Switch to the newly added tool, i.e. set it as the active tool
+        spdlog::info("Switching to tool [{}]", new_tool_name);
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, new_tool_name);
+        }
+
+        // Get and print the current active tool again, should be the new tool
+        for (const auto& [group, _] : single_arm_groups) {
+            spdlog::info(
+                "[{}] Current active tool: {}", rdk::kJointGroupNames.at(group), tool.name(group));
+        }
+
+        // Switch to other tool or no tool (Flange) before removing the current tool
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, "Flange");
+        }
+
+        // Clean up by removing the new tool
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        spdlog::info("Removing tool [{}]", new_tool_name);
+        tool.Remove(new_tool_name);
+
+        spdlog::info("Program finished");
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/basics8_global_variables.cpp b/thirdparty/flexiv_rdk/example/basics8_global_variables.cpp
new file mode 100644
index 0000000..d2f6188
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics8_global_variables.cpp
@@ -0,0 +1,129 @@
+/**
+ * @example basics8_global_variables.cpp
+ * This tutorial shows how to get and set global variables.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to get and set global variables.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Get existing global variables
+        // =========================================================================================
+        auto global_vars = robot.global_variables();
+        if (global_vars.empty()) {
+            spdlog::warn("No global variables available");
+            return 1;
+        } else {
+            spdlog::info("Existing global variables and their original values:");
+            for (const auto& var : global_vars) {
+                std::cout << var.first << ": " << rdk::utility::FlexivTypes2Str(var.second)
+                          << std::endl;
+            }
+        }
+
+        // Set global variables
+        // =========================================================================================
+        ///@warning These specified global variables need to be created first using Flexiv Elements
+        spdlog::info("Setting new values to existing global variables");
+        robot.SetGlobalVariables({
+            {"test_bool", 1},
+            {"test_int", 100},
+            {"test_double", 100.123},
+            {"test_string", "Flexiv"},
+            {"test_int_vec", std::vector<int> {1, 2, 3}},
+            {"test_double_vec", std::vector<double> {1.1, 2.2, 3.3}},
+            {"test_string_vec", std::vector<std::string> {"Go", "Flexiv", "Go!"}},
+            {"test_pose", std::vector<double> {0.1, -0.2, 0.3, -90, -45, 120}},
+            {"test_coord", rdk::Coord({0.1, -0.2, 0.3}, {-90, -45, 120}, {"WORLD", "WORLD_ORIGIN"},
+                               {1, 2, 3, 4, 5, 6, 7}, {10, 20, 0, 0, 0, 0})},
+            {"test_coord_array",
+                std::vector<rdk::Coord> {rdk::Coord({1, 2, 3}, {4, 5, 6}, {"WORK", "WorkCoord0"}),
+                    rdk::Coord({10, 20, 30}, {40, 50, 60}, {"WORLD", "WORLD_ORIGIN"},
+                        {1, 2, 3, 4, 5, 6, 7}, {10, 20, 0, 0, 0, 0}),
+                    rdk::Coord({3, 2, 1}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})}},
+        });
+
+        // Get updated global variables
+        // =========================================================================================
+        global_vars = robot.global_variables();
+        if (global_vars.empty()) {
+            spdlog::warn("No global variables available");
+            return 1;
+        } else {
+            spdlog::info("Updated global variables:");
+            for (const auto& var : global_vars) {
+                std::cout << var.first << ": " << rdk::utility::FlexivTypes2Str(var.second)
+                          << std::endl;
+            }
+        }
+
+        spdlog::info("Program finished");
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/basics9_logging_behavior.cpp b/thirdparty/flexiv_rdk/example/basics9_logging_behavior.cpp
new file mode 100644
index 0000000..b53fbf0
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/basics9_logging_behavior.cpp
@@ -0,0 +1,97 @@
+/**
+ * @example basics9_logging_behavior.cpp
+ * This tutorial shows how to change the logging behaviors of RDK client.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+#include <spdlog/async.h>
+#include <spdlog/sinks/stdout_color_sinks.h>
+#include <spdlog/sinks/basic_file_sink.h>
+
+#include <iostream>
+#include <string>
+
+using namespace flexiv;
+
+namespace {
+constexpr char kDefaultLogPattern[] = "[%Y-%m-%d %H:%M:%S.%e] [%^%l%$] %v";
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to change the logging behaviors of "
+        "RDK client.\n");
+
+    // Suppress log messages from RDK client
+    // =========================================================================================
+    // Change log level to suppress certain type of log messages from RDK client. The log level
+    // setting will apply globally to all following spdlog messages
+    spdlog::set_level(spdlog::level::warn); ///< Suppress info messages
+    // messages spdlog::set_level(spdlog::level::off); ///< Suppress all messages
+
+    // Instantiate RDK client, all info messages are suppressed
+    try {
+        rdk::Robot robot(robot_sn);
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+    }
+
+    // Output all log messages to a file
+    // =========================================================================================
+    // Create a logger that outputs the messages to both the console and a log file
+    auto console_sink = std::make_shared<spdlog::sinks::stdout_color_sink_mt>();
+    auto file_sink = std::make_shared<spdlog::sinks::basic_file_sink_mt>("example.log", true);
+    std::vector<spdlog::sink_ptr> sinks {console_sink, file_sink};
+    // Since writing to file takes some time, so we use async logger to avoid blocking the program
+    spdlog::init_thread_pool(10240, 1);
+    auto tp = spdlog::thread_pool();
+    auto logger
+        = std::make_shared<spdlog::async_logger>("global_logger", sinks.begin(), sinks.end(), tp);
+    // Use default pattern for log messages
+    logger->set_pattern(kDefaultLogPattern);
+
+    // Use the logger configured above as the default global logger
+    spdlog::set_default_logger(logger);
+
+    // Allow all messages
+    spdlog::set_level(spdlog::level::info);
+
+    // Instantiate RDK client again, all messages are printed to console and output to a log file
+    try {
+        rdk::Robot robot(robot_sn);
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+    }
+
+    spdlog::warn("This message should also appear in the log file");
+    spdlog::info("Program finished");
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/intermediate1_realtime_joint_position_control.cpp b/thirdparty/flexiv_rdk/example/intermediate1_realtime_joint_position_control.cpp
new file mode 100644
index 0000000..1cbff6c
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/intermediate1_realtime_joint_position_control.cpp
@@ -0,0 +1,199 @@
+/**
+ * @example intermediate1_realtime_joint_position_control.cpp
+ * This tutorial runs real-time joint position control to hold or sine-sweep all robot joints.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** Sine-sweep trajectory amplitude and frequency */
+constexpr double kSineAmp = 0.035;
+constexpr double kSineFreq = 0.3;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--hold]" << std::endl;
+    std::cout << "    --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
+    const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
+{
+    // Local periodic loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtJointPositionCmd> rt_cmds;
+
+        if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
+            throw std::invalid_argument(
+                "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
+        }
+
+        for (const auto& [group, init_pos] : all_init_pos) {
+            std::vector<double> target_pos(init_pos.size());
+            std::vector<double> target_vel(init_pos.size());
+            std::vector<double> target_acc(init_pos.size());
+
+            if (motion_type == "hold") {
+                target_pos = init_pos;
+            } else {
+                for (size_t i = 0; i < target_pos.size(); ++i) {
+                    target_pos[i]
+                        = init_pos[i]
+                          + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
+                }
+            }
+            rt_cmds[group] = rdk::RtJointPositionCmd(target_pos, target_vel, target_acc);
+        }
+
+        // Send target joint position to RDK server
+        robot.StreamJointPosition(rt_cmds);
+
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time joint position control to hold "
+        "or sine-sweep all robot joints.\n");
+
+    // Type of motion specified by user
+    std::string motion_type = "";
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
+        spdlog::info("Robot holding current pose");
+        motion_type = "hold";
+    } else {
+        spdlog::info("Robot running joint sine-sweep");
+        motion_type = "sine-sweep";
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Real-time Joint Position Control
+        // =========================================================================================
+        // Direct joint control can be executed by single-arm joint groups and the external axis
+        auto exe_groups = robot.info().single_arm_groups;
+        if (exe_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+        // The external axis joint group (if it exists) also supports direct joint control
+        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
+            exe_groups.emplace(rdk::JointGroup::EXT_AXIS,
+                robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
+        }
+
+        // Switch to real-time joint position control mode
+        robot.SwitchMode(rdk::Mode::RT_JOINT_POSITION);
+
+        // Set initial joint positions
+        std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : exe_groups) {
+            all_init_pos[group] = robot_states.at(group).q;
+            spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
+                rdk::utility::Vec2Str(all_init_pos.at(group)));
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
+                              std::cref(all_init_pos)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/intermediate2_realtime_joint_impedance_control.cpp b/thirdparty/flexiv_rdk/example/intermediate2_realtime_joint_impedance_control.cpp
new file mode 100644
index 0000000..1de1104
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/intermediate2_realtime_joint_impedance_control.cpp
@@ -0,0 +1,226 @@
+/**
+ * @example intermediate2_realtime_joint_impedance_control.cpp
+ * This tutorial runs real-time joint impedance control to hold or sine-sweep all robot joints.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** Sine-sweep trajectory amplitude and frequency */
+constexpr double kSineAmp = 0.035;
+constexpr double kSineFreq = 0.3;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--hold]" << std::endl;
+    std::cout << "    --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
+    const std::map<rdk::JointGroup, std::string>& single_arm_groups,
+    const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
+{
+    // Local periodic loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtJointPositionCmd> rt_cmds;
+
+        if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
+            throw std::invalid_argument(
+                "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
+        }
+
+        for (const auto& [group, init_pos] : all_init_pos) {
+            std::vector<double> target_pos(init_pos.size());
+            std::vector<double> target_vel(init_pos.size());
+            std::vector<double> target_acc(init_pos.size());
+
+            if (motion_type == "hold") {
+                target_pos = init_pos;
+            } else {
+                for (size_t i = 0; i < target_pos.size(); ++i) {
+                    target_pos[i]
+                        = init_pos[i]
+                          + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
+                }
+            }
+
+            rt_cmds[group] = rdk::RtJointPositionCmd(target_pos, target_vel, target_acc);
+        }
+
+        // Reduce stiffness to half of nominal values after 5 seconds
+        if (loop_counter == 5000) {
+            for (const auto& [group, _] : single_arm_groups) {
+                auto new_Kq = robot.info().K_q_nom.at(group);
+                for (auto& v : new_Kq) {
+                    v *= 0.5;
+                }
+                robot.SetJointImpedance(group, new_Kq);
+                spdlog::info("[{}] Joint stiffness set to: [{}]", rdk::kJointGroupNames.at(group),
+                    rdk::utility::Vec2Str(new_Kq));
+            }
+        }
+
+        // Reset stiffness to nominal values after another 5 seconds
+        if (loop_counter == 10000) {
+            for (const auto& [group, _] : single_arm_groups) {
+                const auto nominal_Kq = robot.info().K_q_nom.at(group);
+                robot.SetJointImpedance(group, nominal_Kq);
+                spdlog::info("[{}] Joint stiffness reset to nominal: [{}]",
+                    rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(nominal_Kq));
+            }
+        }
+
+        // Send commands
+        robot.StreamJointPosition(rt_cmds);
+
+        // Increment loop counter
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time joint impedance control to "
+        "hold or sine-sweep all robot joints.\n");
+
+    // Type of motion specified by user
+    std::string motion_type = "";
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
+        spdlog::info("Robot holding current pose");
+        motion_type = "hold";
+    } else {
+        spdlog::info("Robot running joint sine-sweep");
+        motion_type = "sine-sweep";
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Real-time Joint Impedance Control
+        // =========================================================================================
+        // Direct joint control can be executed by single-arm joint groups and the external axis
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+        // The external axis joint group (if it exists) also supports direct joint control
+        auto exe_groups = single_arm_groups;
+        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
+            exe_groups.emplace(
+                rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
+        }
+
+        // Switch to real-time joint impedance control mode
+        robot.SwitchMode(rdk::Mode::RT_JOINT_IMPEDANCE);
+
+        // Set initial joint positions
+        std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : exe_groups) {
+            all_init_pos[group] = robot_states.at(group).q;
+            spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
+                rdk::utility::Vec2Str(all_init_pos.at(group)));
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
+                              std::cref(single_arm_groups), std::cref(all_init_pos)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/intermediate3_realtime_joint_torque_control.cpp b/thirdparty/flexiv_rdk/example/intermediate3_realtime_joint_torque_control.cpp
new file mode 100644
index 0000000..219c13d
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/intermediate3_realtime_joint_torque_control.cpp
@@ -0,0 +1,220 @@
+/**
+ * @example intermediate3_realtime_joint_torque_control.cpp
+ * This tutorial runs real-time joint torque control to hold or sine-sweep all robot joints. An
+ * outer position loop is used to generate joint torque commands. This outer position loop + inner
+ * torque loop together is also known as an impedance controller.
+ * @warning The impedance controller in this example is for demo purpose only and has no performance
+ * guarantee. To use the robot's built-in high-performance joint impedance controller, please refer
+ * to intermediate2_realtime_joint_impedance_control.cpp.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** Outer position loop (impedance) gains, values are only for demo purpose */
+const std::vector<double> kImpedanceKp = {3000.0, 3000.0, 800.0, 800.0, 200.0, 200.0, 200.0};
+const std::vector<double> kImpedanceKd = {80.0, 80.0, 40.0, 40.0, 8.0, 8.0, 8.0};
+
+/** Sine-sweep trajectory amplitude and frequency */
+constexpr double kSineAmp = 0.035;
+constexpr double kSineFreq = 0.3;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--hold]" << std::endl;
+    std::cout << "    --hold: robot holds current joint positions, otherwise do a sine-sweep" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot, const std::string& motion_type,
+    const std::map<rdk::JointGroup, std::vector<double>>& all_init_pos)
+{
+    // Local periodic loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        if ((motion_type != "hold") && (motion_type != "sine-sweep")) {
+            throw std::invalid_argument(
+                "PeriodicTask: Unknown motion type. Accepted motion types: hold, sine-sweep");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtJointTorqueCmd> rt_cmds;
+        const auto all_states = robot.states();
+        for (const auto& [group, init_pos] : all_init_pos) {
+            std::vector<double> target_pos(init_pos.size());
+            if (motion_type == "hold") {
+                target_pos = init_pos;
+            } else {
+                for (size_t i = 0; i < target_pos.size(); ++i) {
+                    target_pos[i]
+                        = init_pos[i]
+                          + kSineAmp * sin(2 * M_PI * kSineFreq * loop_counter * kLoopPeriod);
+                }
+            }
+
+            // Compute target joint torque using outer position loop (impedance)
+            std::vector<double> target_torque(target_pos.size());
+            // Set target torque for external axis to zero, as the demo impedance gains are defined
+            // for only the arms
+            if (group != rdk::JointGroup::EXT_AXIS) {
+                const auto& states = all_states.at(group);
+                for (size_t i = 0; i < target_torque.size(); ++i) {
+                    target_torque[i] = kImpedanceKp[i] * (target_pos[i] - states.q[i])
+                                       - kImpedanceKd[i] * states.dtheta[i];
+                }
+            }
+
+            rt_cmds[group] = rdk::RtJointTorqueCmd(target_torque, true, true);
+        }
+
+        // Send target joint torque to RDK server
+        robot.StreamJointTorque(rt_cmds);
+
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time joint torque control to hold "
+        "or sine-sweep all robot joints. An outer position loop is used to generate joint torque "
+        "commands. This outer position loop + inner torque loop together is also known as an "
+        "impedance controller.\n");
+
+    // Type of motion specified by user
+    std::string motion_type = "";
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
+        spdlog::info("Robot holding current pose");
+        motion_type = "hold";
+    } else {
+        spdlog::info("Robot running joint sine-sweep");
+        motion_type = "sine-sweep";
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Real-time Joint Torque Control
+        // =========================================================================================
+        // Direct joint control can be executed by single-arm joint groups and the external axis
+        auto exe_groups = robot.info().single_arm_groups;
+        if (exe_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+        // The external axis joint group (if it exists) also supports direct joint control
+        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
+            exe_groups.emplace(
+                rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
+        }
+
+        // Switch to real-time joint torque control mode
+        robot.SwitchMode(rdk::Mode::RT_JOINT_TORQUE);
+
+        // Set initial joint positions
+        std::map<rdk::JointGroup, std::vector<double>> all_init_pos;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : exe_groups) {
+            all_init_pos[group] = robot_states.at(group).q;
+            spdlog::info("[{}] Initial joint positions: {}", rdk::kJointGroupNames.at(group),
+                rdk::utility::Vec2Str(all_init_pos.at(group)));
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::ref(motion_type),
+                              std::cref(all_init_pos)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/intermediate4_realtime_joint_floating.cpp b/thirdparty/flexiv_rdk/example/intermediate4_realtime_joint_floating.cpp
new file mode 100644
index 0000000..a743e5a
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/intermediate4_realtime_joint_floating.cpp
@@ -0,0 +1,167 @@
+/**
+ * @example intermediate4_realtime_joint_floating.cpp
+ * This tutorial runs real-time joint floating with gentle velocity damping, gravity compensation,
+ * and soft protection against position limits. This example is ideal for verifying the system's
+ * whole-loop real-timeliness, accuracy of the robot dynamics model, and joint torque control
+ * performance. If everything works well, all joints should float smoothly.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** Joint velocity damping gains for floating */
+const std::vector<double> kFloatingDamping = {10.0, 10.0, 5.0, 5.0, 1.0, 1.0, 1.0};
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot, const std::map<rdk::JointGroup, std::string>& exe_groups)
+{
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtJointTorqueCmd> rt_cmds;
+        const auto all_states = robot.states();
+        for (const auto& [group, _] : exe_groups) {
+            const auto& states = all_states.at(group);
+            std::vector<double> target_torque(states.q.size());
+
+            // Don't add damping for external axis
+            if (group != rdk::JointGroup::EXT_AXIS) {
+                // Add some velocity damping
+                for (size_t i = 0; i < target_torque.size(); ++i) {
+                    target_torque[i] += -kFloatingDamping[i] * states.dtheta[i];
+                }
+            }
+
+            rt_cmds[group] = rdk::RtJointTorqueCmd(target_torque, true, true);
+        }
+
+        // Send target joint torque to RDK server, enable gravity compensation and joint limits soft
+        // protection
+        robot.StreamJointTorque(rt_cmds);
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time joint floating with gentle "
+        "velocity damping, gravity compensation, and soft protection against position limits. This "
+        "example is ideal for verifying the system's whole-loop real-timeliness, accuracy of the "
+        "robot dynamics model, and joint torque control performance. If everything works well, all "
+        "joints should float smoothly.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Real-time Joint Floating
+        // =========================================================================================
+        // Direct joint control can be executed by single-arm joint groups and the external axis
+        auto exe_groups = robot.info().single_arm_groups;
+        if (exe_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+        // The external axis joint group (if exists) also supports direct joint control
+        if (robot.info().all_groups.contains(rdk::JointGroup::EXT_AXIS)) {
+            exe_groups.emplace(
+                rdk::JointGroup::EXT_AXIS, robot.info().all_groups.at(rdk::JointGroup::EXT_AXIS));
+        }
+
+        // Switch to real-time joint torque control mode
+        robot.SwitchMode(rdk::Mode::RT_JOINT_TORQUE);
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::cref(exe_groups)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/intermediate5_realtime_cartesian_pure_motion_control.cpp b/thirdparty/flexiv_rdk/example/intermediate5_realtime_cartesian_pure_motion_control.cpp
new file mode 100644
index 0000000..b8fef76
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/intermediate5_realtime_cartesian_pure_motion_control.cpp
@@ -0,0 +1,346 @@
+/**
+ * @example intermediate5_realtime_cartesian_pure_motion_control.cpp
+ * This tutorial runs real-time Cartesian-space pure motion control to hold or sine-sweep the robot
+ * TCP. A simple collision detection is also included.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <cmath>
+#include <thread>
+#include <atomic>
+#include <algorithm>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop frequency [Hz] */
+constexpr size_t kLoopFreq = 1000;
+
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** TCP sine-sweep amplitude [m] */
+constexpr double kSwingAmp = 0.1;
+
+/** TCP sine-sweep frequency [Hz] */
+constexpr double kSwingFreq = 0.3;
+
+/** External TCP force threshold for collision detection, value is only for demo purpose [N] */
+constexpr double kExtForceThreshold = 10.0;
+
+/** External joint torque threshold for collision detection, value is only for demo purpose [Nm] */
+constexpr double kExtTorqueThreshold = 5.0;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--hold] [--collision]" << std::endl;
+    std::cout << "    --hold: robot holds current TCP pose, otherwise do a sine-sweep" << std::endl;
+    std::cout << "    --collision: enable collision detection, robot will stop upon collision" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot,
+    const std::map<rdk::JointGroup, std::string>& single_arm_groups,
+    const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_pose,
+    const std::map<rdk::JointGroup, std::vector<double>>& all_init_q, bool enable_hold,
+    bool enable_collision)
+{
+    // Local periodic loop counter
+    static uint64_t loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
+        for (const auto& [group, init_pose] : all_init_pose) {
+            auto target_pose = init_pose;
+
+            // Sine-sweep TCP along Y axis
+            if (!enable_hold) {
+                target_pose[1]
+                    = init_pose[1]
+                      + kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
+            }
+            // Otherwise robot TCP will hold at initial pose
+
+            rt_cmds[group] = rdk::RtCartesianCmd(target_pose);
+        }
+
+        // Send command. Calling this method with only target pose input results in pure motion
+        // control
+        robot.StreamCartesianMotionForce(rt_cmds);
+
+        // Do the following operations in sequence for every 20 seconds
+        switch (loop_counter % (20 * kLoopFreq)) {
+            // Online change reference joint positions at 3 seconds
+            case (3 * kLoopFreq): {
+                const std::vector<double> ref_q
+                    = {0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658};
+                for (const auto& [group, _] : single_arm_groups) {
+                    robot.SetNullSpacePosture(group, ref_q);
+                }
+                spdlog::info("Reference joint positions updated for all groups");
+            } break;
+            // Online change stiffness to half of nominal at 6 seconds
+            case (6 * kLoopFreq): {
+                for (const auto& [group, _] : single_arm_groups) {
+                    auto new_K = robot.info().K_x_nom.at(group);
+                    for (auto& v : new_K) {
+                        v *= 0.5;
+                    }
+                    robot.SetCartesianImpedance(group, new_K);
+                    spdlog::info("[{}] Cartesian stiffness set to: [{}]",
+                        rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(new_K));
+                }
+            } break;
+            // Online change to another reference joint positions at 9 seconds
+            case (9 * kLoopFreq): {
+                const std::vector<double> ref_q
+                    = {-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658};
+                for (const auto& [group, _] : single_arm_groups) {
+                    robot.SetNullSpacePosture(group, ref_q);
+                }
+                spdlog::info("Reference joint positions updated for all groups");
+            } break;
+            // Online reset stiffness to nominal at 12 seconds
+            case (12 * kLoopFreq): {
+                for (const auto& [group, _] : single_arm_groups) {
+                    const auto nominal_K = robot.info().K_x_nom.at(group);
+                    robot.SetCartesianImpedance(group, nominal_K);
+                    spdlog::info("[{}] Cartesian stiffness reset to nominal: [{}]",
+                        rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(nominal_K));
+                }
+            } break;
+            // Online reset reference joint positions to nominal at 14 seconds
+            case (14 * kLoopFreq): {
+                for (const auto& [group, init_q] : all_init_q) {
+                    robot.SetNullSpacePosture(group, init_q);
+                    spdlog::info("[{}] Reference joint positions reset to initial: [{}]",
+                        rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(init_q));
+                }
+            } break;
+            // Online enable max contact wrench regulation at 16 seconds
+            case (16 * kLoopFreq): {
+                std::array<double, rdk::kCartDoF> max_wrench = {10.0, 10.0, 10.0, 2.0, 2.0, 2.0};
+                for (const auto& [group, _] : single_arm_groups) {
+                    robot.SetMaxContactWrench(group, max_wrench);
+                    spdlog::info("[{}] Max contact wrench set to: {}",
+                        rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(max_wrench));
+                }
+            } break;
+            // Disable max contact wrench regulation at 19 seconds
+            case (19 * kLoopFreq): {
+                std::array<double, rdk::kCartDoF> inf;
+                inf.fill(std::numeric_limits<double>::infinity());
+                for (const auto& [group, _] : single_arm_groups) {
+                    robot.SetMaxContactWrench(group, inf);
+                    spdlog::info("[{}] Max contact wrench regulation is disabled",
+                        rdk::kJointGroupNames.at(group));
+                }
+            } break;
+            default:
+                break;
+        }
+
+        // Simple collision detection: stop robot if collision is detected from either end-effector
+        // or robot body
+        if (enable_collision) {
+            for (const auto& [group, states] : robot.states()) {
+                bool collision_detected = false;
+                Eigen::Vector3d ext_force
+                    = {states.tcp_wrench[0], states.tcp_wrench[1], states.tcp_wrench[2]};
+                if (ext_force.norm() > kExtForceThreshold) {
+                    collision_detected = true;
+                }
+                for (const auto& v : states.tau_ext) {
+                    if (fabs(v) > kExtTorqueThreshold) {
+                        collision_detected = true;
+                    }
+                }
+
+                if (collision_detected) {
+                    robot.Stop();
+                    spdlog::warn("[{}] Collision detected, stopping robot and exit program ...",
+                        rdk::kJointGroupNames.at(group));
+                    g_stop_sched = true;
+                    break;
+                }
+            }
+        }
+
+        // Increment loop counter
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space pure motion "
+        "control to hold or sine-sweep the robot TCP. A simple collision detection is also "
+        "included.\n");
+
+    // Type of motion specified by user
+    bool enable_hold = false;
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--hold")) {
+        spdlog::info("Robot holding current TCP pose");
+        enable_hold = true;
+    } else {
+        spdlog::info("Robot running TCP sine-sweep");
+    }
+
+    // Whether to enable collision detection
+    bool enable_collision = false;
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--collision")) {
+        spdlog::info("Collision detection enabled");
+        enable_collision = true;
+    } else {
+        spdlog::info("Collision detection disabled");
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Zero Force-torque Sensor
+        // =========================================================================================
+        // Direct Cartesian control can only be executed by single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+        // IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
+        std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // WARNING: during the process, the robot must not contact anything, otherwise the result
+        // will be inaccurate and affect following operations
+        spdlog::warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot");
+
+        // Wait for primitive to finish
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Sensor zeroing complete");
+
+        // Configure Motion Control
+        // =========================================================================================
+        // The Cartesian motion force modes do pure motion control out of the box, thus nothing
+        // needs to be explicitly configured
+
+        // NOTE: motion control always uses robot world frame, while force control can use
+        // either world or TCP frame as reference frame
+
+        // Start Pure Motion Control
+        // =========================================================================================
+        // Switch to real-time mode for continuous motion control
+        robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
+
+        // Set all Cartesian axis(s) to motion control
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetForceControlAxis(
+                group, std::array<bool, rdk::kCartDoF> {false, false, false, false, false, false});
+        }
+
+        // Save initial poses and joint positions
+        std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
+        std::map<rdk::JointGroup, std::vector<double>> all_init_q;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : single_arm_groups) {
+            all_init_pose[group] = robot_states.at(group).tcp_pose;
+            all_init_q[group] = robot_states.at(group).q;
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(
+            std::bind(PeriodicTask, std::ref(robot), std::cref(single_arm_groups),
+                std::cref(all_init_pose), std::cref(all_init_q), enable_hold, enable_collision),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/intermediate6_realtime_cartesian_motion_force_control.cpp b/thirdparty/flexiv_rdk/example/intermediate6_realtime_cartesian_motion_force_control.cpp
new file mode 100644
index 0000000..8e726e4
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/intermediate6_realtime_cartesian_motion_force_control.cpp
@@ -0,0 +1,335 @@
+/**
+ * @example intermediate6_realtime_cartesian_motion_force_control.cpp
+ * This tutorial runs real-time Cartesian-space unified motion-force control. The Z axis of the
+ * chosen reference frame will be activated for explicit force control, while the rest axes in the
+ * same reference frame will stay motion controlled.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <cmath>
+#include <thread>
+#include <atomic>
+#include <algorithm>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+constexpr double kLoopPeriod = 0.001;
+
+/** TCP sine-sweep amplitude [m] */
+constexpr double kSwingAmp = 0.1;
+
+/** TCP sine-sweep frequency [Hz] */
+constexpr double kSwingFreq = 0.3;
+
+/** Pressing force to apply during the unified motion-force control [N] */
+constexpr double kPressingForce = 5.0;
+
+/** Cartesian linear velocity used to search for contact [m/s] */
+constexpr double kSearchVelocity = 0.02;
+
+/** Maximum distance to travel when searching for contact [m] */
+constexpr double kSearchDistance = 1.0;
+
+/** Maximum contact wrench during contact search for soft contact */
+const std::array<double, rdk::kCartDoF> kMaxWrenchForContactSearch
+    = {10.0, 10.0, 10.0, 3.0, 3.0, 3.0};
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: [--TCP] [--polish]" << std::endl;
+    std::cout << "    --TCP: use TCP frame as reference frame for force control, otherwise use world frame" << std::endl;
+    std::cout << "    --polish: run a simple polish motion along XY plane in world frame, otherwise hold robot motion in non-force-control axes"
+              << std::endl
+              << std::endl;
+    // clang-format on
+}
+
+/** Callback function for realtime periodic task */
+void PeriodicTask(rdk::Robot& robot,
+    const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_pose,
+    rdk::CoordType force_ctrl_frame, bool enable_polish)
+{
+    // Local periodic loop counter
+    static uint64_t loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        // Set Fz according to reference frame to achieve a "pressing down" behavior
+        double Fz = 0.0;
+        if (force_ctrl_frame == rdk::CoordType::WORLD) {
+            Fz = kPressingForce;
+        } else if (force_ctrl_frame == rdk::CoordType::TCP) {
+            Fz = -kPressingForce;
+        }
+        std::array<double, rdk::kCartDoF> target_wrench = {0.0, 0.0, Fz, 0.0, 0.0, 0.0};
+
+        std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
+        for (const auto& [group, init_pose] : all_init_pose) {
+            auto target_pose = init_pose;
+
+            // Apply constant force along Z axis of chosen reference frame, and do a simple polish
+            // motion along XY plane in robot world frame
+            if (enable_polish) {
+                // Create motion command to sine-sweep along Y direction
+                target_pose[1]
+                    = init_pose[1]
+                      + kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
+            }
+
+            rt_cmds[group] = rdk::RtCartesianCmd(target_pose, target_wrench);
+        }
+
+        // Command target pose and target wrench
+        robot.StreamCartesianMotionForce(rt_cmds);
+
+        // Increment loop counter
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs real-time Cartesian-space unified "
+        "motion-force control. The Z axis of the chosen reference frame will be activated for "
+        "explicit force control, while the rest axes in the same reference frame will stay motion "
+        "controlled.\n");
+
+    // The reference frame used for force control, see Robot::SetForceControlFrame()
+    auto force_ctrl_frame = rdk::CoordType::WORLD;
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--TCP")) {
+        spdlog::info("Reference frame used for force control: robot TCP frame");
+        force_ctrl_frame = rdk::CoordType::TCP;
+    } else {
+        spdlog::info("Reference frame used for force control: robot world frame");
+    }
+
+    // Whether to enable polish motion
+    bool enable_polish = false;
+    if (rdk::utility::ProgramArgsExist(argc, argv, "--polish")) {
+        spdlog::info("Robot will run a polish motion along XY plane in robot world frame");
+        enable_polish = true;
+    } else {
+        spdlog::info("Robot will hold its motion in all non-force-controlled axes");
+    }
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Zero Force-torque Sensor
+        // =========================================================================================
+        // Direct Cartesian control can only be executed by single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        robot.SwitchMode(rdk::Mode::NRT_PRIMITIVE_EXECUTION);
+        // IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
+        std::map<rdk::JointGroup, rdk::PrimitiveArgs> pt_args;
+        for (const auto& [group, _] : single_arm_groups) {
+            pt_args[group] = rdk::PrimitiveArgs("ZeroFTSensor", {});
+        }
+        robot.ExecutePrimitive(pt_args);
+
+        // WARNING: during the process, the robot must not contact anything, otherwise the result
+        // will be inaccurate and affect following operations
+        spdlog::warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot");
+
+        // Wait for primitive to finish
+        while (!rdk::utility::PrimitiveStateTrueForGroups(robot.primitive_states(), "terminated")) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Sensor zeroing complete");
+
+        // Search for Contact
+        // =========================================================================================
+        // NOTE: there are several ways to do contact search, such as using primitives, or real-time
+        // and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian
+        // control for example.
+        spdlog::info("Searching for contact ...");
+
+        // Set initial poses to current TCP poses
+        std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : single_arm_groups) {
+            all_init_pose[group] = robot_states.at(group).tcp_pose;
+            spdlog::info("[{}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {}",
+                rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(all_init_pose.at(group)));
+        }
+
+        // Use non-real-time mode to make the robot go to a set point with its own motion generator
+        robot.SwitchMode(rdk::Mode::NRT_CARTESIAN_MOTION_FORCE);
+
+        // Search for contact with max contact wrench set to a small value for making soft contact
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetMaxContactWrench(group, kMaxWrenchForContactSearch);
+        }
+
+        // Set target point along -Z direction and expect contact to happen during the travel
+        std::map<rdk::JointGroup, rdk::NrtCartesianCmd> nrt_cmds;
+        for (const auto& [group, init_pose] : all_init_pose) {
+            auto target_pose = init_pose;
+            target_pose[2] -= kSearchDistance;
+            nrt_cmds[group] = rdk::NrtCartesianCmd(target_pose, {}, {}, kSearchVelocity);
+        }
+
+        // Send target point to robot to start searching for contact and limit the velocity. Keep
+        // target wrench 0 at this stage since we are not doing force control yet
+        robot.SendCartesianMotionForce(nrt_cmds);
+
+        // Use a while loop to poll robot states and check if a contact is made
+        bool is_contacted = false;
+        while (!is_contacted) {
+            // Compute norm of sensed external force applied on robot TCP
+            for (const auto& [group, states] : robot.states()) {
+                Eigen::Vector3d ext_force
+                    = {states.tcp_wrench[0], states.tcp_wrench[1], states.tcp_wrench[2]};
+
+                // Contact is considered to be made if sensed TCP force exceeds the threshold
+                if (ext_force.norm() > kPressingForce) {
+                    is_contacted = true;
+                    spdlog::info(
+                        "[{}] Contact detected at robot TCP", rdk::kJointGroupNames.at(group));
+                    break;
+                }
+            }
+            // Check at 1ms interval
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+
+        // Configure Force Control
+        // =========================================================================================
+        // Switch to real-time mode for continuous motion force control
+        robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
+
+        // Set force control reference frame based on program argument. See function doc for more
+        // details
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetForceControlFrame(group, force_ctrl_frame);
+        }
+
+        // Set which Cartesian axis(s) to activate for force control. See function doc for more
+        // details. Here we only active Z axis
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetForceControlAxis(
+                group, std::array<bool, rdk::kCartDoF> {false, false, true, false, false, false});
+        }
+
+        // Uncomment the following line to enable passive force control, otherwise active force
+        // control is used by default. See function doc for more details
+        /*
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetPassiveForceControl(group, true);
+        }
+        */
+
+        // NOTE: motion control always uses robot world frame, while force control can use
+        // either world or TCP frame as reference frame
+
+        // Start Unified Motion Force Control
+        // =========================================================================================
+        // Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis
+        // is activated (i.e. motion control disabled in Z axis) and the motion force control mode
+        // is entered, this way the contact force along Z axis is explicitly regulated and will not
+        // spike after the max contact wrench regulation for motion control is disabled
+        std::array<double, rdk::kCartDoF> inf;
+        inf.fill(std::numeric_limits<double>::infinity());
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetMaxContactWrench(group, inf);
+        }
+
+        // Update initial poses to current TCP poses
+        for (auto& [group, pose] : all_init_pose) {
+            pose = robot.states().at(group).tcp_pose;
+        }
+
+        // Create real-time scheduler to run periodic tasks
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::cref(all_init_pose),
+                              std::ref(force_ctrl_frame), enable_polish),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example/intermediate7_robot_dynamics.cpp b/thirdparty/flexiv_rdk/example/intermediate7_robot_dynamics.cpp
new file mode 100644
index 0000000..342522f
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example/intermediate7_robot_dynamics.cpp
@@ -0,0 +1,158 @@
+/**
+ * @example intermediate7_robot_dynamics.cpp
+ * This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and
+ * gravity torques. Also checks reachability of a Cartesian pose.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/model.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <chrono>
+#include <mutex>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain "
+        "robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian "
+        "pose.\n");
+
+    try {
+        // RDK Initialization
+        // =========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        spdlog::info("Moving to home pose");
+        robot.Home();
+
+        // Robot Dynamics
+        // =========================================================================================
+        // Initialize dynamics engine
+        rdk::Model model(robot);
+
+        // Step dynamics engine 5 times
+        for (size_t i = 0; i < 5; i++) {
+            // Mark timer start point
+            auto tic = std::chrono::high_resolution_clock::now();
+
+            // Update robot model in dynamics engine
+            const auto robot_states = robot.states();
+            model.Update(robot_states.at(rdk::JointGroup::ALL).q,
+                robot_states.at(rdk::JointGroup::ALL).dq);
+
+            // Compute gravity vector
+            auto g = model.g();
+
+            // Compute mass matrix
+            auto M = model.M();
+
+            // Compute Jacobian
+            auto J = model.J("flange");
+
+            // Mark timer end point and get loop time
+            auto toc = std::chrono::high_resolution_clock::now();
+            auto computation_time
+                = std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
+
+            // Print time used to compute g, M, J
+            spdlog::info("Computation time = {} us", computation_time);
+            // Print gravity
+            std::cout << "g = \n"
+                      << std::fixed << std::setprecision(5) << g.transpose() << std::endl;
+            // Print mass matrix
+            std::cout << "M = \n" << std::fixed << std::setprecision(5) << M << std::endl;
+            // Print Jacobian
+            std::cout << "J = \n" << std::fixed << std::setprecision(5) << J << std::endl;
+            std::cout << std::endl;
+        }
+
+        // Check IK feasibility for a nearby Cartesian pose on all available single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        const auto robot_states = robot.states();
+        std::map<rdk::JointGroup, rdk::IKParams> ik_params_by_group;
+        for (const auto& [group, _] : single_arm_groups) {
+            auto pose_to_check = robot_states.at(group).tcp_pose;
+            pose_to_check[0] += 0.1;
+            spdlog::info("[{}] Checking IK feasibility of Cartesian pose [{}]",
+                rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(pose_to_check));
+
+            rdk::IKParams ik_params;
+            ik_params.cartesian_pose = pose_to_check;
+            ik_params.seed_q = robot_states.at(group).q;
+            ik_params.free_orientation = false;
+            ik_params_by_group[group] = ik_params;
+        }
+
+        // Print result
+        auto result = model.SolveConstrainedIK(ik_params_by_group);
+        spdlog::info("IK result success = {}", result.success);
+        for (const auto& [group, q] : result.solved_q) {
+            spdlog::info(
+                "[{}] solved_q = [{}]", rdk::kJointGroupNames.at(group), rdk::utility::Vec2Str(q));
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/example_py/AGENT_REFERENCE_primitives_only.py b/thirdparty/flexiv_rdk/example_py/AGENT_REFERENCE_primitives_only.py
new file mode 100644
index 0000000..945828a
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/AGENT_REFERENCE_primitives_only.py
@@ -0,0 +1,409 @@
+#!/usr/bin/env python
+
+"""AGENT_REFERENCE_primitives_only.py
+
+Comprehensive primitive-only reference script for code-generation agents.
+
+Primitives demonstrated (in order):
+    1. Home          - move robot to the platform-defined home posture.
+    2. ZeroFTSensor  - zero force/torque sensors for all active single-arm groups.
+    3. MoveJ         - joint-space move with waypoints and joint-velocity scaling.
+    4. MoveL         - Cartesian linear move in WORLD frame with velocity, blending (zoneRadius).
+    5. MoveL         - multi-waypoint sweep in WORLD frame.
+    6. Home          - return to home before TCP-frame relative move.
+    7. MoveL         - relative move in TCP frame (TRAJ::START), orientation-only rotation.
+    8. MoveJ         - return to upright posture (all joints 0).
+    9. Home          - return to home.
+
+Key patterns shown:
+    - Fault clear, enable, wait operational, mode switch (essential setup).
+    - exec_prim() wrapper accepts group-indexed PrimitiveArgs for per-group primitives.
+    - Transition keys must be looked up per primitive from Flexiv primitive documentation:
+        https://www.flexiv.com/software/primitives
+    - Reading tcp_pose from each joint group to extract live orientation.
+    - Converting quaternion to Euler ZYX degrees using utility.quat2eulerZYX for MoveL orientation.
+    - ZeroFTSensor is executed for all selected groups with explicit completion checks.
+    - MoveL with optional velocity, zoneRadius blending, and waypoints.
+    - MoveJ with optional waypoints list and jntVelScale.
+    - All JPos calls use 7-element arm-joint list; second (external-axis) argument is optional.
+    - All Coord calls use positional args: (position, orientation_eulerZYX_deg, ref_frame_list).
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import argparse
+import time
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+import utility
+
+
+# ──────────────────────────────────────────────────────────────────────────────
+# Helpers
+# ──────────────────────────────────────────────────────────────────────────────
+def normalize_euler_deg(euler_deg):
+    """Normalize one angle or a 3-element Euler list to the robot's [-180, 180] convention."""
+
+    def normalize_angle(angle_deg):
+        normalized = (angle_deg + 180.0) % 360.0 - 180.0
+        if normalized == -180.0 and angle_deg > 0:
+            return 180.0
+        return normalized
+
+    if isinstance(euler_deg, (int, float)):
+        return normalize_angle(euler_deg)
+
+    return [normalize_angle(angle) for angle in euler_deg]
+
+
+def wait_until_operational(robot, dt=1.0, timeout_s=120.0):
+    """Block until robot.operational() is True, or raise TimeoutError."""
+    t0 = time.time()
+    while not robot.operational():
+        if time.time() - t0 > timeout_s:
+            raise TimeoutError("Timed out waiting for robot to become operational")
+        time.sleep(dt)
+
+
+def wait_primitive_transition(robot, transition_keys_by_group, dt=0.2, timeout_s=60.0):
+    """Block until primitive_states() for all groups satisfy their transition key.
+
+    Use the primitive's default transition key unless a custom transition condition
+    is intended. Check primitive docs to confirm available primitive state keys.
+    """
+    t0 = time.time()
+    while True:
+        if utility.primitive_state_true_for_groups(
+            robot.primitive_states(),
+            transition_keys_by_group,
+        ):
+            return
+
+        if time.time() - t0 > timeout_s:
+            raise TimeoutError(
+                "Timed out waiting for primitive transition states: "
+                f"{ {flexivrdk.kJointGroupNames[g]: k for g, k in transition_keys_by_group.items()} }"
+            )
+        time.sleep(dt)
+
+
+def exec_prim(robot, primitive_args_by_group, transition_keys_by_group):
+    """Execute primitive command(s) and block until all transitions are satisfied.
+
+    Args:
+        robot:          flexivrdk.Robot instance.
+        primitive_args_by_group:
+            Dict[JointGroup, flexivrdk.PrimitiveArgs] for per-group primitive commands.
+        transition_keys_by_group:
+            Dict[JointGroup, str] where each value is the transition key for that group.
+            This can be the primitive's default key or another primitive state key.
+            Look up primitive states from:
+            https://www.flexiv.com/software/primitives
+
+        Note:
+        This helper waits on primitive_states() only. If transition depends on
+        robot states, implement a custom wait loop at call site.
+    """
+    if not primitive_args_by_group:
+        raise ValueError("primitive_args_by_group cannot be empty")
+
+    if set(primitive_args_by_group.keys()) != set(transition_keys_by_group.keys()):
+        raise ValueError(
+            "primitive_args_by_group and transition_keys_by_group must have identical groups"
+        )
+
+    robot.ExecutePrimitive(primitive_args_by_group)
+    wait_primitive_transition(robot, transition_keys_by_group)
+
+
+def prepare_robot(robot_sn, logger):
+    """Connect, clear faults, enable robot, and switch to primitive execution mode."""
+    robot = flexivrdk.Robot(robot_sn)
+
+    if robot.fault():
+        logger.warn("Fault detected on connected robot, trying to clear")
+        if not robot.ClearFault():
+            raise RuntimeError("Failed to clear robot fault")
+        logger.info("Fault cleared")
+
+    logger.info("Enabling robot")
+    robot.ServoOn()
+    wait_until_operational(robot)
+    logger.info("Robot is operational")
+
+    robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
+    return robot
+
+
+def current_euler_zyx_deg(robot, group):
+    """Return live TCP orientation of a specific joint group as Euler ZYX [x,y,z] degrees.
+
+    tcp_pose layout: [x, y, z, q_w, q_x, q_y, q_z] (SI units, quaternion).
+    utility.quat2eulerZYX() expects [w, x, y, z] order.
+    The returned Euler values are normalized to the robot's [-180, 180] convention.
+    """
+    all_states = robot.states()
+    if group not in all_states:
+        raise ValueError(
+            f"Requested joint group has no available robot state: "
+            f"{flexivrdk.kJointGroupNames[group]}"
+        )
+
+    pose = all_states[group].tcp_pose
+    return utility.quat2eulerZYX([pose[3], pose[4], pose[5], pose[6]], degree=True)
+
+
+# ──────────────────────────────────────────────────────────────────────────────
+# Main sequence
+# ──────────────────────────────────────────────────────────────────────────────
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove spaces, e.g. Enlight-L-123456",
+    )
+    args = parser.parse_args()
+
+    logger = spdlog.ConsoleLogger("AgentReference")
+
+    try:
+        robot = prepare_robot(args.robot_sn, logger)
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # ── 1) Move to home pose ──────────────────────────────────────────────
+        logger.info("Step 1: Move to home pose")
+        robot.Home()
+
+        # ── 2) ZeroFTSensor ───────────────────────────────────────────────────
+        # Use this primitive's documented default transition condition.
+        # The robot must not be in contact with anything during zeroing.
+        logger.info("Step 2: ZeroFTSensor")
+        logger.warn("Zeroing F/T sensors - ensure nothing contacts the robot")
+        exec_prim(
+            robot,
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "ZeroFTSensor",
+                    {
+                        "dataCollectTime": 0.2,
+                        "enableStaticCheck": False,
+                        "calibExtraPayload": False,
+                    },
+                )
+                for group in single_arm_groups
+            },
+            {group: "terminated" for group in single_arm_groups},
+        )
+        logger.info("F/T sensor zeroing complete")
+
+        # ── 3) MoveJ with waypoints and velocity scaling ──────────────────────
+        # target / waypoints: flexivrdk.JPos(q_m) where q_m is 7-element degrees list.
+        # jntVelScale: percentage of max joint speed [1-100].
+        logger.info("Step 3: MoveJ with waypoints")
+        exec_prim(
+            robot,
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveJ",
+                    {
+                        "target": flexivrdk.JPos([30, -45, 0, 90, 0, 40, 30]),
+                        "waypoints": [
+                            flexivrdk.JPos([10, -30, 10, 30, 10, 15, 10]),
+                            flexivrdk.JPos([20, -60, -10, 60, -10, 30, 20]),
+                        ],
+                        "jntVelScale": 50,  # 50% of max joint speed
+                        "zoneRadius": "Z50",
+                        "targetTolerLevel": 1,
+                        "enableRelativeMove": False,
+                        "jntAccMultiplier": 1,
+                    },
+                )
+                for group in single_arm_groups
+            },
+            {group: "reachedTarget" for group in single_arm_groups},
+        )
+
+        # ── 4) MoveL in WORLD frame with velocity and blending ────────────────
+        # target / waypoints: flexivrdk.Coord(position_m, orientation_eulerZYX_deg, ref_frame).
+        # ref_frame is a 2-element list: [frame_type, frame_origin].
+        # vel: TCP linear speed in m/s.
+        # zoneRadius: blending zone size string, e.g. "Z50". Use "Z0" for exact stops.
+        step4_primitive_args_by_group = {}
+        for group in single_arm_groups:
+            live_euler = current_euler_zyx_deg(robot, group)
+            wp1_euler = normalize_euler_deg(
+                [live_euler[0] + 20.0, live_euler[1] - 20.0, live_euler[2] + 10.0]
+            )
+            wp2_euler = normalize_euler_deg(
+                [live_euler[0] - 30.0, live_euler[1] + 30.0, live_euler[2] - 30.0]
+            )
+            target_euler = normalize_euler_deg(
+                [live_euler[0] + 40.0, live_euler[1] - 20.0, live_euler[2] + 20.0]
+            )
+            logger.info(
+                f"Step 4 [{flexivrdk.kJointGroupNames[group]}] live Euler ZYX (deg): {live_euler}"
+            )
+            logger.info(
+                f"Step 4 [{flexivrdk.kJointGroupNames[group]}] waypoint1 Euler ZYX (deg): {wp1_euler}"
+            )
+            logger.info(
+                f"Step 4 [{flexivrdk.kJointGroupNames[group]}] waypoint2 Euler ZYX (deg): {wp2_euler}"
+            )
+            logger.info(
+                f"Step 4 [{flexivrdk.kJointGroupNames[group]}] target Euler ZYX (deg): {target_euler}"
+            )
+
+            step4_primitive_args_by_group[group] = flexivrdk.PrimitiveArgs(
+                "MoveL",
+                {
+                    "target": flexivrdk.Coord(
+                        [0.65, -0.3, 0.3], target_euler, ["WORLD", "WORLD_ORIGIN"]
+                    ),
+                    "waypoints": [
+                        flexivrdk.Coord(
+                            [0.45, 0.1, 0.3], wp1_euler, ["WORLD", "WORLD_ORIGIN"]
+                        ),
+                        flexivrdk.Coord(
+                            [0.45, -0.3, 0.3], wp2_euler, ["WORLD", "WORLD_ORIGIN"]
+                        ),
+                    ],
+                    "vel": 0.3,  # m/s TCP linear speed
+                    "zoneRadius": "Z50",  # blended corners; use "Z0" for hard stops
+                    "targetTolerLevel": 3,
+                    "acc": 1.5,
+                    "angVel": 150,
+                    "enableFixRefJntPos": False,
+                    "refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
+                    "jerk": 50,
+                    "configOptObj": [0, 0, 0.5],
+                    "enableSixAxisJntCtrl": False,
+                    "enableNullspaceTraj": False,
+                },
+            )
+
+        logger.info("Step 4: MoveL in WORLD frame with speed and blending")
+        exec_prim(
+            robot,
+            step4_primitive_args_by_group,
+            {group: "reachedTarget" for group in single_arm_groups},
+        )
+
+        # ── 5) MoveL multi-point sweep, exact stops ───────────────────────────
+        logger.info("Step 5: MoveL sweep with exact stops")
+        for target_pos in [
+            [0.5, 0.2, 0.4],
+            [0.5, -0.2, 0.4],
+            [0.5, -0.2, 0.2],
+            [0.5, 0.2, 0.2],
+        ]:
+            exec_prim(
+                robot,
+                {
+                    group: flexivrdk.PrimitiveArgs(
+                        "MoveL",
+                        {
+                            "target": flexivrdk.Coord(
+                                target_pos, [180, 0, 180], ["WORLD", "WORLD_ORIGIN"]
+                            ),
+                            "vel": 0.2,
+                            "zoneRadius": "Z0",  # exact stop at each corner
+                            "targetTolerLevel": 3,
+                            "acc": 1.5,
+                            "angVel": 150,
+                            "enableFixRefJntPos": False,
+                            "refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
+                            "jerk": 50,
+                            "configOptObj": [0, 0, 0.5],
+                            "enableSixAxisJntCtrl": False,
+                            "enableNullspaceTraj": False,
+                        },
+                    )
+                    for group in single_arm_groups
+                },
+                {group: "reachedTarget" for group in single_arm_groups},
+            )
+
+        # ── 6) Home before TCP-frame relative move ────────────────────────────
+        logger.info("Step 6: Move to home pose")
+        robot.Home()
+
+        # ── 7) MoveL in TCP frame (TRAJ::START) - relative orientation change ─
+        # TRAJ::START means both translation and orientation targets are relative
+        # to the current TCP pose at primitive start.
+        # Setting position to [0, 0, 0] keeps the TCP at the same location.
+        # Orientation [20, 0, 0] means a +20 deg relative rotation around X.
+        logger.info("Step 7: MoveL relative orientation-only rotation in TCP frame")
+        logger.info("Relative target Euler ZYX (deg): [20.0, 0.0, 0.0]")
+        exec_prim(
+            robot,
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveL",
+                    {
+                        # Zero position delta: stay in place. Apply a 20-degree rotation around X.
+                        "target": flexivrdk.Coord(
+                            [0.0, 0.0, 0.0],
+                            [20.0, 0.0, 0.0],
+                            ["TRAJ", "START"],
+                        ),
+                        "vel": 0.1,
+                        "zoneRadius": "Z0",
+                        "targetTolerLevel": 3,
+                        "acc": 1.0,
+                        "angVel": 120,
+                        "enableFixRefJntPos": False,
+                        "refJntPos": flexivrdk.JPos([0, -40, 0, 90, 0, 40, 0]),
+                        "jerk": 50,
+                        "configOptObj": [0, 0, 0.5],
+                        "enableSixAxisJntCtrl": False,
+                        "enableNullspaceTraj": False,
+                    },
+                )
+                for group in single_arm_groups
+            },
+            {group: "reachedTarget" for group in single_arm_groups},
+        )
+
+        # ── 8) MoveJ to upright posture ───────────────────────────────────────
+        # Upright convention: all arm joints at 0 degrees.
+        logger.info("Step 8: MoveJ to upright posture")
+        exec_prim(
+            robot,
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveJ",
+                    {
+                        "target": flexivrdk.JPos([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0]),
+                        "jntVelScale": 40,
+                        "zoneRadius": "Z50",
+                        "targetTolerLevel": 1,
+                        "enableRelativeMove": False,
+                        "jntAccMultiplier": 1,
+                    },
+                )
+                for group in single_arm_groups
+            },
+            {group: "reachedTarget" for group in single_arm_groups},
+        )
+
+        # ── 9) Home ───────────────────────────────────────────────────────────
+        logger.info("Step 9: Move to home pose")
+        robot.Home()
+
+        logger.info("Reference sequence completed successfully")
+        robot.Stop()
+
+    except Exception as e:
+        logger.error(f"Unhandled exception: {e}")
+        return 1
+
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())
diff --git a/thirdparty/flexiv_rdk/example_py/basics1_display_robot_states.py b/thirdparty/flexiv_rdk/example_py/basics1_display_robot_states.py
new file mode 100644
index 0000000..e41a283
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/basics1_display_robot_states.py
@@ -0,0 +1,156 @@
+#!/usr/bin/env python
+
+"""basics1_display_robot_states.py
+
+This tutorial does the very first thing: check connection with the robot server and print
+received robot states.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import threading
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def print_robot_states(robot, logger, stop_event):
+    """
+    Print robot states data @ 1Hz.
+
+    """
+
+    while not stop_event.is_set():
+        # Print available joint groups
+        joint_groups_str = " ".join(
+            [f"[{name}]" for name in robot.info().all_groups.values()]
+        )
+        logger.info(f"Available joint groups: {joint_groups_str}")
+
+        # Print all robot states in JSON format using the built-in __str__ overloading
+        for group, states in robot.states().items():
+            logger.info(f"[{flexivrdk.kJointGroupNames[group]}] robot states:")
+            # fmt: off
+            print("{")
+            print(f"timestamp: [{states.timestamp[0]}, {states.timestamp[1]}]")
+            print(f"q: {['%.3f' % i for i in states.q]}")
+            print(f"theta: {['%.3f' % i for i in states.theta]}")
+            print(f"dq: {['%.3f' % i for i in states.dq]}")
+            print(f"dtheta: {['%.3f' % i for i in states.dtheta]}")
+            print(f"tau: {['%.3f' % i for i in states.tau]}")
+            print(f"tau_dot: {['%.3f' % i for i in states.tau_dot]}")
+            print(f"tau_ext: {['%.3f' % i for i in states.tau_ext]}")
+            print(f"tau_interact: {['%.3f' % i for i in states.tau_interact]}")
+            print(f"temperature: {['%.3f' % i for i in states.temperature]}")
+            print(f"flange_pose: {['%.3f' % i for i in states.flange_pose]}")
+            print(f"tcp_pose: {['%.3f' % i for i in states.tcp_pose]}")
+            print(f"tcp_twist: {['%.3f' % i for i in states.tcp_twist]}")
+            print(f"tcp_wrench: {['%.3f' % i for i in states.tcp_wrench]}")
+            print(f"tcp_wrench_local: {['%.3f' % i for i in states.tcp_wrench_local]}")
+            print(f"raw_tcp_wrench: {['%.3f' % i for i in states.raw_tcp_wrench]}")
+            print(f"raw_tcp_wrench_local: {['%.3f' % i for i in states.raw_tcp_wrench_local]}")
+            print(f"raw_ft_sensor: {['%.3f' % i for i in states.raw_ft_sensor]}")
+            print("}", flush=True)
+            # fmt: on
+
+        # Print all robot actions in JSON format using the built-in __str__ overloading
+        for group, actions in robot.actions().items():
+            logger.info(f"[{flexivrdk.kJointGroupNames[group]}] robot actions:")
+            # fmt: off
+            print("{")
+            print(f"timestamp: [{actions.timestamp[0]}, {actions.timestamp[1]}]")
+            print(f"q_d: {['%.3f' % i for i in actions.q_d]}")
+            print(f"dq_d: {['%.3f' % i for i in actions.dq_d]}")
+            print(f"tau_d: {['%.3f' % i for i in actions.tau_d]}")
+            print(f"tcp_pose_d: {['%.3f' % i for i in actions.tcp_pose_d]}")
+            print(f"tcp_twist_d: {['%.3f' % i for i in actions.tcp_twist_d]}")
+            print(f"tcp_wrench_d: {['%.3f' % i for i in actions.tcp_wrench_d]}")
+            print("}", flush=True)
+            # fmt: on
+
+        # Print digital inputs and outputs
+        logger.info("Digital inputs:")
+        print(robot.digital_inputs())
+        logger.info("Digital outputs:")
+        print(robot.digital_outputs())
+        time.sleep(1)
+
+
+def main():
+    # Create an event to signal the thread to stop
+    stop_event = threading.Event()
+
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial does the very first thing: check connection "
+        "with the robot server and print received robot states.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+    # Print States
+    # =============================================================================
+    # Thread for printing robot states
+    print_thread = threading.Thread(
+        target=print_robot_states, args=[robot, logger, stop_event]
+    )
+    print_thread.start()
+
+    # Use main thread to catch keyboard interrupt and exit thread
+    try:
+        while not stop_event.is_set():
+            time.sleep(0.1)
+    except KeyboardInterrupt:
+        # Send signal to exit thread
+        logger.info("Stopping print thread")
+        stop_event.set()
+
+    # Wait for thread to exit
+    print_thread.join()
+    logger.info("Print thread exited")
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/basics2_clear_fault.py b/thirdparty/flexiv_rdk/example_py/basics2_clear_fault.py
new file mode 100644
index 0000000..60ca6bf
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/basics2_clear_fault.py
@@ -0,0 +1,62 @@
+#!/usr/bin/env python
+
+"""basics2_clear_fault.py
+
+This tutorial clears minor or critical faults, if any, of the connected robot.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial clears minor or critical faults, if any, of "
+        "the connected robot.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Fault Clearing
+        # ==========================================================================================
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+        else:
+            logger.info("No fault on the connected robot")
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/basics3_primitive_execution.py b/thirdparty/flexiv_rdk/example_py/basics3_primitive_execution.py
new file mode 100644
index 0000000..85417fc
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/basics3_primitive_execution.py
@@ -0,0 +1,223 @@
+#!/usr/bin/env python
+
+"""basics3_primitive_execution.py
+
+This tutorial executes several basic robot primitives (unit skills). For detailed documentation
+on all available primitives, please see [Flexiv Primitives](https://www.flexiv.com/primitives/).
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+import utility
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial executes several basic robot primitives (unit "
+        "skills). For detailed documentation on all available primitives, please see [Flexiv "
+        "Primitives](https://www.flexiv.com/primitives/).\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Execute Primitives
+        # ==========================================================================================
+        # Primitives can only be executed on single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # (1) Move robot to home pose
+        # ------------------------------------------------------------------------------------------
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Switch to primitive execution mode
+        robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
+
+        # (2) Move robot joints to target positions
+        # ------------------------------------------------------------------------------------------
+        # Required parameters:
+        #     target: final joint positions, unit: degrees
+        #         {arm joint positions}, {external axis joint positions, optional}
+        # Optional parameters:
+        #     waypoints: waypoints to pass before reaching the target
+        #         (same format as above, but can be more than one)
+        #     vel: TCP linear velocity, unit: m/s
+        logger.info("Executing primitive: MoveJ")
+
+        # Send command to robot
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveJ",
+                    {
+                        "target": flexivrdk.JPos(
+                            [30, -45, 0, 90, 0, 40, 30], [-50, 30, 0, 0, 0, 0]
+                        ),
+                        "waypoints": [
+                            flexivrdk.JPos(
+                                [10, -30, 10, 30, 10, 15, 10], [-15, 10, 0, 0, 0, 0]
+                            ),
+                            flexivrdk.JPos(
+                                [20, -60, -10, 60, -10, 30, 20], [-30, 20, 0, 0, 0, 0]
+                            ),
+                        ],
+                    },
+                )
+                for group in single_arm_groups
+            }
+        )
+        # Most primitives won't exit by themselves and require users to explicitly trigger
+        # transitions based on specific primitive states. Here we check if the primitive state
+        # [reachedTarget] becomes true and trigger the transition manually by sending a new
+        # primitive command.
+        while True:
+            primitive_states = robot.primitive_states()
+            if utility.primitive_state_true_for_groups(
+                primitive_states, "reachedTarget"
+            ):
+                break
+            # Print current primitive states
+            logger.info("Current primitive states:")
+            for group, pt_states in primitive_states.items():
+                print(f"{flexivrdk.kJointGroupNames[group]}:")
+                print(f"primitiveName: {pt_states.pt_name}")
+                for name, value in pt_states.names_and_values.items():
+                    print(f"{name}: {value}")
+            time.sleep(1)
+
+        # (3) Move robot TCP to a target pose in world (base) frame
+        # ------------------------------------------------------------------------------------------
+        # Required parameters:
+        #     target: final TCP pose, unit: m and degrees
+        #         {pos_x, pos_y, pos_z}, {rot_x, rot_y, rot_z}, {ref_frame, ref_point}
+        # Optional parameters:
+        #     waypoints: waypoints to pass before reaching the target
+        #         (same format as above, but can be more than one)
+        #     vel: TCP linear velocity, unit: m/s
+        # NOTE: The rotations use Euler ZYX convention, rot_x means Euler ZYX angle around X axis
+        logger.info("Executing primitive: MoveL")
+
+        # Send command to robot
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveL",
+                    {
+                        "target": flexivrdk.Coord(
+                            [0.3, -0.1, 0.2], [160, 20, 180], ["WORLD", "WORLD_ORIGIN"]
+                        ),
+                        "waypoints": [
+                            flexivrdk.Coord(
+                                [0.1, 0.1, 0.1],
+                                [-160, -20, 180],
+                                ["WORLD", "WORLD_ORIGIN"],
+                            ),
+                            flexivrdk.Coord(
+                                [0.3, 0.2, 0.1],
+                                [180, 0, 180],
+                                ["WORLD", "WORLD_ORIGIN"],
+                            ),
+                        ],
+                        "vel": 0.6,
+                        "zoneRadius": "Z50",
+                    },
+                )
+                for group in single_arm_groups
+            }
+        )
+        # Wait for reached target
+        while not utility.primitive_state_true_for_groups(
+            robot.primitive_states(), "reachedTarget"
+        ):
+            time.sleep(1)
+
+        # (4) Another MoveL that uses TCP frame
+        # ------------------------------------------------------------------------------------------
+        # In this example the reference frame is changed from WORLD::WORLD_ORIGIN to TRAJ::START,
+        # which represents the current TCP frame
+        logger.info("Executing primitive: MoveL")
+
+        # Example to convert target quaternion [w,x,y,z] to Euler ZYX using scipy package's 'xyz'
+        # extrinsic rotation
+        target_quat = [0.9185587, 0.1767767, 0.3061862, 0.1767767]
+        # ZYX = [30, 30, 30] degrees
+        eulerZYX_deg = utility.quat2eulerZYX(target_quat, degree=True)
+
+        # Send command to robot. This motion will hold current TCP position and only do rotation
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs(
+                    "MoveL",
+                    {
+                        "target": flexivrdk.Coord(
+                            [0.0, 0.0, 0.0], eulerZYX_deg, ["TRAJ", "START"]
+                        ),
+                        "vel": 0.2,
+                    },
+                )
+                for group in single_arm_groups
+            }
+        )
+
+        # Wait for reached target
+        while not utility.primitive_state_true_for_groups(
+            robot.primitive_states(), "reachedTarget"
+        ):
+            time.sleep(1)
+
+        # All done, stop robot and put into IDLE mode
+        robot.Stop()
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/basics4_plan_execution.py b/thirdparty/flexiv_rdk/example_py/basics4_plan_execution.py
new file mode 100644
index 0000000..995a6b6
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/basics4_plan_execution.py
@@ -0,0 +1,147 @@
+#!/usr/bin/env python
+
+"""basics4_plan_execution.py
+
+This tutorial executes a plan selected by the user from a list of available plans. A plan is a
+pre-written script to execute a series of robot primitives with pre-defined transition conditions
+between 2 adjacent primitives. Users can use Flexiv Elements to compose their own plan and assign
+to the robot, which will appear in the plan list.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial executes a plan selected by the user from a "
+        "list of available plans. A plan is a pre-written script to execute a series of robot "
+        "primitives with pre-defined transition conditions between 2 adjacent primitives. Users "
+        "can use Flexiv Elements to compose their own plan and assign to the robot, which "
+        "will appear in the plan list.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Execute Plans
+        # ==========================================================================================
+        # Switch to plan execution mode
+        robot.SwitchMode(mode.NRT_PLAN_EXECUTION)
+
+        while True:
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            # Get user input
+            logger.info("Choose an action:")
+            print("[1] Show available plans")
+            print("[2] Execute a plan by index")
+            print("[3] Execute a plan by name")
+            user_input = int(input())
+
+            # Get and show plan list
+            if user_input == 1:
+                plan_list = robot.plan_list()
+                for i in range(len(plan_list)):
+                    print(f"[{i}] {plan_list[i]}")
+                print("")
+
+            # Execute plan by index
+            elif user_input == 2:
+                index = int(input("Enter plan index to execute:\n"))
+                # Allow the plan to continue its execution even if the RDK program is closed or
+                # the connection is lost
+                robot.ExecutePlan(index, True)
+
+                # Print plan info while the current plan is running
+                while robot.busy():
+                    plan_info = robot.plan_info()
+                    logger.info("Current plan info:")
+                    print(f"assigned_plan_name: {plan_info.assigned_plan_name}")
+                    print(f"pt_name: {plan_info.pt_name}")
+                    print(f"node_name: {plan_info.node_name}")
+                    print(f"node_path: {plan_info.node_path}")
+                    print(f"node_path_time_period: {plan_info.node_path_time_period}")
+                    print(f"node_path_number: {plan_info.node_path_number}")
+                    print(f"velocity_scale: {plan_info.velocity_scale}")
+                    print(f"waiting_for_step: {plan_info.waiting_for_step}")
+                    print("", flush=True)
+                    time.sleep(1)
+
+            # Execute plan by name
+            elif user_input == 3:
+                name = str(input("Enter plan name to execute:\n"))
+                # Allow the plan to continue its execution even if the RDK program is closed or
+                # the connection is lost
+                robot.ExecutePlan(name, True)
+
+                # Print plan info while the current plan is running
+                while robot.busy():
+                    plan_info = robot.plan_info()
+                    logger.info("Current plan info:")
+                    print(f"assigned_plan_name: {plan_info.assigned_plan_name}")
+                    print(f"pt_name: {plan_info.pt_name}")
+                    print(f"node_name: {plan_info.node_name}")
+                    print(f"node_path: {plan_info.node_path}")
+                    print(f"node_path_time_period: {plan_info.node_path_time_period}")
+                    print(f"node_path_number: {plan_info.node_path_number}")
+                    print(f"velocity_scale: {plan_info.velocity_scale}")
+                    print(f"waiting_for_step: {plan_info.waiting_for_step}")
+                    print("", flush=True)
+                    time.sleep(1)
+
+            else:
+                logger.warn("Invalid input")
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/basics5_zero_force_torque_sensors.py b/thirdparty/flexiv_rdk/example_py/basics5_zero_force_torque_sensors.py
new file mode 100644
index 0000000..82c525e
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/basics5_zero_force_torque_sensors.py
@@ -0,0 +1,112 @@
+#!/usr/bin/env python
+
+"""basics5_zero_force_torque_sensors.py
+
+This tutorial zeros the robot's force and torque sensors, which is a recommended (but not
+mandatory) step before any operations that require accurate force/torque measurement.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+import utility
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial zeros the robot's force and torque sensors, "
+        "which is a recommended (but not mandatory) step before any operations that require "
+        "accurate force/torque measurement.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Zero Sensors
+        # ==========================================================================================
+        # Get and print the current TCP force/moment readings
+        for group, states in robot.states().items():
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] TCP force and moment reading in world frame BEFORE sensor zeroing: {states.tcp_wrench} N-Nm"
+            )
+
+        # Primitives can only be executed on single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # Run the "ZeroFTSensor" primitive to automatically zero force and torque sensors
+        robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
+                for group in single_arm_groups
+            }
+        )
+
+        # WARNING: during the process, the robot must not contact anything, otherwise the result
+        # will be inaccurate and affect following operations
+        logger.warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot"
+        )
+
+        # Wait for primitive to finish
+        while not utility.primitive_state_true_for_groups(robot.primitive_states(), "terminated"):
+            time.sleep(1)
+        logger.info("Sensor zeroing complete")
+
+        # Get and print the current TCP force/moment readings
+        for group, states in robot.states().items():
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] TCP force and moment reading in world frame AFTER sensor zeroing: {states.tcp_wrench} N-Nm"
+            )
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/basics6_gripper_control.py b/thirdparty/flexiv_rdk/example_py/basics6_gripper_control.py
new file mode 100644
index 0000000..18645c4
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/basics6_gripper_control.py
@@ -0,0 +1,230 @@
+#!/usr/bin/env python
+
+"""basics6_gripper_control.py
+
+This tutorial does position and force (if available) control of grippers supported by Flexiv.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import sys
+import argparse
+import threading
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def print_gripper_states(gripper, logger, stop_event):
+    """
+    Print gripper states data @ 1Hz.
+
+    """
+    while not stop_event.is_set():
+        # Print all gripper states, round all float values to 2 decimals
+        logger.info("Current gripper states:")
+        gripper_states = gripper.states()
+        for group, states in gripper_states.items():
+            print(f"[{flexivrdk.kJointGroupNames[group]}]")
+            print(f"width: {round(states.width, 2)}")
+            print(f"force: {round(states.force, 2)}")
+            print(f"is_moving: {states.is_moving}")
+        print("", flush=True)
+        time.sleep(1)
+
+
+def main():
+    # Create an event to signal the thread to stop
+    stop_event = threading.Event()
+
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "gripper_device_name",
+        help="Full name of the device representing the gripper, can be found in Flexiv Elements->Settings->Device",
+    )
+    argparser.add_argument(
+        "gripper_tool_name",
+        help="Full name of the tool representing the gripper, can be found in Flexiv Elements->Settings->Tool",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial does position and force (if available) "
+        "control of grippers supported by Flexiv.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Gripper Control
+        # ==========================================================================================
+        # Instantiate gripper control interface
+        gripper = flexivrdk.Gripper(robot)
+
+        # Instantiate tool interface. Gripper is categorized as both a device and a tool. The
+        # device attribute allows a gripper to be interactively controlled by the user; whereas the
+        # tool attribute tells the robot to account for its mass properties and TCP location.
+        tool = flexivrdk.Tool(robot)
+
+        # Grippers can only be assigned to single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # Enable the specified gripper as a device. This is equivalent to enabling the specified
+        # gripper in Flexiv Elements -> Settings -> Device
+        logger.info(
+            f"Enabling gripper device [{args.gripper_device_name}] for all available single-arm joint groups"
+        )
+        for group in single_arm_groups:
+            gripper.Enable(group, args.gripper_device_name)
+
+        # Print parameters of the enabled gripper
+        logger.info("Gripper params:")
+        gripper_params = gripper.params()
+        for group, params in gripper_params.items():
+            print(f"[{flexivrdk.kJointGroupNames[group]}]")
+            print(f"name: {params.name}")
+            print(f"min_width: {round(params.min_width, 2)}")
+            print(f"max_width: {round(params.max_width, 2)}")
+            print(f"min_force: {round(params.min_force, 2)}")
+            print(f"max_force: {round(params.max_force, 2)}")
+            print(f"min_vel: {round(params.min_vel, 2)}")
+            print(f"max_vel: {round(params.max_vel, 2)}")
+            print("", flush=True)
+
+        # Switch robot tool to gripper so the gravity compensation and TCP location is updated
+        logger.info(
+            f"Switching robot tool to [{args.gripper_tool_name}] for all available single-arm joint groups"
+        )
+        for group in single_arm_groups:
+            tool.Switch(group, args.gripper_tool_name)
+
+        # User needs to determine if this gripper requires manual initialization
+        logger.info(
+            "Manually trigger initialization for the gripper now? Choose Yes if it's a 48v Grav "
+            "gripper"
+        )
+        print("[1] No, it has already initialized automatically when power on")
+        print("[2] Yes, it does not initialize itself when power on")
+        choice = int(input(""))
+
+        # Trigger manual initialization based on input
+        if choice == 1:
+            logger.info("Skipped manual initialization")
+        elif choice == 2:
+            for group in single_arm_groups:
+                gripper.Init(group)
+            # User determines if the manual initialization is finished
+            logger.info(
+                "Triggered manual initialization, press Enter when the initialization is finished to continue"
+            )
+            input()
+        else:
+            logger.error("Invalid choice")
+            return 1
+
+        # Start a separate thread to print gripper states
+        print_thread = threading.Thread(
+            target=print_gripper_states, args=[gripper, logger, stop_event]
+        )
+        print_thread.start()
+
+        # Position control
+        logger.info("Closing gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(2)
+        logger.info("Opening gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(2)
+
+        # Stop
+        logger.info("Closing gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(0.5)
+        logger.info("Stopping gripper")
+        for group in gripper_params:
+            gripper.Stop(group)
+        time.sleep(2)
+        logger.info("Closing gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.min_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(2)
+        logger.info("Opening gripper")
+        for group, params in gripper_params.items():
+            gripper.Move(group, params.max_width, params.max_vel, 0.25 * params.max_force)
+        time.sleep(0.5)
+        logger.info("Stopping gripper")
+        for group in gripper_params:
+            gripper.Stop(group)
+        time.sleep(2)
+
+        # Force control, if available (sensed force is not zero)
+        gripper_states = gripper.states()
+        has_force_control = any(
+            abs(states.force) > sys.float_info.epsilon
+            for states in gripper_states.values()
+        )
+        if has_force_control:
+            logger.info("Gripper running zero force control")
+            for group in gripper_states:
+                gripper.Grasp(group, 0)
+            # Exit after 10 seconds
+            time.sleep(10)
+
+        # Finished
+        for group in gripper_states:
+            gripper.Stop(group)
+
+        # Stop all threads
+        logger.info("Stopping print thread")
+        stop_event.set()
+        print_thread.join()
+        logger.info("Print thread exited")
+        logger.info("Program finished")
+
+    except Exception as e:
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/basics7_update_robot_tool.py b/thirdparty/flexiv_rdk/example_py/basics7_update_robot_tool.py
new file mode 100644
index 0000000..f3fbd2a
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/basics7_update_robot_tool.py
@@ -0,0 +1,159 @@
+#!/usr/bin/env python
+
+"""basics7_update_robot_tool.py
+
+This tutorial shows how to online update and interact with the robot tools. All changes made to
+the robot tool system will take effect immediately without needing to reboot. However, the robot
+must be put into IDLE mode when making these changes.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to online update and interact with "
+        "the robot tools. All changes made to the robot tool system will take effect immediately "
+        "without needing to reboot. However, the robot must be put into IDLE mode when making "
+        "these changes.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Update Robot Tool
+        # ==========================================================================================
+        # Make sure the robot is in IDLE mode
+        robot.SwitchMode(mode.IDLE)
+
+        # Instantiate tool interface
+        tool = flexivrdk.Tool(robot)
+
+        # Tools can only be assigned to single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        # Get and print a list of already configured tools currently in the robot's tools pool
+        logger.info("All configured tools:")
+        tool_list = tool.list()
+        for i in range(len(tool_list)):
+            print(f"[{i}] {tool_list[i]}")
+        print()
+
+        # Get and print the current active tool
+        for group in single_arm_groups:
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Current active tool: [{tool.name(group)}]"
+            )
+
+        # Set name and parameters for a new tool
+        new_tool_name = "ExampleTool1"
+        new_tool_params = flexivrdk.ToolParams()
+        new_tool_params.mass = 0.9
+        new_tool_params.CoM = [0.0, 0.0, 0.057]
+        new_tool_params.inertia = [2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0]
+        new_tool_params.tcp_location = [
+            0.0,
+            -0.207,
+            0.09,
+            0.7071068,
+            0.7071068,
+            0.0,
+            0.0,
+        ]
+
+        # If there's already a tool with the same name in the robot's tools pool, then remove it
+        # first, because duplicate tool names are not allowed
+        if tool.exist(new_tool_name):
+            logger.warn(
+                f"Tool with the same name [{new_tool_name}] already exists, removing it now"
+            )
+            # Switch to other tool or no tool (Flange) before removing the current tool
+            for group in single_arm_groups:
+                tool.Switch(group, "Flange")
+            tool.Remove(new_tool_name)
+
+        # Add the new tool
+        logger.info(f"Adding new tool [{new_tool_name}] to the robot")
+        tool.Add(new_tool_name, new_tool_params)
+
+        # Get and print the tools list again, the new tool should appear at the end
+        logger.info("All configured tools:")
+        tool_list = tool.list()
+        for i in range(len(tool_list)):
+            print(f"[{i}] {tool_list[i]}")
+        print()
+
+        # Switch to the newly added tool, i.e. set it as the active tool
+        logger.info(f"Switching to tool [{new_tool_name}]")
+        for group in single_arm_groups:
+            tool.Switch(group, new_tool_name)
+
+        # Get and print the current active tool again, should be the new tool
+        for group in single_arm_groups:
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Current active tool: [{tool.name(group)}]"
+            )
+
+        # Switch to other tool or no tool (Flange) before removing the current tool
+        for group in single_arm_groups:
+            tool.Switch(group, "Flange")
+
+        # Clean up by removing the new tool
+        time.sleep(2)
+        logger.info(f"Removing tool [{new_tool_name}]")
+        tool.Remove(new_tool_name)
+
+        logger.info("Program finished")
+
+    except Exception as e:
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/basics8_global_variables.py b/thirdparty/flexiv_rdk/example_py/basics8_global_variables.py
new file mode 100644
index 0000000..e4bf0e7
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/basics8_global_variables.py
@@ -0,0 +1,129 @@
+#!/usr/bin/env python
+
+"""basics8_global_variables.py
+
+This tutorial shows how to get and set global variables.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial shows how to get and set global variables.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Get existing global variables
+        # =========================================================================================
+        global_vars = robot.global_variables()
+        if not global_vars:
+            logger.warn("No global variables available")
+            return 1
+        else:
+            logger.info("Existing global variables and their original values:")
+            for key, value in global_vars.items():
+                print(f"{key}: {value}")
+
+        # Set global variables
+        # =========================================================================================
+        # WARNING: These specified global variables need to be created first using Flexiv Elements
+        logger.info("Setting new values to existing global variables")
+        robot.SetGlobalVariables(
+            {
+                "test_bool": 1,
+                "test_int": 100,
+                "test_double": 100.123,
+                "test_string": "Flexiv",
+                "test_int_vec": [1, 2, 3],
+                "test_double_vec": [1.1, 2.2, 3.3],
+                "test_string_vec": ["Go", "Flexiv", "Go!"],
+                "test_pose": [0.1, -0.2, 0.3, -90, -45, 120],
+                "test_coord": flexivrdk.Coord(
+                    [0.1, -0.2, 0.3],
+                    [-90, -45, 120],
+                    ["WORLD", "WORLD_ORIGIN"],
+                    [1, 2, 3, 4, 5, 6, 7],
+                    [10, 20, 0, 0, 0, 0],
+                ),
+                "test_coord_array": [
+                    flexivrdk.Coord([1, 2, 3], [4, 5, 6], ["WORK", "WorkCoord0"]),
+                    flexivrdk.Coord(
+                        [10, 20, 30],
+                        [40, 50, 60],
+                        ["WORLD", "WORLD_ORIGIN"],
+                        [1, 2, 3, 4, 5, 6, 7],
+                        [10, 20, 0, 0, 0, 0],
+                    ),
+                    flexivrdk.Coord(
+                        [3, 2, 1], [180, 0, 180], ["WORLD", "WORLD_ORIGIN"]
+                    ),
+                ],
+            }
+        )
+
+        # Get updated global variables
+        # =========================================================================================
+        global_vars = robot.global_variables()
+        if not global_vars:
+            logger.warn("No global variables available")
+            return 1
+        else:
+            logger.info("Updated global variables:")
+            for key, value in global_vars.items():
+                print(f"{key}: {value}")
+
+        logger.info("Program finished")
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/intermediate1_non_realtime_joint_position_control.py b/thirdparty/flexiv_rdk/example_py/intermediate1_non_realtime_joint_position_control.py
new file mode 100644
index 0000000..4ba48b8
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/intermediate1_non_realtime_joint_position_control.py
@@ -0,0 +1,154 @@
+#!/usr/bin/env python
+
+"""intermediate1_non_realtime_joint_position_control.py
+
+This tutorial runs non-real-time joint position control to hold or sine-sweep all robot joints.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    # Required arguments
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "frequency", help="Command frequency, 1 to 100 [Hz]", type=int
+    )
+    # Optional arguments
+    argparser.add_argument(
+        "--hold",
+        action="store_true",
+        help="Robot holds current joint positions, otherwise do a sine-sweep",
+    )
+    args = argparser.parse_args()
+
+    # Check if arguments are valid
+    frequency = args.frequency
+    assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs non-real-time joint position control to "
+        "hold or sine-sweep all robot joints.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Non-real-time Joint Position Control
+        # ==========================================================================================
+        # Direct joint control can be executed by single-arm joint groups and the external axis
+        exe_groups = robot.info().single_arm_groups
+        if not exe_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+        # The external axis joint group (if it exists) also supports direct joint control
+        if flexivrdk.JointGroup.EXT_AXIS in robot.info().all_groups:
+            exe_groups[flexivrdk.JointGroup.EXT_AXIS] = robot.info().all_groups[
+                flexivrdk.JointGroup.EXT_AXIS
+            ]
+
+        # Switch to non-real-time joint position control mode
+        robot.SwitchMode(mode.NRT_JOINT_POSITION)
+
+        period = 1.0 / frequency
+        loop_time = 0
+        logger.info(
+            f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
+        )
+
+        # Use current robot joint positions as initial positions
+        all_init_pos = {}
+        robot_states = robot.states()
+        for group in exe_groups:
+            all_init_pos[group] = robot_states[group].q.copy()
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Initial joint positions: {all_init_pos[group]}"
+            )
+
+        # Joint sine-sweep amplitude [rad]
+        SWING_AMP = 0.1
+
+        # TCP sine-sweep frequency [Hz]
+        SWING_FREQ = 0.3
+
+        # Send command periodically at user-specified frequency
+        while True:
+            # Use sleep to control loop period
+            time.sleep(period)
+
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            cmds = {}
+            sine_offset = SWING_AMP * math.sin(2 * math.pi * SWING_FREQ * loop_time)
+            for group, init_pos in all_init_pos.items():
+                target_pos = init_pos.copy()
+                if not args.hold:
+                    for i in range(len(target_pos)):
+                        target_pos[i] += sine_offset
+
+                zero_vel = [0.0] * len(init_pos)
+                max_vel = [2.0] * len(init_pos)
+                max_acc = [3.0] * len(init_pos)
+                cmds[group] = flexivrdk.NrtJointPositionCmd(
+                    target_pos, zero_vel, max_vel, max_acc
+                )
+
+            robot.SendJointPosition(cmds)
+
+            # Increment loop time
+            loop_time += period
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/intermediate2_non_realtime_joint_impedance_control.py b/thirdparty/flexiv_rdk/example_py/intermediate2_non_realtime_joint_impedance_control.py
new file mode 100644
index 0000000..0186288
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/intermediate2_non_realtime_joint_impedance_control.py
@@ -0,0 +1,177 @@
+#!/usr/bin/env python
+
+"""intermediate2_non_realtime_joint_impedance_control.py
+
+This tutorial runs non-real-time joint impedance control to hold or sine-sweep all robot joints.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install spdlog
+import numpy as np  # pip install numpy
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    # Required arguments
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "frequency", help="Command frequency, 1 to 100 [Hz]", type=int
+    )
+    # Optional arguments
+    argparser.add_argument(
+        "--hold",
+        action="store_true",
+        help="Robot holds current joint positions, otherwise do a sine-sweep",
+    )
+    args = argparser.parse_args()
+
+    # Check if arguments are valid
+    frequency = args.frequency
+    assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs non-real-time joint impedance control to "
+        "hold or sine-sweep all robot joints.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Non-real-time Joint Impedance Control
+        # ==========================================================================================
+        # Switch to non-real-time joint impedance control mode
+        robot.SwitchMode(mode.NRT_JOINT_IMPEDANCE)
+
+        # Direct joint control can be executed by single-arm joint groups and the external axis
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+        # The external axis joint group (if it exists) also supports direct joint control
+        exe_groups = dict(single_arm_groups)
+        if flexivrdk.JointGroup.EXT_AXIS in robot.info().all_groups:
+            exe_groups[flexivrdk.JointGroup.EXT_AXIS] = robot.info().all_groups[
+                flexivrdk.JointGroup.EXT_AXIS
+            ]
+
+        period = 1.0 / frequency
+        loop_counter = 0
+        logger.info(
+            f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
+        )
+
+        # Use current robot joint positions as initial positions
+        all_init_pos = {}
+        robot_states = robot.states()
+        for group in exe_groups:
+            all_init_pos[group] = robot_states[group].q.copy()
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Initial joint positions: {all_init_pos[group]}"
+            )
+
+        # Joint sine-sweep amplitude [rad]
+        SWING_AMP = 0.1
+
+        # TCP sine-sweep frequency [Hz]
+        SWING_FREQ = 0.3
+
+        # Send command periodically at user-specified frequency
+        while True:
+            # Use sleep to control loop period
+            time.sleep(period)
+
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            # Reduce stiffness to half of nominal values after 5 seconds
+            if loop_counter == 5 / period:
+                for group in single_arm_groups:
+                    new_Kq = np.multiply(robot.info().K_q_nom[group], 0.5).tolist()
+                    robot.SetJointImpedance(group, new_Kq)
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Joint stiffness set to: {new_Kq}"
+                    )
+
+            # Reset stiffness to nominal values after another 5 seconds
+            if loop_counter == 10 / period:
+                for group in single_arm_groups:
+                    nominal_Kq = robot.info().K_q_nom[group]
+                    robot.SetJointImpedance(group, nominal_Kq)
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Joint stiffness reset to nominal: {nominal_Kq}"
+                    )
+
+            # Send commands
+            cmds = {}
+            sine_offset = SWING_AMP * math.sin(
+                2 * math.pi * SWING_FREQ * loop_counter * period
+            )
+            for group, init_pos in all_init_pos.items():
+                target_pos = init_pos.copy()
+                if not args.hold:
+                    for i in range(len(target_pos)):
+                        target_pos[i] += sine_offset
+
+                zero_vel = [0.0] * len(init_pos)
+                max_vel = [2.0] * len(init_pos)
+                max_acc = [3.0] * len(init_pos)
+                cmds[group] = flexivrdk.NrtJointPositionCmd(
+                    target_pos, zero_vel, max_vel, max_acc
+                )
+
+            robot.SendJointPosition(cmds)
+
+            # Increment loop counter
+            loop_counter += 1
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/intermediate3_non_realtime_cartesian_pure_motion_control.py b/thirdparty/flexiv_rdk/example_py/intermediate3_non_realtime_cartesian_pure_motion_control.py
new file mode 100644
index 0000000..a20055d
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/intermediate3_non_realtime_cartesian_pure_motion_control.py
@@ -0,0 +1,280 @@
+#!/usr/bin/env python
+
+"""intermediate3_non_realtime_cartesian_pure_motion_control.py
+
+This tutorial runs non-real-time Cartesian-space pure motion control to hold or sine-sweep the robot
+TCP. A simple collision detection is also included.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install spdlog
+import numpy as np  # pip install numpy
+import flexivrdk  # pip install flexivrdk
+import utility
+
+# Global constants
+# ==================================================================================================
+# TCP sine-sweep amplitude [m]
+SWING_AMP = 0.1
+
+# TCP sine-sweep frequency [Hz]
+SWING_FREQ = 0.3
+
+# External TCP force threshold for collision detection, value is only for demo purpose [N]
+EXT_FORCE_THRESHOLD = 10.0
+
+# External joint torque threshold for collision detection, value is only for demo purpose [Nm]
+EXT_TORQUE_THRESHOLD = 5.0
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    # Required arguments
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "frequency", help="Command frequency, 1 to 100 [Hz]", type=int
+    )
+    # Optional arguments
+    argparser.add_argument(
+        "--hold",
+        action="store_true",
+        help="Robot holds current TCP pose, otherwise do a sine-sweep",
+    )
+    argparser.add_argument(
+        "--collision",
+        action="store_true",
+        help="Enable collision detection, robot will stop upon collision",
+    )
+    args = argparser.parse_args()
+
+    # Check if arguments are valid
+    frequency = args.frequency
+    assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs non-real-time Cartesian-space pure "
+        "motion control to hold or sine-sweep the robot TCP. A simple collision detection is also "
+        "included.\n"
+    )
+
+    # Print based on arguments
+    if args.hold:
+        logger.info("Robot holding current TCP pose")
+    else:
+        logger.info("Robot running TCP sine-sweep")
+
+    if args.collision:
+        logger.info("Collision detection enabled")
+    else:
+        logger.info("Collision detection disabled")
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Zero Force-torque Sensor
+        # =========================================================================================
+        # Direct Cartesian control can only be executed by single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
+        # IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
+                for group in single_arm_groups
+            }
+        )
+
+        # WARNING: during the process, the robot must not contact anything, otherwise the result
+        # will be inaccurate and affect following operations
+        logger.warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot"
+        )
+
+        # Wait for primitive to finish
+        while not utility.primitive_state_true_for_groups(robot.primitive_states(), "terminated"):
+            time.sleep(1)
+        logger.info("Sensor zeroing complete")
+
+        # Configure Motion Control
+        # =========================================================================================
+        # The Cartesian motion force modes do pure motion control out of the box, thus nothing
+        # needs to be explicitly configured
+
+        # NOTE: motion control always uses robot world frame, while force control can use
+        # either world or TCP frame as reference frame
+
+        # Start Pure Motion Control
+        # =========================================================================================
+        # Switch to non-real-time mode for discrete motion control
+        robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
+
+        # Set all Cartesian axis(s) to motion control
+        for group in single_arm_groups:
+            robot.SetForceControlAxis(group, [False, False, False, False, False, False])
+
+        # Save initial poses and joint positions
+        all_init_pose = {}
+        all_init_q = {}
+        robot_states = robot.states()
+        for group in single_arm_groups:
+            all_init_pose[group] = robot_states[group].tcp_pose.copy()
+            all_init_q[group] = robot_states[group].q.copy()
+
+        # Periodic Task
+        # =========================================================================================
+        # Set loop period
+        period = 1.0 / frequency
+        loop_counter = 0
+        logger.info(
+            f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
+        )
+
+        # Send command periodically at user-specified frequency
+        while True:
+            # Use sleep to control loop period
+            time.sleep(period)
+
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            cmds = {}
+            sine_offset = SWING_AMP * math.sin(
+                2 * math.pi * SWING_FREQ * loop_counter * period
+            )
+            for group, init_pose in all_init_pose.items():
+                target_pose = init_pose.copy()
+
+                # Sine-sweep TCP along Y axis
+                if not args.hold:
+                    target_pose[1] += sine_offset
+
+                cmds[group] = flexivrdk.NrtCartesianCmd(target_pose)
+
+            # Send command. Calling this method with only target pose input results
+            # in pure motion control
+            robot.SendCartesianMotionForce(cmds)
+
+            #  Do the following operations in sequence for every 20 seconds
+            time_elapsed = loop_counter * period
+            # Online change reference joint positions at 3 seconds
+            if time_elapsed % 20.0 == 3.0:
+                ref_q = [0.938, -1.108, -1.254, 1.464, 1.073, 0.278, -0.658]
+                for group in single_arm_groups:
+                    robot.SetNullSpacePosture(group, ref_q)
+                logger.info(f"Reference joint positions set to: {ref_q}")
+            # Online change stiffness to half of nominal at 6 seconds
+            elif time_elapsed % 20.0 == 6.0:
+                for group in single_arm_groups:
+                    new_K = np.multiply(robot.info().K_x_nom[group], 0.5).tolist()
+                    robot.SetCartesianImpedance(group, new_K)
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Cartesian stiffness set to: {new_K}"
+                    )
+            # Online change to another reference joint positions at 9 seconds
+            elif time_elapsed % 20.0 == 9.0:
+                ref_q = [-0.938, -1.108, 1.254, 1.464, -1.073, 0.278, 0.658]
+                for group in single_arm_groups:
+                    robot.SetNullSpacePosture(group, ref_q)
+                logger.info(f"Reference joint positions set to: {ref_q}")
+            # Online reset stiffness to nominal at 12 seconds
+            elif time_elapsed % 20.0 == 12.0:
+                for group in single_arm_groups:
+                    nominal_K = robot.info().K_x_nom[group]
+                    robot.SetCartesianImpedance(group, nominal_K)
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Cartesian stiffness reset to nominal: {nominal_K}"
+                    )
+            # Online reset reference joint positions to nominal at 14 seconds
+            elif time_elapsed % 20.0 == 14.0:
+                for group, init_q in all_init_q.items():
+                    robot.SetNullSpacePosture(group, init_q)
+                logger.info("Reference joint positions are reset")
+            # Online enable max contact wrench regulation at 16 seconds
+            elif time_elapsed % 20.0 == 16.0:
+                max_wrench = [10.0, 10.0, 10.0, 2.0, 2.0, 2.0]
+                for group in single_arm_groups:
+                    robot.SetMaxContactWrench(group, max_wrench)
+                logger.info(f"Max contact wrench set to: {max_wrench}")
+            # Disable max contact wrench regulation at 19 seconds
+            elif time_elapsed % 20.0 == 19.0:
+                for group in single_arm_groups:
+                    robot.SetMaxContactWrench(group, [float("inf")] * 6)
+                logger.info("Max contact wrench regulation is disabled")
+
+            # Simple collision detection: stop robot if collision is detected at
+            # end-effector
+            if args.collision:
+                for group, states in robot.states().items():
+                    collision_detected = False
+                    ext_force = np.array(states.tcp_wrench[:3])
+                    if np.linalg.norm(ext_force) > EXT_FORCE_THRESHOLD:
+                        collision_detected = True
+
+                    for v in states.tau_ext:
+                        if abs(v) > EXT_TORQUE_THRESHOLD:
+                            collision_detected = True
+
+                    if collision_detected:
+                        robot.Stop()
+                        logger.warn(
+                            f"[{flexivrdk.kJointGroupNames[group]}] Collision detected, stopping robot and exit program ..."
+                        )
+                        return
+
+            # Increment loop counter
+            loop_counter += 1
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/intermediate4_non_realtime_cartesian_motion_force_control.py b/thirdparty/flexiv_rdk/example_py/intermediate4_non_realtime_cartesian_motion_force_control.py
new file mode 100644
index 0000000..81b7cbe
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/intermediate4_non_realtime_cartesian_motion_force_control.py
@@ -0,0 +1,300 @@
+#!/usr/bin/env python
+
+"""intermediate4_non_realtime_cartesian_motion_force_control.py
+
+This tutorial runs non-real-time Cartesian-space unified motion-force control. The Z axis of the
+chosen reference frame will be activated for explicit force control, while the rest axes in the
+same reference frame will stay motion controlled.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install spdlog
+import numpy as np  # pip install numpy
+import flexivrdk  # pip install flexivrdk
+import utility
+
+# Global constants
+# ==================================================================================================
+# TCP sine-sweep amplitude [m]
+SWING_AMP = 0.1
+
+# TCP sine-sweep frequency [Hz]
+SWING_FREQ = 0.3
+
+# Pressing force to apply during the unified motion-force control [N]
+PRESSING_FORCE = 5.0
+
+# Cartesian linear velocity used to search for contact [m/s]
+SEARCH_VELOCITY = 0.02
+
+# Maximum distance to travel when searching for contact [m]
+SEARCH_DISTANCE = 1.0
+
+# Maximum contact wrench during contact search for soft contact
+MAX_WRENCH_FOR_CONTACT_SEARCH = [10.0, 10.0, 10.0, 3.0, 3.0, 3.0]
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    # Required arguments
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    argparser.add_argument(
+        "frequency", help="Command frequency, 1 to 100 [Hz]", type=int
+    )
+    # Optional arguments
+    argparser.add_argument(
+        "--TCP",
+        action="store_true",
+        help="Use TCP frame as reference frame for force control, otherwise use world frame",
+    )
+    argparser.add_argument(
+        "--polish",
+        action="store_true",
+        help="Run a simple polish motion along XY plane in world frame, otherwise hold robot motion in non-force-control axes",
+    )
+    args = argparser.parse_args()
+
+    # Check if arguments are valid
+    frequency = args.frequency
+    assert frequency >= 1 and frequency <= 100, "Invalid <frequency> input"
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs non-real-time Cartesian-space unified "
+        "motion-force control. The Z axis of the chosen reference frame will be activated for "
+        "explicit force control, while the rest axes in the same reference frame will stay motion "
+        "controlled.\n"
+    )
+
+    # The reference frame to use, see Robot::SendCartesianMotionForce() for more details
+    force_ctrl_frame = flexivrdk.CoordType.WORLD
+    if args.TCP:
+        logger.info("Reference frame used for force control: robot TCP frame")
+        force_ctrl_frame = flexivrdk.CoordType.TCP
+    else:
+        logger.info("Reference frame used for force control: robot world frame")
+
+    # Whether to enable polish motion
+    if args.polish:
+        logger.info(
+            "Robot will run a polish motion along XY plane in robot world frame"
+        )
+    else:
+        logger.info("Robot will hold its motion in all non-force-controlled axes")
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Zero Force-torque Sensor
+        # =========================================================================================
+        # Direct Cartesian control can only be executed by single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        robot.SwitchMode(mode.NRT_PRIMITIVE_EXECUTION)
+        # IMPORTANT: must zero force/torque sensor offset for accurate force/torque measurement
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs("ZeroFTSensor", dict())
+                for group in single_arm_groups
+            }
+        )
+
+        # WARNING: during the process, the robot must not contact anything, otherwise the result
+        # will be inaccurate and affect following operations
+        logger.warn(
+            "Zeroing force/torque sensors, make sure nothing is in contact with the robot"
+        )
+
+        # Wait for primitive to finish
+        while not utility.primitive_state_true_for_groups(
+            robot.primitive_states(), "terminated"
+        ):
+            time.sleep(1)
+        logger.info("Sensor zeroing complete")
+
+        # Search for Contact
+        # =========================================================================================
+        # NOTE: there are several ways to do contact search, such as using primitives, or real-time
+        # and non-real-time direct motion controls, etc. Here we use non-real-time direct Cartesian
+        # control for example.
+        logger.info("Searching for contact ...")
+
+        # Set initial poses to current TCP poses
+        all_init_pose = {}
+        for group in single_arm_groups:
+            all_init_pose[group] = robot.states()[group].tcp_pose.copy()
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {all_init_pose[group]}"
+            )
+
+        # Use non-real-time mode to make the robot go to a set point with its own motion generator
+        robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
+
+        # Search for contact with max contact wrench set to a small value for making soft contact
+        for group in single_arm_groups:
+            robot.SetMaxContactWrench(group, MAX_WRENCH_FOR_CONTACT_SEARCH)
+
+        # Set target point along -Z direction and expect contact to happen during the travel
+        cmds = {}
+        for group, init_pose in all_init_pose.items():
+            target_pose = init_pose.copy()
+            target_pose[2] -= SEARCH_DISTANCE
+            cmds[group] = flexivrdk.NrtCartesianCmd(
+                target_pose, [0.0] * 6, [0.0] * 6, SEARCH_VELOCITY
+            )
+
+        # Send target point to robot to start searching for contact and limit the velocity. Keep
+        # target wrench 0 at this stage since we are not doing force control yet
+        robot.SendCartesianMotionForce(cmds)
+
+        # Use a while loop to poll robot states and check if a contact is made
+        is_contacted = False
+        while not is_contacted:
+            # Compute norm of sensed external force applied on robot TCP
+            for group, states in robot.states().items():
+                ext_force = np.array(states.tcp_wrench[:3])
+
+                # Contact is considered to be made if sensed TCP force exceeds the threshold
+                if np.linalg.norm(ext_force) > PRESSING_FORCE:
+                    is_contacted = True
+                    logger.info(
+                        f"[{flexivrdk.kJointGroupNames[group]}] Contact detected at robot TCP"
+                    )
+                    break
+
+            # Check at 1ms interval
+            time.sleep(0.001)
+
+        # Configure Force Control
+        # =========================================================================================
+        # Switch to non-real-time mode for discrete motion force control
+        robot.SwitchMode(mode.NRT_CARTESIAN_MOTION_FORCE)
+
+        # Set force control reference frame based on program argument. See function doc for more
+        # details
+        for group in single_arm_groups:
+            robot.SetForceControlFrame(group, force_ctrl_frame)
+
+        # Set which Cartesian axis(s) to activate for force control. See function doc for more
+        # details. Here we only active Z axis
+        for group in single_arm_groups:
+            robot.SetForceControlAxis(group, [False, False, True, False, False, False])
+
+        # Uncomment the following line to enable passive force control, otherwise active force
+        # control is used by default. See function doc for more details
+        # for group in single_arm_groups:
+        #     robot.SetPassiveForceControl(group, True)
+
+        # NOTE: motion control always uses robot world frame, while force control can use
+        # either world or TCP frame as reference frame
+
+        # Start Unified Motion Force Control
+        # =========================================================================================
+        # Disable max contact wrench regulation. Need to do this AFTER the force control in Z axis
+        # is activated (i.e. motion control disabled in Z axis) and the motion force control mode
+        # is entered, this way the contact force along Z axis is explicitly regulated and will not
+        # spike after the max contact wrench regulation for motion control is disabled
+        for group in single_arm_groups:
+            robot.SetMaxContactWrench(group, [float("inf")] * 6)
+
+        # Update initial poses to current TCP poses
+        for group in single_arm_groups:
+            all_init_pose[group] = robot.states()[group].tcp_pose.copy()
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Initial TCP pose [position 3x1, rotation (quaternion) 4x1]: {all_init_pose[group]}"
+            )
+
+        # Periodic Task
+        # =========================================================================================
+        # Set loop period
+        period = 1.0 / frequency
+        logger.info(
+            f"Sending command to robot at {frequency} Hz, or {period} seconds interval"
+        )
+
+        # Periodic loop counter
+        loop_counter = 0
+
+        # Send command periodically at user-specified frequency
+        while True:
+            # Use sleep to control loop period
+            time.sleep(period)
+
+            # Monitor fault on the connected robot
+            if robot.fault():
+                raise Exception("Fault occurred on the connected robot, exiting ...")
+
+            # Set Fz according to reference frame to achieve a "pressing down" behavior
+            Fz = 0.0
+            if force_ctrl_frame == flexivrdk.CoordType.WORLD:
+                Fz = PRESSING_FORCE
+            elif force_ctrl_frame == flexivrdk.CoordType.TCP:
+                Fz = -PRESSING_FORCE
+            target_wrench = [0.0, 0.0, Fz, 0.0, 0.0, 0.0]
+
+            cmds = {}
+            for group, init_pose in all_init_pose.items():
+                target_pose = init_pose.copy()
+                if args.polish:
+                    target_pose[1] += SWING_AMP * math.sin(
+                        2 * math.pi * SWING_FREQ * loop_counter * period
+                    )
+
+                cmds[group] = flexivrdk.NrtCartesianCmd(target_pose, target_wrench)
+
+            robot.SendCartesianMotionForce(cmds)
+
+            # Increment loop counter
+            loop_counter += 1
+
+    except Exception as e:
+        # Print exception error message
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/intermediate5_robot_dynamics.py b/thirdparty/flexiv_rdk/example_py/intermediate5_robot_dynamics.py
new file mode 100644
index 0000000..ba1fdb4
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/intermediate5_robot_dynamics.py
@@ -0,0 +1,132 @@
+#!/usr/bin/env python
+
+"""intermediate5_robot_dynamics.py
+
+This tutorial runs the integrated dynamics engine to obtain robot Jacobian, mass matrix, and
+gravity torques. Also checks reachability of a Cartesian pose.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import argparse
+import spdlog  # pip install spdlog
+import flexivrdk  # pip install flexivrdk
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456",
+    )
+    args = argparser.parse_args()
+
+    # Define alias
+    logger = spdlog.ConsoleLogger("Example")
+    mode = flexivrdk.Mode
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nThis tutorial runs the integrated dynamics engine to obtain "
+        "robot Jacobian, mass matrix, and gravity torques. Also checks reachability of a Cartesian "
+        "pose.\n"
+    )
+
+    try:
+        # RDK Initialization
+        # ==========================================================================================
+        # Instantiate robot interface
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        # Clear fault on the connected robot if any
+        if robot.fault():
+            logger.warn("Fault occurred on the connected robot, trying to clear ...")
+            # Try to clear the fault
+            if not robot.ClearFault():
+                logger.error("Fault cannot be cleared, exiting ...")
+                return 1
+            logger.info("Fault on the connected robot is cleared")
+
+        # Servo on the robot, make sure the E-stop is released
+        logger.info("Servo on the robot ...")
+        robot.ServoOn()
+
+        # Wait for the robot to become operational
+        while not robot.operational():
+            time.sleep(1)
+
+        logger.info("Robot is now operational")
+
+        # Move robot to home pose
+        logger.info("Moving to home pose")
+        robot.Home()
+
+        # Robot Dynamics
+        # ==========================================================================================
+        # Initialize dynamics engine
+        model = flexivrdk.Model(robot)
+
+        # Step dynamics engine 5 times
+        for i in range(5):
+            # Update robot model in dynamics engine
+            robot_states = robot.states()
+            model.Update(
+                robot_states[flexivrdk.JointGroup.ALL].q,
+                robot_states[flexivrdk.JointGroup.ALL].dq,
+            )
+
+            # Compute gravity vector
+            g = model.g()
+
+            # Compute mass matrix
+            M = model.M()
+
+            # Compute Jacobian
+            J = model.J("flange")
+
+            # Print result
+            logger.info("g = ")
+            print(g, flush=True)
+            logger.info("M = ")
+            print(M, flush=True)
+            logger.info("J = ")
+            print(J, flush=True)
+            print()
+
+        # Check IK feasibility for a nearby Cartesian pose on all available single-arm joint groups
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        robot_states = robot.states()
+        ik_params_by_group = {}
+        for group in single_arm_groups:
+            pose_to_check = robot_states[group].tcp_pose.copy()
+            pose_to_check[0] += 0.1
+            logger.info(
+                f"[{flexivrdk.kJointGroupNames[group]}] Checking IK feasibility of Cartesian pose {pose_to_check}"
+            )
+            ik_params = flexivrdk.IKParams()
+            ik_params.cartesian_pose = pose_to_check
+            ik_params.seed_q = robot_states[group].q.copy()
+            ik_params.free_orientation = False
+            ik_params_by_group[group] = ik_params
+
+        # Print result
+        result = model.SolveConstrainedIK(ik_params_by_group)
+        logger.info(f"IK result success = {result.success}")
+        for group, q in result.solved_q.items():
+            logger.info(f"[{flexivrdk.kJointGroupNames[group]}] solved_q = {q}")
+
+    except Exception as e:
+        logger.error(str(e))
+        return 1
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_rdk/example_py/utility.py b/thirdparty/flexiv_rdk/example_py/utility.py
new file mode 100644
index 0000000..ec9ba0e
--- /dev/null
+++ b/thirdparty/flexiv_rdk/example_py/utility.py
@@ -0,0 +1,109 @@
+#!/usr/bin/env python
+
+"""utility.py
+
+Utility methods.
+
+Import this module as `import utility` and use functions through the module namespace,
+for example `utility.quat2eulerZYX(...)` and `utility.primitive_state_true_for_groups(...)`.
+"""
+
+__copyright__ = "Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import math
+
+# pip install scipy
+from scipy.spatial.transform import Rotation as R
+
+
+def quat2eulerZYX(quat, degree=False):
+    """
+    Convert quaternion to Euler angles with ZYX axis rotations.
+
+    Parameters
+    ----------
+    quat : float list
+        Quaternion input in [w,x,y,z] order.
+    degree : bool
+        Return values in degrees, otherwise in radians.
+
+    Returns
+    ----------
+    float list
+        Euler angles in [x,y,z] order, radian by default unless specified otherwise.
+        Returned angles are normalized by scipy to a principal range. In degree mode,
+        this is typically within [-180, 180].
+    """
+
+    # Convert target quaternion to Euler ZYX using scipy package's 'xyz' extrinsic rotation
+    # NOTE: scipy uses [x,y,z,w] order to represent quaternion. The returned Euler values
+    # are a normalized representation of the same physical orientation.
+    eulerZYX = (
+        R.from_quat([quat[1], quat[2], quat[3], quat[0]])
+        .as_euler("xyz", degrees=degree)
+        .tolist()
+    )
+
+    return eulerZYX
+
+
+def primitive_state_true_for_groups(primitive_states, state_name):
+    """
+    Check whether primitive states are true for all groups included in primitive_states.
+
+    Parameters
+    ----------
+    primitive_states : dict
+        Primitive states keyed by joint group.
+    state_name : str or dict
+        Primitive state name to check for all groups, or per-group state names keyed by joint
+        group, e.g. "reachedTarget" or {group: "terminated"}.
+
+    Returns
+    ----------
+    bool
+        True if all included groups contain the requested primitive state and its integer value is
+        non-zero.
+    """
+
+    def primitive_state_true(primitive_state, requested_state_name):
+        if requested_state_name not in primitive_state.names_and_values:
+            return False
+
+        value = primitive_state.names_and_values[requested_state_name]
+        return isinstance(value, int) and value != 0
+
+    if isinstance(state_name, dict):
+        return all(
+            group in state_name
+            and primitive_state_true(primitive_state, state_name[group])
+            for group, primitive_state in primitive_states.items()
+        )
+
+    return all(
+        primitive_state_true(primitive_state, state_name)
+        for primitive_state in primitive_states.values()
+    )
+
+
+def list2str(ls):
+    """
+    Convert a list to a string.
+
+    Parameters
+    ----------
+    ls : list
+        Source list of any size.
+
+    Returns
+    ----------
+    str
+        A string with format "ls[0] ls[1] ... ls[n] ", i.e. each value
+        followed by a space, including the last one.
+    """
+
+    ret_str = ""
+    for i in ls:
+        ret_str += str(i) + " "
+    return ret_str
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/data.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/data.hpp
new file mode 100644
index 0000000..90c450f
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/data.hpp
@@ -0,0 +1,896 @@
+/**
+ * @file data.hpp
+ * @brief Header file containing various constant expressions, data structures, and enums.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_DATA_HPP_
+#define FLEXIV_RDK_DATA_HPP_
+
+#include <array>
+#include <vector>
+#include <string>
+#include <ostream>
+#include <variant>
+#include <chrono>
+#include <map>
+
+namespace flexiv::rdk {
+/** Cartesian-space degrees of freedom */
+constexpr size_t kCartDoF = 6;
+
+/** Joint-space degrees of freedom of Flexiv's serial robots */
+constexpr size_t kSerialJointDoF = 7;
+
+/** Size of pose array (3 position + 4 quaternion) */
+constexpr size_t kPoseSize = 7;
+
+/** Number of digital IO ports: 16 on the control box + 2 in each wrist connector * maximum 2 wrists
+ */
+constexpr size_t kIOPorts = 16 + 2 * 2;
+
+/** Maximum number of external axes */
+constexpr size_t kMaxExtAxes = 6;
+
+/**
+ * @brief All supported product models of the robot.
+ */
+enum class ProductModel
+{
+    UNKNOWN = 0,
+    Enlight_L,  /// Enlight-L standard version
+    Enlight_LL, /// Enlight-LL: Dual Enlight-L with customizable mounting poses
+    MICO_Core,  /// MICO-Core: Dual Enlight-L with fixed-mounting upper body
+    MICO_Plus,  /// MICO-Plus: MICO-Core with pan-tilt torso
+    MICO_Ultra, /// MICO-Ultra: MICO-Plus with mobile base
+};
+
+/** Map ProductModel enums to strings */
+inline const std::map<ProductModel, std::string> kProductModelNames
+    = {{ProductModel::UNKNOWN, "UNKNOWN"}, {ProductModel::Enlight_L, "Enlight-L"},
+        {ProductModel::Enlight_LL, "Enlight-LL"}, {ProductModel::MICO_Core, "MICO-Core"},
+        {ProductModel::MICO_Plus, "MICO-Plus"}, {ProductModel::MICO_Ultra, "MICO-Ultra"}};
+
+/**
+ * @brief All possible joint groups of the robot.
+ */
+enum class JointGroup
+{
+    UNKNOWN = -1, ///< Unknown group
+    ALL = 0,      ///< The full system, including all actuated joints
+    ARMS = 2,     ///< The dual arms as a whole, only applicable to dual-arm robots
+    ARM_1 = 3,    ///< The 1st single arm in a dual-arm robot or the only arm in a single-arm robot
+    ARM_2 = 4,    ///< The 2nd single arm in a dual-arm robot, not applicable to single-arm robots
+    EXT_AXIS = 5, ///< External axis(es) for workspace extension
+
+    FIRST = ALL,
+    LAST = EXT_AXIS,
+};
+
+/** Map JointGroup enums to strings */
+inline const std::map<JointGroup, std::string> kJointGroupNames {
+    {JointGroup::UNKNOWN, "UNKNOWN"},
+    {JointGroup::ALL, "ALL"},
+    {JointGroup::ARMS, "ARMS"},
+    {JointGroup::ARM_1, "ARM_1"},
+    {JointGroup::ARM_2, "ARM_2"},
+    {JointGroup::EXT_AXIS, "EXT_AXIS"},
+};
+
+/**
+ * @brief All possible operational statuses of the robot. Except for the first two, the other
+ * enumerators indicate the cause of the robot being not ready to operate.
+ * @see Robot::operational_status().
+ */
+enum class OperationalStatus
+{
+    UNKNOWN = 0,        ///< Unknown status.
+    READY,              ///< Ready to be operated.
+    BOOTING,            ///< System still booting, please wait.
+    ESTOP_NOT_RELEASED, ///< E-Stop is not released.
+    NOT_SERVO_ON,       ///< Not servo on, call ServoOn() to send the signal.
+    RELEASING_BRAKE,    ///< Brake release in progress, please wait.
+    MINOR_FAULT,        ///< Minor fault occurred, call ClearFault() to try clearing it.
+    CRITICAL_FAULT,     ///< Critical fault occurred, call ClearFault() to try clearing it.
+    IN_REDUCED_STATE,   ///< In reduced state, see reduced().
+    IN_RECOVERY_STATE,  ///< In recovery state, see recovery().
+    IN_MANUAL_MODE,     ///< In Manual mode, need to switch to Auto (Remote) mode.
+    IN_AUTO_MODE,       ///< In regular Auto mode, need to switch to Auto (Remote) mode.
+};
+
+/** Map OperationalStatus enums to strings */
+inline const std::map<OperationalStatus, std::string> kOpStatusNames {
+    {OperationalStatus::UNKNOWN, "Unknown status"},
+    {OperationalStatus::READY, "Ready"},
+    {OperationalStatus::BOOTING, "System booting"},
+    {OperationalStatus::ESTOP_NOT_RELEASED, "E-Stop not released"},
+    {OperationalStatus::NOT_SERVO_ON, "Not servo on"},
+    {OperationalStatus::RELEASING_BRAKE, "Releasing brakes"},
+    {OperationalStatus::MINOR_FAULT, "Minor fault occurred"},
+    {OperationalStatus::CRITICAL_FAULT, "Critical fault occurred"},
+    {OperationalStatus::IN_REDUCED_STATE, "In reduced state"},
+    {OperationalStatus::IN_RECOVERY_STATE, "In recovery state"},
+    {OperationalStatus::IN_MANUAL_MODE, "In Manual mode"},
+    {OperationalStatus::IN_AUTO_MODE, "In regular Auto mode"},
+};
+
+/**
+ * @brief Type of commonly-used reference coordinates.
+ */
+enum class CoordType
+{
+    WORLD, ///< World frame (fixed).
+    TCP,   ///< TCP frame (move with the robot's end effector).
+};
+
+/**
+ * @struct RobotEvent
+ * @brief Information about a robot event.
+ * @see Robot::event_log().
+ */
+struct RobotEvent
+{
+    enum Level
+    {
+        UNKNOWN = 0, ///< Not set
+        INFO,        ///< Informational event
+        WARNING,     ///< Warning event
+        ERROR,       ///< Error event
+        CRITICAL,    ///< Critical error event
+    };
+
+    /** Level of the event */
+    Level level = UNKNOWN;
+
+    /** Unique ID of the event */
+    int id = 0;
+
+    /** Brief description of the event */
+    std::string description = "";
+
+    /** Consequences caused by the event */
+    std::string consequences = "";
+
+    /** Probable causes of the event */
+    std::string probable_causes = "";
+
+    /** Recommended actions after the event */
+    std::string recommended_actions = "";
+
+    /** Timestamp (since epoch) of the event */
+    std::chrono::time_point<std::chrono::system_clock> timestamp;
+};
+
+/**
+ * @struct RobotInfo
+ * @brief General information about the connected robot.
+ * @see Robot::info().
+ */
+struct RobotInfo
+{
+    /** Product model of the robot */
+    ProductModel product_model = {};
+
+    /** Serial number of the robot */
+    std::string serial_num = {};
+
+    /** Software version of the robot */
+    std::string software_ver = {};
+
+    /** Type of license */
+    std::string license_type = {};
+
+    /** All available joint groups in the robot and their names */
+    std::map<JointGroup, std::string> all_groups = {};
+
+    /** Available single-arm joint groups in the robot and their names */
+    std::map<JointGroup, std::string> single_arm_groups = {};
+
+    /** Joint-space degrees of freedom for each joint group */
+    std::map<JointGroup, size_t> DoF = {};
+
+    /** Nominal Cartesian motion stiffness of all available single-arm joint groups when in
+     * Cartesian motion-force control modes: \f$ K_x^{nom} \in \mathbb{R}^{6 \times 1} \f$. Consists
+     * of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$ \mathbb{R}^{3 \times 1} \f$
+     * angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, k_{Rz}]^T \f$.
+     * Unit: \f$ [N/m]:[Nm/rad] \f$. */
+    std::map<JointGroup, std::array<double, kCartDoF>> K_x_nom = {};
+
+    /** Nominal joint motion stiffness of all available single-arm joint groups when in joint
+     * impedance control modes: \f$ K_q^{nom} \in \mathbb{R}^{n \times 1} \f$.
+     * Unit: \f$ [Nm/rad] \f$. */
+    std::map<JointGroup, std::vector<double>> K_q_nom = {};
+
+    /** Lower software limits of joint positions of all available joint groups: \f$ q_{min} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> q_min = {};
+
+    /** Upper software limits of joint positions of all available joint groups: \f$ q_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> q_max = {};
+
+    /** Upper software limits of joint velocities of all available joint groups: \f$ \dot{q}_{max}
+     * \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$. */
+    std::map<JointGroup, std::vector<double>> dq_max = {};
+
+    /** Upper software limits of joint torques of all available joint groups: \f$ \tau_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$. */
+    std::map<JointGroup, std::vector<double>> tau_max = {};
+};
+
+/**
+ * @struct RobotStates
+ * @brief Robot states data in joint and Cartesian space for a joint group.
+ */
+struct RobotStates
+{
+    /** Current time since epoch of the robot system. The pair consists of {seconds since epoch,
+     * nanoseconds since last full second} */
+    std::pair<int, int> timestamp = {};
+
+    /**
+     * Measured joint positions from the link-side encoder: \f$ q \in \mathbb{R}^{n \times 1} \f$.
+     * This is the direct measurement of joint positions. Unit: \f$ [rad] or [m] \f$.
+     * @note If a joint has only one encoder, then \f$ \theta = q \f$.
+     */
+    std::vector<double> q = {};
+
+    /**
+     * Measured joint positions from the motor-side encoder: \f$ \theta \in \mathbb{R}^{n \times 1}
+     * \f$. This is the indirect measurement of joint positions. \f$ \theta = q + \Delta \f$, where
+     * \f$ \Delta \f$ is the joint's internal deflection between motor and link. Unit: \f$ [rad] or
+     * [m] \f$.
+     * @note If a joint has only one encoder, then \f$ \theta = q \f$.
+     */
+    std::vector<double> theta = {};
+
+    /**
+     * Measured joint velocities from the link-side encoder: \f$ \dot{q} \in \mathbb{R}^{n \times
+     * 1} \f$. This is the direct but more noisy measurement of joint velocities. Unit: \f$ [rad/s]
+     * or [m/s] \f$.
+     * @note If a joint has only one encoder, then \f$ \dot{\theta} = \dot{q} \f$.
+     */
+    std::vector<double> dq = {};
+
+    /**
+     * Measured joint velocities from the motor-side encoder: \f$ \dot{\theta} \in \mathbb{R}^{n
+     * \times 1} \f$. This is the indirect but less noisy measurement of joint velocities. Unit: \f$
+     * [rad/s] or [m/s] \f$.
+     * @note If a joint has only one encoder, then \f$ \dot{\theta} = \dot{q} \f$.
+     */
+    std::vector<double> dtheta = {};
+
+    /**
+     * Measured joint torques: \f$ \tau \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
+     * @note If a joint has no torque measurement, the corresponding value will be 0.
+     */
+    std::vector<double> tau = {};
+
+    /**
+     * Numerical derivative of measured joint torques: \f$ \dot{\tau} \in \mathbb{R}^{n \times 1}
+     * \f$. Unit: \f$ [Nm/s] \f$.
+     * @note If a joint has no torque measurement, the corresponding value will be 0.
+     */
+    std::vector<double> tau_dot = {};
+
+    /**
+     * Estimated external joint torques: \f$ \hat \tau_{ext} \in \mathbb{R}^{n \times 1} \f$.
+     * Produced by any external contact (with robot body or end-effector) that does not belong to
+     * the known robot model. Unit: \f$ [Nm] \f$.
+     * @note If a joint has no torque measurement, the corresponding value will be 0.
+     */
+    std::vector<double> tau_ext = {};
+
+    /**
+     * Estimated interaction joint torques: \f$ \hat \tau_{int} \in \mathbb{R}^{n \times 1} \f$.
+     * Produced by any interaction forces at the TCP. Unit: \f$ [Nm] \f$.
+     * @note If a joint has no torque measurement, the corresponding value will be 0.
+     */
+    std::vector<double> tau_interact = {};
+
+    /**
+     * Measured joint temperatures: \f$ temp \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [°C] \f$.
+     * @note If a joint has no temperature measurement, the corresponding value will be 0.
+     */
+    std::vector<double> temperature = {};
+
+    /**
+     * Measured flange pose w.r.t. world frame: \f$ ^{O}T_{flange} \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
+     */
+    std::array<double, kPoseSize> flange_pose = {};
+
+    /**
+     * Measured TCP pose w.r.t. world frame: \f$ ^{O}T_{TCP} \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
+     */
+    std::array<double, kPoseSize> tcp_pose = {};
+
+    /**
+     * Measured TCP twist w.r.t. world frame: \f$ ^{O}\dot{X} \in \mathbb{R}^{6 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear velocity and \f$ \mathbb{R}^{3 \times 1}
+     * \f$ angular velocity: \f$ [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T \f$. Unit: \f$
+     * [m/s]:[rad/s] \f$.
+     */
+    std::array<double, kCartDoF> tcp_twist = {};
+
+    /**
+     * Measured or estimated external wrench applied on TCP w.r.t. world frame: \f$ ^{O}F_{ext} \in
+     * \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
+     * \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
+     * Unit: \f$ [N]:[Nm] \f$.
+     */
+    std::array<double, kCartDoF> tcp_wrench = {};
+
+    /**
+     * Measured or estimated external wrench applied on TCP w.r.t. local frame: \f$ ^{TCP}F_{ext}
+     * \in \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
+     * \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
+     * Unit: \f$ [N]:[Nm] \f$.
+     */
+    std::array<double, kCartDoF> tcp_wrench_local = {};
+
+    /**
+     * Unfiltered tcp_wrench. The data is more noisy but has no filter latency.
+     */
+    std::array<double, kCartDoF> raw_tcp_wrench = {};
+
+    /**
+     * Unfiltered tcp_wrench_local. The data is more noisy but has no filter latency.
+     */
+    std::array<double, kCartDoF> raw_tcp_wrench_local = {};
+
+    /**
+     * Raw reading from the force-torque (FT) sensor w.r.t. flange frame: \f$ ^{flange}F_{raw}
+     * \in \mathbb{R}^{6 \times 1} \f$. The value is 0 if no FT sensor is installed. Consists of
+     * \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$
+     * [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$ [N]:[Nm] \f$.
+     */
+    std::array<double, kCartDoF> raw_ft_sensor = {};
+};
+
+/**
+ * @struct RobotActions
+ * @brief Robot actions data in joint and Cartesian space for a joint group.
+ */
+struct RobotActions
+{
+    /** Current time since epoch of the robot system. The pair consists of {seconds since epoch,
+     * nanoseconds since last full second} */
+    std::pair<int, int> timestamp = {};
+
+    /**
+     * Desired joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] or [m] \f$.
+     */
+    std::vector<double> q_d = {};
+
+    /**
+     * Desired joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] or
+     * [m/s] \f$.
+     */
+    std::vector<double> dq_d = {};
+
+    /**
+     * Desired joint torques excluding the compensation of nonlinear dynamics: \f$ \tau_d \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
+     * @note Nonlinear dynamics include: gravity, centrifugal, and Coriolis. If the robot is static,
+     * tau_d will be zeros.
+     * @note If a joint has no torque control capability, the corresponding value will be 0.
+     */
+    std::vector<double> tau_d = {};
+
+    /**
+     * Desired TCP pose w.r.t. world frame: \f$ {^{O}T_{TCP}}_d \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]~[] \f$.
+     */
+    std::array<double, kPoseSize> tcp_pose_d = {};
+
+    /**
+     * Desired TCP twist w.r.t. world frame: \f$ {^{O}\dot{X}}_d \in \mathbb{R}^{6 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear velocity and \f$ \mathbb{R}^{3 \times 1}
+     * \f$ angular velocity: \f$ [v_x, v_y, v_z, \omega_x, \omega_y, \omega_z]^T \f$. Unit: \f$
+     * [m/s]:[rad/s] \f$.
+     */
+    std::array<double, kCartDoF> tcp_twist_d = {};
+
+    /**
+     * Desired TCP wrench w.r.t. the current force control frame: \f$ {^{ctrl}F_{ext}}_d \in
+     * \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ force and \f$
+     * \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$. Unit: \f$
+     * [N]:[Nm] \f$.
+     */
+    std::array<double, kCartDoF> tcp_wrench_d = {};
+};
+
+/**
+ * @struct PlanInfo
+ * @brief Information of the on-going primitive/plan.
+ * @see Robot::plan_info().
+ */
+struct PlanInfo
+{
+    /** Current primitive name */
+    std::string pt_name = {};
+
+    /** Current node name */
+    std::string node_name = {};
+
+    /** Current node path */
+    std::string node_path = {};
+
+    /** Current node path time period */
+    std::string node_path_time_period = {};
+
+    /** Current node path number */
+    std::string node_path_number = {};
+
+    /** Assigned plan name */
+    std::string assigned_plan_name = {};
+
+    /** Velocity scale */
+    double velocity_scale = {};
+
+    /** Waiting for user signal to step the breakpoint */
+    bool waiting_for_step = {};
+};
+
+/**
+ * @struct JPos
+ * @brief Data structure representing the customized data type "JPOS" in Flexiv Elements.
+ * @warning Here [m] is used as the unit of length, whereas [mm] is used in Flexiv Elements. The
+ * conversion is automatically done when exchanging "JPOS" data type with the robot via functions
+ * like Robot::ExecutePrimitive(), Robot::SetGlobalVariables(), etc.
+ */
+struct JPos
+{
+    /** Default constructor */
+    JPos() = default;
+
+    /**
+     * @brief Custom constructor.
+     * @param[in] q_m Sets struct member [q_m].
+     * @param[in] q_e Sets struct member [q_e]. Leave empty if there's no external axis.
+     */
+    JPos(const std::array<double, kSerialJointDoF>& q_m,
+        const std::array<double, kMaxExtAxes>& q_e = {})
+    : q_m(q_m)
+    , q_e(q_e)
+    {
+    }
+
+    /** Joint positions of the robot manipulator. Unit: [degree] */
+    std::array<double, kSerialJointDoF> q_m = {};
+
+    /** Joint positions (linear or angular) of the external axes. Unit: [m] or [degree]
+     * @note If the number of external axes \f$ n_e < kMaxExtAxes \f$, set the first \f$ n_e \f$
+     * elements and leave the rest 0. Leave the whole array empty if there's no external axis. */
+    std::array<double, kMaxExtAxes> q_e = {};
+
+    /** String representation of all data in the struct, separated by space */
+    std::string str() const;
+};
+
+/**
+ * @struct Coord
+ * @brief Data structure representing the customized data type "COORD" in Flexiv Elements.
+ * @warning Here [m] is used as the unit of length, whereas [mm] is used in Flexiv Elements. The
+ * conversion is automatically done when exchanging "COORD" data type with the robot via functions
+ * like Robot::ExecutePrimitive(), Robot::SetGlobalVariables(), Robot::global_variables(), etc.
+ */
+struct Coord
+{
+    /** Default constructor */
+    Coord() = default;
+
+    /**
+     * @brief Custom constructor.
+     * @param[in] position Sets struct member [position].
+     * @param[in] orientation Sets struct member [orientation].
+     * @param[in] ref_frame Sets struct member [ref_frame].
+     * @param[in] ref_q_m Sets struct member [ref_q_m]. Leave empty to use default values.
+     * @param[in] ref_q_e Sets struct member [ref_q_e]. Leave empty if there's no external axis.
+     */
+    Coord(const std::array<double, kCartDoF / 2>& position,
+        const std::array<double, kCartDoF / 2>& orientation,
+        const std::array<std::string, 2>& ref_frame,
+        const std::array<double, kSerialJointDoF>& ref_q_m = {},
+        const std::array<double, kMaxExtAxes>& ref_q_e = {})
+    : position(position)
+    , orientation(orientation)
+    , ref_frame(ref_frame)
+    , ref_q_m(ref_q_m)
+    , ref_q_e(ref_q_e)
+    {
+    }
+
+    /** Position in [ref_frame]. Unit: [m] */
+    std::array<double, kCartDoF / 2> position = {};
+
+    /** Orientation in terms of Euler angles in [ref_frame]. Unit: [degree] */
+    std::array<double, kCartDoF / 2> orientation = {};
+
+    /** Name of the reference frame "root::branch" represented as {"root", "branch"}.
+     *  Refer to Flexiv Elements for available options. Some common ones are:
+     * - World origin: {"WORLD", "WORLD_ORIGIN"}
+     * - Current pose: {"TRAJ", "START"}
+     * - A work coordinate: {"WORK", "WorkCoord0"}
+     * - A global variable: {"GVAR", "MyCoord0"}
+     */
+    std::array<std::string, 2> ref_frame = {};
+
+    /** Reference joint positions of the robot manipulator. Only effective on robots with redundant
+     * degrees of freedom. Unit: [degree]
+     * @note Leave empty to use default values. However, this array cannot be empty if [ref_q_e] has
+     * values */
+    std::array<double, kSerialJointDoF> ref_q_m = {};
+
+    /** Reference joint positions (linear or angular) of the external axes. Only effective on
+     * robots with redundant degrees of freedom and external axes. Unit: [m] or [degree]
+     * @note If the number of external axes \f$ n_e < kMaxExtAxes \f$, set the first \f$ n_e \f$
+     * elements and leave the rest 0. Leave the whole array empty if there's no external axis. */
+    std::array<double, kMaxExtAxes> ref_q_e = {};
+
+    /** String representation of all data in the struct, separated by space */
+    std::string str() const;
+};
+
+/**
+ * @brief Modes for synchronous motions.
+ * @see PrimitiveArgs::sync_motion_mode
+ */
+enum class SyncMotionMode
+{
+    DISABLE = 0,    ///< Don't sync with any target
+    ARM1_TCP = 1,   ///< Sync with arm1 tcp
+    ARM2_TCP = 2,   ///< sync with arm2 tcp
+    POSITIONER = 3, ///< sync with positioner
+};
+
+/** Alias of the variant that holds all possible types of data exchanged with Flexiv robots */
+using FlexivDataTypes = std::variant<int, double, std::string, rdk::JPos, rdk::Coord,
+    std::vector<int>, std::vector<double>, std::vector<std::string>, std::vector<rdk::JPos>,
+    std::vector<rdk::Coord>>;
+
+/**
+ * @struct PrimitiveArgs
+ * @brief Arguments of a primitive command.
+ * @see Robot::ExecutePrimitive().
+ * @see [Flexiv Primitives documentation](https://www.flexiv.com/primitives/)
+ */
+struct PrimitiveArgs
+{
+    /** Default constructor */
+    PrimitiveArgs() = default;
+
+    /** Custom constructor */
+    PrimitiveArgs(
+        const std::string& pt_name, const std::map<std::string, FlexivDataTypes>& input_params)
+    : pt_name(pt_name)
+    , input_params(input_params)
+    {
+    }
+
+    /** Name of the primitive to execute. For example, "Home", "MoveL", "ZeroFTSensor", etc. */
+    std::string pt_name = {};
+
+    /** Input parameter names and values of this primitive. Use int 1 and 0 to represent booleans.
+     * E.g. {{"target", rdk::Coord({0.65, -0.3, 0.2}, {180, 0, 180}, {"WORLD", "WORLD_ORIGIN"})},
+     * {"vel", 0.6}, {"zoneRadius", "Z50"}}.
+     * @warning The primitive input parameters may not use SI units, please refer to the Flexiv
+     * Primitives documentation for exact unit definition. */
+    std::map<std::string, FlexivDataTypes> input_params = {};
+
+    /** True: The external axis motion is actively controlled by this primitive. When this primitive
+     * is executed, the external axis will move to the position defined in target points or
+     * waypoints. False: The external axis motion is not actively controlled by this primitive.
+     * @note This setting is only effective when the external axis exists. */
+    bool external_axis_control = true;
+
+    /** Mode for synchronous motions between the TCP of this primitive and the selected arm or
+     * positioner. Once configured, the motion of the TCP will be automatically adjusted in
+     * accordance with the movement of the selected arm or positioner.
+     * @note Only certain primitives support this feature, this setting will be ignored if this
+     * primitive does not support it. */
+    SyncMotionMode sync_motion_mode = SyncMotionMode::DISABLE;
+};
+
+/**
+ * @struct PrimitiveStates
+ * @brief States data of a primitive.
+ * @see Robot::primitive_states().
+ */
+struct PrimitiveStates
+{
+    /** Name of the currently running primitive */
+    std::string pt_name = {};
+
+    /** Names and corresponding values of this primitive's states. Booleans are represented by int 1
+     * and 0. For example:
+     * {{"reachedTarget", 1}, {"timePeriod", 5.6}, {"forceOffset", {0.1, 0.2, -1.3}}}.
+     */
+    std::map<std::string, FlexivDataTypes> names_and_values = {};
+};
+
+/**
+ * @struct RtJointTorqueCmd
+ * @brief Commands data for real-time joint torque control.
+ * @see Robot::StreamJointTorque().
+ */
+struct RtJointTorqueCmd
+{
+    /** Default constructor */
+    RtJointTorqueCmd() = default;
+
+    /** Custom constructor */
+    RtJointTorqueCmd(const std::vector<double>& tau_d, bool enable_gravity_comp = true,
+        bool enable_soft_limits = true, double friction_comp_scale = 100.0)
+    : tau_d(tau_d)
+    , enable_gravity_comp(enable_gravity_comp)
+    , enable_soft_limits(enable_soft_limits)
+    , friction_comp_scale(friction_comp_scale)
+    {
+    }
+
+    /** Target joint torques: \f$ \tau_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$ */
+    std::vector<double> tau_d = {};
+
+    /** Enable/disable robot gravity compensation */
+    bool enable_gravity_comp = true;
+
+    /** Enable/disable soft limits to keep the joints from moving outside allowed position range,
+     * which will trigger a safety fault that requires recovery operation */
+    bool enable_soft_limits = true;
+
+    /** Percentage of joint friction to be compensated. Valid range: [0, 100]. Setting to 100 means
+     * to compensate all joint friction, and 0 means no friction compensation at all.
+     * Under-compensation increases natural damping of the joints, which can be useful in some
+     * cases, e.g. zero-torque floating. */
+    double friction_comp_scale = 100.0;
+};
+
+/**
+ * @struct RtJointPositionCmd
+ * @brief Commands data for real-time joint position control.
+ * @see Robot::StreamJointPosition().
+ */
+struct RtJointPositionCmd
+{
+    /** Default constructor */
+    RtJointPositionCmd() = default;
+
+    /** Custom constructor */
+    RtJointPositionCmd(const std::vector<double>& q_d, const std::vector<double>& dq_d,
+        const std::vector<double>& ddq_d)
+    : q_d(q_d)
+    , dq_d(dq_d)
+    , ddq_d(ddq_d)
+    {
+    }
+
+    /** Target joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$ */
+    std::vector<double> q_d = {};
+
+    /** Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s]
+     * \f$ */
+    std::vector<double> dq_d = {};
+
+    /** Target joint accelerations: \f$ \ddot{q}_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$
+     * [rad/s^2] \f$ */
+    std::vector<double> ddq_d = {};
+};
+
+/**
+ * @struct NrtJointPositionCmd
+ * @brief Commands data for non-real-time joint position control.
+ * @see Robot::SendJointPosition().
+ */
+struct NrtJointPositionCmd
+{
+    /** Default constructor */
+    NrtJointPositionCmd() = default;
+
+    /** Custom constructor */
+    NrtJointPositionCmd(const std::vector<double>& q_d, const std::vector<double>& dq_d,
+        const std::vector<double>& dq_max, const std::vector<double>& ddq_max)
+    : q_d(q_d)
+    , dq_d(dq_d)
+    , dq_max(dq_max)
+    , ddq_max(ddq_max)
+    {
+    }
+
+    /** Target joint positions: \f$ q_d \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$ */
+    std::vector<double> q_d = {};
+
+    /** Target joint velocities: \f$ \dot{q}_d \in \mathbb{R}^{n \times 1} \f$. Each joint will
+     * maintain this amount of velocity when it reaches the target position. Unit: \f$ [rad/s] \f$
+     */
+    std::vector<double> dq_d = {};
+
+    /** Maximum joint velocities for the planned trajectory: \f$ \dot{q}_{max} \in \mathbb{R}^{n
+     * \times 1} \f$. Unit: \f$ [rad/s] \f$ */
+    std::vector<double> dq_max = {};
+
+    /** Maximum joint accelerations for the planned trajectory: \f$ \ddot{q}_{max} \in \mathbb{R}^{n
+     * \times 1} \f$. Unit: \f$ [rad/s^2] \f$ */
+    std::vector<double> ddq_max = {};
+};
+
+/**
+ * @struct RtCartesianCmd
+ * @brief Commands data for real-time Cartesian motion-force control.
+ * @see Robot::StreamCartesianMotionForce().
+ */
+struct RtCartesianCmd
+{
+    /** Default constructor */
+    RtCartesianCmd() = default;
+
+    /** Custom constructor */
+    RtCartesianCmd(const std::array<double, kPoseSize>& pose_d,
+        const std::array<double, kCartDoF>& wrench_d = {},
+        const std::array<double, kCartDoF>& twist_d = {},
+        const std::array<double, kCartDoF>& acc_d = {})
+    : pose_d(pose_d)
+    , wrench_d(wrench_d)
+    , twist_d(twist_d)
+    , acc_d(acc_d)
+    {
+    }
+
+    /** Target TCP pose in world frame: \f$ {^{O}T_{TCP}}_{d} \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
+    std::array<double, kPoseSize> pose_d = {};
+
+    /** Target TCP wrench in the force control reference frame (configured by
+     * SetForceControlFrame()): \f$ ^{O}F_d \in \mathbb{R}^{6 \times 1} \f$. The robot will track
+     * the target wrench using an explicit force controller. Consists of \f$ \mathbb{R}^{3 \times 1}
+     * \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T
+     * \f$. Unit: \f$ [N]:[Nm] \f$ */
+    std::array<double, kCartDoF> wrench_d = {};
+
+    /** Target TCP twist in world frame: \f$ ^{O}\dot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
+     * Providing properly calculated target twist can improve the robot's overall tracking
+     * performance at the cost of reduced robustness. Leaving this input 0 can maximize robustness
+     * at the cost of reduced tracking performance. Consists of \f$ \mathbb{R}^{3 \times 1} \f$
+     * linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular velocity. Unit: \f$ [m/s]:[rad/s] \f$ */
+    std::array<double, kCartDoF> twist_d = {};
+
+    /** Target TCP acceleration in world frame: \f$ ^{O}\ddot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
+     * Feeding forward target acceleration can improve the robot's tracking performance for highly
+     * dynamic motions, but it's also okay to leave this input 0. Consists of \f$ \mathbb{R}^{3
+     * \times 1} \f$ linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular acceleration. Unit: \f$
+     * [m/s^2]:[rad/s^2] \f$ */
+    std::array<double, kCartDoF> acc_d = {};
+};
+
+/**
+ * @struct NrtCartesianCmd
+ * @brief Commands data for non-real-time Cartesian motion-force control.
+ * @see Robot::SendCartesianMotionForce().
+ */
+struct NrtCartesianCmd
+{
+    /** Default constructor */
+    NrtCartesianCmd() = default;
+
+    /** Custom constructor */
+    NrtCartesianCmd(const std::array<double, kPoseSize>& pose_d,
+        const std::array<double, kCartDoF>& wrench_d = {},
+        const std::array<double, kCartDoF>& twist_d = {}, double max_linear_vel = 0.5,
+        double max_angular_vel = 1.0, double max_linear_acc = 2.0, double max_angular_acc = 5.0)
+    : pose_d(pose_d)
+    , wrench_d(wrench_d)
+    , twist_d(twist_d)
+    , max_linear_vel(max_linear_vel)
+    , max_angular_vel(max_angular_vel)
+    , max_linear_acc(max_linear_acc)
+    , max_angular_acc(max_angular_acc)
+    {
+    }
+
+    /** Target TCP pose in world frame: \f$ {^{O}T_{TCP}}_{d} \in \mathbb{R}^{7 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$
+     * quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
+    std::array<double, kPoseSize> pose_d = {};
+
+    /** Target TCP wrench in the force control reference frame (configured by
+     * SetForceControlFrame()): \f$ ^{O}F_d \in \mathbb{R}^{6 \times 1} \f$. The robot will track
+     * the target wrench using an explicit force controller. Consists of \f$ \mathbb{R}^{3 \times 1}
+     * \f$ force and \f$ \mathbb{R}^{3 \times 1} \f$ moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T
+     * \f$. Unit: \f$ [N]:[Nm] \f$ */
+    std::array<double, kCartDoF> wrench_d = {};
+
+    /** Target TCP twist in world frame: \f$ ^{O}\dot{x}_d \in \mathbb{R}^{6 \times 1} \f$.
+     * Providing properly calculated target twist can improve the robot's overall tracking
+     * performance at the cost of reduced robustness. Leaving this input 0 can maximize robustness
+     * at the cost of reduced tracking performance. Consists of \f$ \mathbb{R}^{3 \times 1} \f$
+     * linear and \f$ \mathbb{R}^{3 \times 1} \f$ angular velocity. Unit: \f$ [m/s]:[rad/s] \f$ */
+    std::array<double, kCartDoF> twist_d = {};
+
+    /** Maximum Cartesian linear velocity when moving to the target pose. A safe value is provided
+     * as default. Unit: \f$ [m/s] \f$ */
+    double max_linear_vel = 0.5;
+
+    /** Maximum Cartesian angular velocity when moving to the target pose. A safe value is provided
+     * as default. Unit: \f$ [rad/s] \f$ */
+    double max_angular_vel = 1.0;
+
+    /** Maximum Cartesian linear acceleration when moving to the target pose. A safe value is
+     * provided as default. Unit: \f$ [m/s^2] \f$ */
+    double max_linear_acc = 2.0;
+
+    /** Maximum Cartesian angular acceleration when moving to the target pose. A safe value is
+     * provided as default. Unit: \f$ [rad/s^2] \f$ */
+    double max_angular_acc = 5.0;
+};
+
+/**
+ * @struct NrtCartesianMultiWaypointCmd
+ * @brief Commands data for non-real-time Cartesian multi-waypoint motion-force control.
+ * @see Robot::SendCartesianMotionForceMultiWaypoint().
+ */
+struct NrtCartesianMultiWaypointCmd
+{
+    /** Default constructor */
+    NrtCartesianMultiWaypointCmd() = default;
+
+    /** Custom constructor */
+    NrtCartesianMultiWaypointCmd(const std::vector<NrtCartesianCmd>& waypoints)
+    : waypoints(waypoints)
+    {
+    }
+
+    /** Sequence of Cartesian waypoints to execute. Each waypoint uses the same data layout as
+     * NrtCartesianCmd. */
+    std::vector<NrtCartesianCmd> waypoints = {};
+};
+
+/**
+ * @brief Operator overloading to out stream all members of RobotEvent in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] robot_event RobotEvent data structure to out stream.
+ * @return Updated ostream instance.
+ * @note The event timestamp is converted to local timezone when printed.
+ */
+std::ostream& operator<<(std::ostream& ostream, const RobotEvent& robot_event);
+
+/**
+ * @brief Operator overloading to out stream all members of RobotInfo in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] robot_info RobotInfo data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const RobotInfo& robot_info);
+
+/**
+ * @brief Operator overloading to out stream all members of RobotStates in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] robot_states RobotStates data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const RobotStates& robot_states);
+
+/**
+ * @brief Operator overloading to out stream all members of RobotActions in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] robot_actions RobotActions data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const RobotActions& robot_actions);
+
+/**
+ * @brief Operator overloading to out stream all members of PlanInfo in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] plan_info PlanInfo data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const PlanInfo& plan_info);
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_DATA_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/device.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/device.hpp
new file mode 100644
index 0000000..94a04ae
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/device.hpp
@@ -0,0 +1,114 @@
+/**
+ * @file device.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_DEVICE_HPP_
+#define FLEXIV_RDK_DEVICE_HPP_
+
+#include "robot.hpp"
+#include <map>
+
+namespace flexiv::rdk {
+
+using DeviceParamDataTypes
+    = std::variant<int, double, std::string, std::vector<double>, std::vector<std::string>>;
+
+/**
+ * @class Device
+ * @brief Interface to control the peripheral device(s) connected to the robot.
+ */
+class Device
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the device control interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Device(const Robot& robot);
+    virtual ~Device();
+
+    /**
+     * @brief [Non-blocking] A list of all existing devices.
+     * @return Device names as a string list.
+     */
+    std::vector<std::string> list() const;
+
+    /**
+     * @brief [Non-blocking] Whether the specified device already exists.
+     * @param[in] name Name of the device to check.
+     * @return True if the specified device exists.
+     */
+    bool exist(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Whether the specified device is enabled.
+     * @param[in] name Name of the device to check.
+     * @return True if the specified device is enabled.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    bool enabled(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Whether the specified device is connected.
+     * @param[in] name Name of the device to check.
+     * @return True if the specified device is connected.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    bool connected(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Configuration parameters of the specified device.
+     * @param[in] name Name of the device to get parameters for.
+     * @return A map of {param_name, param_value}. Booleans are represented by int 1 and 0. For
+     * example, {{"maxVel", 0.5}, {"absolutePosition", {0.7, -0.4, 0.05}}, {"conveyorName",
+     * "conveyor0"}}.
+     * @throw std::invalid_argument if the specified device does not exist.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::map<std::string, DeviceParamDataTypes> params(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Enable the specified device.
+     * @param[in] name Name of the device to enable.
+     * @throw std::invalid_argument if the specified device does not exist.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Enable(const std::string& name);
+
+    /**
+     * @brief [Blocking] Disable the specified device.
+     * @param[in] name Name of the device to disable.
+     * @throw std::invalid_argument if the specified device does not exist.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Disable(const std::string& name);
+
+    /**
+     * @brief [Blocking] Send command(s) to the specified device.
+     * @param[in] name Name of the device to send command(s) to.
+     * @param[in] commands A map of {command_name, command_value}. For example, {{"setSpeed", 6000},
+     * {"openLaser", true}}. All commands in the map will be sent to the device simultaneously. Make
+     * sure the command name(s) are valid and can be accepted by the specified device.
+     * @throw std::invalid_argument if the specified device does not exist.
+     * @throw std::logic_error if the specified device is not enabled yet.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Command(const std::string& name,
+        const std::map<std::string, std::variant<bool, int, double>>& commands);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_DEVICE_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/file_io.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/file_io.hpp
new file mode 100644
index 0000000..b99e303
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/file_io.hpp
@@ -0,0 +1,128 @@
+/**
+ * @file file_io.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_FILE_IO_HPP_
+#define FLEXIV_RDK_FILE_IO_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/**
+ * @class FileIO
+ * @brief Interface to exchange files with the robot. Only certain types of file can be transferred.
+ */
+class FileIO
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the file exchange interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    FileIO(const Robot& robot);
+    virtual ~FileIO();
+
+    /**
+     * @brief [Blocking] A list of all trajectory files currently stored in the connected robot.
+     * @return Trajectory filenames as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> traj_files_list() const;
+
+    /**
+     * @brief [Blocking] A list of all user projects currently stored in the connected robot.
+     * @return Project names as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> projects_list() const;
+
+    /**
+     * @brief [Blocking] Upload a local trajectory file (.traj) to the robot.
+     * @param[in] file_dir Relative or absolute path of the directory that contains the file to
+     * upload, e.g. /home/user/Documents/. Do not include the file name here.
+     * @param[in] file_name Full name of the trajectory file to upload, including the extension.
+     * E.g. PolishSpiral.traj. Do not include the directory path here.
+     * @throw std::invalid_argument if failed to find or load the specified file.
+     * @throw std::runtime_error if failed to upload the file.
+     * @note This function blocks until the file is successfully uploaded.
+     */
+    void UploadTrajFile(const std::string& file_dir, const std::string& file_name);
+
+    /**
+     * @brief [Blocking] Download a trajectory file (.traj) from the robot.
+     * @param[in] file_name Full name of the trajectory file to download, including the extension.
+     * E.g. PolishSpiral.traj.
+     * @return String content of the trajectory file.
+     * @throw std::invalid_argument if the specified file does not exist.
+     * @throw std::runtime_error if failed to download the file.
+     * @note This function blocks until the file is successfully downloaded.
+     */
+    std::string DownloadTrajFile(const std::string& file_name);
+
+    /**
+     * @brief [Blocking] Download a trajectory file (.traj) from the robot and save to the specified
+     * directory.
+     * @param[in] file_name Full name of the trajectory file to download, including the extension.
+     * E.g. "PolishSpiral.traj".
+     * @param[in] save_dir Relative or absolute path of the directory to save the downloaded file.
+     * E.g. /home/user/Documents/. Do not include the file name here. The specified directory will
+     * be created if not yet exist.
+     * @throw std::invalid_argument if the specified file does not exist.
+     * @throw std::runtime_error if failed to download or save the file.
+     * @throw std::filesystem::filesystem_error if failed to create the specified directory.
+     * @note This function blocks until the file is successfully downloaded.
+     */
+    void DownloadTrajFile(const std::string& file_name, const std::string& save_dir);
+
+    /**
+     * @brief [Blocking] Upload a local project to the robot.
+     * @param[in] project_dir Relative or absolute path of the project directory, which should
+     * contain a .proj file and a .plan file.
+     * @throw std::invalid_argument if failed to find the specified directory or the project files
+     * are incomplete.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to upload the project.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the project is successfully uploaded.
+     * @warning If the uploaded project was created by Flexiv Elements with a software package
+     * version different from the connected robot, the project might not be executable before it's
+     * converted to the compatible version.
+     */
+    void UploadProject(const std::string& project_dir);
+
+    /**
+     * @brief [Blocking] Download a project from the robot and save to the specified directory.
+     * @param[in] project_name Full name of the project to download. E.g. "polish_table".
+     * @param[in] save_dir Relative or absolute path of the directory to save the downloaded
+     * project. E.g. /home/user/Documents/.
+     * @throw std::invalid_argument if the specified project does not exist.
+     * @throw std::runtime_error if failed to download or save the project.
+     * @throw std::filesystem::filesystem_error if failed to create the specified directory.
+     * @note This function blocks until the project is successfully downloaded.
+     */
+    void DownloadProject(const std::string& project_name, const std::string& save_dir);
+
+    /**
+     * @brief [Blocking] Download all collision mesh files of the connected robot and save to the
+     * specified directory.
+     * @param[in] save_dir Relative or absolute path of the directory to save the downloaded
+     * mesh files. E.g. /home/user/Documents/.
+     * @throw std::runtime_error if failed to download or save the mesh files.
+     * @throw std::filesystem::filesystem_error if failed to create the specified directory.
+     * @note This function blocks until all collision mesh files are successfully downloaded.
+     */
+    void DownloadCollisionMesh(const std::string& save_dir);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_FILE_IO_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/gripper.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/gripper.hpp
new file mode 100644
index 0000000..01e0c3c
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/gripper.hpp
@@ -0,0 +1,182 @@
+/**
+ * @file gripper.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_GRIPPER_HPP_
+#define FLEXIV_RDK_GRIPPER_HPP_
+
+#include "robot.hpp"
+#include <map>
+#include <memory>
+
+namespace flexiv::rdk {
+
+/**
+ * @struct GripperParams
+ * @brief Data structure containing the gripper parameters.
+ * @see Gripper::params().
+ */
+struct GripperParams
+{
+    /** Gripper name */
+    std::string name = {};
+
+    /** Minimum finger opening width [m] */
+    double min_width = {};
+
+    /** Maximum finger opening width [m] */
+    double max_width = {};
+
+    /** Minimum finger moving velocity [m/s] */
+    double min_vel = {};
+
+    /** Maximum finger moving velocity [m/s] */
+    double max_vel = {};
+
+    /** Minimum grasping force [N] */
+    double min_force = {};
+
+    /** Maximum grasping force [N] */
+    double max_force = {};
+};
+
+/**
+ * @struct GripperStates
+ * @brief Data structure containing the gripper states.
+ * @see Gripper::states().
+ */
+struct GripperStates
+{
+    /** Measured finger opening width [m] */
+    double width = {};
+
+    /** Measured finger force. Positive: opening force, negative: closing force.
+     * Reads 0 if the enabled gripper has no force sensing capability [N] */
+    double force = {};
+
+    /** Whether the gripper fingers are moving */
+    bool is_moving = {};
+};
+
+/**
+ * @brief Operator overloading to out stream all members of GripperStates in JSON format.
+ * @param[in] ostream Ostream instance.
+ * @param[in] gripper_states GripperStates data structure to out stream.
+ * @return Updated ostream instance.
+ */
+std::ostream& operator<<(std::ostream& ostream, const GripperStates& gripper_states);
+
+/**
+ * @class Gripper
+ * @brief Interface to control the gripper installed on the robot. Because gripper is also a type of
+ * robot device, this API uses the same underlying infrastructure as rdk::Device, but with functions
+ * tailored specifically for gripper controls.
+ */
+class Gripper
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the gripper control interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Gripper(const Robot& robot);
+    virtual ~Gripper();
+
+    /**
+     * @brief [Blocking] Enable the specified gripper as a device used by the specified joint group.
+     * @param[in] group Joint group that uses this gripper. Only existing single-arm joint groups
+     * like ARM_1 and ARM_2 are accepted.
+     * @param[in] name Name of the gripper to enable.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if the specified gripper does not exist.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot or failed
+     * to sync gripper parameters.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning There's no enforced check on whether the enabled device is a gripper or not. Using
+     * this function to enable a non-gripper device will likely lead to undefined behaviors.
+     */
+    void Enable(JointGroup group, const std::string& name);
+
+    /**
+     * @brief [Blocking] Disable the gripper currently used by the specified joint group.
+     * @param[in] group Joint group whose gripper to be disabled.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Disable(JointGroup group);
+
+    /**
+     * @brief [Blocking] Manually trigger the initialization of the enabled gripper used by the
+     * specified joint group. This step is not needed for grippers that automatically initialize
+     * upon power-on.
+     * @param[in] group Joint group whose gripper is to be initialized.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning This function does not wait for the initialization sequence to finish, the user may
+     * need to implement wait after calling this function before commanding the gripper.
+     */
+    void Init(JointGroup group);
+
+    /**
+     * @brief [Blocking] Command the gripper used by the specified joint group to grasp with direct
+     * force control. This function requires the enabled gripper to support direct force control.
+     * @param[in] group Joint group whose gripper to send this command to.
+     * @param[in] force Target gripping force. Positive: closing force, negative: opening force.
+     * Valid range: [GripperParams::min_force, GripperParams::max_force]. Unit: \f$ [N] \f$.
+     * @throw std::invalid_argument if [force] is outside the valid range.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Grasp(JointGroup group, double force);
+
+    /**
+     * @brief [Blocking] Command the gripper used by the specified joint group to move the fingers
+     * with position control.
+     * @param[in] group Joint group whose gripper to send this command to.
+     * @param[in] width Target opening width. Valid range: [GripperParams::min_width,
+     * GripperParams::max_width]. Unit: \f$ [m] \f$.
+     * @param[in] velocity Closing/opening velocity, cannot be 0. Valid range:
+     * [GripperParams::min_vel, GripperParams::max_vel]. Unit: \f$ [m/s] \f$.
+     * @param[in] force_limit Maximum contact force during movement. Valid range:
+     * [GripperParams::min_force, GripperParams::max_force]. Unit: \f$ [N] \f$.
+     * @throw std::invalid_argument if any input parameter is outside its valid range.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Move(JointGroup group, double width, double velocity, double force_limit);
+
+    /**
+     * @brief [Blocking] Stop and hold the gripper used by the specified joint group.
+     * @param[in] group Joint group whose gripper to send this command to.
+     * @throw std::logic_error if no gripper is enabled for the specified joint group.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Stop(JointGroup group);
+
+    /**
+     * @brief [Non-blocking] Parameters of all enabled grippers.
+     * @return Gripper parameters mapped by joint group.
+     */
+    std::map<JointGroup, GripperParams> params() const;
+
+    /**
+     * @brief [Non-blocking] Current states data of all enabled grippers.
+     * @return Gripper states data mapped by joint group.
+     */
+    std::map<JointGroup, GripperStates> states() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_GRIPPER_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/maintenance.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/maintenance.hpp
new file mode 100644
index 0000000..efcd515
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/maintenance.hpp
@@ -0,0 +1,64 @@
+/**
+ * @file maintenance.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_MAINTENANCE_HPP_
+#define FLEXIV_RDK_MAINTENANCE_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/**
+ * @class Maintenance
+ * @brief Interface to run maintenance operations on the robot.
+ */
+class Maintenance
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the robot maintenance interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Maintenance(const Robot& robot);
+    virtual ~Maintenance();
+
+    /**
+     * @brief [Blocking] Calibrate all joint torque sensors. The robot will first move to a proper
+     * calibration posture, then start the low-level calibration of all joint torque sensors.
+     * Trigger this calibration if the sensed joint torques have noticeable deviations from true
+     * values. See below for more details.
+     * @param[in] group The joint group to run calibration for.
+     * @param[in] cali_posture Joint positions to move to before starting the calibration: \f$
+     * q_cali \in \mathbb{R}^{n \times 1} \f$. If left empty, the robot will use the recommended
+     * upright posture for calibration. Otherwise the specified posture will be used, which is NOT
+     * recommended. Valid range: [RobotInfo::q_min, RobotInfo::q_max]. Unit: \f$ [rad] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, if [cali_posture] contains any value outside the valid range, or if its size
+     * does not match robot DoF.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if fault occurred during the calibration or failed to save the
+     * calibration result.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the calibration is finished.
+     * @warning The robot needs to be rebooted for the calibration result to take effect.
+     * @par How to determine when this calibration is needed?
+     * 1. When the robot is static and there's no payload or external force exerted on it, if
+     * RobotStates::tcp_wrench_local still gives greater than 5N reading, then this calibration
+     * should be triggered once.
+     * 2. When running the "intermediate4_realtime_joint_floating.cpp" example, if the joints drift
+     * swiftly toward one direction, then this calibration should be triggered once.
+     */
+    void CalibrateJointTorqueSensors(
+        JointGroup group, const std::vector<double>& cali_posture = {});
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_MAINTENANCE_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/mode.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/mode.hpp
new file mode 100644
index 0000000..f506ff1
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/mode.hpp
@@ -0,0 +1,126 @@
+/**
+ * @file mode.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_MODE_HPP_
+#define FLEXIV_RDK_MODE_HPP_
+
+#include <array>
+#include <map>
+#include <string>
+
+namespace flexiv::rdk {
+
+/**
+ * @enum Mode
+ * @brief Robot control modes. The robot needs to be switched into the correct control mode before
+ * the corresponding commands can be sent. Refer to Flexiv RDK Manual for more details.
+ * @see Robot::SwitchMode().
+ */
+enum class Mode
+{
+    /** Mode not set */
+    UNKNOWN = 0,
+
+    /**
+     * No operation to execute, the robot holds position and waits for new command.
+     */
+    IDLE,
+
+    /**
+     * Run real-time joint torque control to track continuous commands @1kHz.
+     * @note Real-time (RT) mode.
+     * @see Robot::StreamJointTorque().
+     */
+    RT_JOINT_TORQUE,
+
+    /**
+     * Run real-time joint impedance control to track continuous commands @ 1kHz.
+     * @note Real-time (RT) mode.
+     * @see Robot::StreamJointPosition().
+     */
+    RT_JOINT_IMPEDANCE,
+
+    /**
+     * Run non-real-time joint impedance control to track discrete commands (smoothened by internal
+     * motion generator).
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::SendJointPosition().
+     */
+    NRT_JOINT_IMPEDANCE,
+
+    /**
+     * Run real-time joint position control to track continuous commands @ 1kHz.
+     * @note Real-time (RT) mode.
+     * @see Robot::StreamJointPosition().
+     */
+    RT_JOINT_POSITION,
+
+    /**
+     * Run non-real-time joint position control to track discrete commands (smoothened by internal
+     * motion generator).
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::SendJointPosition().
+     */
+    NRT_JOINT_POSITION,
+
+    /**
+     * Execute pre-configured robot task plans.
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::ExecutePlan().
+     */
+    NRT_PLAN_EXECUTION,
+
+    /**
+     * Execute robot primitives (unit skills).
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::ExecutePrimitive().
+     * @see [Flexiv Primitives](https://www.flexiv.com/primitives/) documentation.
+     */
+    NRT_PRIMITIVE_EXECUTION,
+
+    /**
+     * Run real-time Cartesian motion-force control to track continuous commands @ 1kHz.
+     * @note Real-time (RT) mode.
+     * @see Robot::StreamCartesianMotionForce().
+     */
+    RT_CARTESIAN_MOTION_FORCE,
+
+    /**
+     * Run non-real-time Cartesian motion-force control to track discrete commands, smoothened by
+     * internal motion generator.
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::SendCartesianMotionForce().
+     */
+    NRT_CARTESIAN_MOTION_FORCE,
+
+    /**
+     * Run non-real-time super primitives, i.e. composite tasks that consist of various components
+     * like motion, force, vision, planning, etc.
+     * @note Non-real-time (NRT) mode.
+     * @see Robot::SendCartesianMotionForce().
+     * @see Robot::SendCartesianMotionForceMultiWaypoint().
+     */
+    NRT_SUPER_PRIMITIVE,
+};
+
+/** Map Mode enums to strings */
+inline const std::map<Mode, std::string> kModeNames {
+    {Mode::UNKNOWN, "UNKNOWN"},
+    {Mode::IDLE, "IDLE"},
+    {Mode::RT_JOINT_TORQUE, "RT_JOINT_TORQUE"},
+    {Mode::RT_JOINT_IMPEDANCE, "RT_JOINT_IMPEDANCE"},
+    {Mode::NRT_JOINT_IMPEDANCE, "NRT_JOINT_IMPEDANCE"},
+    {Mode::RT_JOINT_POSITION, "RT_JOINT_POSITION"},
+    {Mode::NRT_JOINT_POSITION, "NRT_JOINT_POSITION"},
+    {Mode::NRT_PLAN_EXECUTION, "NRT_PLAN_EXECUTION"},
+    {Mode::NRT_PRIMITIVE_EXECUTION, "NRT_PRIMITIVE_EXECUTION"},
+    {Mode::RT_CARTESIAN_MOTION_FORCE, "RT_CARTESIAN_MOTION_FORCE"},
+    {Mode::NRT_CARTESIAN_MOTION_FORCE, "NRT_CARTESIAN_MOTION_FORCE"},
+    {Mode::NRT_SUPER_PRIMITIVE, "NRT_SUPER_PRIMITIVE"},
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_MODE_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/model.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/model.hpp
new file mode 100644
index 0000000..ef394d8
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/model.hpp
@@ -0,0 +1,254 @@
+/**
+ * @file model.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_MODEL_HPP_
+#define FLEXIV_RDK_MODEL_HPP_
+
+#include "robot.hpp"
+#include <Eigen/Eigen>
+#include <memory>
+
+namespace flexiv::rdk {
+
+/**
+ * @struct IKParams
+ * @brief Per-joint-group IK input parameters for SolveConstrainedIK().
+ */
+struct IKParams
+{
+    /** Cartesian pose to be solved. */
+    std::array<double, kPoseSize> cartesian_pose = {};
+
+    /** Seed joint positions for IK solver. Unit: [rad]. */
+    std::vector<double> seed_q = {};
+
+    /** Whether to only constrain Cartesian position and allow orientation to vary. */
+    bool free_orientation = false;
+};
+
+/**
+ * @struct IKResult
+ * @brief Result from SolveConstrainedIK().
+ */
+struct IKResult
+{
+    /** Whether the IK solution is successful. */
+    bool success = false;
+
+    /** Solved joint positions by active joint group. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> solved_q = {};
+};
+
+/**
+ * @class Model
+ * @brief Interface to obtain certain model data of the robot, including kinematics and dynamics.
+ */
+class Model
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the robot model interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @param[in] gravity_vector Earth's gravity vector in world frame. Default to \f$ [0.0, 0.0,
+     * -9.81]^T \f$. Unit: \f$ [m/s^2] \f$.
+     * @throw std::runtime_error if the initialization sequence failed.
+     * @throw std::logic_error if the connected robot does not have an RDK professional license; or
+     * the parsed robot model is not supported.
+     */
+    Model(const Robot& robot,
+        const Eigen::Vector3d& gravity_vector = Eigen::Vector3d(0.0, 0.0, -9.81));
+    virtual ~Model();
+
+    /**
+     * @brief [Non-blocking] Names of all links in the robot model.
+     * @return Name vector in the same order as the robot's kinematic chain.
+     */
+    std::vector<std::string> link_names() const;
+
+    /**
+     * @brief [Non-blocking] Names of all actuated joints in the robot model.
+     * @return Name vector in the same order as the robot's kinematic chain.
+     */
+    std::vector<std::string> joint_names() const;
+
+    /**
+     * @brief [Blocking] Reload (refresh) parameters of the robot model stored locally in this class
+     * using the latest data synced from the connected robot. Tool model is also synced.
+     * @throw std::runtime_error if failed to sync model data.
+     * @throw std::logic_error if the synced robot model contains invalid data.
+     * @note This function blocks until the model parameters are synced and reloaded.
+     * @note This function does not affect the kinematics functions.
+     * @warning Parameters of the locally-stored robot model must be manually refreshed using this
+     * function whenever a physical change is made to the connected robot (e.g. a tool is added or
+     * changed). Otherwise the locally computed functions will return incorrect results.
+     */
+    void Reload();
+
+    /**
+     * @brief [Non-blocking] Update the configuration (posture) of the locally-stored robot model so
+     * that the locally computed functions return results based on the updated configuration.
+     * @param[in] full_q Current joint positions of the whole robot and matching the order of
+     * joint_names(): \f$ q \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$.
+     * @param[in] full_dq Current joint velocities of the whole robot and matching the order of
+     * joint_names(): \f$ \dot{q} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$.
+     * @throw std::invalid_argument if size of any input vector does not match robot full DoF.
+     */
+    void Update(const std::vector<double>& full_q, const std::vector<double>& full_dq);
+
+    //========================================== DYNAMICS ==========================================
+    /**
+     * @brief [Non-blocking] Compute the time derivative of Jacobian matrix at the specified frame
+     * w.r.t. world frame.
+     * @param[in] link_name Name of the link whose frame is the specified one.
+     * @return Time derivative of the Jacobian matrix: \f$ ^{O}\dot{J_i} \in \mathbb{R}^{m \times n}
+     * \f$.
+     * @throw std::invalid_argument if [link_name] does not exist.
+     * @note Call Update() before this function.
+     * @see link_names().
+     */
+    Eigen::MatrixXd dJ(const std::string& link_name);
+
+    /**
+     * @brief [Non-blocking] Compute the mass matrix in generalized coordinates, i.e. joint space.
+     * @return Symmetric positive definite mass matrix: \f$ M(q) \in \mathbb{S}^{n \times n}_{++}
+     * \f$. Unit: \f$ [kgm^2] \f$.
+     * @note Call Update() before this function.
+     */
+    Eigen::MatrixXd M();
+
+    /**
+     * @brief [Non-blocking] Compute the Coriolis/centripetal matrix in generalized coordinates,
+     * i.e. joint space.
+     * @return Coriolis/centripetal matrix: \f$ C(q,\dot{q}) \in \mathbb{R}^{n \times n} \f$.
+     * @note Call Update() before this function.
+     */
+    Eigen::MatrixXd C();
+
+    /**
+     * @brief [Non-blocking] Compute the gravity force vector in generalized coordinates, i.e.
+     * joint space.
+     * @return Gravity force vector: \f$ g(q) \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [Nm] \f$.
+     * @note Call Update() before this function.
+     */
+    Eigen::VectorXd g();
+
+    /**
+     * @brief [Non-blocking] Compute the Coriolis force vector in generalized coordinates, i.e.
+     * joint space.
+     * @return Coriolis force vector: \f$ c(q,\dot{q}) \in \mathbb{R}^{n \times 1} \f$. Unit: \f$
+     * [Nm] \f$.
+     * @note Call Update() before this function.
+     */
+    Eigen::VectorXd c();
+
+    //========================================= KINEMATICS =========================================
+    /**
+     * @brief [Non-blocking] Compute the Jacobian matrix at the specified frame w.r.t. world frame.
+     * @param[in] link_name Name of the link whose frame is the specified one.
+     * @return Jacobian matrix: \f$ ^{O}J_i \in \mathbb{R}^{m \times n} \f$.
+     * @throw std::invalid_argument if [link_name] does not exist.
+     * @note Call Update() before this function.
+     * @see link_names().
+     */
+    Eigen::MatrixXd J(const std::string& link_name);
+
+    /**
+     * @brief [Non-blocking] Compute the transformation matrix of the specified frame w.r.t. world
+     * frame.
+     * @param[in] link_name Name of the link whose frame is the specified one.
+     * @return Transformation matrix: \f$ ^{O}T_i \in \mathbb{R}^{4 \times 4} \f$.
+     * @throw std::invalid_argument if [link_name] does not exist.
+     * @note Call Update() before this function.
+     * @see link_names().
+     */
+    Eigen::Isometry3d T(const std::string& link_name);
+
+    /**
+     * @brief [Blocking] Sync the actual kinematic parameters of the connected robot into the
+     * template URDF file.
+     * @param[in] template_urdf_path Path to the template URDF file that can be generated in
+     * [flexiv_description](https://github.com/flexivrobotics/flexiv_description). This template
+     * URDF file will be updated when the sync is finished.
+     * @throw std::invalid_argument if failed to load the template URDF file.
+     * @throw std::runtime_error if failed to sync the URDF file.
+     * @note This function blocks until the sync is finished.
+     * @par Why is this function needed?
+     * The template URDF contains kinematic parameters of the latest version of robot hardware,
+     * which might be different from older versions. This function is therefore provided to sync the
+     * actual kinematic parameters of the connected robot into the template URDF.
+     */
+    void SyncURDF(const std::string& template_urdf_path);
+
+    /**
+     * @brief [Blocking] Sync the actual kinematic parameters of the connected robot into the
+     * template YAML file.
+     * @param[in] template_yaml_path Path to the template YAML file located at
+     * [flexiv_description/config/.../default_kinematics.yaml]. This template YAML file will be
+     * updated when the sync is finished.
+     * @return Total number of joints that have been successfully synced.
+     * @throw std::invalid_argument if failed to load the template YAML file.
+     * @throw std::runtime_error if failed to sync the YAML file.
+     * @note This function blocks until the sync is finished.
+     */
+    size_t SyncKinematicsYAML(const std::string& template_yaml_path);
+
+    /**
+     * @brief [Blocking] Solve constrained IK using one or more active joint groups.
+     * @param[in] ik_params_by_group IK input parameters mapped by active joint group. Joint groups
+     * not included in this map are treated as inactive. Only single-arm joint groups like ARM_1 and
+     * ARM_2 are accepted.
+     * @return Solver result. [solved_q] contains one entry per requested active joint group.
+     * @throw std::invalid_argument if input map is empty, if any joint group is not an existing
+     * single-arm joint group, or if any [seed_q] size does not match DoF of its joint group.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    IKResult SolveConstrainedIK(const std::map<JointGroup, IKParams>& ik_params_by_group);
+
+    /**
+     * @brief [Blocking] Score of each joint group's current configuration (posture), calculated
+     * from the manipulability measurements.
+     * @return Configuration score mapped by joint group as {translation_score,
+     * orientation_score}. The quality of configuration based on the score can be interpreted as:
+     * poor = [0, 20), medium = [20, 40), good = [40, 100].
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     * @warning A poor configuration score means the robot is near or at singularity, which can lead
+     * to degraded Cartesian performance. Use configuration with high scores for better
+     * manipulability and task results.
+     */
+    std::map<JointGroup, std::pair<double, double>> configuration_score() const;
+
+    //======================================= MULTI-CONTACT ========================================
+    /**
+     * @brief [Non-blocking] Estimated multi-contact forces applied on each link of applicable joint
+     * groups, calculated using the force-torque sensors installed in every joint of the robot.
+     * @return \f$ f_c \in \mathbb{R}^{n \times 1} \f$ mapped by joint group. Each vector element is
+     * a \f$ \mathbb{R}^{3 \times 1} \f$ force vector w.r.t. the corresponding link frame. Only
+     * contains joint groups that are capable of multi-contact estimation.
+     * @warning This data is only available on certain robot models. An empty vector will be
+     * returned if the connected robot does not support multi-contact estimation.
+     */
+    std::map<JointGroup, std::vector<Eigen::Vector3d>> multi_contact_forces() const;
+
+    /**
+     * @brief [Non-blocking] Estimated multi-contact positions on each link of applicable joint
+     * groups, calculated using the force-torque sensors installed in every joint of the robot.
+     * @return \f$ p_c \in \mathbb{R}^{n \times 1} \f$ mapped by joint group. Each vector element is
+     * a \f$ \mathbb{R}^{3 \times 1} \f$ position vector w.r.t. the corresponding link frame. Only
+     * contains joint groups that are capable of multi-contact estimation.
+     * @warning This data is only available on certain robot models. An empty vector will be
+     * returned if the connected robot does not support multi-contact estimation.
+     */
+    std::map<JointGroup, std::vector<Eigen::Vector3d>> multi_contact_positions() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_MODEL_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/robot.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/robot.hpp
new file mode 100644
index 0000000..5bf00c1
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/robot.hpp
@@ -0,0 +1,896 @@
+/**
+ * @file robot.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_ROBOT_HPP_
+#define FLEXIV_RDK_ROBOT_HPP_
+
+#include "data.hpp"
+#include "mode.hpp"
+#include <vector>
+#include <memory>
+#include <exception>
+#include <map>
+
+namespace flexiv::rdk {
+
+/**
+ * @class Robot
+ * @brief Main interface to control the robot, containing several function categories and background
+ * services.
+ */
+class Robot
+{
+public:
+    /**
+     * @brief [Blocking] Instantiate the robot control interface. RDK services will be started and
+     * establish connection with the target robot.
+     * @param[in] robot_sn Serial number of the robot to connect. The accepted formats are:
+     * "Enlight-L-123456" and "Enlight L-123456". See ProductModel enum for all supported models.
+     * @param[in] verbose Enable/disable info and warning prints.
+     * @param[in] lite Whether to create a lite instance. See details below. A normal instance must
+     * have already connected to the robot for lite instances to function properly.
+     * @throw std::invalid_argument if the format of [robot_sn] is invalid.
+     * @throw std::runtime_error if the initialization sequence failed.
+     * @throw std::logic_error if the connected robot does not have a valid RDK license, or is
+     * incompatible with this RDK library version, or is an unsupported product model.
+     * @warning This constructor blocks until the initialization sequence is successfully finished
+     * and connection with the robot is established.
+     * @par Lite Instance
+     * Creating multiple instances of rdk::Robot that connect to the same robot is allowed. However,
+     * every additional instance brings extra overhead that can restrict the total number of
+     * instances due to limited computation power and network bandwidth. To mitigate the performance
+     * impact, it is recommended to create a lite instance of rdk::Robot if it only needs to send
+     * one-shot requests, which are functions marked with the [Blocking] tag. A lite instance does
+     * not receive robot states or system status, nor send motion commands.
+     */
+    Robot(const std::string& robot_sn, bool verbose = true, bool lite = false);
+    virtual ~Robot();
+
+    //========================================= ACCESSORS ==========================================
+    /**
+     * @brief [Non-blocking] Whether this rdk::Robot instance is a lite one.
+     * @return True: is a lite instance; false: not a lite instance.
+     */
+    bool lite() const;
+
+    /**
+     * @brief [Non-blocking] Whether the connection with the robot is established.
+     * @return True: connected; false: disconnected.
+     */
+    bool connected() const;
+
+    /**
+     * @brief [Non-blocking] General information about the robot.
+     * @return Value copy of RobotInfo struct.
+     */
+    RobotInfo info() const;
+
+    /**
+     * @brief [Non-blocking] Current control mode of the robot.
+     * @return Mode enum.
+     */
+    Mode mode() const;
+
+    /**
+     * @brief [Non-blocking] Current states data of all existing joint groups of the robot.
+     * @return Robot states data mapped by joint group.
+     * @warning Cartesian states of non-single-arm joint groups are not populated.
+     */
+    std::map<JointGroup, RobotStates> states() const;
+
+    /**
+     * @brief [Non-blocking] Current actions data of all existing joint groups of the robot.
+     * @return Robot actions data mapped by joint group.
+     * @warning Cartesian actions of non-single-arm joint groups are not populated.
+     */
+    std::map<JointGroup, RobotActions> actions() const;
+
+    /**
+     * @brief [Non-blocking] Whether specific joint groups of the robot have come to a complete
+     * stop.
+     * @return Stopped flag mapped by joint group. True: this joint group has stopped; false: this
+     * joint group is still moving.
+     */
+    std::map<JointGroup, bool> stopped() const;
+
+    /**
+     * @brief [Non-blocking] Whether all joint groups of the robot have come to a complete stop.
+     * @return True: all stopped; false: not all stopped.
+     */
+    bool all_stopped() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is ready to be operated, which requires the
+     * following conditions to be met: enabled, brakes fully released, in auto mode, no fault, and
+     * not in reduced state.
+     * @return True: operational (operational_status() == READY); false: not operational.
+     * @warning The robot won't execute any user command until it's ready to be operated.
+     */
+    bool operational() const;
+
+    /**
+     * @brief [Non-blocking] Current operational status of the robot.
+     * @return OperationalStatus enum.
+     */
+    OperationalStatus operational_status() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is busy. This includes any user commanded operations
+     * that requires the robot to execute. For example, plans, primitives, Cartesian and joint
+     * motions, etc.
+     * @return True: busy; false: idle.
+     * @warning Some exceptions exist for primitives, see ExecutePrimitive() for more details.
+     */
+    bool busy() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is in fault state.
+     * @return True: robot has fault; false: robot normal.
+     */
+    bool fault() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is in reduced state.
+     * @return True: in reduced state; false: not in reduced state.
+     * @par Reduced state
+     * The robot will enter reduced state if a) the safety input for reduced state goes low or b)
+     * robot TCP passes through any safety plane. The safety limits are lowered in reduced state
+     * compared to normal state. Specific values for the safety limits can be configured in Flexiv
+     * Elements under Settings -> Safety Configuration. Please refer to the robot user manual for
+     * more details about system reduced state.
+     */
+    bool reduced() const;
+
+    /**
+     * @brief [Non-blocking] Whether the robot is in recovery state.
+     * @return True: in recovery state; false: not in recovery state.
+     * @note Please use Flexiv Elements to get the robot out of recovery state.
+     * @par Recovery state
+     * The robot will enter recovery state if it needs to recover from joint position limit
+     * violation (a critical system fault that requires a recovery operation, during which the
+     * joints that moved outside the allowed position range will need to move very slowly back into
+     * the allowed range). Please refer to the robot user manual for more details about system
+     * recovery state.
+     */
+    bool recovery() const;
+
+    /**
+     * @brief [Non-blocking] Whether all connected emergency stops (E-stops) are released.
+     * @return True: released; false: pressed.
+     */
+    bool estop_released() const;
+
+    /**
+     * @brief [Non-blocking] Whether the 3-position enabling device (e.g. enabling button on the
+     * motion bar) is in the ON position (the middle position).
+     * @return True: ON; false: OFF.
+     */
+    bool enabling_device_on() const;
+
+    /**
+     * @brief [Non-blocking] Whether the timeliness failure limit has been reached within a
+     * 1-second moving window.
+     * @return True: reached limit; false: not reached limit.
+     * @see SetTimelinessFailureLimit().
+     */
+    bool reached_timeliness_failure_limit() const;
+
+    /**
+     * @brief [Non-blocking] Robot events stored since the last successful instantiation of this
+     * class.
+     * @return Event log as a vector of RobotEvent, with the first element being the
+     * oldest event and the last element being the latest event.
+     * @warning Events before the last successful instantiation of this class are not stored.
+     */
+    std::vector<RobotEvent> event_log() const;
+
+    //======================================= SYSTEM CONTROL =======================================
+    /**
+     * @brief [Blocking] Servo on the robot. If E-stop is released and there's no fault, the robot
+     * will release brakes, and becomes operational a few seconds later.
+     * @throw std::logic_error if the robot is not connected or E-stop is not released.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void ServoOn();
+
+    /**
+     * @brief [Blocking] Move the specified joint groups to the home posture using Home primitive.
+     * @param[in] groups Joint group(s) to home. Only existing single-arm joint groups like ARM_1
+     * and ARM_2 are accepted. If left empty, home all applicable joint groups.
+     * @throw std::invalid_argument if [groups] contains joint groups that are not existing
+     * single-arm joint groups in the connected robot.
+     * @throw std::runtime_error if failed to execute the Home primitive.
+     * @note The robot's control mode will be temporarily switched to NRT_PRIMITIVE_EXECUTION and
+     * restored to the previous mode before returning.
+     * @note This function blocks until all specified joint groups have reached the home posture.
+     */
+    void Home(const std::vector<JointGroup>& groups = {});
+
+    /**
+     * @brief [Blocking] Force robot brakes to engage or release during normal operation.
+     * Restrictions apply, see warning.
+     * @param[in] engage True: engage brakes; false: release brakes.
+     * @throw std::logic_error if the connected robot is not a medical model or is moving.
+     * @throw std::runtime_error if failed to engage/release the brakes.
+     * @note This function blocks until the brakes are successfully engaged/released.
+     * @warning This function is accessible only if a) the connected robot is a medical model AND
+     * b) the robot is not moving.
+     */
+    void Brake(bool engage);
+
+    /**
+     * @brief [Blocking] Switch the robot to a new control mode and wait for the mode transition
+     * to finish.
+     * @param[in] mode Control mode to switch to.
+     * @throw std::invalid_argument if the requested mode is invalid or unlicensed.
+     * @throw std::logic_error if robot is in an unknown control mode or is not operational.
+     * @throw std::runtime_error if failed to transit the robot into the specified control mode
+     * after several attempts.
+     * @note This function blocks until the robot has successfully transited into the specified
+     * control mode.
+     * @warning If the robot is still moving when this function is called, it will automatically
+     * stop before making the mode transition.
+     */
+    void SwitchMode(Mode mode);
+
+    /**
+     * @brief [Blocking] Stop the robot and transit its control mode to IDLE.
+     * @throw std::runtime_error if failed to stop the robot.
+     * @note This function blocks until the robot comes to a complete stop.
+     */
+    void Stop();
+
+    /**
+     * @brief [Blocking] Try to clear minor or critical fault for the robot without a power cycle.
+     * @param[in] timeout_sec Maximum time in seconds to wait for the fault to be successfully
+     * cleared. Normally, a minor fault should take no more than 3 seconds to clear, and a critical
+     * fault should take no more than 30 seconds to clear.
+     * @return True: successfully cleared fault; false: failed to clear fault.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the fault is successfully cleared or [timeout_sec] has
+     * elapsed.
+     * @warning Clearing a critical fault through this function without a power cycle requires a
+     * dedicated device, which may not be installed in older robot models.
+     */
+    bool ClearFault(unsigned int timeout_sec = 30);
+
+    /**
+     * @brief [Blocking] Set values to global variables that already exist in the robot.
+     * @param[in] global_vars A map of {global_var_name, global_var_value(s)}. Use int 1 and 0 to
+     * represent booleans. For example, {{"camera_offset", {0.1, -0.2, 0.3}}, {"start_plan", 1}}.
+     * @throw std::length_error if [global_vars] is empty.
+     * @throw std::invalid_argument if any of the specified global variables does not exist.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the global variables are successfully set.
+     * @warning The specified global variables need to be created first using Flexiv Elements.
+     * @see global_variables().
+     */
+    void SetGlobalVariables(const std::map<std::string, FlexivDataTypes>& global_vars);
+
+    /**
+     * @brief [Blocking] Existing global variables and their current values.
+     * @return A map of {global_var_name, global_var_value(s)}. Booleans are represented by int 1
+     * and 0. For example, {{"camera_offset", {0.1, -0.2, 0.3}}, {"start_plan", 1}}.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     * @see SetGlobalVariables().
+     */
+    std::map<std::string, FlexivDataTypes> global_variables() const;
+
+    /**
+     * @brief [Non-blocking] Set the maximum tolerable percentage of real-time commands that arrived
+     * the robot too late within a 1-second moving window. This only applies to the real-time
+     * control modes and functions marked as "Real-time (RT)" will throw an exception when this
+     * limit is reached.
+     * @param[in] percentage Percentage limit, default to 2%. Valid range: [0, 100].
+     * @throw std::invalid_argument if [percentage] is outside the valid range.
+     * @see reached_timeliness_failure_limit().
+     */
+    void SetTimelinessFailureLimit(double percentage = 2.0);
+
+    //======================================= PLAN EXECUTION =======================================
+    /**
+     * @brief [Blocking] Execute a plan by specifying its index.
+     * @param[in] index Index of the plan to execute, can be obtained via plan_list().
+     * @param[in] continue_exec Whether to continue executing the plan when the RDK program is
+     * closed or the connection is lost.
+     * @param[in] block_until_started Whether to wait for the commanded plan to finish loading
+     * and start execution before the function returns. Depending on the amount of computation
+     * needed to get the plan ready, the loading process typically takes no more than 200 ms.
+     * @throw std::invalid_argument if [index] is outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot or the
+     * robot is not operational.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered if
+     * [block_until_started] is disabled, or until the plan has started execution if
+     * [block_until_started] is enabled.
+     * @note busy() can be used to check if the plan has finished.
+     */
+    void ExecutePlan(
+        unsigned int index, bool continue_exec = false, bool block_until_started = true);
+
+    /**
+     * @brief [Blocking] Execute a plan by specifying its name.
+     * @param[in] name Name of the plan to execute, can be obtained via plan_list().
+     * @param[in] continue_exec Whether to continue executing the plan when the RDK program is
+     * closed or the connection is lost.
+     * @param[in] block_until_started Whether to wait for the commanded plan to finish loading
+     * and start execution before the function returns. Depending on the amount of computation
+     * needed to get the plan ready, the loading process typically takes no more than 200 ms.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot or the
+     * robot is not operational.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered if
+     * [block_until_started] is disabled, or until the plan has started execution if
+     * [block_until_started] is enabled.
+     * @note busy() can be used to check if the plan has finished.
+     */
+    void ExecutePlan(
+        const std::string& name, bool continue_exec = false, bool block_until_started = true);
+
+    /**
+     * @brief [Blocking] Pause or resume the execution of the current plan.
+     * @param[in] toggle True: pause plan; false: resume plan.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning Internal plans (not created by user) cannot be resumed due to safety concerns.
+     */
+    void PausePlan(bool toggle);
+
+    /**
+     * @brief [Blocking] Stop the execution of the current plan.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void StopPlan();
+
+    /**
+     * @brief [Blocking] A list of all available plans.
+     * @return Available plans as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> plan_list() const;
+
+    /**
+     * @brief [Blocking] Detailed information about the executing plan. Contains plan name,
+     * primitive name, node name, node path, node path time period, etc.
+     * @return PlanInfo value copy.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until a reply is received.
+     */
+    PlanInfo plan_info() const;
+
+    /**
+     * @brief [Blocking] Enable or disable the breakpoint mode during plan execution. When enabled,
+     * the executing plan will pause at the pre-defined breakpoints. Use StepBreakpoint() to
+     * continue the execution and pause at the next breakpoint.
+     * @param[in] is_enabled True: enable; false: disable. By default, breakpoint mode is disabled.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void SetBreakpointMode(bool is_enabled);
+
+    /**
+     * @brief [Blocking] If breakpoint mode is enabled, step to the next breakpoint. The plan
+     * execution will continue and pause at the next breakpoint.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     * @note Use PlanInfo::waiting_for_step to check if the plan is waiting for user signal to step
+     * the breakpoint.
+     */
+    void StepBreakpoint();
+
+    /**
+     * @brief [Blocking] Set overall velocity scale of robot motions during plan and primitive
+     * execution.
+     * @param[in] velocity_scale Percentage scale to adjust the overall velocity of robot motions.
+     * Valid range: [0, 100]. Setting to 100 means to move with 100% of specified motion velocity,
+     * and 0 means not moving at all.
+     * @throw std::invalid_argument if [velocity_scale] is outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: NRT_PLAN_EXECUTION, NRT_PRIMITIVE_EXECUTION.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void SetVelocityScale(unsigned int velocity_scale);
+
+    //==================================== PRIMITIVE EXECUTION =====================================
+    /**
+     * @brief [Blocking] Execute primitive(s) on specified joint group(s).
+     * @param[in] primitive_args Primitive arguments mapped by joint group. Only existing
+     * single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] block_until_started Whether to wait for the commanded primitive to finish loading
+     * and start execution before the function returns. Depending on the amount of computation
+     * needed to get the primitive ready, the loading process typically takes no more than 200 ms.
+     * @throw std::invalid_argument if [primitive_args] contains joint groups that are not
+     * existing single-arm joint groups in the connected robot.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot or the
+     * robot is not operational.
+     * @note Applicable control modes: NRT_PRIMITIVE_EXECUTION.
+     * @note This function blocks until the request is successfully delivered if
+     * [block_until_started] is disabled, or until the primitive has started execution if
+     * [block_until_started] is enabled. After this function returns, the primitive will still be
+     * running, thus the users need to implement their own blocking after calling this function. See
+     * examples for reference.
+     * @warning The primitive input parameters may not use SI units, please refer to the Flexiv
+     * Primitives documentation for exact unit definition.
+     * @warning Most primitives won't exit by themselves and require users to explicitly trigger
+     * transitions based on specific primitive states. In such case, busy() will stay true even if
+     * it seems everything is done for that primitive.
+     */
+    void ExecutePrimitive(
+        const std::map<JointGroup, PrimitiveArgs>& primitive_args, bool block_until_started = true);
+
+    /**
+     * @brief [Blocking] States data of the primitive(s) that are currently running.
+     * @return Primitive states data mapped by joint group. Only joint groups that are currently
+     * running primitives will be included.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::map<JointGroup, PrimitiveStates> primitive_states() const;
+
+    //==================================== DIRECT JOINT CONTROL ====================================
+    /**
+     * @brief [Non-blocking] Continuously stream joint torque commands of the specified joint
+     * group(s) to the robot. The commands are tracked by a high-performance joint torque
+     * controller.
+     * @param[in] cmds Real-time joint torque commands mapped by joint group. Only existing
+     * single-arm joint groups and external axis joint groups are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm or external axis joint group in the connected robot, if size of any input vector
+     * does not match robot DoF, or [friction_comp_scale] is outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: RT_JOINT_TORQUE.
+     * @note Real-time (RT).
+     * @warning Always stream smooth and continuous commands to avoid sudden movements.
+     */
+    void StreamJointTorque(const std::map<JointGroup, RtJointTorqueCmd>& cmds);
+
+    /**
+     * @brief [Non-blocking] Continuously stream joint position, velocity, and acceleration commands
+     * of the specified joint group(s) to the robot. The commands are tracked by either the joint
+     * impedance controller or the joint position controller, depending on the control mode.
+     * @param[in] cmds Real-time joint position-velocity-acceleration commands mapped by joint
+     * group. Only existing single-arm joint groups and external axis joint groups are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm or external axis joint group in the connected robot, or if size of any input
+     * vector does not match robot DoF.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: RT_JOINT_IMPEDANCE, RT_JOINT_POSITION.
+     * @note Real-time (RT).
+     * @warning Always stream smooth and continuous commands to avoid sudden movements.
+     * @see SetJointImpedance().
+     */
+    void StreamJointPosition(const std::map<JointGroup, RtJointPositionCmd>& cmds);
+
+    /**
+     * @brief [Non-blocking] Discretely send joint position and velocity commands of the specified
+     * joint group(s) to the robot. An internal motion generator will smoothen the discrete
+     * commands, which are then tracked by either the joint impedance controller or the joint
+     * position controller, depending on the control mode.
+     * @param[in] cmds Non-real-time joint position-velocity-acceleration commands mapped by joint
+     * group. Only existing single-arm joint groups and external axis joint groups are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm or external axis joint group in the connected robot, if size of any input vector
+     * does not match robot DoF, or [dq_max] or [ddq_max] contains any non-positive value.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: NRT_JOINT_IMPEDANCE, NRT_JOINT_POSITION.
+     * @warning Calling this function a second time while the motion from the previous call is still
+     * ongoing will trigger an online re-planning of the joint trajectory, such that the previous
+     * command is aborted and the new command starts to execute.
+     * @see SetJointImpedance().
+     */
+    void SendJointPosition(const std::map<JointGroup, NrtJointPositionCmd>& cmds);
+
+    /**
+     * @brief [Blocking] Set impedance properties of the robot's joint motion controller used in
+     * the joint impedance control modes.
+     * @param[in] group The joint group to set impedance properties for. Only existing single-arm
+     * joint groups and external axis joint groups are accepted.
+     * @param[in] K_q Joint motion stiffness: \f$ K_q \in \mathbb{R}^{n \times 1} \f$.
+     * Setting motion stiffness of a joint axis to 0 will make this axis free-floating. Valid range:
+     * [0, RobotInfo::K_q_nom]. Unit: \f$ [Nm/rad] \f$.
+     * @param[in] Z_q Joint motion damping ratio: \f$ Z_q \in \mathbb{R}^{n \times 1} \f$.
+     * Valid range: [0.3, 0.8]. The default value 0.7 will be used if left empty.
+     * @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
+     * group in the connected robot, if [K_q] or [Z_q] contains any value outside the valid range,
+     * or if size of any input vector does not match robot DoF.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning Changing damping ratio [Z_q] to a non-nominal value may lead to performance and
+     * stability issues, please use with caution.
+     */
+    void SetJointImpedance(
+        JointGroup group, const std::vector<double>& K_q, const std::vector<double>& Z_q = {});
+
+    /**
+     * @brief [Blocking] Set maximum contact torques for the robot's joint motion controller used in
+     * the joint impedance control modes. The controller will regulate its output to maintain
+     * contact torques with the environment under the set values.
+     * @param[in] group The joint group to set maximum contact torques for. Only existing single-arm
+     * joint groups and external axis joint groups are accepted.
+     * @param[in] max_torques Maximum contact torques: \f$ tau_q \in \mathbb{R}^{n \times 1} \f$.
+     * Valid range: [0, RobotInfo::tau_max]. Unit: \f$ [Nm] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
+     * group in the connected robot, if [max_torques] contains any value outside the valid range, or
+     * if its size does not match robot DoF.
+     * @throw std::logic_error if the robot is not in an applicable control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void SetMaxContactTorque(JointGroup group, const std::vector<double>& max_torques);
+
+    /**
+     * @brief [Blocking] Set inertia shaping scales for the robot's joint motion controller used in
+     * the joint impedance control modes.
+     * @param[in] group The joint group to set inertia shaping scales for. Only existing single-arm
+     * joint groups and external axis joint groups are accepted.
+     * @param[in] inertia_scales Inertia shaping scales: \f$ \sigma_q \in \mathbb{R}^{n \times 1}
+     * \f$. Valid range: [0.75, 1.0]. The nominal (safe) value is 1.0, which means no shaping.
+     * @throw std::invalid_argument if [group] is not an existing single-arm or external axis joint
+     * group in the connected robot, if [inertia_scales] contains any value outside the valid range,
+     * or if its size does not match robot DoF.
+     * @throw std::logic_error if the robot is not in an applicable control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_JOINT_IMPEDANCE, NRT_JOINT_IMPEDANCE.
+     * @note This function blocks until the request is successfully delivered.
+     * @par Joint inertia shaping
+     * In joint impedance control modes, it is possible to shape down the natural inertia of the
+     * joints to make them behave as if they are lighter. The parameter [inertia_scales] sets the
+     * scale of shaped/natural inertia for each joint. Smaller scale corresponds to lighter inertia.
+     */
+    void SetJointInertiaScale(JointGroup group, const std::vector<double>& inertia_scales);
+
+    //================================== DIRECT CARTESIAN CONTROL ==================================
+    /**
+     * @brief [Non-blocking] Continuously stream Cartesian motion and/or force commands of the
+     * specified joint group(s) to the robot. The commands are tracked by a unified motion-force
+     * controller that allows force control in zero or more Cartesian axes and motion control in
+     * the other axes.
+     * @param[in] cmds Real-time Cartesian motion/force commands mapped by joint group. Only
+     * existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm joint group in the connected robot.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE.
+     * @note Real-time (RT).
+     * @warning Always stream smooth and continuous motion commands to avoid sudden movements. The
+     * force commands don't need to be continuous.
+     * @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
+     * instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
+     * towards -Z direction, so that upon contact, the sensed force will be +5 N.
+     * @par How to achieve pure motion control?
+     * Use SetForceControlAxis() to disable force control for all Cartesian axes to achieve pure
+     * motion control. This function does pure motion control by default.
+     * @par How to achieve pure force control?
+     * Use SetForceControlAxis() to enable force control for all Cartesian axes to achieve pure
+     * force control, active or passive.
+     * @par How to achieve unified motion-force control?
+     * Use SetForceControlAxis() to enable force control for one or more Cartesian axes and leave
+     * the rest axes motion-controlled, then provide target pose for the motion-controlled axes and
+     * target wrench for the force-controlled axes.
+     * @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
+     * SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
+     */
+    void StreamCartesianMotionForce(const std::map<JointGroup, RtCartesianCmd>& cmds);
+
+    /**
+     * @brief [Non-blocking] Discretely send Cartesian motion and/or force commands of the specified
+     * joint group(s) to the robot. An internal motion generator will smoothen the discrete
+     * commands, which are then tracked by a unified motion-force controller that allows force
+     * control in zero or more Cartesian axes and motion control in the other axes. The robot's
+     * internal motion generator will smoothen the discrete commands.
+     * @param[in] cmds Non-real-time Cartesian motion/force commands mapped by joint group. Only
+     * existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm joint group in the connected robot, or if any of the last 4 input parameters is
+     * not positive.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: NRT_CARTESIAN_MOTION_FORCE, NRT_SUPER_PRIMITIVE.
+     * @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
+     * instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
+     * towards -Z direction, so that upon contact, the sensed force will be +5 N.
+     * @par How to achieve pure motion control?
+     * Use SetForceControlAxis() to disable force control for all Cartesian axes to achieve pure
+     * motion control. This function does pure motion control by default.
+     * @par How to achieve pure force control?
+     * Use SetForceControlAxis() to enable force control for all Cartesian axes to achieve pure
+     * force control, active or passive.
+     * @par How to achieve unified motion-force control?
+     * Use SetForceControlAxis() to enable force control for one or more Cartesian axes and leave
+     * the rest axes motion-controlled, then provide target pose for the motion-controlled axes and
+     * target wrench for the force-controlled axes.
+     * @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
+     * SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
+     */
+    void SendCartesianMotionForce(const std::map<JointGroup, NrtCartesianCmd>& cmds);
+
+    /**
+     * @brief [Non-blocking] Discretely send Cartesian multi-waypoint motion and/or force commands
+     * for the robot to track using non-real-time super primitives.
+     * @param[in] cmds Non-real-time Cartesian multi-waypoint commands mapped by joint group. Only
+     * existing single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @throw std::invalid_argument if [cmds] contains a joint group that is not an existing
+     * single-arm joint group in the connected robot, or if any waypoint list is empty or any
+     * waypoint's last 4 input parameters is not positive.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if the robot is not operational.
+     * @note Applicable control modes: NRT_SUPER_PRIMITIVE.
+     * @warning Same as Flexiv Elements, the target wrench is expressed as wrench sensed at TCP
+     * instead of wrench exerted by TCP. E.g. commanding f_z = +5 N will make the end-effector move
+     * towards -Z direction, so that upon contact, the sensed force will be +5 N.
+     * @see SetCartesianImpedance(), SetMaxContactWrench(), SetNullSpacePosture(),
+     * SetForceControlAxis(), SetForceControlFrame(), SetPassiveForceControl().
+     */
+    void SendCartesianMotionForceMultiWaypoint(
+        const std::map<JointGroup, NrtCartesianMultiWaypointCmd>& cmds);
+
+    /**
+     * @brief [Blocking] Set impedance properties of the robot's Cartesian motion controller
+     * used in the Cartesian motion-force control modes.
+     * @param[in] group The joint group to set Cartesian impedance for. Only existing single-arm
+     * joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] K_x Cartesian motion stiffness: \f$ K_x \in \mathbb{R}^{6 \times 1} \f$.
+     * Setting motion stiffness of a motion-controlled Cartesian axis to 0 will make this axis
+     * free-floating. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear stiffness and \f$
+     * \mathbb{R}^{3 \times 1} \f$ angular stiffness: \f$ [k_x, k_y, k_z, k_{Rx}, k_{Ry}, k_{Rz}]^T
+     * \f$. Valid range: [0, RobotInfo::K_x_nom]. Unit: \f$ [N/m]:[Nm/rad] \f$.
+     * @param[in] Z_x Cartesian motion damping ratio: \f$ Z_x \in \mathbb{R}^{6 \times 1} \f$.
+     * Consists of \f$ \mathbb{R}^{3 \times 1} \f$ linear damping ratio and \f$ \mathbb{R}^{3 \times
+     * 1} \f$ angular damping ratio: \f$ [\zeta_x, \zeta_y, \zeta_z, \zeta_{Rx}, \zeta_{Ry},
+     * \zeta_{Rz}]^T \f$. Valid range: [0.3, 0.8]. The nominal (safe) value is provided as default.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if [K_x] or [Z_x] contains any value outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning Changing damping ratio [Z_x] to a non-nominal value may lead to performance and
+     * stability issues, please use with caution.
+     */
+    void SetCartesianImpedance(JointGroup group, const std::array<double, kCartDoF>& K_x,
+        const std::array<double, kCartDoF>& Z_x = {0.7, 0.7, 0.7, 0.7, 0.7, 0.7});
+
+    /**
+     * @brief [Blocking] Set maximum contact wrench for the motion control part of the Cartesian
+     * motion-force control modes. The controller will regulate its output to maintain contact
+     * wrench (force and moment) with the environment under the set values.
+     * @param[in] group The joint group to set maximum contact wrench for. Only existing
+     * single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] max_wrench Maximum contact wrench (force and moment): \f$ F_{max} \in
+     * \mathbb{R}^{6 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ maximum force and
+     * \f$ \mathbb{R}^{3 \times 1} \f$ maximum moment: \f$ [f_x, f_y, f_z, m_x, m_y, m_z]^T \f$.
+     * Unit: \f$ [N]:[Nm] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if [max_wrench] contains any negative value.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note The maximum contact wrench regulation only applies to the motion control part.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning The maximum contact wrench regulation cannot be enabled if any of the rotational
+     * Cartesian axes is enabled for moment control.
+     */
+    void SetMaxContactWrench(JointGroup group, const std::array<double, kCartDoF>& max_wrench);
+
+    /**
+     * @brief [Blocking] Set reference joint positions for the null-space posture control module
+     * used in the Cartesian motion-force control modes.
+     * @param[in] group The joint group to set null-space posture for. Only existing single-arm
+     * joint groups and external axis joint groups are accepted.
+     * @param[in] ref_positions Reference joint positions for the null-space posture control:
+     * \f$ q_{ns} \in \mathbb{R}^{n \times 1} \f$. Valid range: [RobotInfo::q_min,
+     * RobotInfo::q_max]. Unit: \f$ [rad] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm or external axis
+     * joint group in the connected robot, if [ref_positions] contains any value outside the valid
+     * range, or if its size does not match robot DoF.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning The reference joint positions will be automatically reset to the robot's current
+     * joint positions upon re-entering the applicable control modes.
+     * @par Null-space posture control
+     * Similar to human arm, a robotic arm with redundant joint-space degree(s) of freedom (DoF > 6)
+     * can change its overall posture without affecting the ongoing primary task. This is achieved
+     * through a technique called "null-space control". After the reference joint positions of a
+     * desired robot posture are set using this function, the robot's null-space control module will
+     * try to pull the arm as close to this posture as possible without affecting the primary
+     * Cartesian motion-force control task.
+     */
+    void SetNullSpacePosture(JointGroup group, const std::vector<double>& ref_positions);
+
+    /**
+     * @brief [Blocking] Set weights of the three optimization objectives while computing the
+     * robot's null-space posture. Change the weights to optimize robot performance for different
+     * use cases.
+     * @param[in] group The joint group to set null-space objectives for. Only existing
+     * single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] linear_manipulability Increase this weight to improve the robot's capability to
+     * translate freely in Cartesian space, i.e. a broader range of potential translation movements.
+     * Valid range: [0.0, 1.0].
+     * @param[in] angular_manipulability Increase this weight to improve the robot's capability to
+     * rotate freely in Cartesian space, i.e. a broader range of potential rotation movements. Valid
+     * range: [0.0, 1.0].
+     * @param[in] ref_positions_tracking Increase this weight to make the robot track closer to the
+     * reference joint positions specified using SetNullSpacePosture(). Valid range: [0.1, 1.0].
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if any of the input parameters is outside its valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note The default value is provided for each parameter.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning The optimization weights will be automatically reset to the provided default values
+     * upon re-entering the applicable control modes.
+     */
+    void SetNullSpaceObjectives(JointGroup group, double linear_manipulability = 0.0,
+        double angular_manipulability = 0.0, double ref_positions_tracking = 0.5);
+
+    /**
+     * @brief [Blocking] Set Cartesian axes to enable force control while in the Cartesian
+     * motion-force control modes. Axes not enabled for force control will be motion-controlled.
+     * @param[in] group The joint group to set force control axes for. Only existing single-arm
+     * joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] enabled_axes Flags to enable/disable force control for certain Cartesian axes in
+     * the force control reference frame (configured by SetForceControlFrame()). The axis order is
+     * \f$ [X, Y, Z, Rx, Ry, Rz] \f$.
+     * @param[in] max_linear_vel For linear Cartesian axes that are enabled for force control, limit
+     * the moving velocity to these values as a protection mechanism in case of contact loss. The
+     * axis order is \f$ [X, Y, Z] \f$. Valid range: [0.005, 2.0]. Unit: \f$ [m/s] \f$.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if [max_linear_vel] contains any value outside the valid range.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @note If not set, force control is disabled for all Cartesian axes by default.
+     * @warning The maximum linear velocity protection for force control axes is only effective
+     * under active force control (i.e. passive force control disabled), see
+     * SetPassiveForceControl().
+     */
+    void SetForceControlAxis(JointGroup group, const std::array<bool, kCartDoF>& enabled_axes,
+        const std::array<double, kCartDoF / 2>& max_linear_vel = {1.0, 1.0, 1.0});
+
+    /**
+     * @brief [Blocking] Set reference frame for force control while in the Cartesian motion-force
+     * control modes. The force control frame is defined by specifying its transformation with
+     * regard to the root coordinate.
+     * @param[in] group The joint group to set force control frame for. Only existing single-arm
+     * joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] root_coord Reference coordinate of [T_in_root].
+     * @param[in] T_in_root Transformation from [root_coord] to the user-defined force control
+     * frame: \f$ ^{root}T_{force} \in \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3
+     * \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w,
+     * q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$. If root coordinate is a fixed one (e.g. WORLD),
+     * then the force control frame will also be fixed; if root coordinate is a moving one (e.g.
+     * TCP), then the force control frame will also be moving with the root coordinate. An identity
+     * transformation is provided as default.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, or if [root_coord] is invalid.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: RT_CARTESIAN_MOTION_FORCE, NRT_CARTESIAN_MOTION_FORCE,
+     * NRT_SUPER_PRIMITIVE.
+     * @note This function blocks until the request is successfully delivered.
+     * @note If not set, the robot will use WORLD origin as the force control frame by default.
+     * @par Force control frame
+     * In Cartesian motion-force control modes, the reference frame of motion control is always the
+     * world frame, but the reference frame of force control can be an arbitrary one. While the
+     * world frame is the commonly used global coordinate, the current TCP frame is a dynamic local
+     * coordinate whose transformation with regard to the world frame changes as the robot TCP
+     * moves. When using world frame with no transformation as the force control frame, the
+     * force-controlled axes and motion-controlled axes are guaranteed to be orthogonal. Otherwise,
+     * the force-controlled axes and motion-controlled axes are NOT guaranteed to be orthogonal
+     * because different reference frames are used. In this case, it's recommended but not required
+     * to set the target pose such that the actual robot motion direction(s) are orthogonal to force
+     * direction(s). If they are not orthogonal, the motion control's vector component(s) in the
+     * force direction(s) will be eliminated.
+     */
+    void SetForceControlFrame(JointGroup group, CoordType root_coord,
+        const std::array<double, kPoseSize>& T_in_root = {0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0});
+
+    /**
+     * @brief [Blocking] Enable or disable passive force control for the Cartesian motion-force
+     * control modes. When enabled, an open-loop force controller will be used to feed forward the
+     * target wrench, i.e. passive force control. When disabled, a closed-loop force controller will
+     * be used to track the target wrench, i.e. active force control.
+     * @param[in] group The joint group to toggle passive force control for. Only existing
+     * single-arm joint groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] is_enabled True: enable; false: disable. By default, passive force control is
+     * disabled and active force control is used.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot.
+     * @throw std::logic_error if the robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @note If not set, the passive force control is disabled by default.
+     * @par Difference between active and passive force control
+     * Active force control uses a feedback loop to reduce the error between target wrench and
+     * measured wrench. This method results in better force tracking performance, but at the cost of
+     * additional Cartesian damping which could potentially decrease motion tracking performance. On
+     * the other hand, passive force control simply feeds forward the target wrench. This methods
+     * results in worse force tracking performance, but is more robust and does not introduce
+     * additional Cartesian damping. The choice of active or passive force control depends on the
+     * actual application.
+     */
+    void SetPassiveForceControl(JointGroup group, bool is_enabled);
+
+    //======================================== IO CONTROL ========================================
+    /**
+     * @brief [Blocking] Set one or more digital output ports, including 16 on the control box plus
+     * 2 inside each wrist connector.
+     * @param[in] digital_outputs A map of {port_index, port_value}. For [port_index], the valid
+     * range is [0, 17]. For [port_value], true: set port high, false: set port low. For example,
+     * {{1, true}, {3, false}, {10, true}}.
+     * @throw std::invalid_argument if any provided port index is outside the valid range.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void SetDigitalOutputs(const std::map<unsigned int, bool>& digital_outputs);
+
+    /**
+     * @brief [Non-blocking] Current reading from all digital input ports, including 16 on the
+     * control box plus 2 in each wrist connector.
+     * @return A boolean array whose index corresponds to that of the digital input ports.
+     * True: port high; false: port low.
+     */
+    std::array<bool, kIOPorts> digital_inputs() const;
+
+    /**
+     * @brief [Non-blocking] Current reading from all digital output ports, including 16 on the
+     * control box plus 2 in each wrist connector.
+     * @return A boolean array whose index corresponds to that of the digital output ports.
+     * True: port high; false: port low.
+     */
+    std::array<bool, kIOPorts> digital_outputs() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+
+    friend class Device;
+    friend class FileIO;
+    friend class Gripper;
+    friend class Maintenance;
+    friend class Model;
+    friend class Safety;
+    friend class Tool;
+    friend class WorkCoord;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_ROBOT_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/safety.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/safety.hpp
new file mode 100644
index 0000000..052acab
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/safety.hpp
@@ -0,0 +1,168 @@
+/**
+ * @file safety.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_SAFETY_HPP_
+#define FLEXIV_RDK_SAFETY_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/** Number of safety IO ports */
+constexpr size_t kSafetyIOPorts = 8;
+
+/**
+ * @struct SafetyLimits
+ * @brief Data structure containing configurable robot safety limits.
+ */
+struct SafetyLimits
+{
+    /** Lower safety limits of joint positions for each joint group: \f$ q_{min} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> q_min = {};
+
+    /** Upper safety limits of joint positions for each joint group: \f$ q_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad] \f$. */
+    std::map<JointGroup, std::vector<double>> q_max = {};
+
+    /** Upper safety limits of joint velocities for each joint group when the robot is in normal
+     * state: \f$ \dot{q}^{normal}_{max} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$. */
+    std::map<JointGroup, std::vector<double>> dq_max_normal = {};
+
+    /** Upper safety limits of joint velocities for each joint group when the robot is in reduced
+     * state: \f$ \dot{q}^{reduced}_{max} \in \mathbb{R}^{n \times 1} \f$. Unit: \f$ [rad/s] \f$.
+     * @see Robot::reduced() */
+    std::map<JointGroup, std::vector<double>> dq_max_reduced = {};
+};
+
+/**
+ * @class Safety
+ * @brief Interface to manage safety settings of the robot. A password is required to authenticate
+ * this interface.
+ * @note As mentioned in the function doc, certain safety settings are only applicable to the arms
+ * but not the external axis.
+ */
+class Safety
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the safety settings interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @param[in] password Password to authorize making changes to the robot's safety settings.
+     * @throw std::invalid_argument if the provided password is incorrect.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Safety(const Robot& robot, const std::string& password);
+    virtual ~Safety();
+
+    /**
+     * @brief [Non-blocking] Default values of the safety limits of the connected robot.
+     * @return SafetyLimits value copy.
+     */
+    SafetyLimits default_limits() const;
+
+    /**
+     * @brief [Non-blocking] Current values of the safety limits of the connected robot.
+     * @return SafetyLimits value copy.
+     */
+    SafetyLimits current_limits() const;
+
+    /**
+     * @brief [Non-blocking] Current reading from all safety input ports.
+     * @return A boolean array whose index corresponds to that of the safety input ports.
+     * True: port high; false: port low.
+     */
+    std::array<bool, kSafetyIOPorts> safety_inputs() const;
+
+    /**
+     * @brief [Blocking] Set safety limits on the joint positions of the manipulator, which will
+     * honor this setting when making movements.
+     * @param[in] min_positions Minimum joint positions: \f$ q_{min} \in \mathbb{R}^{n \times 1}
+     * \f$. Valid range: [default_min_joint_positions, default_max_joint_positions]. Unit: \f$ [rad]
+     * \f$.
+     * @param[in] max_positions Maximum joint positions: \f$ q_{max} \in \mathbb{R}^{n \times 1}
+     * \f$. Valid range: [default_min_joint_positions, default_max_joint_positions]. Unit: \f$ [rad]
+     * \f$.
+     * @param[in] group Joint group to apply the limits to.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, if [min_positions] or [max_positions] contains any value outside the valid
+     * range, or if size of any input vector does not match manipulator DoF.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning A reboot is required for the updated safety settings to take effect. After reboot,
+     * make sure to use current_limits() to double check the updated safety limits are correct.
+     */
+    void SetJointPositionLimits(JointGroup group, const std::vector<double>& min_positions,
+        const std::vector<double>& max_positions);
+
+    /**
+     * @brief [Blocking] Set safety limits on the joint velocities of the manipulator, which will
+     * honor this setting when making movements under the normal state.
+     * @param[in] max_velocities Maximum joint velocities for normal state: \f$ \dot{q}_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Valid range: [0.8727, joint_velocity_normal_limits]. Unit: \f$
+     * [rad/s] \f$.
+     * @param[in] group Joint group to apply the limits to.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, if [max_velocities] contains any value outside the valid range, or if its
+     * size does not match manipulator DoF.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning A reboot is required for the updated safety settings to take effect. After reboot,
+     * make sure to use current_limits() to double check the updated safety limits are correct.
+     */
+    void SetJointVelocityNormalLimits(JointGroup group, const std::vector<double>& max_velocities);
+
+    /**
+     * @brief [Blocking] Set safety limits on the joint velocities of the manipulator, which will
+     * honor this setting when making movements under the reduced state.
+     * @param[in] max_velocities Maximum joint velocities for reduced state: \f$ \dot{q}_{max} \in
+     * \mathbb{R}^{n \times 1} \f$. Valid range: [0.8727, joint_velocity_normal_limits]. Unit: \f$
+     * [rad/s] \f$.
+     * @param[in] group Joint group to apply the limits to.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot, if [max_velocities] contains any value outside the valid range, or if its
+     * size does not match manipulator DoF.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @warning A reboot is required for the updated safety settings to take effect. After reboot,
+     * make sure to use current_limits() to double check the updated safety limits are correct.
+     */
+    void SetJointVelocityReducedLimits(JointGroup group, const std::vector<double>& max_velocities);
+
+    /**
+     * @brief [Blocking] Change settings of the regulator on the joint output torques of the
+     * manipulator and external axes. The regulator will limit the joints' total output torques so
+     * that the measured torques are less likely to exceed safety limits under large payload or
+     * fast/abrupt motions.
+     * @param[in] limiting_factor Factor to limit the total output torques: \f$ \tau^{o}_{max} =
+     * \tau_{max} \times factor \f$.
+     * @param[in] error_threshold If the unregulated output torque of any joint exceeds the
+     * configured regulator limit for more than [error_threshold] cycles consecutively, the robot
+     * will trigger a minor fault and stop.
+     * @throw std::invalid_argument if [limiting_factor] or [error_threshold] is not positive.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     * @note This setting takes effect immediately and doesn't require a reboot.
+     * @warning Setting the limiting factor too small can cause performance degradation.
+     */
+    void SetJointOutputTorqueRegulator(
+        double limiting_factor = 1.3, unsigned int error_threshold = 50);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_SAFETY_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/scheduler.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/scheduler.hpp
new file mode 100644
index 0000000..43701f2
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/scheduler.hpp
@@ -0,0 +1,113 @@
+/**
+ * @file scheduler.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_SCHEDULER_HPP_
+#define FLEXIV_RDK_SCHEDULER_HPP_
+
+#include <string>
+#include <functional>
+#include <memory>
+
+namespace flexiv::rdk {
+
+/**
+ * @class Scheduler
+ * @brief Real-time scheduler that can simultaneously run multiple periodic tasks. Parameters for
+ * each task are configured independently.
+ */
+class Scheduler
+{
+public:
+    /**
+     * @brief [Blocking] Instantiate a real-time scheduler.
+     * @throw std::runtime_error if the initialization sequence failed.
+     * @warning This constructor blocks until the initialization sequence is successfully finished.
+     */
+    Scheduler();
+    virtual ~Scheduler();
+
+    /**
+     * @brief [Non-blocking] Add a new periodic task to the scheduler's task pool. Each task in the
+     * pool is assigned to a dedicated thread with independent thread configuration.
+     * @param[in] callback Callback function of user task.
+     * @param[in] task_name A unique name for this task.
+     * @param[in] interval Execution interval of this periodic task [ms]. The minimum available
+     * interval is 1 ms, equivalent to 1 kHz loop frequency.
+     * @param[in] priority Priority for this task thread, can be set to
+     * min_priority()–max_priority() for real-time scheduling, or 0 for non-real-time scheduling.
+     * When the priority is set to use real-time scheduling, this thread becomes a real-time thread
+     * and can only be interrupted by threads with higher priority. When the priority is set to use
+     * non-real-time scheduling (i.e. 0), this thread becomes a non-real-time thread and can be
+     * interrupted by any real-time threads. The common practice is to set priority of the most
+     * critical tasks to max_priority() or near, and set priority of other non-critical tasks to 0
+     * or near. To avoid race conditions, the same priority should be assigned to only one task.
+     * @param[in] cpu_affinity CPU core for this task thread to bind to, can be set to 2–(num_cores
+     * - 1). This task thread will only run on the specified CPU core. If left with the default
+     * value (-1), then this task thread will not bind to any CPU core, and the system will decide
+     * which core to run this task thread on according to the system's own strategy. The common
+     * practice is to bind the high-priority task to a dedicated spare core, and bind low-priority
+     * tasks to other cores or just leave them unbound (cpu_affinity = -1).
+     * @throw std::logic_error if the scheduler is already started or is not fully initialized yet.
+     * @throw std::invalid_argument if the specified interval/priority/affinity is invalid or the
+     * specified task name is duplicate.
+     * @throw std::runtime_error if an error is triggered by the client computer.
+     * @note Setting CPU affinity on macOS has no effect, as its Mach kernel takes full control of
+     * thread placement so CPU binding is not supported.
+     * @warning Calling this function after start() is not allowed.
+     * @warning For maximum scheduling performance, setting CPU affinity to 0 or 1 is not allowed:
+     * core 0 is usually the default core for system processes and can be crowded; core 1 is
+     * reserved for the scheduler itself.
+     */
+    void AddTask(std::function<void(void)>&& callback, const std::string& task_name, int interval,
+        int priority, int cpu_affinity = -1);
+
+    /**
+     * @brief [Blocking] Start all added tasks. A dedicated thread will be created for each added
+     * task and the periodic execution will begin.
+     * @throw std::logic_error if the scheduler is not initialized yet.
+     * @throw std::runtime_error if failed to start the tasks.
+     * @note This function blocks until all added tasks are started.
+     */
+    void Start();
+
+    /**
+     * @brief [Blocking] Stop all added tasks. The periodic execution will stop and all task threads
+     * will be closed with the resources released.
+     * @throw std::logic_error if the scheduler is not initialized or the tasks are not started yet.
+     * @throw std::runtime_error if failed to stop the tasks.
+     * @note Calling start() again can restart the added tasks.
+     * @note This function blocks until all task threads have exited and resources are released.
+     * @warning This function cannot be called from within a task thread.
+     */
+    void Stop();
+
+    /**
+     * @brief [Non-blocking] Maximum available priority for user tasks.
+     * @return The maximum priority that can be set for a user task with real-time scheduling policy
+     * when calling AddTask().
+     */
+    int max_priority() const;
+
+    /**
+     * @brief [Non-blocking] Minimum available priority for user tasks.
+     * @return The minimum priority that can be set for a user task with real-time scheduling policy
+     * when calling AddTask().
+     */
+    int min_priority() const;
+
+    /**
+     * @brief [Non-blocking] Number of tasks added to the scheduler.
+     * @return Number of added tasks.
+     */
+    size_t num_tasks() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_SCHEDULER_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/tool.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/tool.hpp
new file mode 100644
index 0000000..0adac45
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/tool.hpp
@@ -0,0 +1,187 @@
+/**
+ * @file tool.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_TOOL_HPP_
+#define FLEXIV_RDK_TOOL_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/**
+ * @struct ToolParams
+ * @brief Data structure containing robot tool parameters.
+ * @see Tool::params().
+ */
+struct ToolParams
+{
+    /** Total mass. Unit: \f$ [kg] \f$ */
+    double mass = 0.0;
+
+    /** Center of mass in robot flange frame: \f$ [x, y, z] \f$. Unit: \f$ [m] \f$ */
+    std::array<double, 3> CoM = {};
+
+    /** Inertia at center of mass: \f$ [Ixx, Iyy, Izz, Ixy, Ixz, Iyz] \f$. Unit: \f$ [kg m^2] \f$ */
+    std::array<double, 6> inertia = {};
+
+    /** Position and orientation of the tool center point (TCP) in flange frame. Consists of \f$
+     * \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x,
+     * y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$ */
+    std::array<double, kPoseSize> tcp_location = {};
+};
+
+/**
+ * @class Tool
+ * @brief Interface to manage tools of the robot. All updates take effect immediately without a
+ * power cycle. However, the robot must be in IDLE mode when applying changes.
+ */
+class Tool
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the robot tool interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    Tool(const Robot& robot);
+    virtual ~Tool();
+
+    /**
+     * @brief [Blocking] A list of all configured tools.
+     * @return Tool names as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> list() const;
+
+    /**
+     * @brief [Blocking] Name of the active tool currently used by the specified joint group.
+     * @param[in] group Joint group to get tool name for. Only existing single-arm joint groups
+     * like ARM_1 and ARM_2 are accepted.
+     * @return Name of the active tool. Return "Flange" if there's no active tool.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::string name(JointGroup group) const;
+
+    /**
+     * @brief [Blocking] Whether the specified tool already exists.
+     * @param[in] name Name of the tool to check.
+     * @return True if the specified tool exists.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    bool exist(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Parameters of the active tool currently used by the specified joint group.
+     * @param[in] group Joint group to get tool parameters for. Only existing single-arm joint
+     * groups like ARM_1 and ARM_2 are accepted.
+     * @return ToolParams data struct.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    ToolParams params(JointGroup group) const;
+
+    /**
+     * @brief [Blocking] Parameters of the specified tool.
+     * @param[in] name Name of the tool to get parameters for, must be an existing one.
+     * @return ToolParams data struct.
+     * @throw std::invalid_argument if the specified tool does not exist.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    ToolParams params(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Add a new tool with user-specified parameters.
+     * @param[in] name Name of the new tool, must be unique.
+     * @param[in] params Parameters of the new tool.
+     * @throw std::invalid_argument if the specified tool already exists.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Add(const std::string& name, const ToolParams& params);
+
+    /**
+     * @brief [Blocking] Switch active tool for the specified joint group. All following operations
+     * will default to use this tool.
+     * @param[in] group Joint group to switch active tool for. Only existing single-arm joint
+     * groups like ARM_1 and ARM_2 are accepted.
+     * @param[in] name Name of the tool to switch to, must be an existing one.
+     * @throw std::invalid_argument if [group] is not an existing single-arm joint group in the
+     * connected robot.
+     * @throw std::invalid_argument if the specified tool does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Switch(JointGroup group, const std::string& name);
+
+    /**
+     * @brief [Blocking] Update the parameters of an existing tool.
+     * @param[in] name Name of the tool to update, must be an existing one.
+     * @param[in] params New parameters for the specified tool.
+     * @throw std::invalid_argument if the specified tool does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Update(const std::string& name, const ToolParams& params);
+
+    /**
+     * @brief [Blocking] Remove an existing tool.
+     * @param[in] name Name of the tool to remove, must be an existing one but cannot be "Flange".
+     * @throw std::invalid_argument if the specified tool does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode or trying to remove
+     * "Flange".
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Remove(const std::string& name);
+
+    /**
+     * @brief [Blocking] Calibrate the payload parameters (mass, CoM, and inertia) of a tool.
+     * @param[in] tool_mounted Whether the tool to be calibrated is mounted on the robot flange when
+     * triggering this calibration process. See details below.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if fault occurred during the calibration or failed to get the
+     * calibration result.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the calibration is finished.
+     * @warning [tcp_location] in the returned struct will be zeros and should be ignored.
+     * @par How to properly calibrate the payload parameters of a tool?
+     * 1. Call Switch("Flange") to disable any active tool from the robot software.
+     * 2. Physically mount the tool to be calibrated to robot flange.
+     * 3. Call this function with [tool_mounted] set to TRUE, then wait for completion. If the robot
+     * has a force-torque (FT) sensor, then the returned result will be accurate enough and the
+     * optional steps can be skipped. If the robot does not have an FT sensor, then the optional
+     * steps are recommended to improve accuracy.
+     * 4. (Optional) Physically unmount the tool from robot flange.
+     * 5. (Optional) Call this function again but with [tool_mounted] set to FALSE, then wait for
+     * completion.
+     * 6. Review the returned result and call Add() or Update() to apply the calibrated payload
+     * parameters to a new or existing tool. Note that [tcp_location] in the returned struct is
+     * invalid and cannot be used directly.
+     */
+    ToolParams CalibratePayloadParams(bool tool_mounted);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_TOOL_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/utility.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/utility.hpp
new file mode 100644
index 0000000..7de29c1
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/utility.hpp
@@ -0,0 +1,245 @@
+/**
+ * @file utility.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_UTILITY_HPP_
+#define FLEXIV_RDK_UTILITY_HPP_
+
+#include "data.hpp"
+#include <Eigen/Eigen>
+#include <algorithm>
+#include <sstream>
+
+namespace flexiv::rdk {
+
+namespace utility {
+
+/** @brief Whether the specified group represents a single arm. */
+inline bool IsSingleArmGroup(JointGroup group)
+{
+    return group == JointGroup::ARM_1 || group == JointGroup::ARM_2;
+}
+
+/**
+ * @brief Check whether one primitive state is true.
+ * @param[in] primitive_states Primitive states.
+ * @param[in] state_name Primitive state name to check, e.g. "reachedTarget".
+ * @return True if the state exists and its integer value is non-zero.
+ */
+inline bool PrimitiveStateTrue(
+    const PrimitiveStates& primitive_states, const std::string& state_name)
+{
+    const auto state_it = primitive_states.names_and_values.find(state_name);
+    if (state_it == primitive_states.names_and_values.end()) {
+        return false;
+    }
+
+    if (!std::holds_alternative<int>(state_it->second)) {
+        return false;
+    }
+
+    return std::get<int>(state_it->second) != 0;
+}
+
+/**
+ * @brief Check whether one primitive state is true for all groups included in [primitive_states].
+ * @param[in] primitive_states Primitive states keyed by joint group.
+ * @param[in] state_name Primitive state name to check, e.g. "reachedTarget".
+ * @return True if all included groups contain the state and its integer value is non-zero.
+ */
+inline bool PrimitiveStateTrueForGroups(
+    const std::map<JointGroup, PrimitiveStates>& primitive_states, const std::string& state_name)
+{
+    return std::all_of(primitive_states.begin(), primitive_states.end(),
+        [&](const auto& entry) { return PrimitiveStateTrue(entry.second, state_name); });
+}
+
+/**
+ * @brief Check whether one primitive state is true for each group included in [primitive_states],
+ * using a per-group state name.
+ * @param[in] primitive_states Primitive states keyed by joint group.
+ * @param[in] state_names Primitive state names keyed by joint group.
+ * @return True if every included group has a matching state name entry and its integer value is
+ * non-zero.
+ */
+inline bool PrimitiveStateTrueForGroups(
+    const std::map<JointGroup, PrimitiveStates>& primitive_states,
+    const std::map<JointGroup, std::string>& state_names)
+{
+    return std::all_of(primitive_states.begin(), primitive_states.end(), [&](const auto& entry) {
+        if (!state_names.contains(entry.first)) {
+            return false;
+        }
+        return PrimitiveStateTrue(entry.second, state_names.at(entry.first));
+    });
+}
+
+/**
+ * @brief Convert quaternion to Euler angles with ZYX axis rotations.
+ * @param[in] quat Quaternion input in [w,x,y,z] order.
+ * @return Euler angles in [x,y,z] order [rad].
+ * @note The return value, when converted to degrees, is the same Euler angles used by Move
+ * primitives.
+ */
+inline std::array<double, 3> Quat2EulerZYX(const std::array<double, 4>& quat)
+{
+    // Form quaternion
+    Eigen::Quaterniond q(quat[0], quat[1], quat[2], quat[3]);
+
+    // The returned array is in [z,y,x] order
+    auto euler_ZYX = q.toRotationMatrix().eulerAngles(2, 1, 0);
+
+    // Convert to general [x,y,z] order
+    return (std::array<double, 3> {euler_ZYX[2], euler_ZYX[1], euler_ZYX[0]});
+}
+
+/**
+ * @brief Convert radians to degrees for a single value.
+ */
+inline double Rad2Deg(double rad)
+{
+    constexpr double kPi = 3.14159265358979323846;
+    return (rad / kPi * 180.0);
+}
+
+/**
+ * @brief Convert radians to degrees for an array.
+ */
+template <size_t N>
+inline std::array<double, N> Rad2Deg(const std::array<double, N>& rad_arr)
+{
+    std::array<double, N> deg_arr;
+    for (size_t i = 0; i < N; i++) {
+        deg_arr[i] = Rad2Deg(rad_arr[i]);
+    }
+    return deg_arr;
+}
+
+/**
+ * @brief Convert radians to degrees for a vector.
+ */
+inline std::vector<double> Rad2Deg(const std::vector<double>& rad_vec)
+{
+    std::vector<double> deg_vec;
+    for (const auto& v : rad_vec) {
+        deg_vec.push_back(Rad2Deg(v));
+    }
+    return deg_vec;
+}
+
+/**
+ * @brief Convert an std::vector to a string.
+ * @param[in] vec std::vector of any type and size.
+ * @param[in] decimal Decimal places to keep for each floating-point number in the vector.
+ * @param[in] separator Character to separate between numbers.
+ * @return The converted string.
+ */
+template <typename T>
+inline std::string Vec2Str(
+    const std::vector<T>& vec, size_t decimal = 3, const std::string& separator = " ")
+{
+    std::string padding = "";
+    std::ostringstream oss;
+    oss.precision(decimal);
+    oss << std::fixed;
+
+    for (const auto& v : vec) {
+        oss << padding << v;
+        padding = separator;
+    }
+    return oss.str();
+}
+
+/**
+ * @brief Convert an std::array to a string.
+ * @param[in] arr std::array of any type and size.
+ * @param[in] decimal Decimal places to keep for each floating-point number in the array.
+ * @param[in] separator Character to separate between numbers.
+ * @return The converted string.
+ */
+template <typename T, size_t N>
+inline std::string Arr2Str(
+    const std::array<T, N>& arr, size_t decimal = 3, const std::string& separator = " ")
+{
+    std::vector<T> vec(N);
+    std::copy(arr.begin(), arr.end(), vec.begin());
+    return Vec2Str(vec, decimal, separator);
+}
+
+/**
+ * @brief Convert the commonly used std::variant to a string.
+ * @param[in] variant std::variant used by multiple rdk::Robot functions.
+ * @param[in] decimal Decimal places to keep for each floating-point number in the variant.
+ * @param[in] separator Character to separate between numbers in the vector.
+ * @return The converted string.
+ */
+inline std::string FlexivTypes2Str(
+    const rdk::FlexivDataTypes& variant, size_t decimal = 3, const std::string& separator = " ")
+{
+    if (auto* val = std::get_if<int>(&variant)) {
+        return Vec2Str(std::vector<int> {*val}, decimal);
+    } else if (auto* val = std::get_if<double>(&variant)) {
+        return Vec2Str(std::vector<double> {*val}, decimal);
+    } else if (auto* val = std::get_if<std::string>(&variant)) {
+        return *val;
+    } else if (auto* val = std::get_if<rdk::Coord>(&variant)) {
+        return (*val).str();
+    } else if (auto* vec = std::get_if<std::vector<int>>(&variant)) {
+        return Vec2Str(*vec, decimal, separator);
+    } else if (auto* vec = std::get_if<std::vector<double>>(&variant)) {
+        return Vec2Str(*vec, decimal, separator);
+    } else if (auto* vec = std::get_if<std::vector<std::string>>(&variant)) {
+        return Vec2Str(*vec, decimal, separator);
+    } else if (auto* vec = std::get_if<std::vector<rdk::Coord>>(&variant)) {
+        std::string ret;
+        // Separate two Coord by " : "
+        for (const auto& v : (*vec)) {
+            ret += v.str() + " : ";
+        }
+        // Remove the trailing " : "
+        if (!ret.empty()) {
+            ret.erase(ret.size() - 3);
+        }
+        return ret;
+    }
+    return "";
+}
+
+/**
+ * @brief Check if any provided strings exist in the program arguments.
+ * @param[in] argc Argument count passed to main() of the program.
+ * @param[in] argv Argument vector passed to main() of the program, where argv[0] is the program
+ * name.
+ * @param[in] ref_strings Reference strings to check against.
+ * @return True if the program arguments contain one or more reference strings.
+ */
+inline bool ProgramArgsExistAny(int argc, char** argv, const std::vector<std::string>& ref_strings)
+{
+    for (int i = 0; i < argc; i++) {
+        for (const auto& v : ref_strings) {
+            if (v == std::string(argv[i])) {
+                return true;
+            }
+        }
+    }
+    return false;
+}
+
+/**
+ * @brief Check if one specific string exists in the program arguments.
+ * @param[in] argc Argument count passed to main() of the program.
+ * @param[in] argv Argument vector passed to main() of the program, with argv[0] being the program
+ * name.
+ * @param[in] ref_strings Reference string to check against.
+ * @return True if the program arguments contain this specific reference string.
+ */
+inline bool ProgramArgsExist(int argc, char** argv, const std::string& ref_strings)
+{
+    return ProgramArgsExistAny(argc, argv, {ref_strings});
+}
+
+} /* namespace utility */
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_UTILITY_HPP_ */
diff --git a/thirdparty/flexiv_rdk/include/flexiv/rdk/work_coord.hpp b/thirdparty/flexiv_rdk/include/flexiv/rdk/work_coord.hpp
new file mode 100644
index 0000000..485dd4e
--- /dev/null
+++ b/thirdparty/flexiv_rdk/include/flexiv/rdk/work_coord.hpp
@@ -0,0 +1,105 @@
+/**
+ * @file work_coord.hpp
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_RDK_WORK_COORD_HPP_
+#define FLEXIV_RDK_WORK_COORD_HPP_
+
+#include "robot.hpp"
+
+namespace flexiv::rdk {
+
+/**
+ * @class WorkCoord
+ * @brief Interface to manage work coordinates of the robot. All updates take effect immediately
+ * without a power cycle. However, the robot must be in IDLE mode when applying changes.
+ */
+class WorkCoord
+{
+public:
+    /**
+     * @brief [Non-blocking] Instantiate the work coordinate interface.
+     * @param[in] robot Reference to the instance of flexiv::rdk::Robot.
+     * @throw std::runtime_error if the initialization sequence failed.
+     */
+    WorkCoord(const Robot& robot);
+    virtual ~WorkCoord();
+
+    /**
+     * @brief [Blocking] A list of all configured work coordinates.
+     * @return Work coordinate names as a string list.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::vector<std::string> list() const;
+
+    /**
+     * @brief [Blocking] Whether the specified work coordinate already exists.
+     * @param[in] name Name of the work coordinate to check.
+     * @return True if the specified work coordinate exists.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    bool exist(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Pose of an existing work coordinate.
+     * @param[in] name Name of the work coordinate to get pose for, must be an existing one.
+     * @return Pose of the work coordinate in world frame: \f$ ^{O}T_{work} \in \mathbb{R}^{7 \times
+     * 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$ \mathbb{R}^{4 \times 1}
+     * \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$ [m]:[] \f$.
+     * @throw std::runtime_error if failed to get a reply from the connected robot.
+     * @note This function blocks until a reply is received.
+     */
+    std::array<double, kPoseSize> pose(const std::string& name) const;
+
+    /**
+     * @brief [Blocking] Add a new work coordinate with user-specified parameters.
+     * @param[in] name Name of the new work coordinate, must be unique.
+     * @param[in] pose Pose of the new work coordinate in world frame: \f$ ^{O}T_{work} \in
+     * \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$
+     * \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$
+     * [m]:[] \f$.
+     * @throw std::invalid_argument if the specified work coordinate already exists.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Add(const std::string& name, const std::array<double, kPoseSize>& pose);
+
+    /**
+     * @brief [Blocking] Update the pose of an existing work coordinate.
+     * @param[in] name Name of the work coordinate to update, must be an existing one.
+     * @param[in] pose New pose for the specified work coordinate in world frame: \f$ ^{O}T_{work}
+     * \in \mathbb{R}^{7 \times 1} \f$. Consists of \f$ \mathbb{R}^{3 \times 1} \f$ position and \f$
+     * \mathbb{R}^{4 \times 1} \f$ quaternion: \f$ [x, y, z, q_w, q_x, q_y, q_z]^T \f$. Unit: \f$
+     * [m]:[] \f$.
+     * @throw std::invalid_argument if the specified work coordinate does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Update(const std::string& name, const std::array<double, kPoseSize>& pose);
+
+    /**
+     * @brief [Blocking] Remove an existing work coordinate.
+     * @param[in] name Name of the work coordinate to remove, must be an existing one.
+     * @throw std::invalid_argument if the specified work coordinate does not exist.
+     * @throw std::logic_error if robot is not in the correct control mode.
+     * @throw std::runtime_error if failed to deliver the request to the connected robot.
+     * @note Applicable control modes: IDLE.
+     * @note This function blocks until the request is successfully delivered.
+     */
+    void Remove(const std::string& name);
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace flexiv::rdk */
+
+#endif /* FLEXIV_RDK_WORK_COORD_HPP_ */
diff --git a/thirdparty/flexiv_rdk/lib/dummy.cpp b/thirdparty/flexiv_rdk/lib/dummy.cpp
new file mode 100644
index 0000000..6299cc5
--- /dev/null
+++ b/thirdparty/flexiv_rdk/lib/dummy.cpp
@@ -0,0 +1,5 @@
+/**
+ * @file dummy.cpp
+ * @brief A dummy source file to add portability to the installed flexiv_rdk library
+ */
+void dummy() { }
diff --git a/thirdparty/flexiv_rdk/lib/flexiv_rdk.win_amd64.lib.sha256 b/thirdparty/flexiv_rdk/lib/flexiv_rdk.win_amd64.lib.sha256
new file mode 100644
index 0000000..d339a39
--- /dev/null
+++ b/thirdparty/flexiv_rdk/lib/flexiv_rdk.win_amd64.lib.sha256
@@ -0,0 +1 @@
+8159b1304cd25f4cbde0aafc37d8e41ea8edf77804b0680cee5abf9442ba7557
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a.sha256 b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a.sha256
new file mode 100644
index 0000000..78da2fb
--- /dev/null
+++ b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a.sha256
@@ -0,0 +1 @@
+d1267bfa2f54b30e044c92c5468bdfdeb80126fc99e22d1a9623e73315b070a7
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-qnx.a.sha256 b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-qnx.a.sha256
new file mode 100644
index 0000000..04bdf2a
--- /dev/null
+++ b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-qnx.a.sha256
@@ -0,0 +1 @@
+temporarily_suspended
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/lib/libflexiv_rdk.arm64-darwin.a.sha256 b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.arm64-darwin.a.sha256
new file mode 100644
index 0000000..e2416e5
--- /dev/null
+++ b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.arm64-darwin.a.sha256
@@ -0,0 +1 @@
+24b0f511b6679d935c6d26e7dab512e534b80da32583699e4b1f2872f10eabde
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a.sha256 b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a.sha256
new file mode 100644
index 0000000..4299483
--- /dev/null
+++ b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a.sha256
@@ -0,0 +1 @@
+f5a0aab1fe2195ac28fd10764c6a3deb81d25789d7b04acb1e87e029f6924aea
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-qnx.a.sha256 b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-qnx.a.sha256
new file mode 100644
index 0000000..04bdf2a
--- /dev/null
+++ b/thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-qnx.a.sha256
@@ -0,0 +1 @@
+temporarily_suspended
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/llms.txt b/thirdparty/flexiv_rdk/llms.txt
new file mode 100644
index 0000000..bdf2dec
--- /dev/null
+++ b/thirdparty/flexiv_rdk/llms.txt
@@ -0,0 +1,215 @@
+# llms.txt - Guidance for generating Flexiv robot control code in this repository
+
+Scope: This file captures repository-specific, API-accurate knowledge for LLMs that generate Python control scripts using `flexivrdk`.
+
+## 0) Authoritative References
+- RDK Manual: https://www.flexiv.com/software/rdk/manual
+- API Documentation (latest): https://www.flexiv.com/software/rdk/api
+- Python package page: https://pypi.org/project/flexivrdk
+- Primitive catalog and documentation: https://www.flexiv.com/software/primitives
+
+Use these sources for API behavior details and argument meaning when in doubt.
+
+## 0.1) C++ and Python API Parity
+- In this project, Python RDK is generated from pybind bindings of C++ RDK.
+- Function names and function documentation are expected to match between C++ and Python APIs.
+- Generation rule: if Python docs are sparse, use the corresponding C++ API docs as canonical backup for names, parameters, and semantics.
+- Verification rule: before emitting a Python API call, verify parameter count, optionality, and fixed-size container constraints against API docs or headers.
+- Priority references for verification: API docs first, then C++ headers (for exact sizes/types when docs are ambiguous).
+
+## 1) Preferred Script Pattern (Python)
+- Use examples under `example_py/` as style reference.
+- Typical imports:
+  - `import argparse`
+  - `import time`
+  - `import spdlog`
+  - `import flexivrdk`
+- Typical flow:
+  1. Parse `robot_sn` argument.
+  2. `robot = flexivrdk.Robot(robot_sn)`.
+  3. If `robot.fault()`: call `robot.ClearFault()` and handle failure.
+  4. `robot.ServoOn()` and wait until `robot.operational()`.
+  5. Get applicable joint groups from `robot.info()`:
+     - for primitive execution, use `single_arm_groups = robot.info().single_arm_groups`
+     - for inspection/logging of all groups, use `robot.info().all_groups`
+  6. If needed, use `robot.Home()` (or `robot.Home([...])`) as the blocking convenience wrapper for homing.
+  7. Switch to primitive execution with `robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)`.
+  8. Execute primitives with `robot.ExecutePrimitive({group: flexivrdk.PrimitiveArgs(name, params) for group in single_arm_groups})`.
+  9. Wait for primitive completion using the primitive's documented Default Transition Condition from each running group's primitive state map.
+  10. End with `robot.Stop()`.
+
+## 2) Primitive Execution Conventions
+- Use explicit primitive names such as `Home`, `MoveJ`, `MoveL`.
+- For each primitive call, wrap parameters in `flexivrdk.PrimitiveArgs(name, params)` and send as a map keyed by joint group.
+- Use only existing single-arm joint groups, typically the keys of `robot.info().single_arm_groups`, with `robot.ExecutePrimitive(...)`.
+- Keep transitions explicit in code (sequential primitive calls) unless user asks for asynchronous behavior.
+- Do not assume `robot.ExecutePrimitive(...)` blocks until completion. It blocks until request delivery or primitive start; users must still wait on primitive states.
+- Primitive transition rule:
+  - Each primitive exposes a set of primitive states as listed in Primitive State Parameters.
+  - In most cases, use the primitive's documented Default Transition Condition. But other Primitive State Parameters can also be used as transition conditions.
+  - Do not assume `reachedTarget` is the only valid transition condition for all primitives.
+- Polling loop pattern:
+  - `all_states = robot.primitive_states()`
+  - `robot.primitive_states()` only contains joint groups that are currently running primitives
+  - check the documented state key(s) from `state.names_and_values` for each target joint group
+  - primitive-state booleans are typically read back as integer `1` or `0`
+  - `time.sleep(0.1-1.0)` in the loop
+
+### 2.1) RDK v2.x Joint-Group API Rule
+- `robot.states()` returns `dict[JointGroup, RobotStates]`, not a single `RobotStates`.
+- `robot.primitive_states()` returns `dict[JointGroup, PrimitiveStates]`, not a flat key-value map.
+- `robot.ExecutePrimitive(...)` takes `dict[JointGroup, PrimitiveArgs]`, not `(name, params)`.
+- `robot.Home(groups=...)` is the blocking convenience wrapper for the Home primitive and accepts a list of single-arm joint groups. If omitted, it homes all applicable single-arm groups.
+- When using fields like `tcp_pose`, first choose a specific joint group, usually one from `robot.info().single_arm_groups`.
+
+## 3) Data Type Mapping for `ExecutePrimitive`
+Map plan-like values to RDK Python objects:
+- `JPOS` -> `flexivrdk.JPos(q_m, q_e)` or `flexivrdk.JPos(q_m)`
+- `COORD` -> `flexivrdk.Coord(position, orientation, ref_frame, ref_q_m, ref_q_e)`
+- `ARRAY_COORD` -> Python list of `flexivrdk.Coord(...)`
+- Scalar types:
+  - `BOOL` -> `bool`
+  - `INT` -> `int`
+  - `DOUBLE` -> `float`
+  - `TYPE` -> `str`
+  - `VEC_3d` -> Python list/array length 3
+
+### 3.1) Fixed-Size Argument Checklist (Pybind)
+- Treat constructor/type hints with fixed sizes as strict runtime requirements.
+- `flexivrdk.JPos(q_m)` requires `q_m` length 7. `flexivrdk.JPos(q_m, q_e)` requires `q_e` length 6.
+- `flexivrdk.Coord(position, orientation, ref_frame)` requires lengths 3, 3, and 2 respectively.
+- Optional `Coord` arguments still have fixed sizes when provided: `ref_q_m` length 7 and `ref_q_e` length 6.
+- If `ref_q_e` is provided, also provide `ref_q_m`.
+- Prefer omitting optional constructor arguments entirely over passing empty or wrong-sized placeholders.
+
+## 4) Critical `Coord` Rule (Do Not Drop Provided Fields)
+From `include/flexiv/rdk/data.hpp`, the current `Coord` signature is:
+
+`flexivrdk.Coord(position, orientation, ref_frame, ref_q_m, ref_q_e)`
+
+Its members are, in order:
+1. `position`
+2. `orientation`
+3. `ref_frame`
+4. `ref_q_m`
+5. `ref_q_e`
+
+Generation rule:
+- `ref_q_m` and `ref_q_e` are optional in the constructor, but if source data provides them, populate them.
+- Do not silently drop `ref_q_m` or `ref_q_e`.
+- If specifying `ref_q_e`, also specify `ref_q_m`.
+
+## 5) Units Guidance
+- RDK `JPos` and `Coord` objects use SI length unit `[m]` on the API side.
+- Header note: conversion between RDK `[m]` and Flexiv Elements custom-type `[mm]` is handled automatically for `JPOS` and `COORD` exchanged through APIs such as `ExecutePrimitive()`.
+- Primitive-documentation caveat: primitive input parameters do not universally use SI units.
+  - For scalar primitive parameters, follow the primitive page exactly.
+  - For `flexivrdk.Coord(...)` and `flexivrdk.JPos(...)`, supply values in the RDK object units defined by the headers.
+
+## 5.1) Upright Posture Convention
+- The robot upright posture is defined as all joint positions being 0.
+
+## 5.2) Euler Readback Normalization
+- Robot Euler orientation readback is normalized to the range [-180, 180] degrees.
+- If an Euler angle increases past 180, it wraps to -180. If it decreases past -180, it wraps to 180.
+- This wraparound does not indicate a different physical orientation; it is only a different representation of the same rotation.
+- When comparing Euler angles from commanded values versus robot readback, compare with wraparound awareness instead of raw numeric equality.
+
+## 6) Safety and Robustness Defaults
+- Always include fault clear path before enabling.
+- Always wait for operational state before switching mode or sending primitives.
+- Use try/except and log exceptions with `spdlog`.
+- Avoid busy-spin loops; include `time.sleep(...)` in polling loops.
+- Prefer timeout-backed wait helpers over unbounded polling loops when generating new scripts.
+- Keep command sequence deterministic and human-readable.
+
+## 7) Output Style for Generated Scripts
+- Prefer one self-contained runnable script per task.
+- Include short comments only for non-obvious behavior.
+- Keep numeric trajectory data hardcoded only when user explicitly asks for hardcoding.
+- Do not parse external plan files if the user asks for hardcoded primitive calls.
+
+## 8) Repository Anchors
+- Python examples: `example_py/`
+- Agent primitive-only reference: `example_py/AGENT_REFERENCE_primitives_only.py`
+- Primitive tutorial reference: `example_py/basics3_primitive_execution.py`
+- Primitive wait helper reference: `example_py/utility.py`
+- Core robot API surface: `include/flexiv/rdk/robot.hpp`
+- Core data type definitions: `include/flexiv/rdk/data.hpp`
+- Repo readme and setup guide: `README.md`
+
+## 9) Minimal Skeleton
+```python
+import argparse
+import time
+import spdlog
+import flexivrdk
+import utility
+
+
+def wait_primitive_transition(robot, transition_keys_by_group, dt=0.2, timeout_s=60.0):
+    """Block until primitive_states() for all groups satisfy their transition key.
+
+    Use the primitive's default transition key unless a custom transition condition
+    is intended. Check primitive docs to confirm available primitive state keys.
+    """
+    t0 = time.time()
+    while True:
+        if utility.primitive_state_true_for_groups(
+            robot.primitive_states(),
+            transition_keys_by_group,
+        ):
+            return
+
+        if time.time() - t0 > timeout_s:
+            raise TimeoutError(
+                "Timed out waiting for primitive transition states: "
+                f"{ {flexivrdk.kJointGroupNames[g]: k for g, k in transition_keys_by_group.items()} }"
+            )
+        time.sleep(dt)
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument("robot_sn")
+    args = parser.parse_args()
+
+    logger = spdlog.ConsoleLogger("LLMGenerated")
+    try:
+        robot = flexivrdk.Robot(args.robot_sn)
+
+        if robot.fault():
+            logger.warn("Fault detected, clearing")
+            if not robot.ClearFault():
+                raise RuntimeError("Cannot clear fault")
+
+        robot.ServoOn()
+        while not robot.operational():
+            time.sleep(1)
+
+        single_arm_groups = robot.info().single_arm_groups
+        if not single_arm_groups:
+            raise RuntimeError("No single-arm joint group found on the connected robot")
+
+        robot.SwitchMode(flexivrdk.Mode.NRT_PRIMITIVE_EXECUTION)
+
+        robot.ExecutePrimitive(
+            {
+                group: flexivrdk.PrimitiveArgs("Home", {})
+                for group in single_arm_groups
+            }
+        )
+        wait_primitive_transition(robot, {group: "reachedTarget" for group in single_arm_groups})
+
+        # Add other primitive calls here.
+
+        robot.Stop()
+
+    except Exception as e:
+        logger.error(f"Unhandled exception: {e}")
+        raise
+
+
+if __name__ == "__main__":
+    main()
+```
diff --git a/thirdparty/flexiv_rdk/test/CMakeLists.txt b/thirdparty/flexiv_rdk/test/CMakeLists.txt
new file mode 100644
index 0000000..c3ee831
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/CMakeLists.txt
@@ -0,0 +1,77 @@
+cmake_minimum_required(VERSION 3.22.1)
+project(flexiv_rdk-tests)
+
+# CMake >= 3.30 warns for FindBoost behavior unless CMP0167 is explicitly set.
+if(POLICY CMP0167)
+  cmake_policy(SET CMP0167 OLD)
+endif()
+
+# Show verbose build info
+SET(CMAKE_VERBOSE_MAKEFILE ON)
+
+message("OS: ${CMAKE_SYSTEM_NAME}")
+message("Processor: ${CMAKE_SYSTEM_PROCESSOR}")
+
+# Configure build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
+endif()
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
+
+find_package(flexiv_rdk REQUIRED)
+
+# List of functionality tests
+set(FUNCTIONALITY_TESTS
+  test_dynamics_engine
+  test_repeated_instantiation
+)
+if(CMAKE_HOST_UNIX)
+  list(APPEND FUNCTIONALITY_TESTS
+    test_endurance
+    test_loop_latency
+    test_scheduler
+    test_timeliness_monitor
+  )
+endif()
+
+# Build functionality tests
+foreach(test ${FUNCTIONALITY_TESTS})
+  add_executable(${test} ${test}.cpp)
+  target_link_libraries(${test} flexiv::flexiv_rdk)
+  # C++20 required
+  set_target_properties(${test} PROPERTIES
+    CXX_STANDARD 20
+    CXX_STANDARD_REQUIRED ON)
+endforeach()
+
+
+# List of unit tests
+set(UNIT_TESTS
+  test_robot_constructor
+)
+
+# Download GTest if not found
+include(FetchContent)
+find_package(GTest CONFIG QUIET)
+if(NOT GTest_FOUND)
+  if(MSVC)
+    set(gtest_force_shared_crt ON CACHE BOOL "" FORCE)
+  endif()
+
+  FetchContent_Declare(
+    googletest
+    URL https://github.com/google/googletest/archive/refs/tags/v1.14.0.zip
+  )
+  FetchContent_MakeAvailable(googletest)
+endif()
+
+# Build unit tests
+foreach(test ${UNIT_TESTS})
+  add_executable(${test} ${test}.cpp)
+  target_link_libraries(${test} flexiv::flexiv_rdk GTest::gtest_main)
+  add_test(NAME ${test} COMMAND ${test})
+  # C++20 required
+  set_target_properties(${test} PROPERTIES
+    CXX_STANDARD 20
+    CXX_STANDARD_REQUIRED ON)
+endforeach()
diff --git a/thirdparty/flexiv_rdk/test/test_dynamics_engine.cpp b/thirdparty/flexiv_rdk/test/test_dynamics_engine.cpp
new file mode 100644
index 0000000..e4220dd
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/test_dynamics_engine.cpp
@@ -0,0 +1,256 @@
+/**
+ * @test test_dynamics_engine.cpp
+ * A test to evaluate the dynamics engine (J, M, G), with and without end-effector tool.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/model.hpp>
+#include <flexiv/rdk/tool.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+#include <Eigen/Eigen>
+
+#include <iostream>
+#include <iomanip>
+#include <thread>
+#include <chrono>
+
+namespace {
+/** J, M, G ground truth from MATLAB */
+struct GroundTruth
+{
+    /** Jacobian matrix */
+    Eigen::Matrix<double, 6, 7> J;
+    /** Mass matrix */
+    Eigen::Matrix<double, 7, 7> M;
+    /** Gravity torques */
+    Eigen::Matrix<double, 7, 1> G;
+};
+}
+
+/** Step the dynamics engine once */
+void StepDynamics(flexiv::rdk::Robot& robot, flexiv::rdk::Model& model, const GroundTruth& ref)
+{
+    // Mark timer start point
+    auto tic = std::chrono::high_resolution_clock::now();
+
+    // Update robot model in dynamics engine
+    const auto robot_states = robot.states();
+    model.Update(robot_states.at(flexiv::rdk::JointGroup::ALL).q,
+        robot_states.at(flexiv::rdk::JointGroup::ALL).dtheta);
+
+    // Get J, M, G from dynamic engine
+    Eigen::MatrixXd J = model.J("flange");
+    Eigen::MatrixXd M = model.M();
+    Eigen::VectorXd G = model.g();
+
+    // Get and print computation time
+    auto toc = std::chrono::high_resolution_clock::now();
+    auto computation_time
+        = std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
+    std::cout << "===================================================================" << std::endl;
+    std::cout << "Computation time = " << computation_time << " us" << std::endl;
+
+    // Evaluate J, M, G with true value
+    auto delta_J = J - ref.J;
+    auto delta_M = M - ref.M;
+    auto delta_G = G - ref.G;
+
+    std::cout << std::fixed << std::setprecision(5);
+    std::cout << "Difference of J between ground truth (MATLAB) and integrated dynamics engine = "
+              << std::endl
+              << delta_J << std::endl;
+    std::cout << "Norm of delta J: " << delta_J.norm() << '\n' << std::endl;
+
+    std::cout << "Difference of M between ground truth (MATLAB) and integrated dynamics engine = "
+              << std::endl
+              << delta_M << std::endl;
+    std::cout << "Norm of delta M: " << delta_M.norm() << '\n' << std::endl;
+
+    std::cout << "Difference of G between ground truth (MATLAB) and integrated dynamics engine = "
+              << std::endl
+              << delta_G.transpose() << std::endl;
+    std::cout << "Norm of delta G: " << delta_G.norm() << '\n' << std::endl;
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //==============================================================================================
+    if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    try {
+        // RDK Initialization
+        //==========================================================================================
+        // Instantiate robot interface
+        flexiv::rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        //==========================================================================================
+        robot.Home();
+
+        // Test Dynamics Engine without Tool
+        //==========================================================================================
+        spdlog::info(">>>>> Test 1: no end-effector tool <<<<<");
+
+        // Instantiate dynamics engine
+        flexiv::rdk::Model model(robot);
+
+        // Ground truth from MATLAB without robot tool
+        GroundTruth ref;
+        // clang-format off
+        ref.J <<
+        0.110000000000000,  0.078420538028673,  0.034471999940354, -0.368152340866938, -0.064278760968654,  0.136000000000000, -0.000000000000000,
+        0.687004857483100, -0.000000000000000,  0.576684003660457,  0.000000000000000,  0.033475407198662, -0.000000000000000, -0.000000000000000,
+        0.000000000000000,  0.687004857483100, -0.028925442435894, -0.417782862794537, -0.076604444311898,  0.110000000000000, -0.000000000000000,
+        0.000000000000000, -0.000000000000000,  0.642787609686539,  0.000000000000000,  0.766044443118978, -0.000000000000000,  0.000000000000000,
+                        0, -1.000000000000000, -0.000000000000000,  1.000000000000000,  0.000000000000000, -1.000000000000000,  0.000000000000000,
+        1.000000000000000, -0.000000000000000,  0.766044443118978,  0.000000000000000, -0.642787609686539, -0.000000000000000, -1.000000000000000;
+        ref.M << 
+        2.480084579964625, -0.066276150278304,  1.520475428405090, -0.082258763257690, -0.082884472612488,  0.008542255000000, -0.000900000000000,
+        -0.066276150278304,  2.778187401738267,  0.067350373889147, -1.100953424157635, -0.129191018084721,  0.165182543869134, -0.000000000000000,
+        1.520475428405090,  0.067350373889147,  1.074177611590570, -0.000410055889147, -0.043572229972025, -0.001968185110853, -0.000689439998807,
+        -0.082258763257690, -1.100953424157635, -0.000410055889147,  0.964760218577003,  0.123748338084721, -0.125985653288502,  0.000000000000000,
+        -0.082884472612488, -0.129191018084721, -0.043572229972025,  0.123748338084721,  0.038006882479315, -0.020080821084721,  0.000578508848718,
+        0.008542255000000,  0.165182543869134, -0.001968185110853, -0.125985653288502, -0.020080821084721,  0.034608170000000,                  0,
+        -0.000900000000000, -0.000000000000000, -0.000689439998807,  0.000000000000000,  0.000578508848718,                  0,  0.000900000000000;
+        ref.G << 
+        0.000000000000000, 46.598931189891374, 1.086347692836129, -19.279904969860052, -2.045058704525489,  2.300886450000000,                  0;
+        // clang-format on
+
+        // Step the dynamics engine
+        int i = 0;
+        while (++i <= 3) {
+            StepDynamics(robot, model, ref);
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+
+        // Test Dynamics Engine with Tool
+        //==========================================================================================
+        spdlog::info(">>>>> Test 2: with end-effector tool <<<<<");
+
+        // Ground truth from MATLAB with robot tool
+        // clang-format off
+        ref.M << 
+        2.916316686749461, -0.052869517013466,  1.903540434220357, -0.124348845003517, -0.041914639740668,  0.027649255000000, -0.001464000000000,
+        -0.052869517013466,  3.222619358431081,  0.053667041477633, -1.414236317529289, -0.184390078851855,  0.259867572215541, -0.000000000000000,
+        1.903540434220357,  0.053667041477633,  1.416023230140298, -0.002724223477633,  0.000093550780077,  0.001155982477633, -0.001121489064726,
+        -0.124348845003517, -1.414236317529289, -0.002724223477633,  1.287676548627496,  0.177147398851855, -0.244344118313748,  0.000000000000000,
+        -0.041914639740668, -0.184390078851855,  0.000093550780077,  0.177147398851855,  0.054219756143365, -0.038829881851855,  0.000941041060581,
+        0.027649255000000,  0.259867572215541,  0.001155982477633, -0.244344118313748, -0.038829881851855,  0.082171270000000,                  0,
+        -0.001464000000000, -0.000000000000000, -0.001121489064726,  0.000000000000000,  0.000941041060581,                  0,  0.001464000000000;
+
+        // Matlab value is the joint torque to resist gravity
+        ref.G << 
+        -0.000000000000001,  52.664497076609663,  0.830964961569619, -22.968509865473024, -2.721399343355234, 3.272076450000000,                 0;
+        // clang-format on
+
+        // Instantiate tool interface
+        flexiv::rdk::Tool tool(robot);
+
+        // Tools can only be assigned to single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Set name and parameters for the test tool
+        std::string tool_name = "DynamicsTestTool";
+        flexiv::rdk::ToolParams tool_params;
+        tool_params.mass = 0.9;
+        tool_params.CoM = {0.0, 0.0, 0.057};
+        tool_params.inertia = {2.768e-03, 3.149e-03, 5.64e-04, 0.0, 0.0, 0.0};
+        tool_params.tcp_location = {0.0, -0.207, 0.09, 0.7071068, 0.7071068, 0.0, 0.0};
+
+        // Remove any existing tool with the same name
+        if (tool.exist(tool_name)) {
+            spdlog::warn("Tool with the same name [{}] already exists, removing it now", tool_name);
+            // Switch to other tool or no tool (Flange) before removing the current tool
+            for (const auto& [group, _] : single_arm_groups) {
+                tool.Switch(group, "Flange");
+            }
+            tool.Remove(tool_name);
+        }
+
+        // Add the test tool
+        spdlog::info("Adding test tool [{}] to the robot", tool_name);
+        tool.Add(tool_name, tool_params);
+
+        // Switch to the newly added test tool, i.e. set it as the active tool
+        spdlog::info("Switching to test tool [{}]", tool_name);
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, tool_name);
+        }
+
+        // Get and print the current active tool for each joint group, should be the test tool
+        for (const auto& [group, _] : single_arm_groups) {
+            spdlog::info("[{}] Current active tool: {}", flexiv::rdk::kJointGroupNames.at(group),
+                tool.name(group));
+        }
+
+        // Reload robot + tool model using the latest data synced from the connected robot
+        model.Reload();
+
+        // Step the dynamics engine again
+        i = 0;
+        while (++i <= 3) {
+            StepDynamics(robot, model, ref);
+            std::this_thread::sleep_for(std::chrono::milliseconds(100));
+        }
+
+        // Switch to other tool or no tool (Flange) before removing the current tool
+        for (const auto& [group, _] : single_arm_groups) {
+            tool.Switch(group, "Flange");
+        }
+
+        // Clean up by removing the test tool
+        spdlog::info("Removing tool [{}]", tool_name);
+        tool.Remove(tool_name);
+
+        spdlog::info("Program finished");
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/test/test_endurance.cpp b/thirdparty/flexiv_rdk/test/test_endurance.cpp
new file mode 100644
index 0000000..058c691
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/test_endurance.cpp
@@ -0,0 +1,278 @@
+/**
+ * @test test_endurance.cpp
+ * Endurance test running Cartesian impedance control to slowly sine-sweep near home for a duration
+ * of user-specified hours. Raw data will be logged to CSV files continuously.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+using namespace flexiv;
+
+namespace {
+/** RT loop period [sec] */
+const double kLoopPeriod = 0.001;
+
+/** TCP sine-sweep amplitude [m] */
+const double kSwingAmp = 0.1;
+
+/** TCP sine-sweep frequency [Hz] */
+const double kSwingFreq = 0.025; // = 10mm/s linear velocity
+
+/** Current Cartesian-space pose (position + rotation) of robot TCP */
+std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> g_curr_tcp_pose;
+
+/** Test duration converted from user-specified hours to loop counts */
+uint64_t g_test_duration_loop_counts = 0;
+
+/** Time duration for each log file [loop counts] */
+const unsigned int kLogDurationLoopCounts = 10 * 60 * 1000; // = 10 min/file
+
+/** Data to be logged in low-priority thread */
+struct LogData
+{
+    std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> tcp_pose;
+    std::map<rdk::JointGroup, std::array<double, rdk::kCartDoF>> tcp_force;
+} g_log_data;
+
+/** Atomic signal to stop the test */
+std::atomic<bool> g_stop = {false};
+}
+
+void HighPriorityTask(rdk::Robot& robot,
+    const std::map<rdk::JointGroup, std::string>& single_arm_groups,
+    const std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>>& all_init_poses)
+{
+    // Local periodic loop counter
+    static uint64_t loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "HighPriorityTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        // Swing along Z direction
+        std::map<rdk::JointGroup, rdk::RtCartesianCmd> rt_cmds;
+        for (const auto& [group, _] : single_arm_groups) {
+            auto target_pose = g_curr_tcp_pose.at(group);
+            target_pose[2] = all_init_poses.at(group)[2]
+                             + kSwingAmp * sin(2 * M_PI * kSwingFreq * loop_counter * kLoopPeriod);
+            g_curr_tcp_pose[group] = target_pose;
+            rt_cmds[group] = rdk::RtCartesianCmd(target_pose);
+        }
+        robot.StreamCartesianMotionForce(rt_cmds);
+
+        // Save data to global buffer, not using mutex to avoid interruption on RT loop from
+        // potential priority inversion
+        for (const auto& [group, states] : robot.states()) {
+            g_log_data.tcp_pose[group] = states.tcp_pose;
+            g_log_data.tcp_force[group] = states.tcp_wrench;
+        }
+
+        // Stop after test duration has elapsed
+        if (++loop_counter > g_test_duration_loop_counts) {
+            g_stop = true;
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop = true;
+    }
+}
+
+void LowPriorityTask()
+{
+    // Low-priority task loop counter
+    uint64_t loop_counter = 0;
+
+    // Data logging CSV file
+    std::ofstream csv_file;
+
+    // CSV file name
+    std::string csv_file_name;
+
+    // CSV file counter (data during the test is divided to multiple files)
+    unsigned int file_counter = 0;
+
+    // Wait for a while for the robot states data to be available
+    std::this_thread::sleep_for(std::chrono::seconds(2));
+
+    // Use while loop to prevent this thread from return
+    while (true) {
+        // Log data at 1kHz
+        std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        loop_counter++;
+
+        // Close existing log file and create a new one periodically
+        if (loop_counter % kLogDurationLoopCounts == 0) {
+            if (csv_file.is_open()) {
+                csv_file.close();
+                spdlog::info("Saved log file: {}", csv_file_name);
+            }
+
+            // Increment log file counter
+            file_counter++;
+
+            // Create new file name using the updated counter as suffix
+            csv_file_name = "endurance_test_data_" + std::to_string(file_counter) + ".csv";
+
+            // Open new log file
+            csv_file.open(csv_file_name);
+            if (csv_file.is_open()) {
+                spdlog::info("Created new log file: {}", csv_file_name);
+            } else {
+                spdlog::error("Failed to create log file: {}", csv_file_name);
+            }
+        }
+
+        // Log data to file in CSV format
+        if (csv_file.is_open()) {
+            // Loop counter x1, then grouped TCP pose x7 and TCP wrench x6
+            csv_file << loop_counter << ",";
+            for (const auto& [group, pose] : g_log_data.tcp_pose) {
+                csv_file << rdk::kJointGroupNames.at(group) << ",";
+                for (const auto& i : pose) {
+                    csv_file << i << ",";
+                }
+                for (const auto& i : g_log_data.tcp_force.at(group)) {
+                    csv_file << i << ",";
+                }
+            }
+            // End of line
+            csv_file << '\n';
+        }
+
+        // Check if the test duration has elapsed
+        if (g_stop) {
+            spdlog::info("Test duration has elapsed, saving any open log file ...");
+            // Close log file
+            if (csv_file.is_open()) {
+                csv_file.close();
+                spdlog::info("Saved log file: {}", csv_file_name);
+            }
+            // Exit thread
+            return;
+        }
+    }
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn] [test_hours]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "    test_hours: Duration of the test, can have decimals" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //==============================================================================================
+    if (argc < 3 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // test duration in hours
+    double test_hours = std::stof(argv[2]);
+    // convert duration in hours to loop counts
+    g_test_duration_loop_counts = (uint64_t)(test_hours * 3600.0 * 1000.0);
+    spdlog::info("Test duration: {} hours = {} cycles", test_hours, g_test_duration_loop_counts);
+
+    try {
+        // RDK Initialization
+        //==========================================================================================
+        // Instantiate robot interface
+        rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Move robot to home pose
+        //==========================================================================================
+        // Direct Cartesian control can only be executed by single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        robot.Home();
+
+        // Switch mode after robot is at home
+        robot.SwitchMode(rdk::Mode::RT_CARTESIAN_MOTION_FORCE);
+
+        for (const auto& [group, _] : single_arm_groups) {
+            robot.SetForceControlAxis(
+                group, std::array<bool, rdk::kCartDoF> {false, false, false, false, false, false});
+        }
+
+        // Set initial pose to current TCP pose
+        std::map<rdk::JointGroup, std::array<double, rdk::kPoseSize>> all_init_pose;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : single_arm_groups) {
+            all_init_pose[group] = robot_states.at(group).tcp_pose;
+            g_curr_tcp_pose[group] = robot_states.at(group).tcp_pose;
+            spdlog::info("[{}] Initial TCP pose set to [position 3x1, rotation (quaternion) 4x1]: {}",
+                rdk::kJointGroupNames.at(group), rdk::utility::Arr2Str(all_init_pose.at(group)));
+        }
+
+        // Periodic Tasks
+        //==========================================================================================
+        rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(HighPriorityTask, std::ref(robot), std::cref(single_arm_groups),
+                              std::cref(all_init_pose)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Use std::thread for logging task without strict chronology
+        std::thread low_priority_thread(LowPriorityTask);
+
+        // low_priority_thread is responsible to release blocking
+        low_priority_thread.join();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/test/test_loop_latency.cpp b/thirdparty/flexiv_rdk/test/test_loop_latency.cpp
new file mode 100644
index 0000000..2dc6f2e
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/test_loop_latency.cpp
@@ -0,0 +1,175 @@
+/**
+ * @test test_loop_latency.cpp
+ * A test to benchmark RDK's loop latency, including communication, computation, etc. The
+ * workstation PC's serial port is used as reference, and the robot server's digital out port is
+ * used as test target.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+#include <string.h>
+#include <atomic>
+
+// Standard input/output definitions
+#include <stdio.h>
+// UNIX standard function definitions
+#include <unistd.h>
+// File control definitions
+#include <fcntl.h>
+// Error number definitions
+#include <errno.h>
+// POSIX terminal control definitions
+#include <termios.h>
+
+namespace {
+/** File descriptor of the serial port */
+int g_fd = 0;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+// callback function for realtime periodic task
+void PeriodicTask(flexiv::rdk::Robot& robot)
+{
+    // Loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+
+        // send signal at 1Hz
+        switch (loop_counter % 1000) {
+            case 0: {
+                spdlog::info(
+                    "Sending benchmark signal to both workstation PC's serial "
+                    "port and robot server's digital out port[0]");
+                break;
+            }
+            case 1: {
+                // signal robot server's digital out port
+                robot.SetDigitalOutputs({{0, true}});
+
+                // signal workstation PC's serial port
+                auto n = write(g_fd, "0", 1);
+                if (n < 0) {
+                    spdlog::error("Failed to write to serial port");
+                }
+
+                break;
+            }
+            case 900: {
+                // reset digital out after a few seconds
+                robot.SetDigitalOutputs({{0, false}});
+                break;
+            }
+            default:
+                break;
+        }
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn] [serial_port_name]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "    serial_port_name: /dev/ttyS0 for COM1, /dev/ttyS1 for COM2, /dev/ttyUSB0 for USB-serial converter" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //=============================================================================
+    if (argc < 3 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // serial port name
+    std::string serial_port = argv[2];
+
+    try {
+        // RDK Initialization
+        //=============================================================================
+        // Instantiate robot interface
+        flexiv::rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // Benchmark Signal
+        //=============================================================================
+        // get workstation PC's serial port ready,
+        g_fd = open(serial_port.c_str(), O_RDWR | O_NOCTTY | O_NDELAY | O_EXCL | O_CLOEXEC);
+
+        if (g_fd == -1) {
+            spdlog::error("Unable to open serial port [{}]", serial_port);
+        }
+
+        // print messages
+        spdlog::warn("Benchmark signal will be sent every 1 second");
+
+        // Periodic Tasks
+        //=============================================================================
+        flexiv::rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(
+            std::bind(PeriodicTask, std::ref(robot)), "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/test/test_repeated_instantiation.cpp b/thirdparty/flexiv_rdk/test/test_repeated_instantiation.cpp
new file mode 100644
index 0000000..01b72b6
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/test_repeated_instantiation.cpp
@@ -0,0 +1,68 @@
+/**
+ * @test test_repeated_instantiation.cpp
+ * A test to repeatedly construct and destruct rdk::Robot instance.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+
+using namespace flexiv;
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn] [test_cycles]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "    test_cycles: Number of cycles to repeat the instantiation" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 3 || rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    // Number of cycles
+    unsigned int test_cycles = std::stoi(argv[2]);
+
+    try {
+        for (size_t i = 1; i <= test_cycles; i++) {
+            spdlog::info("Test cycle: {}/{}", i, test_cycles);
+
+            // Instantiate robot interface
+            rdk::Robot robot(robot_sn);
+
+            // Servo on the robot, make sure the E-stop is released
+            spdlog::info("Servo on the robot ...");
+            robot.ServoOn();
+
+            // Wait for the robot to become operational
+            while (!robot.operational()) {
+                std::this_thread::sleep_for(std::chrono::seconds(1));
+            }
+            spdlog::info("Robot is now operational");
+        }
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/test/test_robot_constructor.cpp b/thirdparty/flexiv_rdk/test/test_robot_constructor.cpp
new file mode 100644
index 0000000..4d181cb
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/test_robot_constructor.cpp
@@ -0,0 +1,56 @@
+/**
+ * @test test_robot_constructor.cpp
+ * @brief Test offline construction of flexivrdk.Robot.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ */
+
+#include <flexiv/rdk/robot.hpp>
+
+#include <gtest/gtest.h>
+
+#include <exception>
+#include <string>
+#include <string_view>
+
+namespace {
+
+template <typename ExceptionT>
+void ExpectRobotConstructorFailure(
+    const std::string& robot_sn, std::string_view expected_message_fragment)
+{
+    try {
+        flexiv::rdk::Robot robot(robot_sn, false);
+        FAIL() << "Expected constructor to fail for serial [" << robot_sn << "], but it succeeded";
+    } catch (const ExceptionT& e) {
+        const std::string_view message(e.what());
+        EXPECT_NE(message.find(expected_message_fragment), std::string_view::npos)
+            << "Constructor for serial [" << robot_sn
+            << "] threw the expected exception type but with unexpected message: " << e.what();
+    } catch (const std::exception& e) {
+        FAIL() << "Constructor for serial [" << robot_sn
+               << "] threw an unexpected exception type: " << e.what();
+    }
+}
+
+} // namespace
+
+TEST(RobotConstructor, InvalidSerialNumberFormat)
+{
+    ExpectRobotConstructorFailure<std::invalid_argument>("Enlight412345", "serial number is invalid");
+}
+
+TEST(RobotConstructor, UnsupportedProductModel)
+{
+    for (const std::string robot_sn : {"Enlight-X-123456", "Enlight X-123456"}) {
+        SCOPED_TRACE(robot_sn);
+        ExpectRobotConstructorFailure<std::logic_error>(robot_sn, "is not supported");
+    }
+}
+
+TEST(RobotConstructor, SupportedModelWithoutServer)
+{
+    for (const std::string robot_sn : {"Enlight-L-123456", "Enlight L-123456"}) {
+        SCOPED_TRACE(robot_sn);
+        ExpectRobotConstructorFailure<std::runtime_error>(robot_sn, "Failed to connect");
+    }
+}
\ No newline at end of file
diff --git a/thirdparty/flexiv_rdk/test/test_robot_constructor.py b/thirdparty/flexiv_rdk/test/test_robot_constructor.py
new file mode 100644
index 0000000..d176841
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/test_robot_constructor.py
@@ -0,0 +1,37 @@
+"""Test offline construction of flexivrdk.Robot"""
+
+import unittest
+import flexivrdk
+
+
+class RobotConstructorTest(unittest.TestCase):
+    def assert_robot_constructor_failure(
+        self, robot_sn, expected_exception, message_fragment
+    ):
+        with self.assertRaises(expected_exception) as context:
+            flexivrdk.Robot(robot_sn, False)
+
+        self.assertIn(message_fragment, str(context.exception))
+
+    def test_invalid_format(self):
+        self.assert_robot_constructor_failure(
+            "Enlight412345", ValueError, "serial number is invalid"
+        )
+
+    def test_unsupported_model(self):
+        for robot_sn in ("Enlight-X-123456", "Enlight X-123456"):
+            with self.subTest(robot_sn=robot_sn):
+                self.assert_robot_constructor_failure(
+                    robot_sn, RuntimeError, "is not supported"
+                )
+
+    def test_supported_model_without_server(self):
+        for robot_sn in ("Enlight-L-123456", "Enlight L-123456"):
+            with self.subTest(robot_sn=robot_sn):
+                self.assert_robot_constructor_failure(
+                    robot_sn, RuntimeError, "Failed to connect"
+                )
+
+
+if __name__ == "__main__":
+    unittest.main()
diff --git a/thirdparty/flexiv_rdk/test/test_scheduler.cpp b/thirdparty/flexiv_rdk/test/test_scheduler.cpp
new file mode 100644
index 0000000..88277ec
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/test_scheduler.cpp
@@ -0,0 +1,148 @@
+/**
+ * @test test_scheduler.cpp
+ * A test to evaluate RDK's internal real-time scheduler.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <thread>
+#include <chrono>
+#include <mutex>
+#include <atomic>
+
+namespace {
+/** Data shared between threads */
+struct SharedData
+{
+    int64_t measured_interval = 0;
+} g_data;
+
+/** Mutex on the shared data */
+std::mutex g_mutex;
+
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+/** User-defined high-priority periodic task @ 1kHz */
+void HighPriorityTask()
+{
+    static unsigned int loop_counter = 0;
+
+    // Scheduler loop interval start time point
+    static std::chrono::high_resolution_clock::time_point tic;
+
+    try {
+        // Mark loop interval end point
+        auto toc = std::chrono::high_resolution_clock::now();
+
+        // Calculate scheduler's interrupt interval and print
+        auto measured_interval
+            = std::chrono::duration_cast<std::chrono::microseconds>(toc - tic).count();
+
+        // Safely write shared data
+        {
+            std::lock_guard<std::mutex> lock(g_mutex);
+            g_data.measured_interval = measured_interval;
+        }
+
+        // Stop scheduler after 5 seconds
+        if (++loop_counter > 5000) {
+            loop_counter = 0;
+            g_stop_sched = true;
+        }
+
+        // Mark loop interval start point
+        tic = std::chrono::high_resolution_clock::now();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+/** User-defined low-priority periodic task @1Hz */
+void LowPriorityTask()
+{
+    static uint64_t accumulated_time = 0;
+    static uint64_t num_measures = 0;
+    static float avg_interval = 0.0;
+    int measured_interval = 0;
+
+    // Safely read shared data
+    {
+        std::lock_guard<std::mutex> lock(g_mutex);
+        measured_interval = g_data.measured_interval;
+    }
+
+    // calculate average time interval
+    accumulated_time += measured_interval;
+    num_measures++;
+    avg_interval = (float)accumulated_time / (float)num_measures;
+
+    // print time interval of high-priority periodic task
+    spdlog::info(
+        "High-priority task interval (curr | avg) = {} | {} us", measured_interval, avg_interval);
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: None" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //==============================================================================================
+    if (flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    try {
+        // Periodic Tasks
+        //==========================================================================================
+        flexiv::rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(HighPriorityTask), "HP periodic", 1, scheduler.max_priority());
+        // Add periodic task with 1s interval and lowest applicable priority
+        scheduler.AddTask(std::bind(LowPriorityTask), "LP periodic", 1000, 0);
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+        // Restart scheduler after 2 seconds
+        spdlog::warn("Scheduler will restart in 2 seconds");
+        std::this_thread::sleep_for(std::chrono::seconds(2));
+        g_stop_sched = false;
+        scheduler.Start();
+
+        // Wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks, flexiv::rdk::Scheduler's destructor can also do
+        // the thread exit and resources cleanup
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/test/test_timeliness_monitor.cpp b/thirdparty/flexiv_rdk/test/test_timeliness_monitor.cpp
new file mode 100644
index 0000000..b98ad24
--- /dev/null
+++ b/thirdparty/flexiv_rdk/test/test_timeliness_monitor.cpp
@@ -0,0 +1,160 @@
+/**
+ * @test test_timeliness_monitor.cpp
+ * A test to evaluate RDK's internal timeliness monitor on real-time modes. Bad communication or
+ * insufficient real-time performance of the workstation PC will cause the monitor's timeliness
+ * check to fail. A warning will be issued first, then if the check has failed too many times, the
+ * RDK connection with the server will be closed. During this test, the robot will hold its position
+ * using joint torque streaming mode.
+ * @copyright Copyright (C) 2016-2026 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/rdk/robot.hpp>
+#include <flexiv/rdk/scheduler.hpp>
+#include <flexiv/rdk/utility.hpp>
+#include <spdlog/spdlog.h>
+
+#include <iostream>
+#include <string>
+#include <cmath>
+#include <thread>
+#include <atomic>
+
+namespace {
+/** Atomic signal to stop scheduler tasks */
+std::atomic<bool> g_stop_sched = {false};
+}
+
+// callback function for realtime periodic task
+void PeriodicTask(flexiv::rdk::Robot& robot,
+    const std::map<flexiv::rdk::JointGroup, std::string>& single_arm_groups,
+    const std::map<flexiv::rdk::JointGroup, std::vector<double>>& all_init_pos)
+{
+    // Loop counter
+    static unsigned int loop_counter = 0;
+
+    try {
+        // Monitor fault on the connected robot
+        if (robot.fault()) {
+            throw std::runtime_error(
+                "PeriodicTask: Fault occurred on the connected robot, exiting ...");
+        }
+        // Hold position
+        std::map<flexiv::rdk::JointGroup, flexiv::rdk::RtJointPositionCmd> rt_cmds;
+        for (const auto& [group, _] : single_arm_groups) {
+            const auto& init_pos = all_init_pos.at(group);
+            std::vector<double> target_vel(init_pos.size());
+            std::vector<double> target_acc(init_pos.size());
+            rt_cmds[group] = flexiv::rdk::RtJointPositionCmd(init_pos, target_vel, target_acc);
+        }
+        robot.StreamJointPosition(rt_cmds);
+
+        if (loop_counter == 5000) {
+            spdlog::warn(">>>>> Adding simulated loop delay <<<<<");
+        }
+        // simulate prolonged loop time after 5 seconds
+        else if (loop_counter > 5000) {
+            std::this_thread::sleep_for(std::chrono::microseconds(995));
+        }
+
+        loop_counter++;
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        g_stop_sched = true;
+    }
+}
+
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot to connect. Remove any space, e.g. Enlight-L-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+int main(int argc, char* argv[])
+{
+    // Parse Parameters
+    //==============================================================================================
+    if (argc < 2 || flexiv::rdk::utility::ProgramArgsExistAny(argc, argv, {"-h", "--help"})) {
+        PrintHelp();
+        return 1;
+    }
+
+    // Serial number of the robot to connect to
+    std::string robot_sn = argv[1];
+
+    try {
+        // RDK Initialization
+        //==========================================================================================
+        // Instantiate robot interface
+        flexiv::rdk::Robot robot(robot_sn);
+
+        // Clear fault on the connected robot if any
+        if (robot.fault()) {
+            spdlog::warn("Fault occurred on the connected robot, trying to clear ...");
+            // Try to clear the fault
+            if (!robot.ClearFault()) {
+                spdlog::error("Fault cannot be cleared, exiting ...");
+                return 1;
+            }
+            spdlog::info("Fault on the connected robot is cleared");
+        }
+
+        // Servo on the robot, make sure the E-stop is released
+        spdlog::info("Servo on the robot ...");
+        robot.ServoOn();
+
+        // Wait for the robot to become operational
+        while (!robot.operational()) {
+            std::this_thread::sleep_for(std::chrono::seconds(1));
+        }
+        spdlog::info("Robot is now operational");
+
+        // set mode after robot is operational
+        robot.SwitchMode(flexiv::rdk::Mode::RT_JOINT_POSITION);
+
+        // Direct joint control can only be executed by single-arm joint groups
+        const auto& single_arm_groups = robot.info().single_arm_groups;
+        if (single_arm_groups.empty()) {
+            throw std::runtime_error("No single-arm joint group found on the connected robot");
+        }
+
+        // Set initial joint positions
+        std::map<flexiv::rdk::JointGroup, std::vector<double>> all_init_pos;
+        const auto robot_states = robot.states();
+        for (const auto& [group, _] : single_arm_groups) {
+            all_init_pos[group] = robot_states.at(group).q;
+            spdlog::info("[{}] Initial joint positions: {}",
+                flexiv::rdk::kJointGroupNames.at(group),
+                flexiv::rdk::utility::Vec2Str(all_init_pos.at(group)));
+        }
+        spdlog::warn(">>>>> Simulated loop delay will be added after 5 seconds <<<<<");
+
+        // Periodic Tasks
+        //==========================================================================================
+        flexiv::rdk::Scheduler scheduler;
+        // Add periodic task with 1ms interval and highest applicable priority
+        scheduler.AddTask(std::bind(PeriodicTask, std::ref(robot), std::cref(single_arm_groups),
+                              std::cref(all_init_pos)),
+            "HP periodic", 1, scheduler.max_priority());
+        // Start all added tasks
+        scheduler.Start();
+
+        // Block and wait for signal to stop scheduler tasks
+        while (!g_stop_sched) {
+            std::this_thread::sleep_for(std::chrono::milliseconds(1));
+        }
+        // Received signal to stop scheduler tasks
+        scheduler.Stop();
+
+    } catch (const std::exception& e) {
+        spdlog::error(e.what());
+        return 1;
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_rdk/thirdparty/build_and_install_dependencies.sh b/thirdparty/flexiv_rdk/thirdparty/build_and_install_dependencies.sh
new file mode 100644
index 0000000..05b0c79
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/build_and_install_dependencies.sh
@@ -0,0 +1,77 @@
+#!/usr/bin/env bash
+# Build and install all dependencies of flexiv_rdk.
+echo ">>>>> Start: flexiv_rdk/thirdparty/build_and_install_dependencies.sh <<<<<"
+
+# Absolute path of this script
+script_dir="$(dirname $(readlink -f $0))"
+set -e
+
+# Check script arguments
+if [ "$#" -lt 1 ]; then
+    echo "Error: invalid script argument"
+    echo "Required argument: [install_directory_path]"
+    echo "    install_directory_path: directory to install all dependencies, should be the same as the install directory of flexiv_rdk"
+    echo "Optional arguments: [num_parallel_jobs] [qnx_toolchain_path]"
+    echo "    num_parallel_jobs: number of parallel jobs used to build, use 4 if not specified"
+    echo "    qnx_toolchain_path: path to the QNX toolchain file under cmake/qnx/, required if building for QNX"
+    exit
+fi
+
+# Get dependencies install directory from script argument, should be the same as the install directory of flexiv_rdk
+export INSTALL_DIR=$1
+echo "Dependencies will be installed to: $INSTALL_DIR"
+
+# Use specified number for parallel build jobs, otherwise use 4
+if [ -n "$2" ] ;then
+    export NUM_JOBS=$2
+else
+    export NUM_JOBS=4
+fi
+echo "Number of parallel build jobs: $NUM_JOBS"
+
+# Set shared cmake arguments
+SHARED_CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Release \
+                   -DBUILD_SHARED_LIBS=ON \
+                   -DCMAKE_POSITION_INDEPENDENT_CODE=ON \
+                   -DCMAKE_PREFIX_PATH=$INSTALL_DIR \
+                   -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \
+                   -DBUILD_TESTING=OFF"
+
+# Building for QNX
+if [ -n "$QNX_TARGET" ]; then
+    # Path to the toolchain file must be set
+    if [ -n "$3" ]; then
+        QNX_TOOLCHAIN=$3
+    else
+        echo "Error: path to QNX toolchain file is not provided"
+        exit 1
+    fi
+    # Append toolchain file to cmake arguments
+    SHARED_CMAKE_ARGS="$SHARED_CMAKE_ARGS -DCMAKE_TOOLCHAIN_FILE=$QNX_TOOLCHAIN"
+    # Extract QNX target architecture from toolchain file name, and set it as an environment variable for later use
+    if [[ "$QNX_TOOLCHAIN" == *"x86_64"* ]]; then
+        export QNX_ARCH="x86_64"
+    elif [[ "$QNX_TOOLCHAIN" == *"aarch64"* ]]; then
+        export QNX_ARCH="aarch64"
+    else
+        echo "Error: cannot determine QNX target architecture"
+        exit 1
+    fi
+    echo "Building for QNX $QNX_ARCH target with toolchain file [$QNX_TOOLCHAIN]"
+fi
+export SHARED_CMAKE_ARGS
+
+# Clone all dependencies in a subfolder
+mkdir -p cloned && cd cloned
+
+# Build and install all dependencies to INSTALL_DIR
+bash $script_dir/scripts/install_eigen.sh
+bash $script_dir/scripts/install_spdlog.sh
+bash $script_dir/scripts/install_tinyxml2.sh
+bash $script_dir/scripts/install_yaml-cpp.sh
+bash $script_dir/scripts/install_zenoh.sh
+bash $script_dir/scripts/install_boost.sh
+bash $script_dir/scripts/install_SpaceVecAlg.sh
+bash $script_dir/scripts/install_RBDyn.sh
+
+echo ">>>>> Finished: flexiv_rdk/thirdparty/build_and_install_dependencies.sh <<<<<"
diff --git a/thirdparty/flexiv_rdk/thirdparty/patches/eigen_qnx802.patch b/thirdparty/flexiv_rdk/thirdparty/patches/eigen_qnx802.patch
new file mode 100644
index 0000000..19e6274
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/patches/eigen_qnx802.patch
@@ -0,0 +1,23 @@
+diff --git a/CMakeLists.txt b/CMakeLists.txt
+index f3e69b845..fafcdc3a4 100644
+--- a/CMakeLists.txt
++++ b/CMakeLists.txt
+@@ -76,18 +76,6 @@ endmacro()
+ 
+ check_cxx_compiler_flag("-std=c++11" EIGEN_COMPILER_SUPPORT_CPP11)
+ 
+-if(EIGEN_TEST_CXX11)
+-  set(CMAKE_CXX_STANDARD 11)
+-  set(CMAKE_CXX_EXTENSIONS OFF)
+-  if(EIGEN_COMPILER_SUPPORT_CPP11)
+-    set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
+-  endif()
+-else()
+-  #set(CMAKE_CXX_STANDARD 03)
+-  #set(CMAKE_CXX_EXTENSIONS OFF)
+-  ei_add_cxx_compiler_flag("-std=c++03")
+-endif()
+-
+ # Determine if we should build shared libraries on this platform.
+ get_cmake_property(EIGEN_BUILD_SHARED_LIBS TARGET_SUPPORTS_SHARED_LIBS)
+ 
diff --git a/thirdparty/flexiv_rdk/thirdparty/scripts/install_RBDyn.sh b/thirdparty/flexiv_rdk/thirdparty/scripts/install_RBDyn.sh
new file mode 100644
index 0000000..7146ca2
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/scripts/install_RBDyn.sh
@@ -0,0 +1,29 @@
+#!/usr/bin/env bash
+# Depends on: tinyxml2, yaml-cpp, Eigen3, SpaceVecAlg
+set -e
+repo="RBDyn"
+echo "Installing $repo"
+
+# https://github.com/jrl-umi3218/RBDyn/releases/tag/v1.9.3
+ver_tag=v1.9.3
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone --recurse-submodules https://github.com/jrl-umi3218/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS \
+         -DPYTHON_BINDING=OFF \
+         -DBoost_USE_STATIC_LIBS=OFF \
+         -DCXX_DISABLE_WERROR=ON
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"
diff --git a/thirdparty/flexiv_rdk/thirdparty/scripts/install_SpaceVecAlg.sh b/thirdparty/flexiv_rdk/thirdparty/scripts/install_SpaceVecAlg.sh
new file mode 100644
index 0000000..4521ab5
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/scripts/install_SpaceVecAlg.sh
@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+set -e
+repo="SpaceVecAlg"
+echo "Installing $repo"
+
+# https://github.com/jrl-umi3218/SpaceVecAlg/releases/tag/v1.2.8
+ver_tag=v1.2.8
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone --recurse-submodules https://github.com/jrl-umi3218/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS -DPYTHON_BINDING=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"
diff --git a/thirdparty/flexiv_rdk/thirdparty/scripts/install_boost.sh b/thirdparty/flexiv_rdk/thirdparty/scripts/install_boost.sh
new file mode 100644
index 0000000..e9b71ca
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/scripts/install_boost.sh
@@ -0,0 +1,66 @@
+#!/usr/bin/env bash
+set -e
+repo="boost"
+echo "Installing $repo"
+
+# Function to check if running on Windows
+is_windows() {
+  [[ "$OSTYPE" == "msys"* || "$OSTYPE" == "cygwin"* || "$OSTYPE" == "mingw"* || "$OSTYPE" == "win32"* ]]
+}
+
+# Default version on Ubuntu 24.04
+# Also the minimum version compatible with MSVC v143
+ver_tag=1.83.0
+ver_str=1_83_0
+
+# Download source code, 1.83 
+if [ ! -d ${repo}_${ver_str} ] ; then
+  # download is faster than clone
+  URL="https://archives.boost.io/release/$ver_tag/source/${repo}_${ver_str}.tar.bz2"
+  echo "-- Downloading: $URL"
+  if is_windows; then
+    curl -L -o ${repo}_${ver_str}.tar.bz2 $URL
+  else
+    wget $URL --no-clobber --quiet --show-progress --progress=bar:force 2>&1
+  fi
+  # Unzip
+  echo "-- Extracting: ${repo}_${ver_str}.tar.bz2"
+  tar --bzip2 -xf "${repo}_${ver_str}.tar.bz2"
+  cd ${repo}_${ver_str}
+else
+  cd ${repo}_${ver_str}
+fi
+
+# Bootstrap differently on Unix and Windows
+if is_windows; then
+  # Windows
+  ./bootstrap.bat
+else
+  # Unix
+  ./bootstrap.sh
+fi
+
+# Build differently on QNX and non-QNX
+if [ -n "$QNX_TARGET" ]; then
+  if [ $QNX_ARCH == "x86_64" ]; then
+    # Build for x86_64 target
+    echo "Building $repo for QNX x86_64"
+    ./b2 -j$NUM_JOBS --prefix=$INSTALL_DIR --with-system --with-filesystem variant=release link=shared \
+    toolset=qcc target-os=qnx architecture=x86 address-model=64 \
+    cxxflags="-Vgcc_ntox86_64" linkflags="-Vgcc_ntox86_64" install
+  elif [ $QNX_ARCH == "aarch64" ]; then
+    # Build for aarch64 target
+    echo "Building $repo for QNX aarch64"
+    ./b2 -j$NUM_JOBS --prefix=$INSTALL_DIR --with-system --with-filesystem variant=release link=shared \
+    toolset=qcc target-os=qnx architecture=arm address-model=64 \
+    cxxflags="-Vgcc_ntoaarch64le" linkflags="-Vgcc_ntoaarch64le" install
+  else
+    echo "Error: unknown QNX platform architecture"
+    exit 1
+  fi
+else
+  # Build for non-QNX
+  ./b2 -j$NUM_JOBS --prefix=$INSTALL_DIR --with-system --with-filesystem variant=release link=shared install
+fi
+
+echo "Installed $repo"
diff --git a/thirdparty/flexiv_rdk/thirdparty/scripts/install_eigen.sh b/thirdparty/flexiv_rdk/thirdparty/scripts/install_eigen.sh
new file mode 100644
index 0000000..b5326c3
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/scripts/install_eigen.sh
@@ -0,0 +1,32 @@
+#!/usr/bin/env bash
+set -e
+repo="eigen"
+echo "Installing $repo"
+script_dir="$(dirname $(readlink -f $0))"
+
+# Default version on Ubuntu 22.04
+ver_tag=3.4.0
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone https://gitlab.com/libeigen/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Apply patch if building for QNX
+git reset --hard
+if [ -n "$QNX_TARGET" ]; then
+  git apply $script_dir/../patches/eigen_qnx802.patch
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"
diff --git a/thirdparty/flexiv_rdk/thirdparty/scripts/install_spdlog.sh b/thirdparty/flexiv_rdk/thirdparty/scripts/install_spdlog.sh
new file mode 100644
index 0000000..f0525e0
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/scripts/install_spdlog.sh
@@ -0,0 +1,27 @@
+#!/usr/bin/env bash
+set -e
+repo="spdlog"
+echo "Installing $repo"
+
+# Default version on Ubuntu 22.04
+ver_tag=v1.9.2
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone https://github.com/gabime/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS \
+         -DSPDLOG_BUILD_EXAMPLE=OFF \
+         -DSPDLOG_BUILD_TESTS=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"
diff --git a/thirdparty/flexiv_rdk/thirdparty/scripts/install_tinyxml2.sh b/thirdparty/flexiv_rdk/thirdparty/scripts/install_tinyxml2.sh
new file mode 100644
index 0000000..8407767
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/scripts/install_tinyxml2.sh
@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+set -e
+repo="tinyxml2"
+echo "Installing $repo"
+
+# Default version on Ubuntu 22.04
+ver_tag=9.0.0
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone https://github.com/leethomason/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"
diff --git a/thirdparty/flexiv_rdk/thirdparty/scripts/install_yaml-cpp.sh b/thirdparty/flexiv_rdk/thirdparty/scripts/install_yaml-cpp.sh
new file mode 100644
index 0000000..ac1eae8
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/scripts/install_yaml-cpp.sh
@@ -0,0 +1,25 @@
+#!/usr/bin/env bash
+set -e
+repo="yaml-cpp"
+echo "Installing $repo"
+
+# Default version on Ubuntu 22.04
+ver_tag=yaml-cpp-0.7.0
+
+# Clone source code
+if [ ! -d $repo ] ; then
+  git clone https://github.com/jbeder/$repo.git --branch $ver_tag
+  cd $repo
+else
+  cd $repo
+  git checkout $ver_tag
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS -DYAML_CPP_BUILD_TESTS=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed $repo"
diff --git a/thirdparty/flexiv_rdk/thirdparty/scripts/install_zenoh.sh b/thirdparty/flexiv_rdk/thirdparty/scripts/install_zenoh.sh
new file mode 100644
index 0000000..bc8d3bd
--- /dev/null
+++ b/thirdparty/flexiv_rdk/thirdparty/scripts/install_zenoh.sh
@@ -0,0 +1,141 @@
+#!/usr/bin/env bash
+set -e
+
+# Version for both zenoh-c and zenoh-cpp
+ver_tag="1.9.0"
+
+# Root directory of the script
+root_dir="$(pwd)"
+
+is_windows() {
+  [[ "$OSTYPE" == "msys"* || "$OSTYPE" == "cygwin"* || "$OSTYPE" == "mingw"* || "$OSTYPE" == "win32"* ]]
+}
+
+# Install rust toolchain if cargo is missing.
+ensure_cargo() {
+  # 1) In Windows shells, cargo may be installed under USERPROFILE.
+  # Prepend that location so command lookup can find cargo/rustup immediately.
+  if is_windows && [ -n "$USERPROFILE" ] && command -v cygpath >/dev/null 2>&1; then
+    export PATH="$(cygpath "$USERPROFILE")/.cargo/bin:$PATH"
+  fi
+
+  # 2) Load rustup's env script when present so current shell gets updated PATH.
+  if [ -f "$HOME/.cargo/env" ]; then
+    # shellcheck disable=SC1090
+    source "$HOME/.cargo/env"
+  fi
+
+  # 3) Fast path: cargo already available, nothing else to do.
+  if command -v cargo >/dev/null 2>&1; then
+    echo "Found cargo: $(command -v cargo)"
+    return
+  fi
+
+  echo "cargo is not found, installing rustup and cargo"
+
+  if ! command -v curl >/dev/null 2>&1; then
+    echo "Error: curl is required to install cargo via rustup"
+    exit 1
+  fi
+
+  # 4) Install rustup/cargo non-interactively.
+  curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh -s -- -y
+
+  # 5) Re-load shell env generated by rustup in this same process.
+  if [ -f "$HOME/.cargo/env" ]; then
+    # shellcheck disable=SC1090
+    source "$HOME/.cargo/env"
+  fi
+
+  # 6) Add common cargo bin paths explicitly to handle shell/env variations.
+  export PATH="$HOME/.cargo/bin:$PATH"
+  if is_windows && [ -n "$USERPROFILE" ] && command -v cygpath >/dev/null 2>&1; then
+    export PATH="$(cygpath "$USERPROFILE")/.cargo/bin:$PATH"
+  fi
+
+  # 7) Validate installation before continuing build steps.
+  if ! command -v cargo >/dev/null 2>&1; then
+    echo "Error: cargo installation failed"
+    exit 1
+  fi
+
+  echo "Installed cargo: $(command -v cargo)"
+}
+
+# For Windows builds, force rustup/cargo to use the MSVC toolchain/target.
+# This avoids GNU-target requirements (for example dlltool.exe) during zenoh build.
+configure_rust_windows_toolchain() {
+  if ! is_windows; then
+    return
+  fi
+
+  local rust_msvc_toolchain="stable-x86_64-pc-windows-msvc"
+  export RUSTUP_TOOLCHAIN="$rust_msvc_toolchain"
+  export CARGO_BUILD_TARGET="x86_64-pc-windows-msvc"
+
+  if command -v rustup >/dev/null 2>&1; then
+    rustup toolchain install "$rust_msvc_toolchain" >/dev/null
+    if command -v rustup.exe >/dev/null 2>&1; then
+      rustup.exe toolchain install "$rust_msvc_toolchain" >/dev/null
+    fi
+  else
+    echo "Warning: rustup not found in PATH; cannot enforce MSVC Rust toolchain"
+  fi
+}
+
+# Install zenoh-c backend
+install_zenoh_c() {
+  local repo="zenoh-c"
+  echo "Installing $repo ($ver_tag)"
+  cd "$root_dir"
+
+  if [ ! -d "$repo" ]; then
+    git clone https://github.com/eclipse-zenoh/$repo.git --branch $ver_tag
+    cd "$repo"
+  else
+    cd "$repo"
+    git fetch --all
+    git checkout $ver_tag
+  fi
+
+  mkdir -p build && cd build
+  cmake .. $SHARED_CMAKE_ARGS \
+           -DBUILD_SHARED_LIBS=ON \
+           -DZENOHC_BUILD_WITH_UNSTABLE_API=ON
+
+  cmake --build . --target install --config Release -j $NUM_JOBS
+
+  echo "Installed $repo"
+}
+
+# Install zenoh-cpp aligned to the same version as zenoh-c
+install_zenoh_cpp() {
+  local repo="zenoh-cpp"
+  echo "Installing $repo ($ver_tag)"
+  cd "$root_dir"
+
+  if [ ! -d "$repo" ]; then
+    git clone https://github.com/eclipse-zenoh/$repo.git --branch $ver_tag
+    cd "$repo"
+  else
+    cd "$repo"
+    git fetch --all
+    git checkout $ver_tag
+  fi
+
+  mkdir -p build && cd build
+  cmake .. $SHARED_CMAKE_ARGS \
+           -DBUILD_SHARED_LIBS=ON \
+           -DZENOHCXX_ENABLE_TESTS=OFF \
+           -DZENOHCXX_ENABLE_EXAMPLES=OFF \
+           -DZENOHCXX_EXAMPLES_PROTOBUF=OFF
+
+  cmake --build . --target install --config Release -j $NUM_JOBS
+
+  echo "Installed $repo"
+}
+
+ensure_cargo
+configure_rust_windows_toolchain
+install_zenoh_c
+install_zenoh_cpp
diff --git a/thirdparty/flexiv_sim_plugin b/thirdparty/flexiv_sim_plugin
new file mode 120000
index 0000000..1e3e7f4
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin
@@ -0,0 +1 @@
+/home/cc/文档/Flexiv_Rizon4/flexiv_sim_plugin
\ No newline at end of file
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/.clang-format b/thirdparty/flexiv_sim_plugin.cloned.bak/.clang-format
new file mode 100644
index 0000000..8b12f8e
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/.clang-format
@@ -0,0 +1,65 @@
+---
+Language: Proto
+BasedOnStyle: Google
+---
+Language: Cpp
+AccessModifierOffset: -4
+AlignAfterOpenBracket: false
+AlignConsecutiveAssignments: false
+AlignConsecutiveDeclarations: false
+AlignEscapedNewlinesLeft: false
+AlignOperands: true
+AlignTrailingComments: true
+AllowAllParametersOfDeclarationOnNextLine: false
+AllowShortBlocksOnASingleLine: false
+AllowShortCaseLabelsOnASingleLine: false
+AllowShortFunctionsOnASingleLine: Inline
+AllowShortIfStatementsOnASingleLine: false
+AllowShortLoopsOnASingleLine: false
+AlwaysBreakAfterDefinitionReturnType: None
+AlwaysBreakAfterReturnType: None
+AlwaysBreakBeforeMultilineStrings: true
+AlwaysBreakTemplateDeclarations: true
+BasedOnStyle: WebKit
+BinPackArguments: true
+BinPackParameters: true
+BreakBeforeBraces: Mozilla
+BreakBeforeTernaryOperators: true
+BreakConstructorInitializersBeforeComma: true
+ColumnLimit: 100
+CommentPragmas: "^ IWYU pragma:"
+ConstructorInitializerAllOnOneLineOrOnePerLine: false
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 4
+Cpp11BracedListStyle: true
+DerivePointerAlignment: false
+DisableFormat: false
+ExperimentalAutoDetectBinPacking: false
+ForEachMacros: [foreach, Q_FOREACH, BOOST_FOREACH]
+IndentCaseLabels: true
+IndentWidth: 4
+IndentWrappedFunctionNames: false
+KeepEmptyLinesAtTheStartOfBlocks: true
+MacroBlockBegin: ""
+MacroBlockEnd: ""
+MaxEmptyLinesToKeep: 1
+NamespaceIndentation: None
+PenaltyBreakBeforeFirstCallParameter: 100
+PenaltyBreakComment: 300
+PenaltyBreakFirstLessLess: 120
+PenaltyBreakString: 1000
+PenaltyExcessCharacter: 10000
+PointerAlignment: Left
+ReflowComments: true
+SortIncludes: false
+SpaceAfterCStyleCast: false
+SpaceBeforeAssignmentOperators: true
+SpaceBeforeParens: ControlStatements
+SpaceInEmptyParentheses: false
+SpacesInContainerLiterals: true
+SpacesInCStyleCastParentheses: false
+SpacesInParentheses: false
+SpacesInSquareBrackets: false
+Standard: Cpp11
+TabWidth: 4
+UseTab: Never
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/bug_report.md b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/bug_report.md
new file mode 100644
index 0000000..3e618b3
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/bug_report.md
@@ -0,0 +1,38 @@
+---
+name: Bug Report
+about: Report a bug to help us improve
+title: "[BUG] "
+type: Bug
+labels: bug
+assignees: bsoe-flexiv
+
+---
+
+## Version info
+
+- Sim Plugin: [e.g. v1.2]
+- OS: [e.g. Ubuntu 22.04, x86_64]
+
+## How urgent is this bug? Have you found any workarounds?
+
+On a scale of 1 to 10.
+
+## Describe the issue and expected behavior
+
+The issue is ...
+The correct behavior you expect is  ...
+
+## Steps to reproduce
+
+1. '....'
+2. '....'
+3. '....'
+4. See error
+
+## Screenshots
+
+If applicable, add screenshots to help explain your issue.
+
+## Additional context
+
+Add any other context about the issue.
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/feature_request.md b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/feature_request.md
new file mode 100644
index 0000000..dac2d5a
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/feature_request.md
@@ -0,0 +1,29 @@
+---
+name: Feature Request
+about: Suggest a new feature
+title: "[FEATURE] "
+type: Feature
+labels: enhancement
+assignees: bsoe-flexiv
+
+---
+
+## How urgent is this feature?
+
+On a scale of 1 to 10.
+
+## Is this feature related to a problem?
+
+This new feature is needed so that ... can be solved.
+
+## Describe the solution you'd like
+
+What you want to happen.
+
+## Describe alternatives you've considered
+
+Any alternative solutions or workarounds you've considered.
+
+## Additional context
+
+Add any other context or screenshots about this feature.
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/question.md b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/question.md
new file mode 100644
index 0000000..7a0c332
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/ISSUE_TEMPLATE/question.md
@@ -0,0 +1,25 @@
+---
+name: Question
+about: Ask a question related to Sim Plugin
+title: "[QUESTION] "
+type: Question
+labels: help wanted
+assignees: bsoe-flexiv
+
+---
+
+## There might already be an answer to your question
+
+Someone might have asked similar questions before, you can search in the [Issues](https://github.com/flexivrobotics/flexiv_sim_plugin/issues?q=type%3AQuestion) tab.
+
+## Not sure how to get started?
+
+You can take a look at [Isaac Sim Workspace](https://github.com/flexivrobotics/isaac_sim_ws) which uses this plugin to build the simulation workflow.
+
+## Is this related to a specific version of Sim Plugin?
+
+If yes, please specify.
+
+## Please describe your question
+
+Texts, screenshots, links, code blocks ...
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/.github/workflows/cmake.yml b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/workflows/cmake.yml
new file mode 100644
index 0000000..f268a8c
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/workflows/cmake.yml
@@ -0,0 +1,21 @@
+name: CMake
+
+on:
+  push:
+    branches: [main]
+  pull_request:
+    branches: [main]
+  workflow_dispatch:
+
+env:
+  # Customize the CMake build type here (Release, Debug, RelWithDebInfo, etc.)
+  BUILD_TYPE: Release
+
+jobs:
+  build-ubuntu-22:
+    # GitHub-hosted Ubuntu 22.04 runner
+    runs-on: ubuntu-22.04
+    # Use shared steps
+    steps:
+      - uses: actions/checkout@v3
+      - uses: ./.github/workflows/shared_steps
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/.github/workflows/shared_steps/action.yml b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/workflows/shared_steps/action.yml
new file mode 100644
index 0000000..90defda
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/.github/workflows/shared_steps/action.yml
@@ -0,0 +1,31 @@
+name: "Shared Build Steps"
+runs:
+  using: "composite"
+  # Note: working directory will be reset to the repo root for each new step
+  steps:
+    # Build and install all dependencies to RDK installation directory.
+    - name: Build and install dependencies
+      shell: bash
+      run: |
+        pwd
+        cd thirdparty
+        bash build_and_install_dependencies.sh ~/sim_plugin_install 4
+
+    # Configure CMake, then build and install flexiv_rdk library to RDK installation directory.
+    - name: Build and install library
+      shell: bash
+      run: |
+        pwd
+        mkdir -p build && cd build
+        cmake .. -DCMAKE_INSTALL_PREFIX=~/sim_plugin_install
+        cmake --build . --target install --config Release
+
+    # Find and link to flexiv_rdk library, then build all example programs.
+    - name: Build examples
+      shell: bash
+      run: |
+        pwd
+        cd example
+        mkdir -p build && cd build
+        cmake .. -DCMAKE_PREFIX_PATH=~/sim_plugin_install
+        cmake --build . --config Release -j 4
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/.gitignore b/thirdparty/flexiv_sim_plugin.cloned.bak/.gitignore
new file mode 100644
index 0000000..3325357
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/.gitignore
@@ -0,0 +1,14 @@
+.vscode
+.vs
+.DS_Store
+config.h
+build*/
+install/
+cloned/
+html/
+__pycache__
+
+# Pre-built static libraries are attached to GitHub releases as assets, not
+# committed. Only the .sha256 checksum is tracked; the .a is downloaded by
+# CMake at configure time.
+lib/*.a
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/CMakeLists.txt b/thirdparty/flexiv_sim_plugin.cloned.bak/CMakeLists.txt
new file mode 100644
index 0000000..c03b4b0
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/CMakeLists.txt
@@ -0,0 +1,100 @@
+cmake_minimum_required(VERSION 3.16.3)
+
+# ===================================================================
+#      PROJECT SETUP
+# ===================================================================
+project(flexiv_sim_plugin VERSION 1.3.0)
+
+# Configure build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
+endif()
+set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Release" "Debug" "RelWithDebInfo" "MinSizeRel")
+
+# Set static library according to platform
+message(STATUS "OS: ${CMAKE_SYSTEM_NAME}")
+message(STATUS "Processor: ${CMAKE_SYSTEM_PROCESSOR}")
+if(${CMAKE_SYSTEM_NAME} MATCHES "Linux")
+  if(${CMAKE_SYSTEM_PROCESSOR} MATCHES "x86_64")
+    set(SIM_PLUGIN_LIB "libflexiv_sim_plugin.x86_64-linux-gnu.a")
+  else()
+    message(FATAL_ERROR "Linux with ${CMAKE_SYSTEM_PROCESSOR} processor is currently not supported.")
+  endif()
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "Darwin")
+    message(FATAL_ERROR "macOS is currently not supported.")
+elseif(${CMAKE_SYSTEM_NAME} MATCHES "Windows")
+    message(FATAL_ERROR "Windows is currently not supported.")
+endif()
+
+# Download the pre-built static library from the matching GitHub release and
+# verify it against the committed checksum. The .a itself is not tracked in
+# this repo, only lib/${SIM_PLUGIN_LIB}.sha256.
+file(READ ${CMAKE_CURRENT_SOURCE_DIR}/lib/${SIM_PLUGIN_LIB}.sha256 EXPECTED_HASH)
+set(SIM_PLUGIN_LIB_URL https://github.com/flexivrobotics/flexiv_sim_plugin/releases/download/v${PROJECT_VERSION}/${SIM_PLUGIN_LIB})
+set(SIM_PLUGIN_LIB_PATH ${CMAKE_CURRENT_BINARY_DIR}/${SIM_PLUGIN_LIB})
+message(STATUS "Downloading ${SIM_PLUGIN_LIB_URL} ...")
+file(DOWNLOAD ${SIM_PLUGIN_LIB_URL} ${SIM_PLUGIN_LIB_PATH}
+     SHOW_PROGRESS
+     TLS_VERIFY ON
+     EXPECTED_HASH SHA256=${EXPECTED_HASH}
+     STATUS status)
+list(GET status 0 code)
+if(code EQUAL 0)
+  message(STATUS "Download finished")
+else()
+  message(FATAL_ERROR "Download failed: ${status}")
+endif()
+
+# ===================================================================
+#      PROJECT DEPENDENCIES
+# ===================================================================
+# Threads
+set(THREADS_PREFER_PTHREAD_FLAG ON)
+find_package(Threads REQUIRED)
+message(STATUS "Found Threads")
+
+# spdlog
+find_package(spdlog REQUIRED)
+message(STATUS "Found spdlog v${spdlog_VERSION}: ${spdlog_DIR}")
+
+# Fast-DDS (Fast-RTPS)
+find_package(fastrtps REQUIRED)
+message(STATUS "Found fastrtps v${fastrtps_VERSION}: ${fastrtps_DIR}")
+
+# Fast-CDR
+find_package(fastcdr REQUIRED)
+message(STATUS "Found fastcdr v${fastcdr_VERSION}: ${fastcdr_DIR}")
+
+# ===================================================================
+#      CREATE LIBRARY
+# ===================================================================
+# Create static library target with dummy source
+add_library(${PROJECT_NAME} STATIC ${CMAKE_CURRENT_SOURCE_DIR}/lib/dummy.cpp)
+
+# Create an alias of the target with flexiv namespace
+add_library(flexiv::${PROJECT_NAME} ALIAS ${PROJECT_NAME})
+
+# Set include directories
+target_include_directories(${PROJECT_NAME} PUBLIC
+  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+  $<INSTALL_INTERFACE:include>
+)
+
+# Link to dependencies
+target_link_libraries(${PROJECT_NAME} PUBLIC
+  Threads::Threads
+  spdlog::spdlog
+  fastrtps
+  fastcdr
+)
+
+# Use moderate compiler warning option
+if(CMAKE_HOST_UNIX)
+  target_compile_options(${PROJECT_NAME} PUBLIC -Wall -Wextra)
+else()
+  target_compile_options(${PROJECT_NAME} PUBLIC /W1)
+endif()
+
+# Install the library
+include(${CMAKE_CURRENT_SOURCE_DIR}/cmake/FlexivInstallLibrary.cmake)
+FlexivInstallLibrary()
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/COLCON_IGNORE b/thirdparty/flexiv_sim_plugin.cloned.bak/COLCON_IGNORE
new file mode 100644
index 0000000..e69de29
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/LICENSE b/thirdparty/flexiv_sim_plugin.cloned.bak/LICENSE
new file mode 100644
index 0000000..261eeb9
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/LICENSE
@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/README.md b/thirdparty/flexiv_sim_plugin.cloned.bak/README.md
new file mode 100644
index 0000000..6644f11
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/README.md
@@ -0,0 +1,162 @@
+# Flexiv Sim Plugin
+
+![CMake Badge](https://github.com/flexivrobotics/flexiv_sim_plugin/actions/workflows/cmake.yml/badge.svg)
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://www.apache.org/licenses/LICENSE-2.0.html)
+
+
+## Overview
+
+Flexiv Sim Plugin is a C++/Python library that bridges **Flexiv Elements Studio** and any external physics simulator, so that simulated Flexiv robots are driven by the same high-performance force-torque controller used on real robots — without requiring you to deal with the underlying IPC.
+
+```
+┌──────────────────────────────────────────┐
+│       User Program with RDK Client       │
+│          (send robot commands)           │
+└────────────────────┬─────────────────────┘
+                     │ RDK API
+                     ▼
+┌──────────────────────────────────────────┐
+│         Flexiv Elements Studio           │
+│   (simulated controller + RDK Server)    │
+└────────────────────┬─────────────────────┘
+                     │ Flexiv Sim Plugin API
+                     ▼
+┌────────────────────────────────────────────┐
+│          External Simulator                |
+│ (Flexiv Sim Plugin API + External sim API) │
+└────────────────────────────────────────────┘
+```
+
+## Supported External Simulators
+
+The following external simulators are tested and known to work with Flexiv Sim Plugin:
+
+- NVIDIA [Isaac Sim](https://developer.nvidia.com/isaac/sim) (a template workspace is available [here](https://github.com/flexivrobotics/isaac_sim_ws))
+- [MuJoCo](https://mujoco.org/)
+
+In theory, any simulator that meets the following criteria should work:
+
+1. Has a C++ or Python interface.
+2. Provides joint positions and velocities of the simulated robot.
+3. Actuates the joints of the simulated robot by torque.
+
+
+## Environment Compatibility
+
+| **OS**                | **Platform** | **C++ compiler kit** | **Python interpreter** |
+| --------------------- | ------------ | -------------------- | ---------------------- |
+| Linux (Ubuntu 20.04+) | x86_64       | GCC v9.4+            | 3.10, 3.12, 3.14       |
+
+
+## Quick Start - Python
+
+### Install the Python package
+
+On all supported platforms, the Python package of Sim Plugin for a specific Python version can be installed using the `pip` module:
+
+    python3 -m pip install flexivsimplugin
+
+NOTE: replace python3 with a specific version (e.g. python3.10) if your default python3 is not one of the versions listed in the table above.
+
+### Use the installed Python package
+
+After the ``flexivsimplugin`` Python package is installed, it can be imported from any Python script. Test with the following commands in a new Terminal, which should start Flexiv Sim Plugin:
+
+    python3
+    import flexivsimplugin
+    node = flexivsimplugin.UserNode("Rizon4-123456")
+    print("Connected = ", node.connected())
+
+The program should print some info messages and "Connected = False" at the end.
+
+### Run the example Python script
+
+An example script that mocks an external simulator is provided and can be used to test the plugin with the following steps:
+
+1. Setup and run Flexiv Elements Studio simulation. See [docs/elements_studio_setup.md](docs/elements_studio_setup.md).
+
+2. Start the mock program:
+
+       cd flexiv_sim_plugin/example_py
+       python3 -m pip install spdlog
+       python3 ./mock_external_simulator.py [robot_serial_number]
+
+   NOTE: the robot serial number provided to the program is the same one you noted down when creating the simulated robot in Flexiv Elements Studio.
+
+3. Wait for the connection to establish. If the connection is successful, you should see the visualized robot in Elements Studio moving every joint back and forth. NOTE: a software error should occur in Elements Studio which is expected because the mock external simulator did not close the loop by applying the calculated joint torques command to the simulated robot in it. This won't happen to real external simulators.
+
+
+## Quick Start - C++
+
+### Prepare build tools
+
+#### Linux
+
+1. Install compiler kit using package manager:
+
+       sudo apt install build-essential
+
+2. Install CMake using package manager:
+
+       sudo apt install cmake
+
+### Install the C++ library
+
+The following steps are identical on all supported platforms.
+
+1. Choose a directory for installing the C++ library of Sim Plugin and its dependencies. This directory can be under system path or not, depending on whether you want Sim Plugin to be globally discoverable by CMake. For example, a new folder named ``sim_plugin_install`` under the home directory.
+
+2. In a new Terminal, run the provided script to compile and install all dependencies to the installation directory chosen in step 1:
+
+       cd flexiv_sim_plugin/thirdparty
+       bash build_and_install_dependencies.sh ~/sim_plugin_install
+
+3. In a new Terminal, configure the ``flexiv_sim_plugin`` CMake project:
+
+       cd flexiv_sim_plugin
+       mkdir build && cd build
+       cmake .. -DCMAKE_INSTALL_PREFIX=~/sim_plugin_install
+
+   NOTE: ``-D`` followed by ``CMAKE_INSTALL_PREFIX`` sets the absolute path of the installation directory, which should be the one chosen in step 1.
+
+4. Install ``flexiv_sim_plugin`` C++ library to ``CMAKE_INSTALL_PREFIX`` path, which may or may not be globally discoverable by CMake:
+
+       cd flexiv_sim_plugin/build
+       cmake --build . --target install --config Release
+
+### Use the installed C++ library
+
+After the library is installed as ``flexiv_sim_plugin`` CMake target, it can be linked from any other CMake projects. Using the provided `flexiv_sim_plugin-examples` project for instance:
+
+    cd flexiv_sim_plugin/example
+    mkdir build && cd build
+    cmake .. -DCMAKE_PREFIX_PATH=~/sim_plugin_install
+    cmake --build . --config Release -j 4
+
+NOTE: ``-D`` followed by ``CMAKE_PREFIX_PATH`` tells the user project's CMake where to find the installed C++ library. This argument can be skipped if the Sim Plugin library and its dependencies are installed to a globally discoverable location.
+
+### Run the example C++ program
+
+An example program that mocks an external simulator is provided and can be used to test the plugin with the following steps:
+
+1. Setup and run Flexiv Elements Studio simulation. See [docs/elements_studio_setup.md](docs/elements_studio_setup.md).
+
+2. Start the mock program:
+
+       cd flexiv_sim_plugin/example/build
+       ./mock_external_simulator [robot_serial_number]
+
+   NOTE: the robot serial number provided to the program is the same one you noted down when creating the simulated robot in Flexiv Elements Studio.
+
+3. Wait for the connection to establish. If the connection is successful, you should see the visualized robot in Elements Studio moving every joint back and forth. NOTE: a software error should occur in Elements Studio which is expected because the mock external simulator did not close the loop by applying the calculated joint torques command to the simulated robot in it. This won't happen to real external simulators.
+
+## API Documentation
+
+The API documentation can be generated using Doxygen. For example, on Linux:
+
+    sudo apt install doxygen-latex graphviz
+    cd flexiv_sim_plugin
+    doxygen doc/Doxyfile.in
+
+Open any html file under ``flexiv_sim_plugin/doc/html/`` with your browser to view the doc.
+
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/cmake/FlexivInstallLibrary.cmake b/thirdparty/flexiv_sim_plugin.cloned.bak/cmake/FlexivInstallLibrary.cmake
new file mode 100644
index 0000000..4d558d4
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/cmake/FlexivInstallLibrary.cmake
@@ -0,0 +1,103 @@
+# This macro will install ${PROJECT_NAME} to ${CMAKE_INSTALL_PREFIX} when running make install
+#
+# FlexivInstallLibrary() will install all subfolders of ${CMAKE_CURRENT_SOURCE_DIR}/include
+# FlexivInstallLibrary(install_directories) will install only the specified install_directories
+#
+# Requirements:
+# 1. project structure should resemble:
+#    project
+#     - README.md
+#     - CMakeLists.txt that calls this macro
+#     - cmake/${PROJECT_NAME}-config.cmake.in
+#     - include/subfolder/*.h or *.hpp
+# 2. build the library using cmake target functions
+#    - add_library(${PROJECT_NAME} ...) before calling this macro
+#    - target_include_directories(${PROJECT_NAME} ...)
+#    - target_link_libraries(${PROJECT_NAME} ...)
+#    - target_compile_features(${PROJECT_NAME} ...)
+#    - target_compile_options(${PROJECT_NAME} ...)
+#
+# Installed files:
+# - include/subfolder/*.h or *.hpp
+# - lib/lib{PROJECT_NAME}
+# - lib/cmake/{PROJECT_NAME}/
+
+macro(FlexivInstallLibrary)
+    # copy the executables and libraries to the CMAKE_INSTALL_PREFIX DIRECTORY
+    # GNUInstallDirs will set CMAKE_INSTALL* to be the standard relative paths
+    include(GNUInstallDirs)
+    install(TARGETS ${PROJECT_NAME}
+        EXPORT "${PROJECT_NAME}-targets"
+        RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        )
+
+    if(${ARGC} EQUAL 0)
+        # install all subfolders of ${CMAKE_CURRENT_SOURCE_DIR}/include
+        file(GLOB install_directories ${CMAKE_CURRENT_SOURCE_DIR}/include/*)
+        foreach(install_directory ${install_directories})
+            if(IS_DIRECTORY ${install_directory})
+                install(DIRECTORY ${install_directory}
+                        DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+                        FILES_MATCHING 
+                        PATTERN "*.h"
+                        PATTERN "*.hpp"
+                )
+            endif()
+        endforeach()
+    elseif(${ARGC} EQUAL 1)
+        # install specified directories only
+        foreach(install_directory ${ARGV0})
+            install(DIRECTORY ${install_directory}
+                    DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}
+                    FILES_MATCHING 
+                    PATTERN "*.h"
+                    PATTERN "*.hpp"
+                    )
+        endforeach()
+    else()
+        message(FATAL_ERROR "FlexivInstallLibrary take 0 or 1 argument, but given ${ARGC}")
+    endif()
+
+    # Create a *config-version.cmake file so that find_package can have a version specified
+    include(CMakePackageConfigHelpers)
+    write_basic_package_version_file(
+        "${PROJECT_NAME}-config-version.cmake"
+        VERSION ${PROJECT_VERSION}
+        COMPATIBILITY AnyNewerVersion
+        )
+
+    # Copy the *-targets.cmake file to the CMAKE_INSTALL_PREFIX directory
+    install(EXPORT "${PROJECT_NAME}-targets"
+            FILE "${PROJECT_NAME}-targets.cmake"
+            NAMESPACE "flexiv::"
+            DESTINATION "lib/cmake/${PROJECT_NAME}"
+            )
+
+    # Copy the *.-config file to the CMAKE_INSTALL_PREFIX directory. This will specify the dependencies.
+    configure_file("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${PROJECT_NAME}-config.cmake.in" "${PROJECT_NAME}-config.cmake" @ONLY)
+    install(FILES "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config.cmake"
+                  "${CMAKE_CURRENT_BINARY_DIR}/${PROJECT_NAME}-config-version.cmake"
+            DESTINATION "lib/cmake/${PROJECT_NAME}"
+            )
+
+    # Replace the dummy static lib with the actual static lib 
+    install(CODE 
+            "file(REMOVE ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_LIBDIR}/${CMAKE_STATIC_LIBRARY_PREFIX}${PROJECT_NAME}${CMAKE_STATIC_LIBRARY_SUFFIX})")
+    install(FILES ${SIM_PLUGIN_LIB_PATH}
+            DESTINATION ${CMAKE_INSTALL_LIBDIR}
+            RENAME ${CMAKE_STATIC_LIBRARY_PREFIX}${PROJECT_NAME}${CMAKE_STATIC_LIBRARY_SUFFIX}
+            )
+
+    # Use the CPack Package Generator
+    set(CPACK_PACKAGE_VENDOR "Flexiv")
+    set(CPACK_PACKAGE_CONTACT "support@flexiv.com")
+    set(CPACK_PACKAGE_DESCRIPTION "Flexiv Sim Plugin")
+    set(CPACK_PACKAGE_VERSION_MAJOR ${PROJECT_VERSION_MAJOR})
+    set(CPACK_PACKAGE_VERSION_MINOR ${PROJECT_VERSION_MINOR})
+    set(CPACK_PACKAGE_VERSION_PATCH ${PROJECT_VERSION_PATCH})
+    set(CPACK_RESOURCE_FILE_LICENSE "${CMAKE_CURRENT_SOURCE_DIR}/LICENSE")
+    set(CPACK_RESOURCE_FILE_README  "${CMAKE_CURRENT_SOURCE_DIR}/README.md")
+    include(CPack)
+endmacro()
\ No newline at end of file
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/cmake/flexiv_sim_plugin-config.cmake.in b/thirdparty/flexiv_sim_plugin.cloned.bak/cmake/flexiv_sim_plugin-config.cmake.in
new file mode 100644
index 0000000..4dad5dd
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/cmake/flexiv_sim_plugin-config.cmake.in
@@ -0,0 +1,11 @@
+include(CMakeFindDependencyMacro)
+
+# Find dependency
+set(THREADS_PREFER_PTHREAD_FLAG ON)
+find_dependency(Threads REQUIRED)
+find_dependency(spdlog REQUIRED)
+find_dependency(fastrtps REQUIRED)
+find_dependency(fastcdr REQUIRED)
+
+# Add targets file
+include("${CMAKE_CURRENT_LIST_DIR}/@PROJECT_NAME@-targets.cmake")
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/doc/Doxyfile.in b/thirdparty/flexiv_sim_plugin.cloned.bak/doc/Doxyfile.in
new file mode 100644
index 0000000..1c26f3c
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/doc/Doxyfile.in
@@ -0,0 +1,2666 @@
+# Doxyfile 1.9.1
+
+# This file describes the settings to be used by the documentation system
+# doxygen (www.doxygen.org) for a project.
+#
+# All text after a double hash (##) is considered a comment and is placed in
+# front of the TAG it is preceding.
+#
+# All text after a single hash (#) is considered a comment and will be ignored.
+# The format is:
+# TAG = value [value, ...]
+# For lists, items can also be appended using:
+# TAG += value [value, ...]
+# Values that contain spaces should be placed between quotes (\" \").
+
+#---------------------------------------------------------------------------
+# Project related configuration options
+#---------------------------------------------------------------------------
+
+# This tag specifies the encoding used for all characters in the configuration
+# file that follow. The default is UTF-8 which is also the encoding used for all
+# text before the first occurrence of this tag. Doxygen uses libiconv (or the
+# iconv built into libc) for the transcoding. See
+# https://www.gnu.org/software/libiconv/ for the list of possible encodings.
+# The default value is: UTF-8.
+
+DOXYFILE_ENCODING      = UTF-8
+
+# The PROJECT_NAME tag is a single word (or a sequence of words surrounded by
+# double-quotes, unless you are using Doxywizard) that should identify the
+# project for which the documentation is generated. This name is used in the
+# title of most generated pages and in a few other places.
+# The default value is: My Project.
+
+PROJECT_NAME           = "Flexiv Sim Plugin APIs"
+
+# The PROJECT_NUMBER tag can be used to enter a project or revision number. This
+# could be handy for archiving the generated documentation or if some version
+# control system is used.
+
+PROJECT_NUMBER         = 1.2.0
+
+# Using the PROJECT_BRIEF tag one can provide an optional one line description
+# for a project that appears at the top of each page and should give viewer a
+# quick idea about the purpose of the project. Keep the description short.
+
+PROJECT_BRIEF          =
+
+# With the PROJECT_LOGO tag one can specify a logo or an icon that is included
+# in the documentation. The maximum height of the logo should not exceed 55
+# pixels and the maximum width should not exceed 200 pixels. Doxygen will copy
+# the logo to the output directory.
+
+PROJECT_LOGO           = doc/flexiv_logo.png
+
+# The OUTPUT_DIRECTORY tag is used to specify the (relative or absolute) path
+# into which the generated documentation will be written. If a relative path is
+# entered, it will be relative to the location where doxygen was started. If
+# left blank the current directory will be used.
+
+OUTPUT_DIRECTORY       = doc
+
+# If the CREATE_SUBDIRS tag is set to YES then doxygen will create 4096 sub-
+# directories (in 2 levels) under the output directory of each output format and
+# will distribute the generated files over these directories. Enabling this
+# option can be useful when feeding doxygen a huge amount of source files, where
+# putting all generated files in the same directory would otherwise causes
+# performance problems for the file system.
+# The default value is: NO.
+
+CREATE_SUBDIRS         = NO
+
+# If the ALLOW_UNICODE_NAMES tag is set to YES, doxygen will allow non-ASCII
+# characters to appear in the names of generated files. If set to NO, non-ASCII
+# characters will be escaped, for example _xE3_x81_x84 will be used for Unicode
+# U+3044.
+# The default value is: NO.
+
+ALLOW_UNICODE_NAMES    = NO
+
+# The OUTPUT_LANGUAGE tag is used to specify the language in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all constant output in the proper language.
+# Possible values are: Afrikaans, Arabic, Armenian, Brazilian, Catalan, Chinese,
+# Chinese-Traditional, Croatian, Czech, Danish, Dutch, English (United States),
+# Esperanto, Farsi (Persian), Finnish, French, German, Greek, Hungarian,
+# Indonesian, Italian, Japanese, Japanese-en (Japanese with English messages),
+# Korean, Korean-en (Korean with English messages), Latvian, Lithuanian,
+# Macedonian, Norwegian, Persian (Farsi), Polish, Portuguese, Romanian, Russian,
+# Serbian, Serbian-Cyrillic, Slovak, Slovene, Spanish, Swedish, Turkish,
+# Ukrainian and Vietnamese.
+# The default value is: English.
+
+OUTPUT_LANGUAGE        = English
+
+# The OUTPUT_TEXT_DIRECTION tag is used to specify the direction in which all
+# documentation generated by doxygen is written. Doxygen will use this
+# information to generate all generated output in the proper direction.
+# Possible values are: None, LTR, RTL and Context.
+# The default value is: None.
+
+OUTPUT_TEXT_DIRECTION  = None
+
+# If the BRIEF_MEMBER_DESC tag is set to YES, doxygen will include brief member
+# descriptions after the members that are listed in the file and class
+# documentation (similar to Javadoc). Set to NO to disable this.
+# The default value is: YES.
+
+BRIEF_MEMBER_DESC      = YES
+
+# If the REPEAT_BRIEF tag is set to YES, doxygen will prepend the brief
+# description of a member or function before the detailed description
+#
+# Note: If both HIDE_UNDOC_MEMBERS and BRIEF_MEMBER_DESC are set to NO, the
+# brief descriptions will be completely suppressed.
+# The default value is: YES.
+
+REPEAT_BRIEF           = YES
+
+# This tag implements a quasi-intelligent brief description abbreviator that is
+# used to form the text in various listings. Each string in this list, if found
+# as the leading text of the brief description, will be stripped from the text
+# and the result, after processing the whole list, is used as the annotated
+# text. Otherwise, the brief description is used as-is. If left blank, the
+# following values are used ($name is automatically replaced with the name of
+# the entity):The $name class, The $name widget, The $name file, is, provides,
+# specifies, contains, represents, a, an and the.
+
+ABBREVIATE_BRIEF       = "The $name class" \
+                         "The $name widget" \
+                         "The $name file" \
+                         is \
+                         provides \
+                         specifies \
+                         contains \
+                         represents \
+                         a \
+                         an \
+                         the
+
+# If the ALWAYS_DETAILED_SEC and REPEAT_BRIEF tags are both set to YES then
+# doxygen will generate a detailed section even if there is only a brief
+# description.
+# The default value is: NO.
+
+ALWAYS_DETAILED_SEC    = NO
+
+# If the INLINE_INHERITED_MEMB tag is set to YES, doxygen will show all
+# inherited members of a class in the documentation of that class as if those
+# members were ordinary class members. Constructors, destructors and assignment
+# operators of the base classes will not be shown.
+# The default value is: NO.
+
+INLINE_INHERITED_MEMB  = NO
+
+# If the FULL_PATH_NAMES tag is set to YES, doxygen will prepend the full path
+# before files name in the file list and in the header files. If set to NO the
+# shortest path that makes the file name unique will be used
+# The default value is: YES.
+
+FULL_PATH_NAMES        = YES
+
+# The STRIP_FROM_PATH tag can be used to strip a user-defined part of the path.
+# Stripping is only done if one of the specified strings matches the left-hand
+# part of the path. The tag can be used to show relative paths in the file list.
+# If left blank the directory from which doxygen is run is used as the path to
+# strip.
+#
+# Note that you can specify absolute paths here, but also relative paths, which
+# will be relative from the directory where doxygen is started.
+# This tag requires that the tag FULL_PATH_NAMES is set to YES.
+
+STRIP_FROM_PATH        =
+
+# The STRIP_FROM_INC_PATH tag can be used to strip a user-defined part of the
+# path mentioned in the documentation of a class, which tells the reader which
+# header file to include in order to use a class. If left blank only the name of
+# the header file containing the class definition is used. Otherwise one should
+# specify the list of include paths that are normally passed to the compiler
+# using the -I flag.
+
+STRIP_FROM_INC_PATH    =
+
+# If the SHORT_NAMES tag is set to YES, doxygen will generate much shorter (but
+# less readable) file names. This can be useful is your file systems doesn't
+# support long names like on DOS, Mac, or CD-ROM.
+# The default value is: NO.
+
+SHORT_NAMES            = NO
+
+# If the JAVADOC_AUTOBRIEF tag is set to YES then doxygen will interpret the
+# first line (until the first dot) of a Javadoc-style comment as the brief
+# description. If set to NO, the Javadoc-style will behave just like regular Qt-
+# style comments (thus requiring an explicit @brief command for a brief
+# description.)
+# The default value is: NO.
+
+JAVADOC_AUTOBRIEF      = NO
+
+# If the JAVADOC_BANNER tag is set to YES then doxygen will interpret a line
+# such as
+# /***************
+# as being the beginning of a Javadoc-style comment "banner". If set to NO, the
+# Javadoc-style will behave just like regular comments and it will not be
+# interpreted by doxygen.
+# The default value is: NO.
+
+JAVADOC_BANNER         = NO
+
+# If the QT_AUTOBRIEF tag is set to YES then doxygen will interpret the first
+# line (until the first dot) of a Qt-style comment as the brief description. If
+# set to NO, the Qt-style will behave just like regular Qt-style comments (thus
+# requiring an explicit \brief command for a brief description.)
+# The default value is: NO.
+
+QT_AUTOBRIEF           = NO
+
+# The MULTILINE_CPP_IS_BRIEF tag can be set to YES to make doxygen treat a
+# multi-line C++ special comment block (i.e. a block of //! or /// comments) as
+# a brief description. This used to be the default behavior. The new default is
+# to treat a multi-line C++ comment block as a detailed description. Set this
+# tag to YES if you prefer the old behavior instead.
+#
+# Note that setting this tag to YES also means that rational rose comments are
+# not recognized any more.
+# The default value is: NO.
+
+MULTILINE_CPP_IS_BRIEF = NO
+
+# By default Python docstrings are displayed as preformatted text and doxygen's
+# special commands cannot be used. By setting PYTHON_DOCSTRING to NO the
+# doxygen's special commands can be used and the contents of the docstring
+# documentation blocks is shown as doxygen documentation.
+# The default value is: YES.
+
+PYTHON_DOCSTRING       = YES
+
+# If the INHERIT_DOCS tag is set to YES then an undocumented member inherits the
+# documentation from any documented member that it re-implements.
+# The default value is: YES.
+
+INHERIT_DOCS           = YES
+
+# If the SEPARATE_MEMBER_PAGES tag is set to YES then doxygen will produce a new
+# page for each member. If set to NO, the documentation of a member will be part
+# of the file/class/namespace that contains it.
+# The default value is: NO.
+
+SEPARATE_MEMBER_PAGES  = NO
+
+# The TAB_SIZE tag can be used to set the number of spaces in a tab. Doxygen
+# uses this value to replace tabs by spaces in code fragments.
+# Minimum value: 1, maximum value: 16, default value: 4.
+
+TAB_SIZE               = 4
+
+# This tag can be used to specify a number of aliases that act as commands in
+# the documentation. An alias has the form:
+# name=value
+# For example adding
+# "sideeffect=@par Side Effects:\n"
+# will allow you to put the command \sideeffect (or @sideeffect) in the
+# documentation, which will result in a user-defined paragraph with heading
+# "Side Effects:". You can put \n's in the value part of an alias to insert
+# newlines (in the resulting output). You can put ^^ in the value part of an
+# alias to insert a newline as if a physical newline was in the original file.
+# When you need a literal { or } or , in the value part of an alias you have to
+# escape them by means of a backslash (\), this can lead to conflicts with the
+# commands \{ and \} for these it is advised to use the version @{ and @} or use
+# a double escape (\\{ and \\})
+
+ALIASES                =
+
+# Set the OPTIMIZE_OUTPUT_FOR_C tag to YES if your project consists of C sources
+# only. Doxygen will then generate output that is more tailored for C. For
+# instance, some of the names that are used will be different. The list of all
+# members will be omitted, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_FOR_C  = NO
+
+# Set the OPTIMIZE_OUTPUT_JAVA tag to YES if your project consists of Java or
+# Python sources only. Doxygen will then generate output that is more tailored
+# for that language. For instance, namespaces will be presented as packages,
+# qualified scopes will look different, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_JAVA   = NO
+
+# Set the OPTIMIZE_FOR_FORTRAN tag to YES if your project consists of Fortran
+# sources. Doxygen will then generate output that is tailored for Fortran.
+# The default value is: NO.
+
+OPTIMIZE_FOR_FORTRAN   = NO
+
+# Set the OPTIMIZE_OUTPUT_VHDL tag to YES if your project consists of VHDL
+# sources. Doxygen will then generate output that is tailored for VHDL.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_VHDL   = NO
+
+# Set the OPTIMIZE_OUTPUT_SLICE tag to YES if your project consists of Slice
+# sources only. Doxygen will then generate output that is more tailored for that
+# language. For instance, namespaces will be presented as modules, types will be
+# separated into more groups, etc.
+# The default value is: NO.
+
+OPTIMIZE_OUTPUT_SLICE  = NO
+
+# Doxygen selects the parser to use depending on the extension of the files it
+# parses. With this tag you can assign which parser to use for a given
+# extension. Doxygen has a built-in mapping, but you can override or extend it
+# using this tag. The format is ext=language, where ext is a file extension, and
+# language is one of the parsers supported by doxygen: IDL, Java, JavaScript,
+# Csharp (C#), C, C++, D, PHP, md (Markdown), Objective-C, Python, Slice, VHDL,
+# Fortran (fixed format Fortran: FortranFixed, free formatted Fortran:
+# FortranFree, unknown formatted Fortran: Fortran. In the later case the parser
+# tries to guess whether the code is fixed or free formatted code, this is the
+# default for Fortran type files). For instance to make doxygen treat .inc files
+# as Fortran files (default is PHP), and .f files as C (default is Fortran),
+# use: inc=Fortran f=C.
+#
+# Note: For files without extension you can use no_extension as a placeholder.
+#
+# Note that for custom extensions you also need to set FILE_PATTERNS otherwise
+# the files are not read by doxygen. When specifying no_extension you should add
+# * to the FILE_PATTERNS.
+#
+# Note see also the list of default file extension mappings.
+
+EXTENSION_MAPPING      =
+
+# If the MARKDOWN_SUPPORT tag is enabled then doxygen pre-processes all comments
+# according to the Markdown format, which allows for more readable
+# documentation. See https://daringfireball.net/projects/markdown/ for details.
+# The output of markdown processing is further processed by doxygen, so you can
+# mix doxygen, HTML, and XML commands with Markdown formatting. Disable only in
+# case of backward compatibilities issues.
+# The default value is: YES.
+
+MARKDOWN_SUPPORT       = YES
+
+# When the TOC_INCLUDE_HEADINGS tag is set to a non-zero value, all headings up
+# to that level are automatically included in the table of contents, even if
+# they do not have an id attribute.
+# Note: This feature currently applies only to Markdown headings.
+# Minimum value: 0, maximum value: 99, default value: 5.
+# This tag requires that the tag MARKDOWN_SUPPORT is set to YES.
+
+TOC_INCLUDE_HEADINGS   = 0
+
+# When enabled doxygen tries to link words that correspond to documented
+# classes, or namespaces to their corresponding documentation. Such a link can
+# be prevented in individual cases by putting a % sign in front of the word or
+# globally by setting AUTOLINK_SUPPORT to NO.
+# The default value is: YES.
+
+AUTOLINK_SUPPORT       = YES
+
+# If you use STL classes (i.e. std::string, std::vector, etc.) but do not want
+# to include (a tag file for) the STL sources as input, then you should set this
+# tag to YES in order to let doxygen match functions declarations and
+# definitions whose arguments contain STL classes (e.g. func(std::string);
+# versus func(std::string) {}). This also make the inheritance and collaboration
+# diagrams that involve STL classes more complete and accurate.
+# The default value is: NO.
+
+BUILTIN_STL_SUPPORT    = NO
+
+# If you use Microsoft's C++/CLI language, you should set this option to YES to
+# enable parsing support.
+# The default value is: NO.
+
+CPP_CLI_SUPPORT        = NO
+
+# Set the SIP_SUPPORT tag to YES if your project consists of sip (see:
+# https://www.riverbankcomputing.com/software/sip/intro) sources only. Doxygen
+# will parse them like normal C++ but will assume all classes use public instead
+# of private inheritance when no explicit protection keyword is present.
+# The default value is: NO.
+
+SIP_SUPPORT            = NO
+
+# For Microsoft's IDL there are propget and propput attributes to indicate
+# getter and setter methods for a property. Setting this option to YES will make
+# doxygen to replace the get and set methods by a property in the documentation.
+# This will only work if the methods are indeed getting or setting a simple
+# type. If this is not the case, or you want to show the methods anyway, you
+# should set this option to NO.
+# The default value is: YES.
+
+IDL_PROPERTY_SUPPORT   = YES
+
+# If member grouping is used in the documentation and the DISTRIBUTE_GROUP_DOC
+# tag is set to YES then doxygen will reuse the documentation of the first
+# member in the group (if any) for the other members of the group. By default
+# all members of a group must be documented explicitly.
+# The default value is: NO.
+
+DISTRIBUTE_GROUP_DOC   = YES
+
+# If one adds a struct or class to a group and this option is enabled, then also
+# any nested class or struct is added to the same group. By default this option
+# is disabled and one has to add nested compounds explicitly via \ingroup.
+# The default value is: NO.
+
+GROUP_NESTED_COMPOUNDS = NO
+
+# Set the SUBGROUPING tag to YES to allow class member groups of the same type
+# (for instance a group of public functions) to be put as a subgroup of that
+# type (e.g. under the Public Functions section). Set it to NO to prevent
+# subgrouping. Alternatively, this can be done per class using the
+# \nosubgrouping command.
+# The default value is: YES.
+
+SUBGROUPING            = YES
+
+# When the INLINE_GROUPED_CLASSES tag is set to YES, classes, structs and unions
+# are shown inside the group in which they are included (e.g. using \ingroup)
+# instead of on a separate page (for HTML and Man pages) or section (for LaTeX
+# and RTF).
+#
+# Note that this feature does not work in combination with
+# SEPARATE_MEMBER_PAGES.
+# The default value is: NO.
+
+INLINE_GROUPED_CLASSES = NO
+
+# When the INLINE_SIMPLE_STRUCTS tag is set to YES, structs, classes, and unions
+# with only public data fields or simple typedef fields will be shown inline in
+# the documentation of the scope in which they are defined (i.e. file,
+# namespace, or group documentation), provided this scope is documented. If set
+# to NO, structs, classes, and unions are shown on a separate page (for HTML and
+# Man pages) or section (for LaTeX and RTF).
+# The default value is: NO.
+
+INLINE_SIMPLE_STRUCTS  = NO
+
+# When TYPEDEF_HIDES_STRUCT tag is enabled, a typedef of a struct, union, or
+# enum is documented as struct, union, or enum with the name of the typedef. So
+# typedef struct TypeS {} TypeT, will appear in the documentation as a struct
+# with name TypeT. When disabled the typedef will appear as a member of a file,
+# namespace, or class. And the struct will be named TypeS. This can typically be
+# useful for C code in case the coding convention dictates that all compound
+# types are typedef'ed and only the typedef is referenced, never the tag name.
+# The default value is: NO.
+
+TYPEDEF_HIDES_STRUCT   = NO
+
+# The size of the symbol lookup cache can be set using LOOKUP_CACHE_SIZE. This
+# cache is used to resolve symbols given their name and scope. Since this can be
+# an expensive process and often the same symbol appears multiple times in the
+# code, doxygen keeps a cache of pre-resolved symbols. If the cache is too small
+# doxygen will become slower. If the cache is too large, memory is wasted. The
+# cache size is given by this formula: 2^(16+LOOKUP_CACHE_SIZE). The valid range
+# is 0..9, the default is 0, corresponding to a cache size of 2^16=65536
+# symbols. At the end of a run doxygen will report the cache usage and suggest
+# the optimal cache size from a speed point of view.
+# Minimum value: 0, maximum value: 9, default value: 0.
+
+LOOKUP_CACHE_SIZE      = 0
+
+# The NUM_PROC_THREADS specifies the number threads doxygen is allowed to use
+# during processing. When set to 0 doxygen will based this on the number of
+# cores available in the system. You can set it explicitly to a value larger
+# than 0 to get more control over the balance between CPU load and processing
+# speed. At this moment only the input processing can be done using multiple
+# threads. Since this is still an experimental feature the default is set to 1,
+# which efficively disables parallel processing. Please report any issues you
+# encounter. Generating dot graphs in parallel is controlled by the
+# DOT_NUM_THREADS setting.
+# Minimum value: 0, maximum value: 32, default value: 1.
+
+NUM_PROC_THREADS       = 1
+
+#---------------------------------------------------------------------------
+# Build related configuration options
+#---------------------------------------------------------------------------
+
+# If the EXTRACT_ALL tag is set to YES, doxygen will assume all entities in
+# documentation are documented, even if no documentation was available. Private
+# class members and static file members will be hidden unless the
+# EXTRACT_PRIVATE respectively EXTRACT_STATIC tags are set to YES.
+# Note: This will also disable the warnings about undocumented members that are
+# normally produced when WARNINGS is set to YES.
+# The default value is: NO.
+
+EXTRACT_ALL            = NO
+
+# If the EXTRACT_PRIVATE tag is set to YES, all private members of a class will
+# be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIVATE        = NO
+
+# If the EXTRACT_PRIV_VIRTUAL tag is set to YES, documented private virtual
+# methods of a class will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PRIV_VIRTUAL   = NO
+
+# If the EXTRACT_PACKAGE tag is set to YES, all members with package or internal
+# scope will be included in the documentation.
+# The default value is: NO.
+
+EXTRACT_PACKAGE        = NO
+
+# If the EXTRACT_STATIC tag is set to YES, all static members of a file will be
+# included in the documentation.
+# The default value is: NO.
+
+EXTRACT_STATIC         = NO
+
+# If the EXTRACT_LOCAL_CLASSES tag is set to YES, classes (and structs) defined
+# locally in source files will be included in the documentation. If set to NO,
+# only classes defined in header files are included. Does not have any effect
+# for Java sources.
+# The default value is: YES.
+
+EXTRACT_LOCAL_CLASSES  = YES
+
+# This flag is only useful for Objective-C code. If set to YES, local methods,
+# which are defined in the implementation section but not in the interface are
+# included in the documentation. If set to NO, only methods in the interface are
+# included.
+# The default value is: NO.
+
+EXTRACT_LOCAL_METHODS  = NO
+
+# If this flag is set to YES, the members of anonymous namespaces will be
+# extracted and appear in the documentation as a namespace called
+# 'anonymous_namespace{file}', where file will be replaced with the base name of
+# the file that contains the anonymous namespace. By default anonymous namespace
+# are hidden.
+# The default value is: NO.
+
+EXTRACT_ANON_NSPACES   = NO
+
+# If this flag is set to YES, the name of an unnamed parameter in a declaration
+# will be determined by the corresponding definition. By default unnamed
+# parameters remain unnamed in the output.
+# The default value is: YES.
+
+RESOLVE_UNNAMED_PARAMS = YES
+
+# If the HIDE_UNDOC_MEMBERS tag is set to YES, doxygen will hide all
+# undocumented members inside documented classes or files. If set to NO these
+# members will be included in the various overviews, but no documentation
+# section is generated. This option has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_MEMBERS     = NO
+
+# If the HIDE_UNDOC_CLASSES tag is set to YES, doxygen will hide all
+# undocumented classes that are normally visible in the class hierarchy. If set
+# to NO, these classes will be included in the various overviews. This option
+# has no effect if EXTRACT_ALL is enabled.
+# The default value is: NO.
+
+HIDE_UNDOC_CLASSES     = NO
+
+# If the HIDE_FRIEND_COMPOUNDS tag is set to YES, doxygen will hide all friend
+# declarations. If set to NO, these declarations will be included in the
+# documentation.
+# The default value is: NO.
+
+HIDE_FRIEND_COMPOUNDS  = NO
+
+# If the HIDE_IN_BODY_DOCS tag is set to YES, doxygen will hide any
+# documentation blocks found inside the body of a function. If set to NO, these
+# blocks will be appended to the function's detailed documentation block.
+# The default value is: NO.
+
+HIDE_IN_BODY_DOCS      = NO
+
+# The INTERNAL_DOCS tag determines if documentation that is typed after a
+# \internal command is included. If the tag is set to NO then the documentation
+# will be excluded. Set it to YES to include the internal documentation.
+# The default value is: NO.
+
+INTERNAL_DOCS          = NO
+
+# With the correct setting of option CASE_SENSE_NAMES doxygen will better be
+# able to match the capabilities of the underlying filesystem. In case the
+# filesystem is case sensitive (i.e. it supports files in the same directory
+# whose names only differ in casing), the option must be set to YES to properly
+# deal with such files in case they appear in the input. For filesystems that
+# are not case sensitive the option should be be set to NO to properly deal with
+# output files written for symbols that only differ in casing, such as for two
+# classes, one named CLASS and the other named Class, and to also support
+# references to files without having to specify the exact matching casing. On
+# Windows (including Cygwin) and MacOS, users should typically set this option
+# to NO, whereas on Linux or other Unix flavors it should typically be set to
+# YES.
+# The default value is: system dependent.
+
+CASE_SENSE_NAMES       = NO
+
+# If the HIDE_SCOPE_NAMES tag is set to NO then doxygen will show members with
+# their full class and namespace scopes in the documentation. If set to YES, the
+# scope will be hidden.
+# The default value is: NO.
+
+HIDE_SCOPE_NAMES       = NO
+
+# If the HIDE_COMPOUND_REFERENCE tag is set to NO (default) then doxygen will
+# append additional text to a page's title, such as Class Reference. If set to
+# YES the compound reference will be hidden.
+# The default value is: NO.
+
+HIDE_COMPOUND_REFERENCE= NO
+
+# If the SHOW_INCLUDE_FILES tag is set to YES then doxygen will put a list of
+# the files that are included by a file in the documentation of that file.
+# The default value is: YES.
+
+SHOW_INCLUDE_FILES     = YES
+
+# If the SHOW_GROUPED_MEMB_INC tag is set to YES then Doxygen will add for each
+# grouped member an include statement to the documentation, telling the reader
+# which file to include in order to use the member.
+# The default value is: NO.
+
+SHOW_GROUPED_MEMB_INC  = NO
+
+# If the FORCE_LOCAL_INCLUDES tag is set to YES then doxygen will list include
+# files with double quotes in the documentation rather than with sharp brackets.
+# The default value is: NO.
+
+FORCE_LOCAL_INCLUDES   = NO
+
+# If the INLINE_INFO tag is set to YES then a tag [inline] is inserted in the
+# documentation for inline members.
+# The default value is: YES.
+
+INLINE_INFO            = YES
+
+# If the SORT_MEMBER_DOCS tag is set to YES then doxygen will sort the
+# (detailed) documentation of file and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order.
+# The default value is: YES.
+
+SORT_MEMBER_DOCS       = YES
+
+# If the SORT_BRIEF_DOCS tag is set to YES then doxygen will sort the brief
+# descriptions of file, namespace and class members alphabetically by member
+# name. If set to NO, the members will appear in declaration order. Note that
+# this will also influence the order of the classes in the class list.
+# The default value is: NO.
+
+SORT_BRIEF_DOCS        = NO
+
+# If the SORT_MEMBERS_CTORS_1ST tag is set to YES then doxygen will sort the
+# (brief and detailed) documentation of class members so that constructors and
+# destructors are listed first. If set to NO the constructors will appear in the
+# respective orders defined by SORT_BRIEF_DOCS and SORT_MEMBER_DOCS.
+# Note: If SORT_BRIEF_DOCS is set to NO this option is ignored for sorting brief
+# member documentation.
+# Note: If SORT_MEMBER_DOCS is set to NO this option is ignored for sorting
+# detailed member documentation.
+# The default value is: NO.
+
+SORT_MEMBERS_CTORS_1ST = NO
+
+# If the SORT_GROUP_NAMES tag is set to YES then doxygen will sort the hierarchy
+# of group names into alphabetical order. If set to NO the group names will
+# appear in their defined order.
+# The default value is: NO.
+
+SORT_GROUP_NAMES       = NO
+
+# If the SORT_BY_SCOPE_NAME tag is set to YES, the class list will be sorted by
+# fully-qualified names, including namespaces. If set to NO, the class list will
+# be sorted only by class name, not including the namespace part.
+# Note: This option is not very useful if HIDE_SCOPE_NAMES is set to YES.
+# Note: This option applies only to the class list, not to the alphabetical
+# list.
+# The default value is: NO.
+
+SORT_BY_SCOPE_NAME     = NO
+
+# If the STRICT_PROTO_MATCHING option is enabled and doxygen fails to do proper
+# type resolution of all parameters of a function it will reject a match between
+# the prototype and the implementation of a member function even if there is
+# only one candidate or it is obvious which candidate to choose by doing a
+# simple string match. By disabling STRICT_PROTO_MATCHING doxygen will still
+# accept a match between prototype and implementation in such cases.
+# The default value is: NO.
+
+STRICT_PROTO_MATCHING  = NO
+
+# The GENERATE_TODOLIST tag can be used to enable (YES) or disable (NO) the todo
+# list. This list is created by putting \todo commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TODOLIST      = YES
+
+# The GENERATE_TESTLIST tag can be used to enable (YES) or disable (NO) the test
+# list. This list is created by putting \test commands in the documentation.
+# The default value is: YES.
+
+GENERATE_TESTLIST      = YES
+
+# The GENERATE_BUGLIST tag can be used to enable (YES) or disable (NO) the bug
+# list. This list is created by putting \bug commands in the documentation.
+# The default value is: YES.
+
+GENERATE_BUGLIST       = YES
+
+# The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or disable (NO)
+# the deprecated list. This list is created by putting \deprecated commands in
+# the documentation.
+# The default value is: YES.
+
+GENERATE_DEPRECATEDLIST= YES
+
+# The ENABLED_SECTIONS tag can be used to enable conditional documentation
+# sections, marked by \if <section_label> ... \endif and \cond <section_label>
+# ... \endcond blocks.
+
+ENABLED_SECTIONS       =
+
+# The MAX_INITIALIZER_LINES tag determines the maximum number of lines that the
+# initial value of a variable or macro / define can have for it to appear in the
+# documentation. If the initializer consists of more lines than specified here
+# it will be hidden. Use a value of 0 to hide initializers completely. The
+# appearance of the value of individual variables and macros / defines can be
+# controlled using \showinitializer or \hideinitializer command in the
+# documentation regardless of this setting.
+# Minimum value: 0, maximum value: 10000, default value: 30.
+
+MAX_INITIALIZER_LINES  = 30
+
+# Set the SHOW_USED_FILES tag to NO to disable the list of files generated at
+# the bottom of the documentation of classes and structs. If set to YES, the
+# list will mention the files that were used to generate the documentation.
+# The default value is: YES.
+
+SHOW_USED_FILES        = YES
+
+# Set the SHOW_FILES tag to NO to disable the generation of the Files page. This
+# will remove the Files entry from the Quick Index and from the Folder Tree View
+# (if specified).
+# The default value is: YES.
+
+SHOW_FILES             = YES
+
+# Set the SHOW_NAMESPACES tag to NO to disable the generation of the Namespaces
+# page. This will remove the Namespaces entry from the Quick Index and from the
+# Folder Tree View (if specified).
+# The default value is: YES.
+
+SHOW_NAMESPACES        = YES
+
+# The FILE_VERSION_FILTER tag can be used to specify a program or script that
+# doxygen should invoke to get the current version for each file (typically from
+# the version control system). Doxygen will invoke the program by executing (via
+# popen()) the command command input-file, where command is the value of the
+# FILE_VERSION_FILTER tag, and input-file is the name of an input file provided
+# by doxygen. Whatever the program writes to standard output is used as the file
+# version. For an example see the documentation.
+
+FILE_VERSION_FILTER    =
+
+# The LAYOUT_FILE tag can be used to specify a layout file which will be parsed
+# by doxygen. The layout file controls the global structure of the generated
+# output files in an output format independent way. To create the layout file
+# that represents doxygen's defaults, run doxygen with the -l option. You can
+# optionally specify a file name after the option, if omitted DoxygenLayout.xml
+# will be used as the name of the layout file.
+#
+# Note that if you run doxygen from a directory containing a file called
+# DoxygenLayout.xml, doxygen will parse it automatically even if the LAYOUT_FILE
+# tag is left empty.
+
+LAYOUT_FILE            =
+
+# The CITE_BIB_FILES tag can be used to specify one or more bib files containing
+# the reference definitions. This must be a list of .bib files. The .bib
+# extension is automatically appended if omitted. This requires the bibtex tool
+# to be installed. See also https://en.wikipedia.org/wiki/BibTeX for more info.
+# For LaTeX the style of the bibliography can be controlled using
+# LATEX_BIB_STYLE. To use this feature you need bibtex and perl available in the
+# search path. See also \cite for info how to create references.
+
+CITE_BIB_FILES         =
+
+#---------------------------------------------------------------------------
+# Configuration options related to warning and progress messages
+#---------------------------------------------------------------------------
+
+# The QUIET tag can be used to turn on/off the messages that are generated to
+# standard output by doxygen. If QUIET is set to YES this implies that the
+# messages are off.
+# The default value is: NO.
+
+QUIET                  = NO
+
+# The WARNINGS tag can be used to turn on/off the warning messages that are
+# generated to standard error (stderr) by doxygen. If WARNINGS is set to YES
+# this implies that the warnings are on.
+#
+# Tip: Turn warnings on while writing the documentation.
+# The default value is: YES.
+
+WARNINGS               = YES
+
+# If the WARN_IF_UNDOCUMENTED tag is set to YES then doxygen will generate
+# warnings for undocumented members. If EXTRACT_ALL is set to YES then this flag
+# will automatically be disabled.
+# The default value is: YES.
+
+WARN_IF_UNDOCUMENTED   = YES
+
+# If the WARN_IF_DOC_ERROR tag is set to YES, doxygen will generate warnings for
+# potential errors in the documentation, such as not documenting some parameters
+# in a documented function, or documenting parameters that don't exist or using
+# markup commands wrongly.
+# The default value is: YES.
+
+WARN_IF_DOC_ERROR      = YES
+
+# This WARN_NO_PARAMDOC option can be enabled to get warnings for functions that
+# are documented, but have no documentation for their parameters or return
+# value. If set to NO, doxygen will only warn about wrong or incomplete
+# parameter documentation, but not about the absence of documentation. If
+# EXTRACT_ALL is set to YES then this flag will automatically be disabled.
+# The default value is: NO.
+
+WARN_NO_PARAMDOC       = NO
+
+# If the WARN_AS_ERROR tag is set to YES then doxygen will immediately stop when
+# a warning is encountered. If the WARN_AS_ERROR tag is set to FAIL_ON_WARNINGS
+# then doxygen will continue running as if WARN_AS_ERROR tag is set to NO, but
+# at the end of the doxygen process doxygen will return with a non-zero status.
+# Possible values are: NO, YES and FAIL_ON_WARNINGS.
+# The default value is: NO.
+
+WARN_AS_ERROR          = NO
+
+# The WARN_FORMAT tag determines the format of the warning messages that doxygen
+# can produce. The string should contain the $file, $line, and $text tags, which
+# will be replaced by the file and line number from which the warning originated
+# and the warning text. Optionally the format may contain $version, which will
+# be replaced by the version of the file (if it could be obtained via
+# FILE_VERSION_FILTER)
+# The default value is: $file:$line: $text.
+
+WARN_FORMAT            = "$file:$line: $text"
+
+# The WARN_LOGFILE tag can be used to specify a file to which warning and error
+# messages should be written. If left blank the output is written to standard
+# error (stderr).
+
+WARN_LOGFILE           =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the input files
+#---------------------------------------------------------------------------
+
+# The INPUT tag is used to specify the files and/or directories that contain
+# documented source files. You may enter file names like myfile.cpp or
+# directories like /usr/src/myproject. Separate the files or directories with
+# spaces. See also FILE_PATTERNS and EXTENSION_MAPPING
+# Note: If this tag is empty the current directory is searched.
+
+INPUT                  = README.md \
+                         doc \
+                         example \
+                         include \
+                         lib \
+                         resources \
+                         test
+
+# This tag can be used to specify the character encoding of the source files
+# that doxygen parses. Internally doxygen uses the UTF-8 encoding. Doxygen uses
+# libiconv (or the iconv built into libc) for the transcoding. See the libiconv
+# documentation (see:
+# https://www.gnu.org/software/libiconv/) for the list of possible encodings.
+# The default value is: UTF-8.
+
+INPUT_ENCODING         = UTF-8
+
+# If the value of the INPUT tag contains directories, you can use the
+# FILE_PATTERNS tag to specify one or more wildcard patterns (like *.cpp and
+# *.h) to filter out the source-files in the directories.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# read by doxygen.
+#
+# Note the list of default checked file patterns might differ from the list of
+# default file extension mappings.
+#
+# If left blank the following patterns are tested:*.c, *.cc, *.cxx, *.cpp,
+# *.c++, *.java, *.ii, *.ixx, *.ipp, *.i++, *.inl, *.idl, *.ddl, *.odl, *.h,
+# *.hh, *.hxx, *.hpp, *.h++, *.cs, *.d, *.php, *.php4, *.php5, *.phtml, *.inc,
+# *.m, *.markdown, *.md, *.mm, *.dox (to be provided as doxygen C comment),
+# *.py, *.pyw, *.f90, *.f95, *.f03, *.f08, *.f18, *.f, *.for, *.vhd, *.vhdl,
+# *.ucf, *.qsf and *.ice.
+
+FILE_PATTERNS          = *.c \
+                         *.cc \
+                         *.cxx \
+                         *.cpp \
+                         *.c++ \
+                         *.java \
+                         *.ii \
+                         *.ixx \
+                         *.ipp \
+                         *.i++ \
+                         *.inl \
+                         *.idl \
+                         *.ddl \
+                         *.odl \
+                         *.h \
+                         *.hh \
+                         *.hxx \
+                         *.hpp \
+                         *.h++ \
+                         *.cs \
+                         *.d \
+                         *.php \
+                         *.php4 \
+                         *.php5 \
+                         *.phtml \
+                         *.inc \
+                         *.m \
+                         *.markdown \
+                         *.md \
+                         *.mm \
+                         *.dox \
+                         *.py \
+                         *.pyw \
+                         *.f90 \
+                         *.f95 \
+                         *.f03 \
+                         *.f08 \
+                         *.f \
+                         *.for \
+                         *.tcl \
+                         *.vhd \
+                         *.vhdl \
+                         *.ucf \
+                         *.qsf \
+                         *.proto
+
+# The RECURSIVE tag can be used to specify whether or not subdirectories should
+# be searched for input files as well.
+# The default value is: NO.
+
+RECURSIVE              = YES
+
+# The EXCLUDE tag can be used to specify files and/or directories that should be
+# excluded from the INPUT source files. This way you can easily exclude a
+# subdirectory from a directory tree whose root is specified with the INPUT tag.
+#
+# Note that relative paths are relative to the directory from which doxygen is
+# run.
+
+EXCLUDE                =
+
+# The EXCLUDE_SYMLINKS tag can be used to select whether or not files or
+# directories that are symbolic links (a Unix file system feature) are excluded
+# from the input.
+# The default value is: NO.
+
+EXCLUDE_SYMLINKS       = NO
+
+# If the value of the INPUT tag contains directories, you can use the
+# EXCLUDE_PATTERNS tag to specify one or more wildcard patterns to exclude
+# certain files from those directories.
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories for example use the pattern */test/*
+
+EXCLUDE_PATTERNS       = */build/* \
+                         *.pb.h \
+                         *.pb.cc
+
+# The EXCLUDE_SYMBOLS tag can be used to specify one or more symbol names
+# (namespaces, classes, functions, etc.) that should be excluded from the
+# output. The symbol name can be a fully qualified name, a word, or if the
+# wildcard * is used, a substring. Examples: ANamespace, AClass,
+# AClass::ANamespace, ANamespace::*Test
+#
+# Note that the wildcards are matched against the file with absolute path, so to
+# exclude all test directories use the pattern */test/*
+
+EXCLUDE_SYMBOLS        =
+
+# The EXAMPLE_PATH tag can be used to specify one or more files or directories
+# that contain example code fragments that are included (see the \include
+# command).
+
+EXAMPLE_PATH           = example
+
+# If the value of the EXAMPLE_PATH tag contains directories, you can use the
+# EXAMPLE_PATTERNS tag to specify one or more wildcard pattern (like *.cpp and
+# *.h) to filter out the source-files in the directories. If left blank all
+# files are included.
+
+EXAMPLE_PATTERNS       = *
+
+# If the EXAMPLE_RECURSIVE tag is set to YES then subdirectories will be
+# searched for input files to be used with the \include or \dontinclude commands
+# irrespective of the value of the RECURSIVE tag.
+# The default value is: NO.
+
+EXAMPLE_RECURSIVE      = NO
+
+# The IMAGE_PATH tag can be used to specify one or more files or directories
+# that contain images that are to be included in the documentation (see the
+# \image command).
+
+IMAGE_PATH             =
+
+# The INPUT_FILTER tag can be used to specify a program that doxygen should
+# invoke to filter for each input file. Doxygen will invoke the filter program
+# by executing (via popen()) the command:
+#
+# <filter> <input-file>
+#
+# where <filter> is the value of the INPUT_FILTER tag, and <input-file> is the
+# name of an input file. Doxygen will then use the output that the filter
+# program writes to standard output. If FILTER_PATTERNS is specified, this tag
+# will be ignored.
+#
+# Note that the filter must not add or remove lines; it is applied before the
+# code is scanned, but not when the output code is generated. If lines are added
+# or removed, the anchors will not be placed correctly.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+INPUT_FILTER           =
+
+# The FILTER_PATTERNS tag can be used to specify filters on a per file pattern
+# basis. Doxygen will compare the file name with each pattern and apply the
+# filter if there is a match. The filters are a list of the form: pattern=filter
+# (like *.cpp=my_cpp_filter). See INPUT_FILTER for further information on how
+# filters are used. If the FILTER_PATTERNS tag is empty or if none of the
+# patterns match the file name, INPUT_FILTER is applied.
+#
+# Note that for custom extensions or not directly supported extensions you also
+# need to set EXTENSION_MAPPING for the extension otherwise the files are not
+# properly processed by doxygen.
+
+FILTER_PATTERNS        =
+
+# If the FILTER_SOURCE_FILES tag is set to YES, the input filter (if set using
+# INPUT_FILTER) will also be used to filter the input files that are used for
+# producing the source files to browse (i.e. when SOURCE_BROWSER is set to YES).
+# The default value is: NO.
+
+FILTER_SOURCE_FILES    = NO
+
+# The FILTER_SOURCE_PATTERNS tag can be used to specify source filters per file
+# pattern. A pattern will override the setting for FILTER_PATTERN (if any) and
+# it is also possible to disable source filtering for a specific pattern using
+# *.ext= (so without naming a filter).
+# This tag requires that the tag FILTER_SOURCE_FILES is set to YES.
+
+FILTER_SOURCE_PATTERNS =
+
+# If the USE_MDFILE_AS_MAINPAGE tag refers to the name of a markdown file that
+# is part of the input, its contents will be placed on the main page
+# (index.html). This can be useful if you have a project on for instance GitHub
+# and want to reuse the introduction page also for the doxygen output.
+
+USE_MDFILE_AS_MAINPAGE = README.md
+
+#---------------------------------------------------------------------------
+# Configuration options related to source browsing
+#---------------------------------------------------------------------------
+
+# If the SOURCE_BROWSER tag is set to YES then a list of source files will be
+# generated. Documented entities will be cross-referenced with these sources.
+#
+# Note: To get rid of all source code in the generated output, make sure that
+# also VERBATIM_HEADERS is set to NO.
+# The default value is: NO.
+
+SOURCE_BROWSER         = YES
+
+# Setting the INLINE_SOURCES tag to YES will include the body of functions,
+# classes and enums directly into the documentation.
+# The default value is: NO.
+
+INLINE_SOURCES         = NO
+
+# Setting the STRIP_CODE_COMMENTS tag to YES will instruct doxygen to hide any
+# special comment blocks from generated source code fragments. Normal C, C++ and
+# Fortran comments will always remain visible.
+# The default value is: YES.
+
+STRIP_CODE_COMMENTS    = YES
+
+# If the REFERENCED_BY_RELATION tag is set to YES then for each documented
+# entity all documented functions referencing it will be listed.
+# The default value is: NO.
+
+REFERENCED_BY_RELATION = NO
+
+# If the REFERENCES_RELATION tag is set to YES then for each documented function
+# all documented entities called/used by that function will be listed.
+# The default value is: NO.
+
+REFERENCES_RELATION    = NO
+
+# If the REFERENCES_LINK_SOURCE tag is set to YES and SOURCE_BROWSER tag is set
+# to YES then the hyperlinks from functions in REFERENCES_RELATION and
+# REFERENCED_BY_RELATION lists will link to the source code. Otherwise they will
+# link to the documentation.
+# The default value is: YES.
+
+REFERENCES_LINK_SOURCE = YES
+
+# If SOURCE_TOOLTIPS is enabled (the default) then hovering a hyperlink in the
+# source code will show a tooltip with additional information such as prototype,
+# brief description and links to the definition and documentation. Since this
+# will make the HTML file larger and loading of large files a bit slower, you
+# can opt to disable this feature.
+# The default value is: YES.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+SOURCE_TOOLTIPS        = YES
+
+# If the USE_HTAGS tag is set to YES then the references to source code will
+# point to the HTML generated by the htags(1) tool instead of doxygen built-in
+# source browser. The htags tool is part of GNU's global source tagging system
+# (see https://www.gnu.org/software/global/global.html). You will need version
+# 4.8.6 or higher.
+#
+# To use it do the following:
+# - Install the latest version of global
+# - Enable SOURCE_BROWSER and USE_HTAGS in the configuration file
+# - Make sure the INPUT points to the root of the source tree
+# - Run doxygen as normal
+#
+# Doxygen will invoke htags (and that will in turn invoke gtags), so these
+# tools must be available from the command line (i.e. in the search path).
+#
+# The result: instead of the source browser generated by doxygen, the links to
+# source code will now point to the output of htags.
+# The default value is: NO.
+# This tag requires that the tag SOURCE_BROWSER is set to YES.
+
+USE_HTAGS              = NO
+
+# If the VERBATIM_HEADERS tag is set the YES then doxygen will generate a
+# verbatim copy of the header file for each class for which an include is
+# specified. Set to NO to disable this.
+# See also: Section \class.
+# The default value is: YES.
+
+VERBATIM_HEADERS       = YES
+
+# If the CLANG_ASSISTED_PARSING tag is set to YES then doxygen will use the
+# clang parser (see:
+# http://clang.llvm.org/) for more accurate parsing at the cost of reduced
+# performance. This can be particularly helpful with template rich C++ code for
+# which doxygen's built-in parser lacks the necessary type information.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse_libclang=ON option for CMake.
+# The default value is: NO.
+
+CLANG_ASSISTED_PARSING = NO
+
+# If clang assisted parsing is enabled and the CLANG_ADD_INC_PATHS tag is set to
+# YES then doxygen will add the directory of each input to the include path.
+# The default value is: YES.
+
+CLANG_ADD_INC_PATHS    = YES
+
+# If clang assisted parsing is enabled you can provide the compiler with command
+# line options that you would normally use when invoking the compiler. Note that
+# the include paths will already be set by doxygen for the files and directories
+# specified with INPUT and INCLUDE_PATH.
+# This tag requires that the tag CLANG_ASSISTED_PARSING is set to YES.
+
+CLANG_OPTIONS          =
+
+# If clang assisted parsing is enabled you can provide the clang parser with the
+# path to the directory containing a file called compile_commands.json. This
+# file is the compilation database (see:
+# http://clang.llvm.org/docs/HowToSetupToolingForLLVM.html) containing the
+# options used when the source files were built. This is equivalent to
+# specifying the -p option to a clang tool, such as clang-check. These options
+# will then be passed to the parser. Any options specified with CLANG_OPTIONS
+# will be added as well.
+# Note: The availability of this option depends on whether or not doxygen was
+# generated with the -Duse_libclang=ON option for CMake.
+
+CLANG_DATABASE_PATH    =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the alphabetical class index
+#---------------------------------------------------------------------------
+
+# If the ALPHABETICAL_INDEX tag is set to YES, an alphabetical index of all
+# compounds will be generated. Enable this if the project contains a lot of
+# classes, structs, unions or interfaces.
+# The default value is: YES.
+
+ALPHABETICAL_INDEX     = YES
+
+# In case all classes in a project start with a common prefix, all classes will
+# be put under the same header in the alphabetical index. The IGNORE_PREFIX tag
+# can be used to specify a prefix (or a list of prefixes) that should be ignored
+# while generating the index headers.
+# This tag requires that the tag ALPHABETICAL_INDEX is set to YES.
+
+IGNORE_PREFIX          =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the HTML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_HTML tag is set to YES, doxygen will generate HTML output
+# The default value is: YES.
+
+GENERATE_HTML          = YES
+
+# The HTML_OUTPUT tag is used to specify where the HTML docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_OUTPUT            = html
+
+# The HTML_FILE_EXTENSION tag can be used to specify the file extension for each
+# generated HTML page (for example: .htm, .php, .asp).
+# The default value is: .html.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FILE_EXTENSION    = .html
+
+# The HTML_HEADER tag can be used to specify a user-defined HTML header file for
+# each generated HTML page. If the tag is left blank doxygen will generate a
+# standard header.
+#
+# To get valid HTML the header file that includes any scripts and style sheets
+# that doxygen needs, which is dependent on the configuration options used (e.g.
+# the setting GENERATE_TREEVIEW). It is highly recommended to start with a
+# default header using
+# doxygen -w html new_header.html new_footer.html new_stylesheet.css
+# YourConfigFile
+# and then modify the file new_header.html. See also section "Doxygen usage"
+# for information on how to generate the default header that doxygen normally
+# uses.
+# Note: The header is subject to change so you typically have to regenerate the
+# default header when upgrading to a newer version of doxygen. For a description
+# of the possible markers and block names see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_HEADER            =
+
+# The HTML_FOOTER tag can be used to specify a user-defined HTML footer for each
+# generated HTML page. If the tag is left blank doxygen will generate a standard
+# footer. See HTML_HEADER for more information on how to generate a default
+# footer and what special commands can be used inside the footer. See also
+# section "Doxygen usage" for information on how to generate the default footer
+# that doxygen normally uses.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FOOTER            =
+
+# The HTML_STYLESHEET tag can be used to specify a user-defined cascading style
+# sheet that is used by each HTML page. It can be used to fine-tune the look of
+# the HTML output. If left blank doxygen will generate a default style sheet.
+# See also section "Doxygen usage" for information on how to generate the style
+# sheet that doxygen normally uses.
+# Note: It is recommended to use HTML_EXTRA_STYLESHEET instead of this tag, as
+# it is more robust and this tag (HTML_STYLESHEET) will in the future become
+# obsolete.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_STYLESHEET        =
+
+# The HTML_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# cascading style sheets that are included after the standard style sheets
+# created by doxygen. Using this option one can overrule certain style aspects.
+# This is preferred over using HTML_STYLESHEET since it does not replace the
+# standard style sheet and is therefore more robust against future updates.
+# Doxygen will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list). For an example see the documentation.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_STYLESHEET  =
+
+# The HTML_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the HTML output directory. Note
+# that these files will be copied to the base HTML output directory. Use the
+# $relpath^ marker in the HTML_HEADER and/or HTML_FOOTER files to load these
+# files. In the HTML_STYLESHEET file, use the file name only. Also note that the
+# files will be copied as-is; there are no commands or markers available.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_EXTRA_FILES       =
+
+# The HTML_COLORSTYLE_HUE tag controls the color of the HTML output. Doxygen
+# will adjust the colors in the style sheet and background images according to
+# this color. Hue is specified as an angle on a colorwheel, see
+# https://en.wikipedia.org/wiki/Hue for more information. For instance the value
+# 0 represents red, 60 is yellow, 120 is green, 180 is cyan, 240 is blue, 300
+# purple, and 360 is red again.
+# Minimum value: 0, maximum value: 359, default value: 220.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_HUE    = 220
+
+# The HTML_COLORSTYLE_SAT tag controls the purity (or saturation) of the colors
+# in the HTML output. For a value of 0 the output will use grayscales only. A
+# value of 255 will produce the most vivid colors.
+# Minimum value: 0, maximum value: 255, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_SAT    = 100
+
+# The HTML_COLORSTYLE_GAMMA tag controls the gamma correction applied to the
+# luminance component of the colors in the HTML output. Values below 100
+# gradually make the output lighter, whereas values above 100 make the output
+# darker. The value divided by 100 is the actual gamma applied, so 80 represents
+# a gamma of 0.8, The value 220 represents a gamma of 2.2, and 100 does not
+# change the gamma.
+# Minimum value: 40, maximum value: 240, default value: 80.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_COLORSTYLE_GAMMA  = 80
+
+# If the HTML_TIMESTAMP tag is set to YES then the footer of each generated HTML
+# page will contain the date and time when the page was generated. Setting this
+# to YES can help to show when doxygen was last run and thus if the
+# documentation is up to date.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_TIMESTAMP         = NO
+
+# If the HTML_DYNAMIC_MENUS tag is set to YES then the generated HTML
+# documentation will contain a main index with vertical navigation menus that
+# are dynamically created via JavaScript. If disabled, the navigation index will
+# consists of multiple levels of tabs that are statically embedded in every HTML
+# page. Disable this option to support browsers that do not have JavaScript,
+# like the Qt help browser.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_MENUS     = YES
+
+# If the HTML_DYNAMIC_SECTIONS tag is set to YES then the generated HTML
+# documentation will contain sections that can be hidden and shown after the
+# page has loaded.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_DYNAMIC_SECTIONS  = NO
+
+# With HTML_INDEX_NUM_ENTRIES one can control the preferred number of entries
+# shown in the various tree structured indices initially; the user can expand
+# and collapse entries dynamically later on. Doxygen will expand the tree to
+# such a level that at most the specified number of entries are visible (unless
+# a fully collapsed tree already exceeds this amount). So setting the number of
+# entries 1 will produce a full collapsed tree by default. 0 is a special value
+# representing an infinite number of entries and will result in a full expanded
+# tree by default.
+# Minimum value: 0, maximum value: 9999, default value: 100.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_INDEX_NUM_ENTRIES = 100
+
+# If the GENERATE_DOCSET tag is set to YES, additional index files will be
+# generated that can be used as input for Apple's Xcode 3 integrated development
+# environment (see:
+# https://developer.apple.com/xcode/), introduced with OSX 10.5 (Leopard). To
+# create a documentation set, doxygen will generate a Makefile in the HTML
+# output directory. Running make will produce the docset in that directory and
+# running make install will install the docset in
+# ~/Library/Developer/Shared/Documentation/DocSets so that Xcode will find it at
+# startup. See https://developer.apple.com/library/archive/featuredarticles/Doxy
+# genXcode/_index.html for more information.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_DOCSET        = NO
+
+# This tag determines the name of the docset feed. A documentation feed provides
+# an umbrella under which multiple documentation sets from a single provider
+# (such as a company or product suite) can be grouped.
+# The default value is: Doxygen generated docs.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_FEEDNAME        = "Doxygen generated docs"
+
+# This tag specifies a string that should uniquely identify the documentation
+# set bundle. This should be a reverse domain-name style string, e.g.
+# com.mycompany.MyDocSet. Doxygen will append .docset to the name.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_BUNDLE_ID       = org.doxygen.Project
+
+# The DOCSET_PUBLISHER_ID tag specifies a string that should uniquely identify
+# the documentation publisher. This should be a reverse domain-name style
+# string, e.g. com.mycompany.MyDocSet.documentation.
+# The default value is: org.doxygen.Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_ID    = org.doxygen.Publisher
+
+# The DOCSET_PUBLISHER_NAME tag identifies the documentation publisher.
+# The default value is: Publisher.
+# This tag requires that the tag GENERATE_DOCSET is set to YES.
+
+DOCSET_PUBLISHER_NAME  = Publisher
+
+# If the GENERATE_HTMLHELP tag is set to YES then doxygen generates three
+# additional HTML index files: index.hhp, index.hhc, and index.hhk. The
+# index.hhp is a project file that can be read by Microsoft's HTML Help Workshop
+# (see:
+# https://www.microsoft.com/en-us/download/details.aspx?id=21138) on Windows.
+#
+# The HTML Help Workshop contains a compiler that can convert all HTML output
+# generated by doxygen into a single compiled HTML file (.chm). Compiled HTML
+# files are now used as the Windows 98 help format, and will replace the old
+# Windows help format (.hlp) on all Windows platforms in the future. Compressed
+# HTML files also contain an index, a table of contents, and you can search for
+# words in the documentation. The HTML workshop also contains a viewer for
+# compressed HTML files.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_HTMLHELP      = NO
+
+# The CHM_FILE tag can be used to specify the file name of the resulting .chm
+# file. You can add a path in front of the file if the result should not be
+# written to the html output directory.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_FILE               =
+
+# The HHC_LOCATION tag can be used to specify the location (absolute path
+# including file name) of the HTML help compiler (hhc.exe). If non-empty,
+# doxygen will try to run the HTML help compiler on the generated index.hhp.
+# The file has to be specified with full path.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+HHC_LOCATION           =
+
+# The GENERATE_CHI flag controls if a separate .chi index file is generated
+# (YES) or that it should be included in the main .chm file (NO).
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+GENERATE_CHI           = NO
+
+# The CHM_INDEX_ENCODING is used to encode HtmlHelp index (hhk), content (hhc)
+# and project file content.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+CHM_INDEX_ENCODING     =
+
+# The BINARY_TOC flag controls whether a binary table of contents is generated
+# (YES) or a normal table of contents (NO) in the .chm file. Furthermore it
+# enables the Previous and Next buttons.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+BINARY_TOC             = NO
+
+# The TOC_EXPAND flag can be set to YES to add extra items for group members to
+# the table of contents of the HTML help documentation and to the tree view.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTMLHELP is set to YES.
+
+TOC_EXPAND             = NO
+
+# If the GENERATE_QHP tag is set to YES and both QHP_NAMESPACE and
+# QHP_VIRTUAL_FOLDER are set, an additional index file will be generated that
+# can be used as input for Qt's qhelpgenerator to generate a Qt Compressed Help
+# (.qch) of the generated HTML documentation.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_QHP           = NO
+
+# If the QHG_LOCATION tag is specified, the QCH_FILE tag can be used to specify
+# the file name of the resulting .qch file. The path specified is relative to
+# the HTML output folder.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QCH_FILE               =
+
+# The QHP_NAMESPACE tag specifies the namespace to use when generating Qt Help
+# Project output. For more information please see Qt Help Project / Namespace
+# (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#namespace).
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_NAMESPACE          = org.doxygen.Project
+
+# The QHP_VIRTUAL_FOLDER tag specifies the namespace to use when generating Qt
+# Help Project output. For more information please see Qt Help Project / Virtual
+# Folders (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#virtual-folders).
+# The default value is: doc.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_VIRTUAL_FOLDER     = doc
+
+# If the QHP_CUST_FILTER_NAME tag is set, it specifies the name of a custom
+# filter to add. For more information please see Qt Help Project / Custom
+# Filters (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_NAME   =
+
+# The QHP_CUST_FILTER_ATTRS tag specifies the list of the attributes of the
+# custom filter to add. For more information please see Qt Help Project / Custom
+# Filters (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#custom-filters).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_CUST_FILTER_ATTRS  =
+
+# The QHP_SECT_FILTER_ATTRS tag specifies the list of the attributes this
+# project's filter section matches. Qt Help Project / Filter Attributes (see:
+# https://doc.qt.io/archives/qt-4.8/qthelpproject.html#filter-attributes).
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHP_SECT_FILTER_ATTRS  =
+
+# The QHG_LOCATION tag can be used to specify the location (absolute path
+# including file name) of Qt's qhelpgenerator. If non-empty doxygen will try to
+# run qhelpgenerator on the generated .qhp file.
+# This tag requires that the tag GENERATE_QHP is set to YES.
+
+QHG_LOCATION           =
+
+# If the GENERATE_ECLIPSEHELP tag is set to YES, additional index files will be
+# generated, together with the HTML files, they form an Eclipse help plugin. To
+# install this plugin and make it available under the help contents menu in
+# Eclipse, the contents of the directory containing the HTML and XML files needs
+# to be copied into the plugins directory of eclipse. The name of the directory
+# within the plugins directory should be the same as the ECLIPSE_DOC_ID value.
+# After copying Eclipse needs to be restarted before the help appears.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_ECLIPSEHELP   = NO
+
+# A unique identifier for the Eclipse help plugin. When installing the plugin
+# the directory name containing the HTML and XML files should also have this
+# name. Each documentation set should have its own identifier.
+# The default value is: org.doxygen.Project.
+# This tag requires that the tag GENERATE_ECLIPSEHELP is set to YES.
+
+ECLIPSE_DOC_ID         = org.doxygen.Project
+
+# If you want full control over the layout of the generated HTML pages it might
+# be necessary to disable the index and replace it with your own. The
+# DISABLE_INDEX tag can be used to turn on/off the condensed index (tabs) at top
+# of each HTML page. A value of NO enables the index and the value YES disables
+# it. Since the tabs in the index contain the same information as the navigation
+# tree, you can set this option to YES if you also set GENERATE_TREEVIEW to YES.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+DISABLE_INDEX          = NO
+
+# The GENERATE_TREEVIEW tag is used to specify whether a tree-like index
+# structure should be generated to display hierarchical information. If the tag
+# value is set to YES, a side panel will be generated containing a tree-like
+# index structure (just like the one that is generated for HTML Help). For this
+# to work a browser that supports JavaScript, DHTML, CSS and frames is required
+# (i.e. any modern browser). Windows users are probably better off using the
+# HTML help feature. Via custom style sheets (see HTML_EXTRA_STYLESHEET) one can
+# further fine-tune the look of the index. As an example, the default style
+# sheet generated by doxygen has an example that shows how to put an image at
+# the root of the tree instead of the PROJECT_NAME. Since the tree basically has
+# the same information as the tab index, you could consider setting
+# DISABLE_INDEX to YES when enabling this option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+GENERATE_TREEVIEW      = NO
+
+# The ENUM_VALUES_PER_LINE tag can be used to set the number of enum values that
+# doxygen will group on one line in the generated HTML documentation.
+#
+# Note that a value of 0 will completely suppress the enum values from appearing
+# in the overview section.
+# Minimum value: 0, maximum value: 20, default value: 4.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+ENUM_VALUES_PER_LINE   = 4
+
+# If the treeview is enabled (see GENERATE_TREEVIEW) then this tag can be used
+# to set the initial width (in pixels) of the frame in which the tree is shown.
+# Minimum value: 0, maximum value: 1500, default value: 250.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+TREEVIEW_WIDTH         = 250
+
+# If the EXT_LINKS_IN_WINDOW option is set to YES, doxygen will open links to
+# external symbols imported via tag files in a separate window.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+EXT_LINKS_IN_WINDOW    = NO
+
+# If the HTML_FORMULA_FORMAT option is set to svg, doxygen will use the pdf2svg
+# tool (see https://github.com/dawbarton/pdf2svg) or inkscape (see
+# https://inkscape.org) to generate formulas as SVG images instead of PNGs for
+# the HTML output. These images will generally look nicer at scaled resolutions.
+# Possible values are: png (the default) and svg (looks nicer but requires the
+# pdf2svg or inkscape tool).
+# The default value is: png.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+HTML_FORMULA_FORMAT    = png
+
+# Use this tag to change the font size of LaTeX formulas included as images in
+# the HTML documentation. When you change the font size after a successful
+# doxygen run you need to manually remove any form_*.png images from the HTML
+# output directory to force them to be regenerated.
+# Minimum value: 8, maximum value: 50, default value: 10.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_FONTSIZE       = 10
+
+# Use the FORMULA_TRANSPARENT tag to determine whether or not the images
+# generated for formulas are transparent PNGs. Transparent PNGs are not
+# supported properly for IE 6.0, but are supported on all modern browsers.
+#
+# Note that when changing this option you need to delete any form_*.png files in
+# the HTML output directory before the changes have effect.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+FORMULA_TRANSPARENT    = YES
+
+# The FORMULA_MACROFILE can contain LaTeX \newcommand and \renewcommand commands
+# to create new LaTeX commands to be used in formulas as building blocks. See
+# the section "Including formulas" for details.
+
+FORMULA_MACROFILE      =
+
+# Enable the USE_MATHJAX option to render LaTeX formulas using MathJax (see
+# https://www.mathjax.org) which uses client side JavaScript for the rendering
+# instead of using pre-rendered bitmaps. Use this if you do not have LaTeX
+# installed or if you want to formulas look prettier in the HTML output. When
+# enabled you may also need to install MathJax separately and configure the path
+# to it using the MATHJAX_RELPATH option.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+USE_MATHJAX            = YES
+
+# When MathJax is enabled you can set the default output format to be used for
+# the MathJax output. See the MathJax site (see:
+# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details.
+# Possible values are: HTML-CSS (which is slower, but has the best
+# compatibility), NativeMML (i.e. MathML) and SVG.
+# The default value is: HTML-CSS.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_FORMAT         = HTML-CSS
+
+# When MathJax is enabled you need to specify the location relative to the HTML
+# output directory using the MATHJAX_RELPATH option. The destination directory
+# should contain the MathJax.js script. For instance, if the mathjax directory
+# is located at the same level as the HTML output directory, then
+# MATHJAX_RELPATH should be ../mathjax. The default value points to the MathJax
+# Content Delivery Network so you can quickly see the result without installing
+# MathJax. However, it is strongly recommended to install a local copy of
+# MathJax from https://www.mathjax.org before deployment.
+# The default value is: https://cdn.jsdelivr.net/npm/mathjax@2.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_RELPATH        = https://cdn.mathjax.org/mathjax/latest
+
+# The MATHJAX_EXTENSIONS tag can be used to specify one or more MathJax
+# extension names that should be enabled during MathJax rendering. For example
+# MATHJAX_EXTENSIONS = TeX/AMSmath TeX/AMSsymbols
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_EXTENSIONS     = TeX/AMSmath
+
+# The MATHJAX_CODEFILE tag can be used to specify a file with javascript pieces
+# of code that will be used on startup of the MathJax code. See the MathJax site
+# (see:
+# http://docs.mathjax.org/en/v2.7-latest/output.html) for more details. For an
+# example see the documentation.
+# This tag requires that the tag USE_MATHJAX is set to YES.
+
+MATHJAX_CODEFILE       =
+
+# When the SEARCHENGINE tag is enabled doxygen will generate a search box for
+# the HTML output. The underlying search engine uses javascript and DHTML and
+# should work on any modern browser. Note that when using HTML help
+# (GENERATE_HTMLHELP), Qt help (GENERATE_QHP), or docsets (GENERATE_DOCSET)
+# there is already a search function so this one should typically be disabled.
+# For large projects the javascript based search engine can be slow, then
+# enabling SERVER_BASED_SEARCH may provide a better solution. It is possible to
+# search using the keyboard; to jump to the search box use <access key> + S
+# (what the <access key> is depends on the OS and browser, but it is typically
+# <CTRL>, <ALT>/<option>, or both). Inside the search box use the <cursor down
+# key> to jump into the search results window, the results can be navigated
+# using the <cursor keys>. Press <Enter> to select an item or <escape> to cancel
+# the search. The filter options can be selected when the cursor is inside the
+# search box by pressing <Shift>+<cursor down>. Also here use the <cursor keys>
+# to select a filter and <Enter> or <escape> to activate or cancel the filter
+# option.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_HTML is set to YES.
+
+SEARCHENGINE           = YES
+
+# When the SERVER_BASED_SEARCH tag is enabled the search engine will be
+# implemented using a web server instead of a web client using JavaScript. There
+# are two flavors of web server based searching depending on the EXTERNAL_SEARCH
+# setting. When disabled, doxygen will generate a PHP script for searching and
+# an index file used by the script. When EXTERNAL_SEARCH is enabled the indexing
+# and searching needs to be provided by external tools. See the section
+# "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SERVER_BASED_SEARCH    = NO
+
+# When EXTERNAL_SEARCH tag is enabled doxygen will no longer generate the PHP
+# script for searching. Instead the search results are written to an XML file
+# which needs to be processed by an external indexer. Doxygen will invoke an
+# external search engine pointed to by the SEARCHENGINE_URL option to obtain the
+# search results.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see:
+# https://xapian.org/).
+#
+# See the section "External Indexing and Searching" for details.
+# The default value is: NO.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH        = NO
+
+# The SEARCHENGINE_URL should point to a search engine hosted by a web server
+# which will return the search results when EXTERNAL_SEARCH is enabled.
+#
+# Doxygen ships with an example indexer (doxyindexer) and search engine
+# (doxysearch.cgi) which are based on the open source search engine library
+# Xapian (see:
+# https://xapian.org/). See the section "External Indexing and Searching" for
+# details.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHENGINE_URL       =
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the unindexed
+# search data is written to a file for indexing by an external tool. With the
+# SEARCHDATA_FILE tag the name of this file can be specified.
+# The default file is: searchdata.xml.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+SEARCHDATA_FILE        = searchdata.xml
+
+# When SERVER_BASED_SEARCH and EXTERNAL_SEARCH are both enabled the
+# EXTERNAL_SEARCH_ID tag can be used as an identifier for the project. This is
+# useful in combination with EXTRA_SEARCH_MAPPINGS to search through multiple
+# projects and redirect the results back to the right project.
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTERNAL_SEARCH_ID     =
+
+# The EXTRA_SEARCH_MAPPINGS tag can be used to enable searching through doxygen
+# projects other than the one defined by this configuration file, but that are
+# all added to the same external search index. Each project needs to have a
+# unique id set via EXTERNAL_SEARCH_ID. The search mapping then maps the id of
+# to a relative location where the documentation can be found. The format is:
+# EXTRA_SEARCH_MAPPINGS = tagname1=loc1 tagname2=loc2 ...
+# This tag requires that the tag SEARCHENGINE is set to YES.
+
+EXTRA_SEARCH_MAPPINGS  =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the LaTeX output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_LATEX tag is set to YES, doxygen will generate LaTeX output.
+# The default value is: YES.
+
+GENERATE_LATEX         = NO
+
+# The LATEX_OUTPUT tag is used to specify where the LaTeX docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: latex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_OUTPUT           = latex
+
+# The LATEX_CMD_NAME tag can be used to specify the LaTeX command name to be
+# invoked.
+#
+# Note that when not enabling USE_PDFLATEX the default is latex when enabling
+# USE_PDFLATEX the default is pdflatex and when in the later case latex is
+# chosen this is overwritten by pdflatex. For specific output languages the
+# default can have been set differently, this depends on the implementation of
+# the output language.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_CMD_NAME         = latex
+
+# The MAKEINDEX_CMD_NAME tag can be used to specify the command name to generate
+# index for LaTeX.
+# Note: This tag is used in the Makefile / make.bat.
+# See also: LATEX_MAKEINDEX_CMD for the part in the generated output file
+# (.tex).
+# The default file is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+MAKEINDEX_CMD_NAME     = makeindex
+
+# The LATEX_MAKEINDEX_CMD tag can be used to specify the command name to
+# generate index for LaTeX. In case there is no backslash (\) as first character
+# it will be automatically added in the LaTeX code.
+# Note: This tag is used in the generated output file (.tex).
+# See also: MAKEINDEX_CMD_NAME for the part in the Makefile / make.bat.
+# The default value is: makeindex.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_MAKEINDEX_CMD    = makeindex
+
+# If the COMPACT_LATEX tag is set to YES, doxygen generates more compact LaTeX
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+COMPACT_LATEX          = NO
+
+# The PAPER_TYPE tag can be used to set the paper type that is used by the
+# printer.
+# Possible values are: a4 (210 x 297 mm), letter (8.5 x 11 inches), legal (8.5 x
+# 14 inches) and executive (7.25 x 10.5 inches).
+# The default value is: a4.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PAPER_TYPE             = a4
+
+# The EXTRA_PACKAGES tag can be used to specify one or more LaTeX package names
+# that should be included in the LaTeX output. The package can be specified just
+# by its name or with the correct syntax as to be used with the LaTeX
+# \usepackage command. To get the times font for instance you can specify :
+# EXTRA_PACKAGES=times or EXTRA_PACKAGES={times}
+# To use the option intlimits with the amsmath package you can specify:
+# EXTRA_PACKAGES=[intlimits]{amsmath}
+# If left blank no extra packages will be included.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+EXTRA_PACKAGES         =
+
+# The LATEX_HEADER tag can be used to specify a personal LaTeX header for the
+# generated LaTeX document. The header should contain everything until the first
+# chapter. If it is left blank doxygen will generate a standard header. See
+# section "Doxygen usage" for information on how to let doxygen write the
+# default header to a separate file.
+#
+# Note: Only use a user-defined header if you know what you are doing! The
+# following commands have a special meaning inside the header: $title,
+# $datetime, $date, $doxygenversion, $projectname, $projectnumber,
+# $projectbrief, $projectlogo. Doxygen will replace $title with the empty
+# string, for the replacement values of the other commands the user is referred
+# to HTML_HEADER.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HEADER           =
+
+# The LATEX_FOOTER tag can be used to specify a personal LaTeX footer for the
+# generated LaTeX document. The footer should contain everything after the last
+# chapter. If it is left blank doxygen will generate a standard footer. See
+# LATEX_HEADER for more information on how to generate a default footer and what
+# special commands can be used inside the footer.
+#
+# Note: Only use a user-defined footer if you know what you are doing!
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_FOOTER           =
+
+# The LATEX_EXTRA_STYLESHEET tag can be used to specify additional user-defined
+# LaTeX style sheets that are included after the standard style sheets created
+# by doxygen. Using this option one can overrule certain style aspects. Doxygen
+# will copy the style sheet files to the output directory.
+# Note: The order of the extra style sheet files is of importance (e.g. the last
+# style sheet in the list overrules the setting of the previous ones in the
+# list).
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_STYLESHEET =
+
+# The LATEX_EXTRA_FILES tag can be used to specify one or more extra images or
+# other source files which should be copied to the LATEX_OUTPUT output
+# directory. Note that the files will be copied as-is; there are no commands or
+# markers available.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EXTRA_FILES      =
+
+# If the PDF_HYPERLINKS tag is set to YES, the LaTeX that is generated is
+# prepared for conversion to PDF (using ps2pdf or pdflatex). The PDF file will
+# contain links (just like the HTML output) instead of page references. This
+# makes the output suitable for online browsing using a PDF viewer.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+PDF_HYPERLINKS         = YES
+
+# If the USE_PDFLATEX tag is set to YES, doxygen will use the engine as
+# specified with LATEX_CMD_NAME to generate the PDF file directly from the LaTeX
+# files. Set this option to YES, to get a higher quality PDF documentation.
+#
+# See also section LATEX_CMD_NAME for selecting the engine.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+USE_PDFLATEX           = YES
+
+# If the LATEX_BATCHMODE tag is set to YES, doxygen will add the \batchmode
+# command to the generated LaTeX files. This will instruct LaTeX to keep running
+# if errors occur, instead of asking the user for help. This option is also used
+# when generating formulas in HTML.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BATCHMODE        = NO
+
+# If the LATEX_HIDE_INDICES tag is set to YES then doxygen will not include the
+# index chapters (such as File Index, Compound Index, etc.) in the output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_HIDE_INDICES     = NO
+
+# If the LATEX_SOURCE_CODE tag is set to YES then doxygen will include source
+# code with syntax highlighting in the LaTeX output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_SOURCE_CODE      = NO
+
+# The LATEX_BIB_STYLE tag can be used to specify the style to use for the
+# bibliography, e.g. plainnat, or ieeetr. See
+# https://en.wikipedia.org/wiki/BibTeX and \cite for more info.
+# The default value is: plain.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_BIB_STYLE        = plain
+
+# If the LATEX_TIMESTAMP tag is set to YES then the footer of each generated
+# page will contain the date and time when the page was generated. Setting this
+# to NO can help when comparing the output of multiple runs.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_TIMESTAMP        = NO
+
+# The LATEX_EMOJI_DIRECTORY tag is used to specify the (relative or absolute)
+# path from which the emoji images will be read. If a relative path is entered,
+# it will be relative to the LATEX_OUTPUT directory. If left blank the
+# LATEX_OUTPUT directory will be used.
+# This tag requires that the tag GENERATE_LATEX is set to YES.
+
+LATEX_EMOJI_DIRECTORY  =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the RTF output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_RTF tag is set to YES, doxygen will generate RTF output. The
+# RTF output is optimized for Word 97 and may not look too pretty with other RTF
+# readers/editors.
+# The default value is: NO.
+
+GENERATE_RTF           = NO
+
+# The RTF_OUTPUT tag is used to specify where the RTF docs will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: rtf.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_OUTPUT             = rtf
+
+# If the COMPACT_RTF tag is set to YES, doxygen generates more compact RTF
+# documents. This may be useful for small projects and may help to save some
+# trees in general.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+COMPACT_RTF            = NO
+
+# If the RTF_HYPERLINKS tag is set to YES, the RTF that is generated will
+# contain hyperlink fields. The RTF file will contain links (just like the HTML
+# output) instead of page references. This makes the output suitable for online
+# browsing using Word or some other Word compatible readers that support those
+# fields.
+#
+# Note: WordPad (write) and others do not support links.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_HYPERLINKS         = NO
+
+# Load stylesheet definitions from file. Syntax is similar to doxygen's
+# configuration file, i.e. a series of assignments. You only have to provide
+# replacements, missing definitions are set to their default value.
+#
+# See also section "Doxygen usage" for information on how to generate the
+# default style sheet that doxygen normally uses.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_STYLESHEET_FILE    =
+
+# Set optional variables used in the generation of an RTF document. Syntax is
+# similar to doxygen's configuration file. A template extensions file can be
+# generated using doxygen -e rtf extensionFile.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_EXTENSIONS_FILE    =
+
+# If the RTF_SOURCE_CODE tag is set to YES then doxygen will include source code
+# with syntax highlighting in the RTF output.
+#
+# Note that which sources are shown also depends on other settings such as
+# SOURCE_BROWSER.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_RTF is set to YES.
+
+RTF_SOURCE_CODE        = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the man page output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_MAN tag is set to YES, doxygen will generate man pages for
+# classes and files.
+# The default value is: NO.
+
+GENERATE_MAN           = NO
+
+# The MAN_OUTPUT tag is used to specify where the man pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it. A directory man3 will be created inside the directory specified by
+# MAN_OUTPUT.
+# The default directory is: man.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_OUTPUT             = man
+
+# The MAN_EXTENSION tag determines the extension that is added to the generated
+# man pages. In case the manual section does not start with a number, the number
+# 3 is prepended. The dot (.) at the beginning of the MAN_EXTENSION tag is
+# optional.
+# The default value is: .3.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_EXTENSION          = .3
+
+# The MAN_SUBDIR tag determines the name of the directory created within
+# MAN_OUTPUT in which the man pages are placed. If defaults to man followed by
+# MAN_EXTENSION with the initial . removed.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_SUBDIR             =
+
+# If the MAN_LINKS tag is set to YES and doxygen generates man output, then it
+# will generate one additional man file for each entity documented in the real
+# man page(s). These additional files only source the real man page, but without
+# them the man command would be unable to find the correct page.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_MAN is set to YES.
+
+MAN_LINKS              = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the XML output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_XML tag is set to YES, doxygen will generate an XML file that
+# captures the structure of the code including all documentation.
+# The default value is: NO.
+
+GENERATE_XML           = NO
+
+# The XML_OUTPUT tag is used to specify where the XML pages will be put. If a
+# relative path is entered the value of OUTPUT_DIRECTORY will be put in front of
+# it.
+# The default directory is: xml.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_OUTPUT             = xml
+
+# If the XML_PROGRAMLISTING tag is set to YES, doxygen will dump the program
+# listings (including syntax highlighting and cross-referencing information) to
+# the XML output. Note that enabling this will significantly increase the size
+# of the XML output.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_PROGRAMLISTING     = YES
+
+# If the XML_NS_MEMB_FILE_SCOPE tag is set to YES, doxygen will include
+# namespace members in file scope as well, matching the HTML output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_XML is set to YES.
+
+XML_NS_MEMB_FILE_SCOPE = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the DOCBOOK output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_DOCBOOK tag is set to YES, doxygen will generate Docbook files
+# that can be used to generate PDF.
+# The default value is: NO.
+
+GENERATE_DOCBOOK       = NO
+
+# The DOCBOOK_OUTPUT tag is used to specify where the Docbook pages will be put.
+# If a relative path is entered the value of OUTPUT_DIRECTORY will be put in
+# front of it.
+# The default directory is: docbook.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_OUTPUT         = docbook
+
+# If the DOCBOOK_PROGRAMLISTING tag is set to YES, doxygen will include the
+# program listings (including syntax highlighting and cross-referencing
+# information) to the DOCBOOK output. Note that enabling this will significantly
+# increase the size of the DOCBOOK output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_DOCBOOK is set to YES.
+
+DOCBOOK_PROGRAMLISTING = NO
+
+#---------------------------------------------------------------------------
+# Configuration options for the AutoGen Definitions output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_AUTOGEN_DEF tag is set to YES, doxygen will generate an
+# AutoGen Definitions (see http://autogen.sourceforge.net/) file that captures
+# the structure of the code including all documentation. Note that this feature
+# is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_AUTOGEN_DEF   = NO
+
+#---------------------------------------------------------------------------
+# Configuration options related to the Perl module output
+#---------------------------------------------------------------------------
+
+# If the GENERATE_PERLMOD tag is set to YES, doxygen will generate a Perl module
+# file that captures the structure of the code including all documentation.
+#
+# Note that this feature is still experimental and incomplete at the moment.
+# The default value is: NO.
+
+GENERATE_PERLMOD       = NO
+
+# If the PERLMOD_LATEX tag is set to YES, doxygen will generate the necessary
+# Makefile rules, Perl scripts and LaTeX code to be able to generate PDF and DVI
+# output from the Perl module output.
+# The default value is: NO.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_LATEX          = NO
+
+# If the PERLMOD_PRETTY tag is set to YES, the Perl module output will be nicely
+# formatted so it can be parsed by a human reader. This is useful if you want to
+# understand what is going on. On the other hand, if this tag is set to NO, the
+# size of the Perl module output will be much smaller and Perl will parse it
+# just the same.
+# The default value is: YES.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_PRETTY         = YES
+
+# The names of the make variables in the generated doxyrules.make file are
+# prefixed with the string contained in PERLMOD_MAKEVAR_PREFIX. This is useful
+# so different doxyrules.make files included by the same Makefile don't
+# overwrite each other's variables.
+# This tag requires that the tag GENERATE_PERLMOD is set to YES.
+
+PERLMOD_MAKEVAR_PREFIX =
+
+#---------------------------------------------------------------------------
+# Configuration options related to the preprocessor
+#---------------------------------------------------------------------------
+
+# If the ENABLE_PREPROCESSING tag is set to YES, doxygen will evaluate all
+# C-preprocessor directives found in the sources and include files.
+# The default value is: YES.
+
+ENABLE_PREPROCESSING   = YES
+
+# If the MACRO_EXPANSION tag is set to YES, doxygen will expand all macro names
+# in the source code. If set to NO, only conditional compilation will be
+# performed. Macro expansion can be done in a controlled way by setting
+# EXPAND_ONLY_PREDEF to YES.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+MACRO_EXPANSION        = NO
+
+# If the EXPAND_ONLY_PREDEF and MACRO_EXPANSION tags are both set to YES then
+# the macro expansion is limited to the macros specified with the PREDEFINED and
+# EXPAND_AS_DEFINED tags.
+# The default value is: NO.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_ONLY_PREDEF     = NO
+
+# If the SEARCH_INCLUDES tag is set to YES, the include files in the
+# INCLUDE_PATH will be searched if a #include is found.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SEARCH_INCLUDES        = YES
+
+# The INCLUDE_PATH tag can be used to specify one or more directories that
+# contain include files that are not input files but should be processed by the
+# preprocessor.
+# This tag requires that the tag SEARCH_INCLUDES is set to YES.
+
+INCLUDE_PATH           =
+
+# You can use the INCLUDE_FILE_PATTERNS tag to specify one or more wildcard
+# patterns (like *.h and *.hpp) to filter out the header-files in the
+# directories. If left blank, the patterns specified with FILE_PATTERNS will be
+# used.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+INCLUDE_FILE_PATTERNS  =
+
+# The PREDEFINED tag can be used to specify one or more macro names that are
+# defined before the preprocessor is started (similar to the -D option of e.g.
+# gcc). The argument of the tag is a list of macros of the form: name or
+# name=definition (no spaces). If the definition and the "=" are omitted, "=1"
+# is assumed. To prevent a macro definition from being undefined via #undef or
+# recursively expanded use the := operator instead of the = operator.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+PREDEFINED             =
+
+# If the MACRO_EXPANSION and EXPAND_ONLY_PREDEF tags are set to YES then this
+# tag can be used to specify a list of macro names that should be expanded. The
+# macro definition that is found in the sources will be used. Use the PREDEFINED
+# tag if you want to use a different macro definition that overrules the
+# definition found in the source code.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+EXPAND_AS_DEFINED      =
+
+# If the SKIP_FUNCTION_MACROS tag is set to YES then doxygen's preprocessor will
+# remove all references to function-like macros that are alone on a line, have
+# an all uppercase name, and do not end with a semicolon. Such function macros
+# are typically used for boiler-plate code, and will confuse the parser if not
+# removed.
+# The default value is: YES.
+# This tag requires that the tag ENABLE_PREPROCESSING is set to YES.
+
+SKIP_FUNCTION_MACROS   = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to external references
+#---------------------------------------------------------------------------
+
+# The TAGFILES tag can be used to specify one or more tag files. For each tag
+# file the location of the external documentation should be added. The format of
+# a tag file without this location is as follows:
+# TAGFILES = file1 file2 ...
+# Adding location for the tag files is done as follows:
+# TAGFILES = file1=loc1 "file2 = loc2" ...
+# where loc1 and loc2 can be relative or absolute paths or URLs. See the
+# section "Linking to external documentation" for more information about the use
+# of tag files.
+# Note: Each tag file must have a unique name (where the name does NOT include
+# the path). If a tag file is not located in the directory in which doxygen is
+# run, you must also specify the path to the tagfile here.
+
+TAGFILES               =
+
+# When a file name is specified after GENERATE_TAGFILE, doxygen will create a
+# tag file that is based on the input files it reads. See section "Linking to
+# external documentation" for more information about the usage of tag files.
+
+GENERATE_TAGFILE       =
+
+# If the ALLEXTERNALS tag is set to YES, all external class will be listed in
+# the class index. If set to NO, only the inherited external classes will be
+# listed.
+# The default value is: NO.
+
+ALLEXTERNALS           = NO
+
+# If the EXTERNAL_GROUPS tag is set to YES, all external groups will be listed
+# in the modules index. If set to NO, only the current project's groups will be
+# listed.
+# The default value is: YES.
+
+EXTERNAL_GROUPS        = YES
+
+# If the EXTERNAL_PAGES tag is set to YES, all external pages will be listed in
+# the related pages index. If set to NO, only the current project's pages will
+# be listed.
+# The default value is: YES.
+
+EXTERNAL_PAGES         = YES
+
+#---------------------------------------------------------------------------
+# Configuration options related to the dot tool
+#---------------------------------------------------------------------------
+
+# If the CLASS_DIAGRAMS tag is set to YES, doxygen will generate a class diagram
+# (in HTML and LaTeX) for classes with base or super classes. Setting the tag to
+# NO turns the diagrams off. Note that this option also works with HAVE_DOT
+# disabled, but it is recommended to install and use dot, since it yields more
+# powerful graphs.
+# The default value is: YES.
+
+CLASS_DIAGRAMS         = YES
+
+# You can include diagrams made with dia in doxygen documentation. Doxygen will
+# then run dia to produce the diagram and insert it in the documentation. The
+# DIA_PATH tag allows you to specify the directory where the dia binary resides.
+# If left empty dia is assumed to be found in the default search path.
+
+DIA_PATH               =
+
+# If set to YES the inheritance and collaboration graphs will hide inheritance
+# and usage relations if the target is undocumented or is not a class.
+# The default value is: YES.
+
+HIDE_UNDOC_RELATIONS   = YES
+
+# If you set the HAVE_DOT tag to YES then doxygen will assume the dot tool is
+# available from the path. This tool is part of Graphviz (see:
+# http://www.graphviz.org/), a graph visualization toolkit from AT&T and Lucent
+# Bell Labs. The other options in this section have no effect if this option is
+# set to NO
+# The default value is: YES.
+
+HAVE_DOT               = YES
+
+# The DOT_NUM_THREADS specifies the number of dot invocations doxygen is allowed
+# to run in parallel. When set to 0 doxygen will base this on the number of
+# processors available in the system. You can set it explicitly to a value
+# larger than 0 to get control over the balance between CPU load and processing
+# speed.
+# Minimum value: 0, maximum value: 32, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_NUM_THREADS        = 0
+
+# When you want a differently looking font in the dot files that doxygen
+# generates you can specify the font name using DOT_FONTNAME. You need to make
+# sure dot is able to find the font, which can be done by putting it in a
+# standard location or by setting the DOTFONTPATH environment variable or by
+# setting DOT_FONTPATH to the directory containing the font.
+# The default value is: Helvetica.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTNAME           = Helvetica
+
+# The DOT_FONTSIZE tag can be used to set the size (in points) of the font of
+# dot graphs.
+# Minimum value: 4, maximum value: 24, default value: 10.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTSIZE           = 10
+
+# By default doxygen will tell dot to use the default font as specified with
+# DOT_FONTNAME. If you specify a different font using DOT_FONTNAME you can set
+# the path where dot can find it using this tag.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_FONTPATH           =
+
+# If the CLASS_GRAPH tag is set to YES then doxygen will generate a graph for
+# each documented class showing the direct and indirect inheritance relations.
+# Setting this tag to YES will force the CLASS_DIAGRAMS tag to NO.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CLASS_GRAPH            = YES
+
+# If the COLLABORATION_GRAPH tag is set to YES then doxygen will generate a
+# graph for each documented class showing the direct and indirect implementation
+# dependencies (inheritance, containment, and class references variables) of the
+# class with other documented classes.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+COLLABORATION_GRAPH    = YES
+
+# If the GROUP_GRAPHS tag is set to YES then doxygen will generate a graph for
+# groups, showing the direct groups dependencies.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GROUP_GRAPHS           = YES
+
+# If the UML_LOOK tag is set to YES, doxygen will generate inheritance and
+# collaboration diagrams in a style similar to the OMG's Unified Modeling
+# Language.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+UML_LOOK               = NO
+
+# If the UML_LOOK tag is enabled, the fields and methods are shown inside the
+# class node. If there are many fields or methods and many nodes the graph may
+# become too big to be useful. The UML_LIMIT_NUM_FIELDS threshold limits the
+# number of items for each type to make the size more manageable. Set this to 0
+# for no limit. Note that the threshold may be exceeded by 50% before the limit
+# is enforced. So when you set the threshold to 10, up to 15 fields may appear,
+# but if the number exceeds 15, the total amount of fields shown is limited to
+# 10.
+# Minimum value: 0, maximum value: 100, default value: 10.
+# This tag requires that the tag UML_LOOK is set to YES.
+
+UML_LIMIT_NUM_FIELDS   = 10
+
+# If the DOT_UML_DETAILS tag is set to NO, doxygen will show attributes and
+# methods without types and arguments in the UML graphs. If the DOT_UML_DETAILS
+# tag is set to YES, doxygen will add type and arguments for attributes and
+# methods in the UML graphs. If the DOT_UML_DETAILS tag is set to NONE, doxygen
+# will not generate fields with class member information in the UML graphs. The
+# class diagrams will look similar to the default class diagrams but using UML
+# notation for the relationships.
+# Possible values are: NO, YES and NONE.
+# The default value is: NO.
+# This tag requires that the tag UML_LOOK is set to YES.
+
+DOT_UML_DETAILS        = NO
+
+# The DOT_WRAP_THRESHOLD tag can be used to set the maximum number of characters
+# to display on a single line. If the actual line length exceeds this threshold
+# significantly it will wrapped across multiple lines. Some heuristics are apply
+# to avoid ugly line breaks.
+# Minimum value: 0, maximum value: 1000, default value: 17.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_WRAP_THRESHOLD     = 17
+
+# If the TEMPLATE_RELATIONS tag is set to YES then the inheritance and
+# collaboration graphs will show the relations between templates and their
+# instances.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+TEMPLATE_RELATIONS     = NO
+
+# If the INCLUDE_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are set to
+# YES then doxygen will generate a graph for each documented file showing the
+# direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDE_GRAPH          = YES
+
+# If the INCLUDED_BY_GRAPH, ENABLE_PREPROCESSING and SEARCH_INCLUDES tags are
+# set to YES then doxygen will generate a graph for each documented file showing
+# the direct and indirect include dependencies of the file with other documented
+# files.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INCLUDED_BY_GRAPH      = YES
+
+# If the CALL_GRAPH tag is set to YES then doxygen will generate a call
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable call graphs for selected
+# functions only using the \callgraph command. Disabling a call graph can be
+# accomplished by means of the command \hidecallgraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALL_GRAPH             = NO
+
+# If the CALLER_GRAPH tag is set to YES then doxygen will generate a caller
+# dependency graph for every global function or class method.
+#
+# Note that enabling this option will significantly increase the time of a run.
+# So in most cases it will be better to enable caller graphs for selected
+# functions only using the \callergraph command. Disabling a caller graph can be
+# accomplished by means of the command \hidecallergraph.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+CALLER_GRAPH           = NO
+
+# If the GRAPHICAL_HIERARCHY tag is set to YES then doxygen will graphical
+# hierarchy of all classes instead of a textual one.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GRAPHICAL_HIERARCHY    = YES
+
+# If the DIRECTORY_GRAPH tag is set to YES then doxygen will show the
+# dependencies a directory has on other directories in a graphical way. The
+# dependency relations are determined by the #include relations between the
+# files in the directories.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DIRECTORY_GRAPH        = YES
+
+# The DOT_IMAGE_FORMAT tag can be used to set the image format of the images
+# generated by dot. For an explanation of the image formats see the section
+# output formats in the documentation of the dot tool (Graphviz (see:
+# http://www.graphviz.org/)).
+# Note: If you choose svg you need to set HTML_FILE_EXTENSION to xhtml in order
+# to make the SVG files visible in IE 9+ (other browsers do not have this
+# requirement).
+# Possible values are: png, png:cairo, png:cairo:cairo, png:cairo:gd, png:gd,
+# png:gd:gd, jpg, jpg:cairo, jpg:cairo:gd, jpg:gd, jpg:gd:gd, gif, gif:cairo,
+# gif:cairo:gd, gif:gd, gif:gd:gd, svg, png:gd, png:gd:gd, png:cairo,
+# png:cairo:gd, png:cairo:cairo, png:cairo:gdiplus, png:gdiplus and
+# png:gdiplus:gdiplus.
+# The default value is: png.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_IMAGE_FORMAT       = png
+
+# If DOT_IMAGE_FORMAT is set to svg, then this option can be set to YES to
+# enable generation of interactive SVG images that allow zooming and panning.
+#
+# Note that this requires a modern browser other than Internet Explorer. Tested
+# and working are Firefox, Chrome, Safari, and Opera.
+# Note: For IE 9+ you need to set HTML_FILE_EXTENSION to xhtml in order to make
+# the SVG files visible. Older versions of IE do not have SVG support.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+INTERACTIVE_SVG        = NO
+
+# The DOT_PATH tag can be used to specify the path where the dot tool can be
+# found. If left blank, it is assumed the dot tool can be found in the path.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_PATH               =
+
+# The DOTFILE_DIRS tag can be used to specify one or more directories that
+# contain dot files that are included in the documentation (see the \dotfile
+# command).
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOTFILE_DIRS           =
+
+# The MSCFILE_DIRS tag can be used to specify one or more directories that
+# contain msc files that are included in the documentation (see the \mscfile
+# command).
+
+MSCFILE_DIRS           =
+
+# The DIAFILE_DIRS tag can be used to specify one or more directories that
+# contain dia files that are included in the documentation (see the \diafile
+# command).
+
+DIAFILE_DIRS           =
+
+# When using plantuml, the PLANTUML_JAR_PATH tag should be used to specify the
+# path where java can find the plantuml.jar file. If left blank, it is assumed
+# PlantUML is not used or called during a preprocessing step. Doxygen will
+# generate a warning when it encounters a \startuml command in this case and
+# will not generate output for the diagram.
+
+PLANTUML_JAR_PATH      =
+
+# When using plantuml, the PLANTUML_CFG_FILE tag can be used to specify a
+# configuration file for plantuml.
+
+PLANTUML_CFG_FILE      =
+
+# When using plantuml, the specified paths are searched for files specified by
+# the !include statement in a plantuml block.
+
+PLANTUML_INCLUDE_PATH  =
+
+# The DOT_GRAPH_MAX_NODES tag can be used to set the maximum number of nodes
+# that will be shown in the graph. If the number of nodes in a graph becomes
+# larger than this value, doxygen will truncate the graph, which is visualized
+# by representing a node as a red box. Note that doxygen if the number of direct
+# children of the root node in a graph is already larger than
+# DOT_GRAPH_MAX_NODES then the graph will not be shown at all. Also note that
+# the size of a graph can be further restricted by MAX_DOT_GRAPH_DEPTH.
+# Minimum value: 0, maximum value: 10000, default value: 50.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_GRAPH_MAX_NODES    = 50
+
+# The MAX_DOT_GRAPH_DEPTH tag can be used to set the maximum depth of the graphs
+# generated by dot. A depth value of 3 means that only nodes reachable from the
+# root by following a path via at most 3 edges will be shown. Nodes that lay
+# further from the root node will be omitted. Note that setting this option to 1
+# or 2 may greatly reduce the computation time needed for large code bases. Also
+# note that the size of a graph can be further restricted by
+# DOT_GRAPH_MAX_NODES. Using a depth of 0 means no depth restriction.
+# Minimum value: 0, maximum value: 1000, default value: 0.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+MAX_DOT_GRAPH_DEPTH    = 0
+
+# Set the DOT_TRANSPARENT tag to YES to generate images with a transparent
+# background. This is disabled by default, because dot on Windows does not seem
+# to support this out of the box.
+#
+# Warning: Depending on the platform used, enabling this option may lead to
+# badly anti-aliased labels on the edges of a graph (i.e. they become hard to
+# read).
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_TRANSPARENT        = NO
+
+# Set the DOT_MULTI_TARGETS tag to YES to allow dot to generate multiple output
+# files in one run (i.e. multiple -o and -T options on the command line). This
+# makes dot run faster, but since only newer versions of dot (>1.8.10) support
+# this, this feature is disabled by default.
+# The default value is: NO.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+DOT_MULTI_TARGETS      = NO
+
+# If the GENERATE_LEGEND tag is set to YES doxygen will generate a legend page
+# explaining the meaning of the various boxes and arrows in the dot generated
+# graphs.
+# The default value is: YES.
+# This tag requires that the tag HAVE_DOT is set to YES.
+
+GENERATE_LEGEND        = YES
+
+# If the DOT_CLEANUP tag is set to YES, doxygen will remove the intermediate
+# files that are used to generate the various graphs.
+#
+# Note: This setting is not only used for dot files but also for msc and
+# plantuml temporary files.
+# The default value is: YES.
+
+DOT_CLEANUP            = YES
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/doc/flexiv_logo.png b/thirdparty/flexiv_sim_plugin.cloned.bak/doc/flexiv_logo.png
new file mode 100644
index 0000000..c0b84c1
Binary files /dev/null and b/thirdparty/flexiv_sim_plugin.cloned.bak/doc/flexiv_logo.png differ
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/docs/elements_studio_setup.md b/thirdparty/flexiv_sim_plugin.cloned.bak/docs/elements_studio_setup.md
new file mode 100644
index 0000000..a4933d4
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/docs/elements_studio_setup.md
@@ -0,0 +1,30 @@
+# Flexiv Elements Studio Setup
+
+Flexiv Elements Studio runs the simulated robot controller. 
+Set it up once before running any external-simulator workspace.
+
+## Install Elements Studio on Ubuntu
+
+1. [Contact Flexiv](https://www.flexiv.com/contact) to obtain the installation package of Elements Studio.
+2. Extract the package to a non-root directory.
+3. Install Elements Studio:
+
+       bash setup_FlexivElements.sh
+
+4. Switch physics engine from the default built-in to external:
+
+       bash switch_physics_engine.sh
+
+   Select *External* when prompted.
+
+## Create a simulated robot in Elements Studio
+
+1. Start Flexiv Elements Studio from the application menu.
+2. In the Robot Connection window, select *Simulator*, and click *CREATE*.
+3. Choose "Create according to the selected robot type" and select one from the list, then click *CONFIRM*. A new simulated robot will be added to the simulator list.
+4. Toggle on the *Connect* button for the newly added one, then wait for loading.
+5. When loading is finished, you'll see a robot at its upright pose, with an "Exception" error at the bottom right corner. This is expected because the external simulator is not started yet. But if you see a normally operating robot, that again means you are running the wrong version of Elements Studio that only supports the built-in physics engine.
+6. To enable RDK, Click Settings → Remote Mode → Select a Mode → Ethernet. In the resulting popup window, restart the simulated robot.
+7. Toggle the virtual motion bar slider button to the "auto" position. In the resulting popup window, select "AUTO (REMOTE)".
+8. At the bottom of the window, click on the small robot icon with a "SIM" tag on it, then a small window will pop up, note down the displayed robot serial number.
+
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/example/CMakeLists.txt b/thirdparty/flexiv_sim_plugin.cloned.bak/example/CMakeLists.txt
new file mode 100644
index 0000000..ca19629
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/example/CMakeLists.txt
@@ -0,0 +1,32 @@
+cmake_minimum_required(VERSION 3.16.3)
+project(flexiv_sim_plugin-examples)
+
+# Show verbose build info
+SET(CMAKE_VERBOSE_MAKEFILE ON)
+
+message("OS: ${CMAKE_SYSTEM_NAME}")
+message("Processor: ${CMAKE_SYSTEM_PROCESSOR}")
+
+# Configure build type
+if(NOT CMAKE_BUILD_TYPE)
+  set(CMAKE_BUILD_TYPE Release CACHE STRING "CMake build type" FORCE)
+endif()
+
+# Minimum example list for all OS
+set(EXAMPLE_LIST
+  mock_external_simulator
+)
+
+# Find flexiv_sim_plugin INTERFACE library
+find_package(flexiv_sim_plugin REQUIRED)
+
+# Build all selected examples
+foreach(example ${EXAMPLE_LIST})
+  add_executable(${example} ${example}.cpp)
+  target_link_libraries(${example} flexiv::flexiv_sim_plugin)
+
+  # C++17 required
+  set_target_properties(${example} PROPERTIES
+    CXX_STANDARD 17
+    CXX_STANDARD_REQUIRED ON)
+endforeach()
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/example/mock_external_simulator.cpp b/thirdparty/flexiv_sim_plugin.cloned.bak/example/mock_external_simulator.cpp
new file mode 100644
index 0000000..c2d2c7e
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/example/mock_external_simulator.cpp
@@ -0,0 +1,135 @@
+/**
+ * @example mock_external_simulator.cpp
+ * A simple example demonstrating how to use Flexiv Sim Plugin in an external simulator. Please
+ * refer to README on how to run this example.
+ * @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved.
+ * @author Flexiv
+ */
+
+#include <flexiv/sim_plugin/user_node.hpp>
+#include <spdlog/spdlog.h>
+
+#include <math.h>
+#include <iostream>
+#include <iomanip>
+#include <thread>
+
+using namespace flexiv;
+
+namespace {
+// Frequency of the external simulator's physics loop [Hz]
+constexpr unsigned int kPhysicsFreq = 2000;
+// Period of the external simulator's physics loop [sec]
+constexpr double kPhysicsPeriod = 1.0 / kPhysicsFreq;
+
+// Sine-sweep trajectory amplitude and frequency
+constexpr double kSineAmp = 0.035;
+constexpr double kSineFreq = 0.3;
+
+// Initial joint positions when the robot is at home posture [rad]
+const std::vector<float> kInitQ = {0.0, -0.698132, 0.0, 1.5708, 0.0, 0.698132, 0.0};
+
+// Joint degrees of freedom
+constexpr size_t kJointDof = 7;
+
+// Servo cycle of the physics loop
+unsigned int g_servo_cycle = 0;
+}
+
+/** @brief Print program usage help */
+void PrintHelp()
+{
+    // clang-format off
+    std::cout << "Required arguments: [robot_sn]" << std::endl;
+    std::cout << "    robot_sn: Serial number of the robot in Flexiv Elements Studio to connect. Remove any space, e.g. Rizon4s-123456" << std::endl;
+    std::cout << "Optional arguments: None" << std::endl;
+    std::cout << std::endl;
+    // clang-format on
+}
+
+/** @brief Step once the external simulator's physics loop. */
+void StepPhysics(sim_plugin::UserNode& user_node)
+{
+    // Record time point at the beginning of this cycle
+    auto current_time = std::chrono::steady_clock::now();
+
+    // Step 1: obtain robot states from the external simulator
+    // =============================================================================================
+    // Mocked robot states
+    sim_plugin::SimRobotStates robot_states;
+    robot_states.q.resize(kJointDof);
+    robot_states.dq.resize(kJointDof);
+
+    // Set and increment servo cycle per physics step
+    robot_states.servo_cycle = g_servo_cycle++;
+
+    // Set joint positions to sine waves
+    for (size_t i = 0; i < kInitQ.size(); i++) {
+        robot_states.q[i]
+            = kInitQ[i] + kSineAmp * sin(2 * M_PI * kSineFreq * g_servo_cycle * kPhysicsPeriod);
+    }
+
+    // Set joint velocities to corresponding cosine waves
+    for (size_t i = 0; i < kInitQ.size(); i++) {
+        robot_states.dq[i] = kSineAmp * 2 * M_PI * kSineFreq
+                             * cos(2 * M_PI * kSineFreq * g_servo_cycle * kPhysicsPeriod);
+    }
+
+    // Step 2: send robot states to Flexiv Elements Studio
+    // =============================================================================================
+    if (!user_node.SendRobotStates(robot_states)) {
+        spdlog::error("Failed to send robot states data");
+    }
+
+    // Step 3: wait for Elements Studio to calculate and deliver new robot commands
+    // =============================================================================================
+    // Must set a timeout value to avoid deadlock
+    constexpr unsigned int kWaitTimeoutMs = 100;
+    if (!user_node.WaitForRobotCommands(kWaitTimeoutMs)) {
+        spdlog::error("WaitForRobotCommands() timeout");
+    }
+
+    // Step 4: apply joint torques command to the simulated robot in the external simulator
+    // =============================================================================================
+    auto target_joint_torques = user_node.robot_commands().target_drives;
+    // Call the external simulator's API to apply the target joint torques
+
+    // End the current physics step with kPhysicsPeriod as the total loop period
+    auto loop_period_us = static_cast<unsigned int>(kPhysicsPeriod * 1'000'000);
+    std::this_thread::sleep_until(current_time + std::chrono::microseconds(loop_period_us));
+}
+
+int main(int argc, char* argv[])
+{
+    // Program Setup
+    // =============================================================================================
+    // Parse parameters
+    if (argc < 2) {
+        PrintHelp();
+        return 1;
+    }
+    // Serial number of the robot to connect to. Remove any space, for example: Rizon4s-123456
+    std::string robot_sn = argv[1];
+
+    // Print description
+    spdlog::info(
+        ">>> Tutorial description <<<\nA simple example demonstrating how to use Flexiv Sim Plugin "
+        "in an external simulator.\n");
+
+    // Create user node
+    sim_plugin::UserNode user_node(robot_sn);
+
+    // Wait for connected
+    while (!user_node.connected()) {
+        spdlog::warn("Waiting for connection with Flexiv Elements Studio");
+        std::this_thread::sleep_for(std::chrono::seconds(1));
+    }
+    spdlog::info("Connected with Flexiv Elements Studio, starting mocked physics loop");
+
+    // Run mocked physics loop
+    while (user_node.connected()) {
+        StepPhysics(user_node);
+    }
+
+    return 0;
+}
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/example_py/mock_external_simulator.py b/thirdparty/flexiv_sim_plugin.cloned.bak/example_py/mock_external_simulator.py
new file mode 100644
index 0000000..29a60d3
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/example_py/mock_external_simulator.py
@@ -0,0 +1,130 @@
+#!/usr/bin/env python
+
+"""mock_external_simulator.py
+
+A simple example demonstrating how to use Flexiv Sim Plugin in an external simulator. Please
+refer to README on how to run this example.
+"""
+
+__copyright__ = "Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved."
+__author__ = "Flexiv"
+
+import time
+import math
+import argparse
+import spdlog  # pip install
+import flexivsimplugin  # pip install
+
+# Frequency of the external simulator's physics loop [Hz]
+PHYSICS_FREQ = 2000
+# Period of the external simulator's physics loop [sec]
+PHYSICS_PERIOD = 1.0 / PHYSICS_FREQ
+
+# Sine-sweep trajectory amplitude and frequency
+SINE_AMP = 0.035
+SINE_FREQ = 0.3
+
+# Initial joint positions when the robot is at home posture [rad]
+INIT_Q = [0.0, -0.698132, 0.0, 1.5708, 0.0, 0.698132, 0.0]
+
+# Joint degrees of freedom
+JOINT_DOF = 7
+
+# Servo cycle of the physics loop
+g_servo_cycle = 0
+
+
+def step_physics(user_node, logger):
+    """
+    Step once the external simulator's physics loop.
+
+    """
+
+    # Step 1: obtain robot states from the external simulator
+    # =============================================================================================
+    # Mocked robot states
+    robot_states = flexivsimplugin.SimRobotStates()
+
+    # Set and increment servo cycle per physics step
+    global g_servo_cycle
+    g_servo_cycle += 1
+    robot_states.servo_cycle = g_servo_cycle
+
+    # robot_states.q and dq are fixed-size tuples and don't support per-element assignment
+    temp_list = [0] * JOINT_DOF
+
+    # Set joint positions to sine waves
+    for i in range(JOINT_DOF):
+        temp_list[i] = INIT_Q[i] + SINE_AMP * math.sin(
+            2 * math.pi * SINE_FREQ * g_servo_cycle * PHYSICS_PERIOD
+        )
+    robot_states.q = temp_list
+
+    # Set joint velocities to corresponding cosine waves
+    for i in range(JOINT_DOF):
+        temp_list[i] = (
+            SINE_AMP
+            * 2
+            * math.pi
+            * SINE_FREQ
+            * math.cos(2 * math.pi * SINE_FREQ * g_servo_cycle * PHYSICS_PERIOD)
+        )
+    robot_states.dq = temp_list
+
+    # Step 2: send robot states to Flexiv Elements Studio
+    # =============================================================================================
+    if not user_node.SendRobotStates(robot_states):
+        logger.error("Failed to send robot states data")
+
+    # Step 3: wait for Elements Studio to calculate and deliver new robot commands
+    # =============================================================================================
+    # Must set a timeout value to avoid deadlock
+    WAIT_TIMEOUT_MS = 100
+    if not user_node.WaitForRobotCommands(WAIT_TIMEOUT_MS):
+        logger.error("WaitForRobotCommands() timeout")
+
+    # Step 4: apply joint torques command to the simulated robot in the external simulator
+    # =============================================================================================
+    target_joint_torques = user_node.robot_commands().target_drives
+    # Call the external simulator's API to apply the target joint torques
+
+    # End the current physics step with kPhysicsPeriod as roughly the total loop period
+    time.sleep(PHYSICS_PERIOD)
+
+
+def main():
+    # Program Setup
+    # ==============================================================================================
+    # Parse arguments
+    argparser = argparse.ArgumentParser()
+    argparser.add_argument(
+        "robot_sn",
+        help="Serial number of the robot in Flexiv Elements Studio to connect. Remove any space, e.g. Rizon4s-123456",
+    )
+    args = argparser.parse_args()
+
+    # Create logger
+    logger = spdlog.ConsoleLogger("Example")
+
+    # Print description
+    logger.info(
+        ">>> Tutorial description <<<\nA simple example demonstrating how to use Flexiv Sim Plugin "
+        "in an external simulator.\n"
+    )
+
+    # Create user node
+    user_node = flexivsimplugin.UserNode(args.robot_sn)
+
+    # Wait for connected
+    while not user_node.connected():
+        logger.warn("Waiting for connection with Flexiv Elements Studio")
+        time.sleep(1)
+    logger.info("Connected with Flexiv Elements Studio, starting mocked physics loop")
+
+    # Run mocked physics loop
+    while user_node.connected():
+        step_physics(user_node, logger)
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/include/flexiv/sim_plugin/data.hpp b/thirdparty/flexiv_sim_plugin.cloned.bak/include/flexiv/sim_plugin/data.hpp
new file mode 100644
index 0000000..f546b9c
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/include/flexiv/sim_plugin/data.hpp
@@ -0,0 +1,86 @@
+/**
+ * @file data.hpp
+ * @brief Header file containing various constant expressions and data structures.
+ * @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_SIM_PLUGIN_DATA_HPP_
+#define FLEXIV_SIM_PLUGIN_DATA_HPP_
+
+#include <array>
+#include <vector>
+#include <string>
+
+namespace flexiv {
+namespace sim_plugin {
+
+/** Number of simulated digital IO ports */
+constexpr size_t kIOPorts = 16;
+
+/** States data of a simulated robot in the external simulator */
+struct SimRobotStates
+{
+    /** @brief Customized constructor */
+    SimRobotStates(uint64_t _servo_cycle, const std::vector<float>& _q,
+        const std::vector<float>& _dq, const std::vector<float>& _wrist_force = {},
+        const std::vector<float>& _wrist_torque = {})
+    : servo_cycle(_servo_cycle)
+    , q(_q)
+    , dq(_dq)
+    , wrist_force(_wrist_force)
+    , wrist_torque(_wrist_torque)
+    {
+    }
+
+    /** @brief Default constructor */
+    SimRobotStates() = default;
+
+    /** Servo cycle incremented once per physics step of the external simulator */
+    uint64_t servo_cycle = 0;
+
+    /** Current joint positions of the simulated robot: \f$ q \in \mathbb{R}^{n \times 1} \f$. Unit:
+     * \f$ [rad] or [m] \f$.
+     * @note This contains values for both the external axes (if any) and the robot manipulator. */
+    std::vector<float> q = {};
+
+    /** Current joint velocities of the simulated robot: \f$ \dot{q} \in \mathbb{R}^{n \times 1}
+     * \f$. Unit: \f$ [rad/s] or [m/s] \f$.
+     * @note This contains values for both the external axes (if any) and the robot manipulator. */
+    std::vector<float> dq = {};
+
+    /** Current wrist force sensor reading of the simulated robot: \f$ [f_x, f_y, f_z] \f$. Unit: \f$
+     * [N] \f$. */
+    std::vector<float> wrist_force = {};
+
+    /** Current wrist torque sensor reading of the simulated robot: \f$ [m_x, m_y, m_z] \f$. Unit:
+     * \f$ [Nm] \f$. */
+    std::vector<float> wrist_torque = {};
+};
+
+/** Commands data for a simulated robot in the external simulator */
+struct SimRobotCommands
+{
+    /** @brief Customized constructor */
+    SimRobotCommands(const std::vector<float>& _target_drives,
+        const std::array<bool, kIOPorts>& _digital_outputs)
+    : target_drives(_target_drives)
+    , digital_outputs(_digital_outputs)
+    {
+    }
+    /** @brief Default constructor */
+    SimRobotCommands() = default;
+
+    /** Target joint drives for the simulated robot, can be torques or velocities depending on the
+     * drive type of the joint. Unit: \f$ [Nm] or [rad/s] or [m/s] \f$.
+     * @note This contains values for both the external axes (if any) and the robot manipulator. */
+    std::vector<float> target_drives = {};
+
+    /** Desired digital outputs for the simulated robot. The index of this boolean array corresponds
+     * to that of the digital output ports. True: port high, false: port low. */
+    std::array<bool, kIOPorts> digital_outputs = {};
+};
+
+} /* namespace sim_plugin */
+} /* namespace flexiv */
+
+#endif /* FLEXIV_SIM_PLUGIN_DATA_HPP_ */
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/include/flexiv/sim_plugin/user_node.hpp b/thirdparty/flexiv_sim_plugin.cloned.bak/include/flexiv/sim_plugin/user_node.hpp
new file mode 100644
index 0000000..1d69b49
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/include/flexiv/sim_plugin/user_node.hpp
@@ -0,0 +1,68 @@
+/**
+ * @file user_node.hpp
+ * @copyright Copyright (C) 2016-2025 Flexiv Ltd. All Rights Reserved.
+ */
+
+#ifndef FLEXIV_SIM_PLUGIN_USER_NODE_HPP_
+#define FLEXIV_SIM_PLUGIN_USER_NODE_HPP_
+
+#include "data.hpp"
+#include <memory>
+
+namespace flexiv {
+namespace sim_plugin {
+
+/**
+ * @class UserNode
+ * @brief A communication node for an external simulator to publish robot states to and receive
+ * robot commands from Flexiv Elements Studio.
+ */
+class UserNode final
+{
+public:
+    /**
+     * @brief Create the node.
+     * @param[in] robot_sn Serial number of the simulated robot created in Flexiv Elements Studio.
+     * The accepted formats are: "Rizon 4-123456" and "Rizon4-123456".
+     * @param[in] debug_print Print debug info.
+     */
+    UserNode(const std::string& robot_sn, bool debug_print = false);
+    virtual ~UserNode();
+
+    /**
+     * @brief Whether this node is connected with Flexiv Elements Studio.
+     */
+    bool connected() const;
+
+    /**
+     * @brief Send robot states data to Flexiv Elements Studio.
+     * @param[in] robot_states States data of the simulated robot in the external simulator.
+     * @return True: data sent successfully; false: failed to send data.
+     */
+    bool SendRobotStates(const SimRobotStates& robot_states);
+
+    /**
+     * @brief Blocks until the next data packet of robot commands from Flexiv Elements Studio is
+     * received or timeout.
+     * @param[in] timeout_ms Maximum wait time before this function returns. Must set a value to
+     * avoid deadlock. Unit: [ms].
+     * @return True: packet received before timeout; false: packet not received before timeout.
+     */
+    bool WaitForRobotCommands(unsigned int timeout_ms);
+
+    /**
+     * @brief Current commands data to apply to the simulated robot in the external simulator.
+     * @return Value copy of SimRobotCommands struct.
+     * @warning Call this function after WaitForRobotCommands() to get the latest data.
+     */
+    SimRobotCommands robot_commands() const;
+
+private:
+    class Impl;
+    std::unique_ptr<Impl> pimpl_;
+};
+
+} /* namespace sim_plugin */
+} /* namespace flexiv */
+
+#endif /* FLEXIV_SIM_PLUGIN_USER_NODE_HPP_ */
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/lib/dummy.cpp b/thirdparty/flexiv_sim_plugin.cloned.bak/lib/dummy.cpp
new file mode 100644
index 0000000..6299cc5
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/lib/dummy.cpp
@@ -0,0 +1,5 @@
+/**
+ * @file dummy.cpp
+ * @brief A dummy source file to add portability to the installed flexiv_rdk library
+ */
+void dummy() { }
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/lib/libflexiv_sim_plugin.x86_64-linux-gnu.a.sha256 b/thirdparty/flexiv_sim_plugin.cloned.bak/lib/libflexiv_sim_plugin.x86_64-linux-gnu.a.sha256
new file mode 100644
index 0000000..e33d922
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/lib/libflexiv_sim_plugin.x86_64-linux-gnu.a.sha256
@@ -0,0 +1 @@
+d95da285eac19eac1d64e5e953a2690f26f4791b9c03d5a9896569717fae6fc0
\ No newline at end of file
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/build_and_install_dependencies.sh b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/build_and_install_dependencies.sh
new file mode 100644
index 0000000..a0c4413
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/build_and_install_dependencies.sh
@@ -0,0 +1,49 @@
+#!/bin/sh
+# This script builds from source and installs all dependencies of flexiv_sim_plugin.
+
+# Absolute path of this script
+SCRIPTPATH="$(dirname $(readlink -f $0))"
+set -e
+
+# Check script arguments
+if [ "$#" -lt 1 ]; then
+    echo "Error: invalid script argument"
+    echo "Required argument: [install_directory_path]"
+    echo "    install_directory_path: directory to install all dependencies, should be the same as the install directory of flexiv_sim_plugin"
+    echo "Optional argument: [num_parallel_jobs]"
+    echo "    num_parallel_jobs: number of parallel jobs used to build, use 4 if not specified"
+    exit
+fi
+
+# Get dependencies install directory from script argument, should be the same as the install directory of flexiv_sim_plugin
+INSTALL_DIR=$1
+echo "Dependencies will be installed to: $INSTALL_DIR"
+
+# Use specified number for parallel build jobs, otherwise use 4
+if [ -n "$2" ] ;then
+    export NUM_JOBS=$2
+else
+    export NUM_JOBS=4
+fi
+echo "Number of parallel build jobs: $NUM_JOBS"
+
+# Set shared cmake arguments
+SHARED_CMAKE_ARGS="-DCMAKE_BUILD_TYPE=Release \
+                   -DBUILD_SHARED_LIBS=OFF \
+                   -DCMAKE_POSITION_INDEPENDENT_CODE=ON \
+                   -DCMAKE_PREFIX_PATH=$INSTALL_DIR \
+                   -DCMAKE_INSTALL_PREFIX=$INSTALL_DIR \
+                   -DBUILD_TESTING=OFF"
+
+# Clone all dependencies in a subfolder
+mkdir -p cloned && cd cloned
+export SHARED_CMAKE_ARGS
+
+# Build and install all dependencies to INSTALL_DIR
+bash $SCRIPTPATH/scripts/install_spdlog.sh
+bash $SCRIPTPATH/scripts/install_tinyxml2.sh
+bash $SCRIPTPATH/scripts/install_foonathan_memory.sh
+bash $SCRIPTPATH/scripts/install_Fast-CDR.sh
+bash $SCRIPTPATH/scripts/install_Fast-DDS.sh
+
+echo ">>>>>>>>>> Finished <<<<<<<<<<"
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_Fast-CDR.sh b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_Fast-CDR.sh
new file mode 100644
index 0000000..0e93f81
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_Fast-CDR.sh
@@ -0,0 +1,24 @@
+#!/bin/bash
+set -e
+echo "Installing Fast-CDR"
+
+# Use a specific version
+VER_TAG=v1.0.28
+
+# Clone source code
+if [ ! -d Fast-CDR ] ; then
+  git clone https://github.com/eProsima/Fast-CDR.git --branch $VER_TAG
+  cd Fast-CDR
+else
+  cd Fast-CDR
+  git checkout $VER_TAG
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS -DCOMPILE_EXAMPLES=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed Fast-CDR"
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_Fast-DDS.sh b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_Fast-DDS.sh
new file mode 100644
index 0000000..c0204e1
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_Fast-DDS.sh
@@ -0,0 +1,31 @@
+#!/bin/bash
+# Depends on: foonathan_memory, tinyxml2, Fast-CDR
+set -e
+echo "Installing Fast-DDS"
+
+# Use a specific version
+VER_TAG=v2.6.10
+
+# Clone source code
+if [ ! -d Fast-DDS ] ; then
+  git clone https://github.com/eProsima/Fast-DDS.git --branch $VER_TAG
+  cd Fast-DDS
+else
+  cd Fast-DDS
+  git checkout $VER_TAG
+fi
+
+# Initialize submodules
+git submodule update --init --recursive
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS \
+         -DTHIRDPARTY_Asio=ON \
+         -DCOMPILE_EXAMPLES=OFF \
+         -DSQLITE3_SUPPORT=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed Fast-DDS"
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_foonathan_memory.sh b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_foonathan_memory.sh
new file mode 100644
index 0000000..d0d794e
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_foonathan_memory.sh
@@ -0,0 +1,24 @@
+#!/bin/bash
+set -e
+echo "Installing foonathan_memory"
+
+# Use a specific version
+VER_TAG=v1.3.1
+
+# Clone source code
+if [ ! -d foonathan_memory_vendor ] ; then
+  git clone https://github.com/eProsima/foonathan_memory_vendor.git --branch $VER_TAG
+  cd foonathan_memory_vendor
+else
+  cd foonathan_memory_vendor
+  git checkout $VER_TAG
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed foonathan_memory"
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_spdlog.sh b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_spdlog.sh
new file mode 100644
index 0000000..abd0f9e
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_spdlog.sh
@@ -0,0 +1,25 @@
+#!/bin/bash
+set -e
+
+# Use a specific version
+VER_TAG=v1.14.1
+
+# Clone source code
+if [ ! -d spdlog ] ; then
+  git clone https://github.com/gabime/spdlog.git --branch $VER_TAG
+  cd spdlog
+else
+  cd spdlog
+  git checkout $VER_TAG
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS \
+         -DSPDLOG_BUILD_EXAMPLE=OFF \
+         -DSPDLOG_BUILD_TESTS=OFF
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed spdlog"
diff --git a/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_tinyxml2.sh b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_tinyxml2.sh
new file mode 100644
index 0000000..e443ef8
--- /dev/null
+++ b/thirdparty/flexiv_sim_plugin.cloned.bak/thirdparty/scripts/install_tinyxml2.sh
@@ -0,0 +1,24 @@
+#!/bin/bash
+set -e
+echo "Installing tinyxml2"
+
+# Use a specific version
+VER_TAG=8.0.0
+
+# Clone source code
+if [ ! -d tinyxml2 ] ; then
+  git clone https://github.com/leethomason/tinyxml2.git --branch $VER_TAG
+  cd tinyxml2
+else
+  cd tinyxml2
+  git checkout $VER_TAG
+fi
+
+# Configure CMake
+mkdir -p build && cd build
+cmake .. $SHARED_CMAKE_ARGS
+
+# Build and install
+cmake --build . --target install --config Release -j $NUM_JOBS
+
+echo "Installed tinyxml2"
diff --git a/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/bug_report.md b/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/bug_report.md
new file mode 100644
index 0000000..020620e
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/bug_report.md
@@ -0,0 +1,40 @@
+---
+name: Bug report
+about: Create a report to help us improve
+title: "[BUG] "
+type: Bug
+labels: bug
+assignees: bsoe-flexiv
+
+---
+
+## Version info
+
+- Isaac Sim Workspace: [e.g. v1.0]
+- Flexiv Elements Studio: [e.g. v3.8]
+- NVIDIA Isaac Sim: [e.g. v4.2.0]
+- OS: [e.g. Ubuntu 22.04, x86_64]
+
+## How urgent is this bug? Have you found any workarounds?
+
+On a scale of 1 to 10.
+
+## Describe the issue and expected behavior
+
+The issue is ...
+The correct behavior you expect is  ...
+
+## Steps to reproduce
+
+1. '....'
+2. '....'
+3. '....'
+4. See error
+
+## Screenshots
+
+If applicable, add screenshots to help explain your issue.
+
+## Additional context
+
+Add any other context about the issue.
diff --git a/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/feature_request.md b/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/feature_request.md
new file mode 100644
index 0000000..0d3b83f
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/feature_request.md
@@ -0,0 +1,29 @@
+---
+name: Feature request
+about: Suggest a new feature
+title: "[FEATURE] "
+type: Feature
+labels: enhancement
+assignees: bsoe-flexiv
+
+---
+
+## How urgent is this feature?
+
+On a scale of 1 to 10.
+
+## Is this feature request related to a problem?
+
+This new feature is needed so that ... can be solved.
+
+## Describe the solution you'd like
+
+What you want to happen.
+
+## Describe alternatives you've considered
+
+Any alternative solutions or workarounds you've considered.
+
+## Additional context
+
+Add any other context or screenshots about this feature.
diff --git a/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/question.md b/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/question.md
new file mode 100644
index 0000000..1f448f1
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/.github/ISSUE_TEMPLATE/question.md
@@ -0,0 +1,29 @@
+---
+name: Question
+about: Ask a question
+title: "[QUESTION] "
+type: Question
+labels: help wanted
+assignees: bsoe-flexiv
+
+---
+
+## There might already be an answer to your question
+
+Someone might have asked similar questions before, you can search in the [Issues](https://github.com/flexivrobotics/isaac_sim_ws/issues?q=is%3Aissue) tab.
+
+## Looking for information related to Isaac Sim?
+
+You can find the complete documentation of NVIDIA Isaac Sim [here](https://docs.isaacsim.omniverse.nvidia.com/latest/index.html).
+
+## Not sure how to get started and build your own simulation workflow?
+
+You can [contact Flexiv](https://www.flexiv.com/contact) to seek technical support and explore turnkey solution options.
+
+## Is this related to a specific version of Isaac Sim Workspace?
+
+If yes, please specify.
+
+## Please describe your question
+
+Texts, screenshots, links, code blocks ...
diff --git a/thirdparty/isaac_sim_ws/.gitignore b/thirdparty/isaac_sim_ws/.gitignore
new file mode 100644
index 0000000..1b9c357
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/.gitignore
@@ -0,0 +1,8 @@
+.vscode
+.vs
+.DS_Store
+.thumbs
+extscache/
+cache/
+*.log
+__pycache__
\ No newline at end of file
diff --git a/thirdparty/isaac_sim_ws/LICENSE b/thirdparty/isaac_sim_ws/LICENSE
new file mode 100644
index 0000000..261eeb9
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/LICENSE
@@ -0,0 +1,201 @@
+                                 Apache License
+                           Version 2.0, January 2004
+                        http://www.apache.org/licenses/
+
+   TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION
+
+   1. Definitions.
+
+      "License" shall mean the terms and conditions for use, reproduction,
+      and distribution as defined by Sections 1 through 9 of this document.
+
+      "Licensor" shall mean the copyright owner or entity authorized by
+      the copyright owner that is granting the License.
+
+      "Legal Entity" shall mean the union of the acting entity and all
+      other entities that control, are controlled by, or are under common
+      control with that entity. For the purposes of this definition,
+      "control" means (i) the power, direct or indirect, to cause the
+      direction or management of such entity, whether by contract or
+      otherwise, or (ii) ownership of fifty percent (50%) or more of the
+      outstanding shares, or (iii) beneficial ownership of such entity.
+
+      "You" (or "Your") shall mean an individual or Legal Entity
+      exercising permissions granted by this License.
+
+      "Source" form shall mean the preferred form for making modifications,
+      including but not limited to software source code, documentation
+      source, and configuration files.
+
+      "Object" form shall mean any form resulting from mechanical
+      transformation or translation of a Source form, including but
+      not limited to compiled object code, generated documentation,
+      and conversions to other media types.
+
+      "Work" shall mean the work of authorship, whether in Source or
+      Object form, made available under the License, as indicated by a
+      copyright notice that is included in or attached to the work
+      (an example is provided in the Appendix below).
+
+      "Derivative Works" shall mean any work, whether in Source or Object
+      form, that is based on (or derived from) the Work and for which the
+      editorial revisions, annotations, elaborations, or other modifications
+      represent, as a whole, an original work of authorship. For the purposes
+      of this License, Derivative Works shall not include works that remain
+      separable from, or merely link (or bind by name) to the interfaces of,
+      the Work and Derivative Works thereof.
+
+      "Contribution" shall mean any work of authorship, including
+      the original version of the Work and any modifications or additions
+      to that Work or Derivative Works thereof, that is intentionally
+      submitted to Licensor for inclusion in the Work by the copyright owner
+      or by an individual or Legal Entity authorized to submit on behalf of
+      the copyright owner. For the purposes of this definition, "submitted"
+      means any form of electronic, verbal, or written communication sent
+      to the Licensor or its representatives, including but not limited to
+      communication on electronic mailing lists, source code control systems,
+      and issue tracking systems that are managed by, or on behalf of, the
+      Licensor for the purpose of discussing and improving the Work, but
+      excluding communication that is conspicuously marked or otherwise
+      designated in writing by the copyright owner as "Not a Contribution."
+
+      "Contributor" shall mean Licensor and any individual or Legal Entity
+      on behalf of whom a Contribution has been received by Licensor and
+      subsequently incorporated within the Work.
+
+   2. Grant of Copyright License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      copyright license to reproduce, prepare Derivative Works of,
+      publicly display, publicly perform, sublicense, and distribute the
+      Work and such Derivative Works in Source or Object form.
+
+   3. Grant of Patent License. Subject to the terms and conditions of
+      this License, each Contributor hereby grants to You a perpetual,
+      worldwide, non-exclusive, no-charge, royalty-free, irrevocable
+      (except as stated in this section) patent license to make, have made,
+      use, offer to sell, sell, import, and otherwise transfer the Work,
+      where such license applies only to those patent claims licensable
+      by such Contributor that are necessarily infringed by their
+      Contribution(s) alone or by combination of their Contribution(s)
+      with the Work to which such Contribution(s) was submitted. If You
+      institute patent litigation against any entity (including a
+      cross-claim or counterclaim in a lawsuit) alleging that the Work
+      or a Contribution incorporated within the Work constitutes direct
+      or contributory patent infringement, then any patent licenses
+      granted to You under this License for that Work shall terminate
+      as of the date such litigation is filed.
+
+   4. Redistribution. You may reproduce and distribute copies of the
+      Work or Derivative Works thereof in any medium, with or without
+      modifications, and in Source or Object form, provided that You
+      meet the following conditions:
+
+      (a) You must give any other recipients of the Work or
+          Derivative Works a copy of this License; and
+
+      (b) You must cause any modified files to carry prominent notices
+          stating that You changed the files; and
+
+      (c) You must retain, in the Source form of any Derivative Works
+          that You distribute, all copyright, patent, trademark, and
+          attribution notices from the Source form of the Work,
+          excluding those notices that do not pertain to any part of
+          the Derivative Works; and
+
+      (d) If the Work includes a "NOTICE" text file as part of its
+          distribution, then any Derivative Works that You distribute must
+          include a readable copy of the attribution notices contained
+          within such NOTICE file, excluding those notices that do not
+          pertain to any part of the Derivative Works, in at least one
+          of the following places: within a NOTICE text file distributed
+          as part of the Derivative Works; within the Source form or
+          documentation, if provided along with the Derivative Works; or,
+          within a display generated by the Derivative Works, if and
+          wherever such third-party notices normally appear. The contents
+          of the NOTICE file are for informational purposes only and
+          do not modify the License. You may add Your own attribution
+          notices within Derivative Works that You distribute, alongside
+          or as an addendum to the NOTICE text from the Work, provided
+          that such additional attribution notices cannot be construed
+          as modifying the License.
+
+      You may add Your own copyright statement to Your modifications and
+      may provide additional or different license terms and conditions
+      for use, reproduction, or distribution of Your modifications, or
+      for any such Derivative Works as a whole, provided Your use,
+      reproduction, and distribution of the Work otherwise complies with
+      the conditions stated in this License.
+
+   5. Submission of Contributions. Unless You explicitly state otherwise,
+      any Contribution intentionally submitted for inclusion in the Work
+      by You to the Licensor shall be under the terms and conditions of
+      this License, without any additional terms or conditions.
+      Notwithstanding the above, nothing herein shall supersede or modify
+      the terms of any separate license agreement you may have executed
+      with Licensor regarding such Contributions.
+
+   6. Trademarks. This License does not grant permission to use the trade
+      names, trademarks, service marks, or product names of the Licensor,
+      except as required for reasonable and customary use in describing the
+      origin of the Work and reproducing the content of the NOTICE file.
+
+   7. Disclaimer of Warranty. Unless required by applicable law or
+      agreed to in writing, Licensor provides the Work (and each
+      Contributor provides its Contributions) on an "AS IS" BASIS,
+      WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or
+      implied, including, without limitation, any warranties or conditions
+      of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A
+      PARTICULAR PURPOSE. You are solely responsible for determining the
+      appropriateness of using or redistributing the Work and assume any
+      risks associated with Your exercise of permissions under this License.
+
+   8. Limitation of Liability. In no event and under no legal theory,
+      whether in tort (including negligence), contract, or otherwise,
+      unless required by applicable law (such as deliberate and grossly
+      negligent acts) or agreed to in writing, shall any Contributor be
+      liable to You for damages, including any direct, indirect, special,
+      incidental, or consequential damages of any character arising as a
+      result of this License or out of the use or inability to use the
+      Work (including but not limited to damages for loss of goodwill,
+      work stoppage, computer failure or malfunction, or any and all
+      other commercial damages or losses), even if such Contributor
+      has been advised of the possibility of such damages.
+
+   9. Accepting Warranty or Additional Liability. While redistributing
+      the Work or Derivative Works thereof, You may choose to offer,
+      and charge a fee for, acceptance of support, warranty, indemnity,
+      or other liability obligations and/or rights consistent with this
+      License. However, in accepting such obligations, You may act only
+      on Your own behalf and on Your sole responsibility, not on behalf
+      of any other Contributor, and only if You agree to indemnify,
+      defend, and hold each Contributor harmless for any liability
+      incurred by, or claims asserted against, such Contributor by reason
+      of your accepting any such warranty or additional liability.
+
+   END OF TERMS AND CONDITIONS
+
+   APPENDIX: How to apply the Apache License to your work.
+
+      To apply the Apache License to your work, attach the following
+      boilerplate notice, with the fields enclosed by brackets "[]"
+      replaced with your own identifying information. (Don't include
+      the brackets!)  The text should be enclosed in the appropriate
+      comment syntax for the file format. We also recommend that a
+      file or class name and description of purpose be included on the
+      same "printed page" as the copyright notice for easier
+      identification within third-party archives.
+
+   Copyright [yyyy] [name of copyright owner]
+
+   Licensed under the Apache License, Version 2.0 (the "License");
+   you may not use this file except in compliance with the License.
+   You may obtain a copy of the License at
+
+       http://www.apache.org/licenses/LICENSE-2.0
+
+   Unless required by applicable law or agreed to in writing, software
+   distributed under the License is distributed on an "AS IS" BASIS,
+   WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+   See the License for the specific language governing permissions and
+   limitations under the License.
diff --git a/thirdparty/isaac_sim_ws/README.md b/thirdparty/isaac_sim_ws/README.md
new file mode 100644
index 0000000..b87d43c
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/README.md
@@ -0,0 +1,164 @@
+# Flexiv Isaac Sim Workspace
+
+[![License](https://img.shields.io/badge/License-Apache%202.0-blue.svg)](https://www.apache.org/licenses/LICENSE-2.0.html)
+
+
+## Overview
+
+This workspace is an example integration of **NVIDIA Isaac Sim** with **Flexiv Elements Studio**, connected via [Flexiv Sim Plugin](https://github.com/flexivrobotics/flexiv_sim_plugin). It demonstrates how to use the Flexiv Sim Plugin API alongside the Nvidia Isaac Sim API to relay robot control commands and physics state between Elements Studio and Isaac Sim, so that simulated Flexiv robots are driven by the same force-torque controller used on real robots and can be programmed with the Flexiv RDK.
+
+```
+┌─────────────────────────────────────────┐
+│       User Program with RDK Client      │
+│          (send robot commands)          │
+└───────────────────┬─────────────────────┘
+                    │ RDK API
+                    ▼
+┌─────────────────────────────────────────┐
+│         Flexiv Elements Studio          │
+│   (simulated controller + RDK Server)   │
+└───────────────────┬─────────────────────┘
+                    │ Flexiv Sim Plugin API
+                    ▼
+┌─────────────────────────────────────────┐
+│          Flexiv Isaac Sim Workspace     │
+│ (Flexiv Sim Plugin API + Isaac Sim API) │
+└───────────────────┬─────────────────────┘
+                    │ Isaac Sim API
+                    ▼
+┌─────────────────────────────────────────┐
+│           NVIDIA Isaac Sim              │
+│           (physics engine)              │
+└─────────────────────────────────────────┘
+```
+
+
+## Compatibility
+
+| **Supported OS** | **Supported processor** | **Supported language** |
+| ---------------- | ----------------------- | ---------------------- |
+| Ubuntu 22.04     | x86_64                  | Python                 |
+
+
+## Demos
+
+### 1. Tower of Hanoi
+
+[![Rizon 4 Masters the Tower of Hanoi Game in Issac Sim with the Flexiv-Isaac Sim Bridge App](https://img.youtube.com/vi/jZT6Ei0L3gk/0.jpg)](https://www.youtube.com/watch?v=jZT6Ei0L3gk)
+
+### 2. Peg-in-hole
+
+https://github.com/user-attachments/assets/e575bf70-9ffb-47a5-8aec-4cda8d25c08e
+
+### 3. Single robot polish
+
+https://github.com/user-attachments/assets/a0c39e70-4469-4405-a07d-e0d8a0ad589b
+
+### 4. Dual robot polish
+
+https://github.com/user-attachments/assets/7462a9bd-3cfd-40cc-95f7-b4fda0a74f30
+
+
+## Pre-requisites
+
+Before using the Flexiv Isaac Sim Workspace, follow the setup instructions in
+[Flexiv Sim Plugin](https://github.com/flexivrobotics/flexiv_sim_plugin?tab=readme-ov-file#flexiv-elements-studio-setup).
+
+
+## Workspace setup
+
+1. Install NVIDIA [Isaac Sim](https://docs.isaacsim.omniverse.nvidia.com/latest/installation/index.html).
+2. Note down Isaac Sim's installation directory, e.g. `~/isaacsim`.
+3. Run the following script in this repo to install Flexiv's Isaac Sim extensions and apps to the Isaac Sim installation directory so that Isaac Sim can load them:
+
+       bash install_ws.sh <isaac_sim_root_dir>
+
+   For example:
+
+       bash install_ws.sh ~/isaacsim
+
+## Verify setup
+
+To verify that the workspace setup is successful, run the example Python application:
+
+    cd <isaac_sim_root_dir>
+    ./python.sh standalone_examples/api/isaacsim.robot.manipulators/flexiv/follow_target_with_rmpflow.py exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4.usd
+
+WARNING: When running Isaac Sim for the first time, it takes a couple of minutes to warm up the shader cache. You will notice that the CPU is fully loaded and the Isaac Sim window seems frozen. Please wait patiently and do not force quit the program.
+
+After the example program is up and running, select the `TargetCube` prim under `World` from the Stage view, then drag it around, the robot TCP should follow the cube.
+
+
+### Run Flexiv-Isaac Bridge App
+
+1. Edit the configuration file `standalone_examples/api/isaacsim.robot.manipulators/flexiv/app_config.yaml` according to the instructions in it.
+2. Start Flexiv-Isaac Bridge App using configurations in `app_config.yaml`:
+
+       cd <isaac_sim_root_dir>
+       ./python.sh standalone_examples/api/isaacsim.robot.manipulators/flexiv/flexiv_isaac_bridge_app.py --config standalone_examples/api/isaacsim.robot.manipulators/flexiv/app_config.yaml
+
+3. The app will launch an Isaac Sim window and start the physics loop (i.e. *Play*) automatically.
+4. Go back to Elements Studio, then restart the exited simulator by toggle on the *Connect* button.
+5. Wait for the connection to establish. If the connection is successful, you should see in Elements Studio a robot at home pose with no error.
+
+### Verify everything is working
+
+1. In Elements Studio, use the simulated motion bar to enter free-drive mode.
+2. Use Cartesian or joint jogging to move the robot around, check that the robot in Isaac Sim is also moving.
+3. Jog the robot along Cartesian Z axis and let the robot make contact with the desk in the example environment, check that a large TCP force is rendered as an orange line in Elements Studio visualization.
+4. Create a test project and add some primitives, then execute the project and check that the robot in Isaac Sim is acting as desired.
+
+## After the first run
+
+To restart the whole setup after the first run:
+
+1. Start the Flexiv-Isaac Bridge app first.
+2. Then start the simulated robot in Elements Studio.
+
+If you have made some changes and need to restart the Isaac Sim app:
+
+1. Close Flexiv-Isaac Bridge app.
+2. In Elements Studio, click *CHANGE CONNECTION*, then toggle off the *Connect* button to close the simulated robot. You do not need to exit the whole Elements Studio program.
+3. Restart Flexiv-Isaac Bridge app.
+4. Toggle on the *Connect* button to restart the simulated robot.
+
+Alternatively, you can leave the simulated robot running and just restart the Isaac Sim app. The simulated robot in Elements Studio will sync with Isaac Sim after the app is restarted. However, soft error might occur and you can just clear them from Elements Studio and continue to normal operations.
+
+## Multi-robot support
+
+This framework supports simulating and controlling multiple robots:
+
+1. Add multiple robots in the configuration file `standalone_examples/api/isaacsim.robot.manipulators/flexiv/app_config.yaml`.
+2. Start Flexiv-Isaac Bridge App using the updated configurations in `app_config.yaml`:
+
+       cd <isaac_sim_root_dir>
+       ./python.sh standalone_examples/api/isaacsim.robot.manipulators/flexiv/flexiv_isaac_bridge_app.py --config standalone_examples/api/isaacsim.robot.manipulators/flexiv/app_config.yaml
+
+3. Find a second Ubuntu 22.04 computer, connect it to the first computer via Ethernet cable. Then on the first computer, check that this wired Ethernet connection is visible in the network settings, then change the IPv4 setting of this wired connection to "Shared to other computers". Alternatively, connect both computers to the same network router via **wired** connection.
+4. Make sure both computers are able to ping each other.
+5. Install Flexiv Elements Studio on the **second** computer, then create a new simulated robot. Now each computer has a robot controller with Elements Studio.
+6. Start the first simulated robot on the first computer, then wait for connection with Isaac Sim. You should see one of the robots in Isaac Sim moves a little bit when the connection is established.
+7. Start the second simulated robot on the second computer, then wait for connection with Isaac Sim. You should see the other robot in Isaac Sim moves a little bit when the connection is established.
+8. Execute test projects from both Elements Studios and check that both robots are working in Isaac Sim.
+
+## Collect data from the simulated robot(s)
+
+You can collect data from the simulated robot(s) using [Flexiv DDK](https://github.com/flexivrobotics/flexiv_ddk) (Data Distribution Kit) at up to 1kHz frequency:
+
+1. Set up Flexiv DDK according to the instructions found in the repo.
+2. Start Isaac Sim and Elements Studio.
+3. Run DDK programs to collect data from one or more simulated robots.
+
+Note: the DDK program doesn't have to run on the same computer as the Elements Studio, it can be any computer that's under the same local network as the Elements Studio computer.
+
+## Control the simulated robot(s) programmatically
+
+Besides using the drag-and-drop graphical interface in Elements Studio to create projects to control the simulated robot(s), you can also control them programmatically using [Flexiv RDK](https://github.com/flexivrobotics/flexiv_rdk) (Robotic Development Kit) in a real-time or non-real-time manner:
+
+1. Set up Flexiv RDK according to the instructions found in the repo.
+2. Start Isaac Sim and Elements Studio.
+3. In Elements Studio, go to *Settings* → *Remote Mode*, then enable Remote Mode and select *Ethernet* from the drop-down list.
+4. Restart the simulated robot by clicking *CHANGE CONNECTION*, then toggle off and on the *Connect* button.
+5. Run RDK programs to control one or more simulated robots.
+
+Note: the RDK program doesn't have to run on the same computer as the Elements Studio, it can be any computer that's under the same local network as the Elements Studio computer.
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Grav_gripper.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Grav_gripper.usd
new file mode 100644
index 0000000..25416df
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Grav_gripper.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10.usd
new file mode 100644
index 0000000..87731af
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10_with_Grav.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10_with_Grav.usd
new file mode 100644
index 0000000..d88b0c4
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10_with_Grav.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10s.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10s.usd
new file mode 100644
index 0000000..6728765
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10s.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10s_with_Grav.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10s_with_Grav.usd
new file mode 100644
index 0000000..03c9bce
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon10s_with_Grav.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4.usd
new file mode 100644
index 0000000..8a23d2e
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4R.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4R.usd
new file mode 100644
index 0000000..efb80a1
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4R.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4R_with_Grav.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4R_with_Grav.usd
new file mode 100644
index 0000000..d0dc88e
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4R_with_Grav.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4_with_Grav.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4_with_Grav.usd
new file mode 100644
index 0000000..b4621c7
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4_with_Grav.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4s.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4s.usd
new file mode 100644
index 0000000..2570fb8
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4s.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4s_with_Grav.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4s_with_Grav.usd
new file mode 100644
index 0000000..9704555
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/Rizon4s_with_Grav.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/empty_env.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/empty_env.usd
new file mode 100644
index 0000000..cf05cb8
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/empty_env.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/example_env.usd b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/example_env.usd
new file mode 100644
index 0000000..10a8734
Binary files /dev/null and b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/data/flexiv/example_env.usd differ
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/__init__.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/__init__.py
new file mode 100644
index 0000000..65fa1ab
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/__init__.py
@@ -0,0 +1,10 @@
+# Copyright (c) 2021-2023, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from isaacsim.robot.manipulators.examples.flexiv.flexiv_serial import FlexivSerial
+from isaacsim.robot.manipulators.examples.flexiv.kinematics_solver import KinematicsSolver
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/__init__.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/__init__.py
new file mode 100644
index 0000000..9da24e0
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/__init__.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2021-2023, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from isaacsim.robot.manipulators.examples.flexiv.controllers.pick_place_controller import PickPlaceController
+from isaacsim.robot.manipulators.examples.flexiv.controllers.rmpflow_controller import RMPFlowController
+from isaacsim.robot.manipulators.examples.flexiv.controllers.stacking_controller import StackingController
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/pick_place_controller.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/pick_place_controller.py
new file mode 100644
index 0000000..eb1bdbf
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/pick_place_controller.py
@@ -0,0 +1,48 @@
+# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from typing import List, Optional
+
+import isaacsim.robot.manipulators.controllers as manipulators_controllers
+from isaacsim.core.prims import SingleArticulation
+from isaacsim.robot.manipulators.examples.franka.controllers.rmpflow_controller import RMPFlowController
+from isaacsim.robot.manipulators.grippers.parallel_gripper import ParallelGripper
+
+
+class PickPlaceController(manipulators_controllers.PickPlaceController):
+    """[summary]
+
+    Args:
+        name (str): [description]
+        gripper (ParallelGripper): [description]
+        robot_articulation (SingleArticulation): [description]
+        end_effector_initial_height (Optional[float], optional): [description]. Defaults to None.
+        events_dt (Optional[List[float]], optional): [description]. Defaults to None.
+    """
+
+    def __init__(
+        self,
+        name: str,
+        gripper: ParallelGripper,
+        robot_articulation: SingleArticulation,
+        end_effector_initial_height: Optional[float] = None,
+        events_dt: Optional[List[float]] = None,
+    ) -> None:
+        if events_dt is None:
+            events_dt = [0.008, 0.005, 1, 0.1, 0.05, 0.05, 0.0025, 1, 0.008, 0.08]
+        manipulators_controllers.PickPlaceController.__init__(
+            self,
+            name=name,
+            cspace_controller=RMPFlowController(
+                name=name + "_cspace_controller", robot_articulation=robot_articulation
+            ),
+            gripper=gripper,
+            end_effector_initial_height=end_effector_initial_height,
+            events_dt=events_dt,
+        )
+        return
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/rmpflow_controller.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/rmpflow_controller.py
new file mode 100644
index 0000000..adf2382
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/rmpflow_controller.py
@@ -0,0 +1,57 @@
+# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+import isaacsim.robot_motion.motion_generation as mg
+from isaacsim.core.prims import SingleArticulation
+
+
+class RMPFlowController(mg.MotionPolicyController):
+    """[summary]
+
+    Args:
+        name (str): [description]
+        robot_articulation (SingleArticulation): [description]
+        physics_dt (float, optional): [description]. Defaults to 1.0/60.0.
+    """
+
+    def __init__(
+        self,
+        name: str,
+        robot_articulation: SingleArticulation,
+        physics_dt: float = 1.0 / 60.0,
+    ) -> None:
+        self.rmp_flow_config = (
+            mg.interface_config_loader.load_supported_motion_policy_config(
+                "Rizon4", "RMPflow"
+            )
+        )
+        self.rmp_flow = mg.lula.motion_policies.RmpFlow(**self.rmp_flow_config)
+
+        self.articulation_rmp = mg.ArticulationMotionPolicy(
+            robot_articulation, self.rmp_flow, physics_dt
+        )
+
+        mg.MotionPolicyController.__init__(
+            self, name=name, articulation_motion_policy=self.articulation_rmp
+        )
+        (
+            self._default_position,
+            self._default_orientation,
+        ) = self._articulation_motion_policy._robot_articulation.get_world_pose()
+        self._motion_policy.set_robot_base_pose(
+            robot_position=self._default_position,
+            robot_orientation=self._default_orientation,
+        )
+        return
+
+    def reset(self):
+        mg.MotionPolicyController.reset(self)
+        self._motion_policy.set_robot_base_pose(
+            robot_position=self._default_position,
+            robot_orientation=self._default_orientation,
+        )
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/stacking_controller.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/stacking_controller.py
new file mode 100644
index 0000000..835c178
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/controllers/stacking_controller.py
@@ -0,0 +1,47 @@
+# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from typing import List
+
+import isaacsim.robot.manipulators.controllers as manipulators_controllers
+from isaacsim.core.prims import SingleArticulation
+from isaacsim.robot.manipulators.examples.franka.controllers.pick_place_controller import PickPlaceController
+from isaacsim.robot.manipulators.grippers.parallel_gripper import ParallelGripper
+
+
+class StackingController(manipulators_controllers.StackingController):
+    """[summary]
+
+    Args:
+        name (str): [description]
+        gripper (ParallelGripper): [description]
+        robot_prim_path (str): [description]
+        picking_order_cube_names (List[str]): [description]
+        robot_observation_name (str): [description]
+    """
+
+    def __init__(
+        self,
+        name: str,
+        gripper: ParallelGripper,
+        robot_articulation: SingleArticulation,
+        picking_order_cube_names: List[str],
+        robot_observation_name: str,
+    ) -> None:
+        manipulators_controllers.StackingController.__init__(
+            self,
+            name=name,
+            pick_place_controller=PickPlaceController(
+                name=name + "_pick_place_controller",
+                gripper=gripper,
+                robot_articulation=robot_articulation,
+            ),
+            picking_order_cube_names=picking_order_cube_names,
+            robot_observation_name=robot_observation_name,
+        )
+        return
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/flexiv_serial.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/flexiv_serial.py
new file mode 100644
index 0000000..f235b00
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/flexiv_serial.py
@@ -0,0 +1,211 @@
+# Copyright (c) 2022-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+
+import numpy as np
+import spdlog
+from typing import Optional, List
+from isaacsim.core.api.robots.robot import Robot
+from isaacsim.core.prims import SingleRigidPrim
+from isaacsim.robot.manipulators.grippers.gripper import Gripper
+from isaacsim.robot.manipulators.grippers.parallel_gripper import ParallelGripper
+from isaacsim.robot.manipulators.grippers.surface_gripper import SurfaceGripper
+
+
+class FlexivSerial(Robot):
+    """
+    Control interface for one Flexiv serial robot.
+
+    Params:
+        prim_path (str): Primitive path of this robot articulation in the stage. E.g. /World/Flexiv
+        name (str): name of this robot articulation.
+        end_effector_prim_name (str): name of the primitive in the robot articulation to be used as end effector.
+        arm_dof (int): degrees of freedom of the robotic arm, excluding any gripper DoF.
+        pos_in_world (Optional[List[float]]): position (x, y, z) of the robot in world [m].
+        ori_in_world (Optional[List[float]]): orientation (quaternion w, x, y, z) of the robot in world [].
+        gripper (Optional[Gripper]): Constructed gripper instance.
+    """
+
+    def __init__(
+        self,
+        prim_path: str,
+        name: str,
+        end_effector_prim_name: str,
+        arm_dof: int = 7,
+        pos_in_world: Optional[List[float]] = None,
+        ori_in_world: Optional[List[float]] = None,
+        gripper: Optional[Gripper] = None,
+    ) -> None:
+        self._arm_dof = arm_dof
+        self._gripper = gripper
+        self._default_kps = None
+        self._default_kds = None
+        self._end_effector = None
+        self._end_effector_prim_path = prim_path + "/" + end_effector_prim_name
+        self._logger = spdlog.ConsoleLogger("flexiv::" + name)
+
+        # Construct base
+        super().__init__(
+            prim_path=prim_path,
+            name=name,
+            position=pos_in_world,
+            orientation=ori_in_world,
+        )
+        return
+
+    def switch_control_mode(self, mode: str) -> None:
+        """
+        Switch control mode for all robot joints, gripper excluded.
+
+        Params:
+            mode (str): Desired control mode, options are "position", "velocity", and "effort".
+        """
+        # Reset default gains for articulation view
+        if self._default_kps is not None and self._default_kds is not None:
+            self._articulation_view.set_gains(
+                kps=self._default_kps, kds=self._default_kds
+            )
+
+        # Set control mode for all robot joints, but leave gripper joints unchanged
+        self._articulation_view.switch_control_mode(
+            mode, joint_indices=np.arange(0, self._arm_dof)
+        )
+
+        self._logger.info(f"Control mode switched to [{mode}]")
+        return
+
+    @property
+    def end_effector(self) -> SingleRigidPrim:
+        """
+        Access reference to the _end_effector member.
+
+        Return:
+            SingleRigidPrim: Reference to the end-effector instance.
+        """
+        return self._end_effector
+
+    @property
+    def gripper(self) -> Gripper:
+        """
+        Access reference to the _gripper member.
+
+        Return:
+            Gripper: Reference to the gripper instance.
+        """
+        return self._gripper
+
+    @property
+    def q(self) -> List[float]:
+        """
+        Get current positions of all robot joints, gripper excluded.
+
+        Return:
+            List[float]: Joint positions [rad].
+        """
+        if self._articulation_view.is_physics_handle_valid():
+            return self.get_joint_positions(
+                joint_indices=np.arange(0, self._arm_dof)
+            ).tolist()
+        else:
+            return np.zeros(self._arm_dof).tolist()
+
+    @property
+    def dq(self) -> List[float]:
+        """
+        Get current velocities of all robot joints, gripper excluded.
+
+        Return:
+            List[float]: Joint velocities [rad/s].
+        """
+        if self._articulation_view.is_physics_handle_valid():
+            return self.get_joint_velocities(
+                joint_indices=np.arange(0, self._arm_dof)
+            ).tolist()
+        else:
+            return np.zeros(self._arm_dof).tolist()
+
+    @property
+    def tau(self) -> List[float]:
+        """
+        Get current torques of all robot joints, gripper excluded.
+
+        Return:
+            List[float]: Joint torques [Nm].
+        """
+        if self._articulation_view.is_physics_handle_valid():
+            return self.get_measured_joint_efforts(
+                joint_indices=np.arange(0, self._arm_dof)
+            ).tolist()
+        else:
+            return np.zeros(self._arm_dof).tolist()
+
+    def apply_torques(self, tau_d: List[float]) -> None:
+        """
+        Apply desired torques to all robot joints, gripper excluded.
+
+        Params:
+            tau_d (List[float]): Desired joint torques.
+        """
+        # Apply only to robot joints, leave out gripper joints, which are controlled by gripper controller
+        self.set_joint_efforts(tau_d, joint_indices=np.arange(0, self._arm_dof))
+        return
+
+    def teleport_to(self, q_d: List[float]) -> None:
+        """
+        Instantly teleport all robot joints to desired positions. This is usually used to set robot to initial positions.
+        No control is involved in the process. Call apply_torques() immediately after to kick in the joint controls.
+
+        Params:
+            q_d (List[float]): Desired joint positions.
+        """
+        self.set_joint_positions(q_d, joint_indices=np.arange(0, self._arm_dof))
+        return
+
+    def initialize(self, physics_sim_view=None) -> None:
+        """
+        Initialize the articulation interface, set up torque drive mode
+        """
+        super().initialize(physics_sim_view=physics_sim_view)
+
+        # Initialize end-effector
+        self._end_effector = SingleRigidPrim(
+            prim_path=self._end_effector_prim_path, name=self.name + "_end_effector"
+        )
+        self._end_effector.initialize(physics_sim_view)
+
+        # Initialize gripper if any
+        if isinstance(self._gripper, ParallelGripper):
+            self._gripper.initialize(
+                physics_sim_view=physics_sim_view,
+                articulation_apply_action_func=self.apply_action,
+                get_joint_positions_func=self.get_joint_positions,
+                set_joint_positions_func=self.set_joint_positions,
+                dof_names=self.dof_names,
+            )
+        elif isinstance(self._gripper, SurfaceGripper):
+            self._gripper.initialize(
+                physics_sim_view=physics_sim_view, articulation_num_dofs=self.num_dof
+            )
+
+        # Joints output torque instead of acceleration
+        self.get_articulation_controller().set_effort_modes("force")
+
+        # Save default gains because calling _articulation_view.switch_control_mode() will change _articulation_view._default_kps,
+        # which makes switching control mode from "effort" back to "position" not possible
+        self._default_kps, self._default_kds = self._articulation_view.get_gains()
+        return
+
+    def post_reset(self) -> None:
+        """
+        Post reset articulation
+        """
+        super().post_reset()
+        self._end_effector.post_reset()
+        if self._gripper is not None:
+            self._gripper.post_reset()
+        return
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/kinematics_solver.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/kinematics_solver.py
new file mode 100644
index 0000000..82ea8f7
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/kinematics_solver.py
@@ -0,0 +1,44 @@
+# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from typing import Optional
+
+import isaacsim.robot_motion.motion_generation.interface_config_loader as interface_config_loader
+from isaacsim.core.prims import SingleArticulation
+from isaacsim.robot_motion.motion_generation.articulation_kinematics_solver import ArticulationKinematicsSolver
+from isaacsim.robot_motion.motion_generation.lula.kinematics import LulaKinematicsSolver
+
+class KinematicsSolver(ArticulationKinematicsSolver):
+    """Kinematics Solver for Flexiv robot.  This class loads a LulaKinematicsSovler object
+
+    Args:
+        robot_articulation (SingleArticulation): An initialized Articulation object representing this Flexiv
+        end_effector_frame_name (Optional[str]): The name of the Flexiv end effector.  If None, an end effector link will
+            be automatically selected.  Defaults to None.
+    """
+
+    def __init__(
+        self,
+        robot_articulation: SingleArticulation,
+        end_effector_frame_name: Optional[str] = None,
+    ) -> None:
+        kinematics_config = (
+            interface_config_loader.load_supported_lula_kinematics_solver_config(
+                "Rizon4"
+            )
+        )
+        self._kinematics = LulaKinematicsSolver(**kinematics_config)
+
+        if end_effector_frame_name is None:
+            end_effector_frame_name = "flange"
+
+        ArticulationKinematicsSolver.__init__(
+            self, robot_articulation, self._kinematics, end_effector_frame_name
+        )
+
+        return
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/__init__.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/__init__.py
new file mode 100644
index 0000000..649248f
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/__init__.py
@@ -0,0 +1,11 @@
+# Copyright (c) 2021-2023, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from isaacsim.robot.manipulators.examples.flexiv.tasks.follow_target import FollowTarget
+from isaacsim.robot.manipulators.examples.flexiv.tasks.pick_place import PickPlace
+from isaacsim.robot.manipulators.examples.flexiv.tasks.stacking import Stacking
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/follow_target.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/follow_target.py
new file mode 100644
index 0000000..7a7899a
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/follow_target.py
@@ -0,0 +1,84 @@
+# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from typing import Optional
+
+import isaacsim.core.api.tasks as tasks
+import numpy as np
+from isaacsim.core.utils.prims import is_prim_path_valid
+from isaacsim.core.utils.string import find_unique_string_name
+from isaacsim.core.utils.stage import add_reference_to_stage
+from isaacsim.robot.manipulators.examples.flexiv import FlexivSerial
+
+
+class FollowTarget(tasks.FollowTarget):
+    """[summary]
+
+    Args:
+        name (str, optional): [description]. Defaults to "flexiv_follow_target".
+        target_prim_path (Optional[str], optional): [description]. Defaults to None.
+        target_name (Optional[str], optional): [description]. Defaults to None.
+        target_position (Optional[np.ndarray], optional): [description]. Defaults to None.
+        target_orientation (Optional[np.ndarray], optional): [description]. Defaults to None.
+        offset (Optional[np.ndarray], optional): [description]. Defaults to None.
+        flexiv_prim_path (Optional[str], optional): [description]. Defaults to None.
+        flexiv_robot_name (Optional[str], optional): [description]. Defaults to None.
+    """
+
+    def __init__(
+        self,
+        name: str = "flexiv_follow_target",
+        target_prim_path: Optional[str] = None,
+        target_name: Optional[str] = None,
+        target_position: Optional[np.ndarray] = None,
+        target_orientation: Optional[np.ndarray] = None,
+        offset: Optional[np.ndarray] = None,
+        flexiv_prim_path: Optional[str] = None,
+        flexiv_robot_name: Optional[str] = None,
+        usd_path: Optional[str] = None,
+    ) -> None:
+        tasks.FollowTarget.__init__(
+            self,
+            name=name,
+            target_prim_path=target_prim_path,
+            target_name=target_name,
+            target_position=target_position,
+            target_orientation=target_orientation,
+            offset=offset,
+        )
+        self._flexiv_prim_path = flexiv_prim_path
+        self._flexiv_robot_name = flexiv_robot_name
+        self._usd_path = usd_path
+        return
+
+    def set_robot(self) -> FlexivSerial:
+        """[summary]
+
+        Returns:
+            FlexivSerial: [description]
+        """
+        if self._flexiv_prim_path is None:
+            self._flexiv_prim_path = find_unique_string_name(
+                initial_name="/World/Flexiv",
+                is_unique_fn=lambda x: not is_prim_path_valid(x),
+            )
+        if self._flexiv_robot_name is None:
+            self._flexiv_robot_name = find_unique_string_name(
+                initial_name="Rizon4",
+                is_unique_fn=lambda x: not self.scene.object_exists(x),
+            )
+
+        add_reference_to_stage(
+            usd_path=self._usd_path, prim_path=self._flexiv_prim_path
+        )
+
+        return FlexivSerial(
+            prim_path=self._flexiv_prim_path,
+            name=self._flexiv_robot_name,
+            end_effector_prim_name="flange",
+        )
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/pick_place.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/pick_place.py
new file mode 100644
index 0000000..c6ca348
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/pick_place.py
@@ -0,0 +1,64 @@
+# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from typing import Optional
+
+import isaacsim.core.api.tasks as tasks
+import numpy as np
+from isaacsim.core.utils.prims import is_prim_path_valid
+from isaacsim.core.utils.string import find_unique_string_name
+from isaacsim.robot.manipulators.examples.flexiv import FlexivSerial
+
+
+class PickPlace(tasks.PickPlace):
+    """[summary]
+
+    Args:
+        name (str, optional): [description]. Defaults to "flexiv_pick_place".
+        cube_initial_position (Optional[np.ndarray], optional): [description]. Defaults to None.
+        cube_initial_orientation (Optional[np.ndarray], optional): [description]. Defaults to None.
+        target_position (Optional[np.ndarray], optional): [description]. Defaults to None.
+        cube_size (Optional[np.ndarray], optional): [description]. Defaults to None.
+        offset (Optional[np.ndarray], optional): [description]. Defaults to None.
+    """
+
+    def __init__(
+        self,
+        name: str = "flexiv_pick_place",
+        cube_initial_position: Optional[np.ndarray] = None,
+        cube_initial_orientation: Optional[np.ndarray] = None,
+        target_position: Optional[np.ndarray] = None,
+        cube_size: Optional[np.ndarray] = None,
+        offset: Optional[np.ndarray] = None,
+    ) -> None:
+        tasks.PickPlace.__init__(
+            self,
+            name=name,
+            cube_initial_position=cube_initial_position,
+            cube_initial_orientation=cube_initial_orientation,
+            target_position=target_position,
+            cube_size=cube_size,
+            offset=offset,
+        )
+        return
+
+    def set_robot(self) -> FlexivSerial:
+        """[summary]
+
+        Returns:
+            FlexivSerial: [description]
+        """
+        flexiv_prim_path = find_unique_string_name(
+            initial_name="/World/Flexiv",
+            is_unique_fn=lambda x: not is_prim_path_valid(x),
+        )
+        flexiv_robot_name = find_unique_string_name(
+            initial_name="Rizon4",
+            is_unique_fn=lambda x: not self.scene.object_exists(x),
+        )
+        return FlexivSerial(prim_path=flexiv_prim_path, name=flexiv_robot_name)
diff --git a/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/stacking.py b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/stacking.py
new file mode 100644
index 0000000..e495a41
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv/tasks/stacking.py
@@ -0,0 +1,64 @@
+# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+from typing import Optional
+
+import numpy as np
+from isaacsim.core.api.tasks import Stacking as BaseStacking
+from isaacsim.core.utils.prims import is_prim_path_valid
+from isaacsim.core.utils.stage import get_stage_units
+from isaacsim.core.utils.string import find_unique_string_name
+from isaacsim.robot.manipulators.examples.flexiv import FlexivSerial
+
+
+class Stacking(BaseStacking):
+    """[summary]
+
+    Args:
+        name (str, optional): [description]. Defaults to "flexiv_stacking".
+        target_position (Optional[np.ndarray], optional): [description]. Defaults to None.
+        cube_size (Optional[np.ndarray], optional): [description]. Defaults to None.
+        offset (Optional[np.ndarray], optional): [description]. Defaults to None.
+    """
+
+    def __init__(
+        self,
+        name: str = "flexiv_stacking",
+        target_position: Optional[np.ndarray] = None,
+        cube_size: Optional[np.ndarray] = None,
+        offset: Optional[np.ndarray] = None,
+    ) -> None:
+        if target_position is None:
+            target_position = np.array([0.5, 0.5, 0]) / get_stage_units()
+        BaseStacking.__init__(
+            self,
+            name=name,
+            cube_initial_positions=np.array([[0.3, 0.3, 0.3], [0.3, -0.3, 0.3]])
+            / get_stage_units(),
+            cube_initial_orientations=None,
+            stack_target_position=target_position,
+            cube_size=cube_size,
+            offset=offset,
+        )
+        return
+
+    def set_robot(self) -> FlexivSerial:
+        """[summary]
+
+        Returns:
+            FlexivSerial: [description]
+        """
+        flexiv_prim_path = find_unique_string_name(
+            initial_name="/World/Flexiv",
+            is_unique_fn=lambda x: not is_prim_path_valid(x),
+        )
+        flexiv_robot_name = find_unique_string_name(
+            initial_name="Rizon4",
+            is_unique_fn=lambda x: not self.scene.object_exists(x),
+        )
+        return FlexivSerial(prim_path=flexiv_prim_path, name=flexiv_robot_name)
diff --git a/thirdparty/isaac_sim_ws/install_ws.sh b/thirdparty/isaac_sim_ws/install_ws.sh
new file mode 100644
index 0000000..c5f5b56
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/install_ws.sh
@@ -0,0 +1,19 @@
+#!/bin/bash
+# Install Flexiv's Isaac Sim workspace to the NVIDIA Isaac Sim installation directory.
+
+# Absolute path of this script
+SCRIPT_PATH="$(dirname $(readlink -f $0))"
+set -e
+
+# Check script arguments
+if [ "$#" -lt 1 ]; then
+    echo "Error: invalid script argument"
+    echo "Required argument: [isaac_sim_root]"
+    echo "    isaac_sim_root: absolute path to Isaac Sim installation root directory"
+    exit 1
+fi
+ISAAC_ROOT=$1
+
+# Install files
+cp -r $SCRIPT_PATH/exts/ $ISAAC_ROOT
+cp -r $SCRIPT_PATH/standalone_examples/ $ISAAC_ROOT
diff --git a/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/app_config.yaml b/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/app_config.yaml
new file mode 100644
index 0000000..b371b3f
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/app_config.yaml
@@ -0,0 +1,71 @@
+# One or more robots to add to the scene.
+# NOTE: Add or remove [serial_number] blocks to control which robots are added to the scene.
+robots:
+  - serial_number: "Rizon4-00001" # Serial number of robot 1
+    usd: "/path/to/robot1.usd"    # Absolute path to the usd file of robot 1, see exts/isaacsim.robot.manipulators.examples/data/flexiv/
+    position:                     # Position in world frame to put robot 1
+      x: 0.0
+      y: 0.0
+      z: 0.0
+    orientation:                  # Orientation (quaternion) in world frame to put robot 1
+      w: 1.0
+      x: 0.0
+      y: 0.0
+      z: 0.0
+
+  - serial_number: "Rizon4-00002" # Serial number of robot 2
+    usd: "/path/to/robot2.usd"    # Absolute path to the usd file of robot 2
+    position:                     # Position in world frame to put robot 2
+      x: 1.0
+      y: 0.0
+      z: 0.0
+    orientation:                  # Orientation (quaternion) in world frame to put robot 2
+      w: 1.0
+      x: 0.0
+      y: 0.0
+      z: 0.0
+  
+  # More robots ...
+
+# Absolute path to the usd file of the environment to load. 
+# If not provided, an empty environment with default ground plane will be used. You can also start with the example environment 
+# located at exts/isaacsim.robot.manipulators.examples/data/flexiv/example_env.usd
+env_usd: "/path/to/environment.usd"
+
+# Enable GPU dynamics. Use this argument if any object in the scene requires GPU for dynamics computation. 
+# For example, deformable body material and SDF mesh collider.
+gpu_dynamics: false
+
+# One or more cameras to add to the scene .
+# NOTE: Add or remove [name] blocks to control which cameras are added to the scene. 
+#       Remove or comment out the whole [cameras] section if no camera is needed.
+cameras:
+  - name: "camera_1"        # Unique name of camera 1
+    resolution: [1280, 720] # Resolution of camera 1
+    fps: 30                 # Frames per second of camera 1
+    focal_length: 2.5       # Focal length in meter of camera 1
+    position:               # Position in world frame to put camera 1
+      x: 0.0
+      y: 0.0
+      z: 0.0
+    orientation:            # Orientation (quaternion) in USD frame to put camera 1
+      w: 1.0
+      x: 0.0
+      y: 0.0
+      z: 0.0
+
+  - name: "camera_2"        # Unique name of camera 2
+    resolution: [1280, 720] # Resolution of camera 2
+    fps: 30                 # Frames per second of camera 2
+    focal_length: 2.5       # Focal length in meter of camera 2
+    position:               # Position in world frame to put camera 2
+      x: 0.0
+      y: 0.0
+      z: 0.0
+    orientation:            # Orientation (quaternion) in USD frame to put camera 2
+      w: 1.0
+      x: 0.0
+      y: 0.0
+      z: 0.0
+
+  # More cameras ...
diff --git a/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/flexiv_isaac_bridge_app.py b/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/flexiv_isaac_bridge_app.py
new file mode 100644
index 0000000..01d507f
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/flexiv_isaac_bridge_app.py
@@ -0,0 +1,352 @@
+# Copyright (c) 2021-2023, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+
+# App version
+APP_VERSION = "1.3"
+
+# Compatible flexivsimplugin version
+COMPATIBLE_SIM_PLUGIN_VER = "1.2.0"
+
+import yaml
+import spdlog
+import numpy as np
+from typing import List, Dict
+from enum import Enum
+from argparse import ArgumentParser
+from dataclasses import dataclass
+from isaacsim import SimulationApp
+
+# Middleware plugin for connecting to Flexiv Elements Studio
+import flexivsimplugin
+
+# Check version
+if flexivsimplugin.__version__ != COMPATIBLE_SIM_PLUGIN_VER:
+    raise ImportError(
+        f"flexivsimplugin=={COMPATIBLE_SIM_PLUGIN_VER} is required, but found {flexivsimplugin.__version__}"
+    )
+
+
+# Load config file
+argparser = ArgumentParser()
+argparser.add_argument("--config", required=True, help="Path to YAML config file")
+args = argparser.parse_args()
+
+# Start simulation main window
+simulation_app = SimulationApp({"headless": False, "width": 1920, "height": 1080})
+
+# Import isaac modules after SimulationApp is started
+from isaacsim.core.api import World
+from isaacsim.core.utils.stage import add_reference_to_stage
+from isaacsim.sensors.camera import Camera
+from isaacsim.robot.manipulators.examples.flexiv import FlexivSerial
+from isaacsim.robot.manipulators.grippers.parallel_gripper import ParallelGripper
+
+# Physics and render loop period [sec]
+RENDER_FREQ = 60.0
+PHYSICS_FREQ = 2000.0
+
+
+# Gripper status
+class GripperStatus(Enum):
+    INIT = 0
+    OPENED = 1
+    CLOSED = 2
+
+
+class BridgeRunner(object):
+    """
+    Set up world and run joint impedance control.
+
+    Params:
+        physics_dt (float): Physics loop period of the scene [sec].
+        render_dt (float): Render loop period of the scene [sec].
+        config (Dict): Configurations parsed from the config file.
+        initial_q (List[float], optional): Initial joint positions [rad].
+    """
+
+    # Data struct for a single robot
+    @dataclass
+    class SingleRobotData:
+        name: str
+        instance: FlexivSerial
+        sim_plugin: flexivsimplugin.UserNode
+        last_connected: bool
+        gripper_status: GripperStatus
+
+    # Robot degrees of freedom
+    ROBOT_DOF = 7
+
+    def __init__(
+        self,
+        physics_dt,
+        render_dt,
+        config: Dict,
+        initial_q: List[float] = [0.0] * ROBOT_DOF,
+    ) -> None:
+        # Initialize logger
+        self._logger = spdlog.ConsoleLogger("Flexiv-Isaac Bridge App")
+
+        # fmt: off
+        self._logger.info("——————————————————————————————————————————————————————————")
+        self._logger.info(f"———            Flexiv-Isaac Bridge App v{APP_VERSION}            ———")
+        self._logger.info("——————————————————————————————————————————————————————————")
+        # fmt: on
+
+        # Save initial q
+        self._initial_q = initial_q
+
+        # Create world
+        self._world = World(
+            stage_units_in_meters=1.0,
+            physics_dt=physics_dt,
+            rendering_dt=render_dt,
+            set_defaults=False,
+        )
+
+        # Enable GPU dynamics if specified
+        if config.get("gpu_dynamics", False):
+            self._world.get_physics_context().enable_gpu_dynamics(True)
+
+        # Load environment and reset world
+        env_usd = config.get("env_usd", "")
+        if env_usd:
+            # Add user-provided environment to stage
+            add_reference_to_stage(usd_path=env_usd, prim_path="/World")
+        else:
+            # Add empty environment to stage
+            self._world.scene.add_default_ground_plane()
+        self._world.reset()
+
+        # Add cameras
+        self._cameras = []
+        for c in config.get("cameras", []):
+            cam_name = c["name"]
+            pos_in_world = [float(c["position"][i]) for i in ["x", "y", "z"]]
+            ori_in_world = [float(c["orientation"][i]) for i in ["w", "x", "y", "z"]]
+            camera = Camera(
+                prim_path="/World/" + cam_name,
+                frequency=c["fps"],
+                resolution=tuple(c["resolution"]),
+                position=pos_in_world,
+                orientation=ori_in_world,
+            )
+            camera.set_focal_length(c["focal_length"])
+            # Use the same camera axes as the Isaac Sim UI
+            camera.set_world_pose(
+                position=pos_in_world, orientation=ori_in_world, camera_axes="usd"
+            )
+            self._cameras.append(camera)
+            self._logger.info(
+                f"Added camera [/World/{cam_name}] located at {pos_in_world} {ori_in_world} in world"
+            )
+
+        # Create data struct for all robots and add them to stage
+        self._robots = []
+        for r in config.get("robots", []):
+            # Parse config of this robot
+            serial_num = r["serial_number"]
+            usd_path = r["usd"]
+            pos_in_world = [float(r["position"][i]) for i in ["x", "y", "z"]]
+            ori_in_world = [float(r["orientation"][i]) for i in ["w", "x", "y", "z"]]
+
+            # Replace dash with underscore in serial number to avoid prim path error
+            serial_num = serial_num.replace("-", "_")
+
+            # Add this robot to stage
+            prim_path = "/World/Flexiv/" + serial_num
+            self._logger.info(
+                f"Adding robot usd [{usd_path}] to stage at prim path [{prim_path}]"
+            )
+            add_reference_to_stage(usd_path=usd_path, prim_path=prim_path)
+
+            # Configure gripper if the usd name suggests a gripper exists in the model
+            gripper = None
+            end_effector_prim_name = "flange"
+            if "Grav" in usd_path:
+                # This gripper has only one actuation joint, but the API requires two,
+                # thus providing a non-actuation joint (gains = 0) as a place holder
+                end_effector_prim_name = "Grav_gripper/right_finger_tip"
+                gripper = ParallelGripper(
+                    end_effector_prim_path=prim_path + "/" + end_effector_prim_name,
+                    joint_prim_names=["finger_joint", "right_outer_knuckle_joint"],
+                    joint_opened_positions=np.array([45.0, 0]),
+                    joint_closed_positions=np.array([-8.88, 0]),
+                )
+                self._logger.info(
+                    "The usd name suggests a Grav gripper exists in the model, gripper control will be enabled"
+                )
+            elif "Robotiq" in usd_path:
+                # This gripper has only one actuation joint, but the API requires two,
+                # thus providing a non-actuation joint (gains = 0) as a place holder
+                end_effector_prim_name = (
+                    "Robotiq_2F_85_flattened/Robotiq_2F_85/right_inner_finger"
+                )
+                gripper = ParallelGripper(
+                    end_effector_prim_path=prim_path + "/" + end_effector_prim_name,
+                    joint_prim_names=["finger_joint", "right_inner_finger_joint"],
+                    joint_opened_positions=np.array([0, 0]),
+                    joint_closed_positions=np.array([45, 0]),
+                )
+                self._logger.info(
+                    "The usd name suggests a Robotiq gripper exists in the model, gripper control will be enabled"
+                )
+            else:
+                self._logger.info("Gripper control is not enabled")
+
+            # Add robot to stage
+            robot = self._world.scene.add(
+                FlexivSerial(
+                    prim_path=prim_path,
+                    name=serial_num,
+                    end_effector_prim_name=end_effector_prim_name,
+                    arm_dof=BridgeRunner.ROBOT_DOF,
+                    pos_in_world=pos_in_world,
+                    ori_in_world=ori_in_world,
+                    gripper=gripper,
+                )
+            )
+            self._logger.info(
+                f"Added robot [/World/Flexiv/{serial_num}] located at {pos_in_world} {ori_in_world} in world"
+            )
+
+            # Append single robot data struct
+            self._robots.append(
+                self.SingleRobotData(
+                    name=serial_num,
+                    instance=robot,
+                    sim_plugin=flexivsimplugin.UserNode(serial_num),
+                    last_connected=False,
+                    gripper_status=GripperStatus.INIT,
+                )
+            )
+
+        # Add physics callback
+        self._world.add_physics_callback("robot_step", callback_fn=self.on_physics_step)
+
+        # Reset world once
+        self._world.reset()
+
+        # Initialize cameras
+        for cam in self._cameras:
+            cam.initialize()
+
+        # Initialize other members
+        self._reset_needed = False
+        self._servo_cycle = 0
+
+        # Put robot to initial pose
+        for robot in self._robots:
+            robot.instance.teleport_to(self._initial_q)
+
+    def on_physics_step(self, dt) -> None:
+        """
+        Physics call back to host the real-time joint control loop. The loop period of this callback is provided by [dt].
+
+        Params:
+            dt (float): Loop period, same as [physics_dt] [sec].
+        """
+        for robot in self._robots:
+            # Publish fresh robot states to all Flexiv Nodes before doing anything else
+            robot.sim_plugin.SendRobotStates(
+                flexivsimplugin.SimRobotStates(
+                    self._servo_cycle,
+                    robot.instance.q,
+                    robot.instance.dq,
+                )
+            )
+
+        for robot in self._robots:
+            if robot.sim_plugin.connected():
+                # Upon reconnection, set joint torque control mode
+                if not robot.last_connected:
+                    self._logger.info(f"Connected to robot [{robot.name}]")
+                    robot.instance.switch_control_mode("effort")
+
+                # Wait for new commands to arrive before proceeding current cycle
+                timeout_ms = 100
+                if robot.sim_plugin.WaitForRobotCommands(timeout_ms):
+                    # Apply joint torques
+                    robot.instance.apply_torques(
+                        robot.sim_plugin.robot_commands().target_drives
+                    )
+                else:
+                    self._logger.warn(f"Missed 1 message from [{robot.name}]")
+
+                # Gripper control based on digital output signal
+                dout_list = list(
+                    robot.sim_plugin.robot_commands().digital_outputs
+                )  # Convert map to list
+                if dout_list:
+                    # DOUT[0] high = open gripper
+                    if dout_list[0]:
+                        # Ignore if already opened
+                        if robot.gripper_status != GripperStatus.OPENED:
+                            self._logger.info("Opening gripper")
+                            robot.instance.gripper.open()
+                            robot.gripper_status = GripperStatus.OPENED
+                    # DOUT[1] high = close gripper
+                    if dout_list[1]:
+                        # Ignore if already closed
+                        if robot.gripper_status != GripperStatus.CLOSED:
+                            self._logger.info("Closing gripper")
+                            robot.instance.gripper.close()
+                            robot.gripper_status = GripperStatus.CLOSED
+
+                # Set last connected status
+                robot.last_connected = True
+
+            else:
+                if robot.last_connected:
+                    # Upon disconnection, transit this robot from torque control to position control to hold its current pose
+                    self._logger.error(f"Disconnected from robot [{robot.name}]")
+                    robot.instance.switch_control_mode("position")
+                    robot.instance.teleport_to(robot.instance.q)
+                    robot.gripper_status = GripperStatus.INIT
+
+                # Set last connected status
+                robot.last_connected = False
+
+        # Increment server cycle
+        self._servo_cycle += 1
+
+    def run(self) -> None:
+        """
+        Poll world step, which will step physics and rendering with specified physics_dt and render_dt.
+        """
+        while simulation_app.is_running():
+            self._world.step(render=True)
+            # Reset world if needed
+            if self._world.is_stopped() and not self._reset_needed:
+                self._reset_needed = True
+                for robot in self._robots:
+                    robot.last_connected = False
+            if self._world.is_playing():
+                if self._reset_needed:
+                    self._world.reset()
+                    self._reset_needed = False
+                    # Put robot to initial pose
+                    for robot in self._robots:
+                        robot.instance.switch_control_mode("position")
+                        robot.instance.teleport_to(self._initial_q)
+
+
+def main():
+    # Create runner to handle everything
+    runner = BridgeRunner(
+        physics_dt=1.0 / PHYSICS_FREQ,
+        render_dt=1.0 / RENDER_FREQ,
+        config=yaml.safe_load(open(args.config)),
+        initial_q=[0.0, -0.698132, 0.0, 1.5708, 0.0, 0.698132, 0.0],
+    )
+    runner.run()
+    simulation_app.close()
+
+
+if __name__ == "__main__":
+    main()
diff --git a/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/follow_target_with_rmpflow.py b/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/follow_target_with_rmpflow.py
new file mode 100644
index 0000000..879ee7e
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/standalone_examples/api/isaacsim.robot.manipulators/flexiv/follow_target_with_rmpflow.py
@@ -0,0 +1,54 @@
+# Copyright (c) 2021-2024, NVIDIA CORPORATION. All rights reserved.
+#
+# NVIDIA CORPORATION and its licensors retain all intellectual property
+# and proprietary rights in and to this software, related documentation
+# and any modifications thereto. Any use, reproduction, disclosure or
+# distribution of this software and related documentation without an express
+# license agreement from NVIDIA CORPORATION is strictly prohibited.
+#
+
+# Start simulation
+from isaacsim import SimulationApp
+
+simulation_app = SimulationApp({"headless": False})
+
+from isaacsim.core.api import World
+from isaacsim.robot.manipulators.examples.flexiv.controllers.rmpflow_controller import (
+    RMPFlowController,
+)
+from isaacsim.robot.manipulators.examples.flexiv.tasks import FollowTarget
+from isaacsim.storage.native import get_assets_root_path
+
+# Use Rizon4 usd stored in Isaac Sim assets
+usd_path = get_assets_root_path() + "/Isaac/Robots/Flexiv/Rizon4/flexiv_rizon4.usd"
+
+my_world = World(stage_units_in_meters=1.0)
+my_task = FollowTarget(name="flexiv_follow_target", usd_path=usd_path)
+my_world.add_task(my_task)
+my_world.reset()
+task_params = my_world.get_task("flexiv_follow_target").get_params()
+flexiv_name = task_params["robot_name"]["value"]
+target_name = task_params["target_name"]["value"]
+my_flexiv = my_world.scene.get_object(flexiv_name)
+my_controller = RMPFlowController(
+    name="target_follower_controller", robot_articulation=my_flexiv
+)
+articulation_controller = my_flexiv.get_articulation_controller()
+reset_needed = False
+while simulation_app.is_running():
+    my_world.step(render=True)
+    if my_world.is_stopped() and not reset_needed:
+        reset_needed = True
+    if my_world.is_playing():
+        if reset_needed:
+            my_world.reset()
+            my_controller.reset()
+            reset_needed = False
+        observations = my_world.get_observations()
+        actions = my_controller.forward(
+            target_end_effector_position=observations[target_name]["position"],
+            target_end_effector_orientation=observations[target_name]["orientation"],
+        )
+        articulation_controller.apply_action(actions)
+
+simulation_app.close()
diff --git a/thirdparty/isaac_sim_ws/uninstall_ws.sh b/thirdparty/isaac_sim_ws/uninstall_ws.sh
new file mode 100644
index 0000000..1d17b5c
--- /dev/null
+++ b/thirdparty/isaac_sim_ws/uninstall_ws.sh
@@ -0,0 +1,20 @@
+#!/bin/bash
+# Uninstall Flexiv's Isaac Sim workspace from the NVIDIA Isaac Sim installation directory.
+
+# Absolute path of this script
+SCRIPT_PATH="$(dirname $(readlink -f $0))"
+set -e
+
+# Check script arguments
+if [ "$#" -lt 1 ]; then
+    echo "Error: invalid script argument"
+    echo "Required argument: [isaac_sim_root]"
+    echo "    isaac_sim_root: absolute path to Isaac Sim installation root directory"
+    exit 1
+fi
+ISAAC_ROOT=$1
+
+# Uninstalled files
+rm -rf $ISAAC_ROOT/exts/isaacsim.robot.manipulators.examples/isaacsim/robot/manipulators/examples/flexiv
+rm -rf $ISAAC_ROOT/exts/isaacsim.robot.manipulators.examples/data/flexiv
+rm -rf $ISAAC_ROOT/standalone_examples/api/isaacsim.robot.manipulators/flexiv
diff --git a/vendor/rdk_libs/libflexiv_rdk.x86_64-linux-gnu.a b/vendor/rdk_libs/libflexiv_rdk.x86_64-linux-gnu.a
new file mode 100644
index 0000000..19e2cb2
Binary files /dev/null and b/vendor/rdk_libs/libflexiv_rdk.x86_64-linux-gnu.a differ