克隆下来必须带上正在用的视觉权重和本机 site.yaml，否则感知只能指到一台机器上的路径，别人起不来。

顶视迁臂改走厂家 MoveL，D405 框心对准后再对孔；夹爪停在 caught 时 open 不再把相同目标当成已经张开，init 会真的回零。

Co-authored-by: Cursor <cursoragent@cursor.com>

.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

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。

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=开（‰）

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）没有
+一阶段默认 **D405 实时推理**，D435 待命。YOLO-pose 这一路不关。连续
-conf≥`perception.wrist_handoff_conf`（0.5）的目标，才用顶视 D435 **拍一张** 把工件 / k0
+`D405_MISS_BEFORE_D435_S`（0.5 s）没有 conf≥`perception.wrist_handoff_conf`（0.5）
-定到基座系（眼在手外 T，不再乘法兰），左臂按二阶段同一套 **`FollowJointTrajectory` action**
+的目标，才用顶视 D435 **拍一张**，把点用眼在手外 T 变到基座（不再乘法兰）。
-先收齐锁高，再把全部 XY 串成一条关节折线，余弦加速（100 mm）/
+这一段运动是厂家 `MoveL`：先放开 `ros2_control` 占着的 RDK，XY 对准该点，
-巡航封顶 60 mm/s / 余弦减速（80 mm），头尾速度为 0。中途不重测、不停住。
+工具尖高度保持当前（点的 z 是工件表面），不按直线距离拒绝；走完再用
-40 mm 爬到 80 mm/s 会在真机上像弹射，那是加速段太短，不是初速度或角速度。
+`run_real.sh --no-stop` 把控制栈拉回来（不扫感知进程）。走到该点并且法兰 XY
-工件上方（剩差 ≤80 mm）巡航改 30 mm/s，只走 XY，不混 Z、不夹收垂直。
+连续 `servo.lock_still_sec`（3 s）位移不超过 `lock_still_move_m`（15 mm）之前，
-结果 + 0.25 s 再重测 TF（带符号 `dxy`）。冻结 J 若还剩 >15 mm，只允许 **一刀补**
+D405 的点不下发。停稳之后改回 D405，跟踪点是工件框的矩形中心，不追 k0。
-（`tag=fix`）。XY 进 15 mm 门槛后再单独收法兰朝下。结果回来之前不往话题插点。
+框心对准连续 `servo.wrist_align_sec`（1 s）→ `/visual_servo/locked` → 二阶段对小孔。
-到位成功不再发话题 hold。几乎没走近就钉住，不许按同一剩差再刷。
+对孔只用关键点像素交到 `workpiece_surface_z_m`，不读深度图。下臂停在
-走到该点并且法兰 XY 连续 `servo.lock_still_sec`（3 s）位移不超过
+`tool10120938` 的 TCP 长度（0.2245 m）加上 `final_gap_mm` 之上，不按旧 tool1 的 0.223 m。
-`lock_still_move_m`（15 mm）之前，D405 的点不下发（推理不关）。停稳之后改回 D405
+只测 D435 这一段位移时加 `--d435-only`：不交 D405、不进二阶段，并且要求先停 `run_real.sh`。
-跟踪；再按同一静止判据 → `/visual_servo/locked` → 二阶段对孔。
-只测 D435 这一段位移时加 `--d435-only`：D405 点不下发，到位停稳后钉住并结束一阶段，
-不交 D405、不进二阶段。眼外标定误差只会让冻结点整体偏一截，不会造成中途启停；
-启停来自以前按 80/15 mm 一跳一跳 rest-to-rest。
 
 ```bash
 bash ~/ros2/scripts/run_all.sh --arm 89 --no-camera --d435-only

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]

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]

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())

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"

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

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    姿态保持当前")

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())

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 都要耗满

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

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:
-            try:
+            cur = self.get_position_per_mille()
-                cur = self.get_position_per_mille()
+        except Exception:
-            except Exception:
+            cur = value
-                cur = value
+        bridge = latched_position_break(cmd, cur, value)
-            nudge = cur + (40 if value >= cur else -40)
+        if bridge is not None:
-            nudge = _clamp_int(nudge, 0, 1000)
+            self.write_register(REG_POSITION, bridge)
-            if nudge == value:
+            deadline = time.monotonic() + 0.6
-                nudge = 0 if value != 0 else 40
+            echoed = False
-            self.write_register(REG_POSITION, nudge)
+            while time.monotonic() < deadline:
-            time.sleep(0.06)
+                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
 

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:
+    def initialize(
-        """写 0xA5，然后等到初始化状态 == 完成（对应官方上电流程）。"""
+        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:
-            try:
+            cur = self.get_current_position(retries=2)
-                cur = self.get_current_position(retries=2)
+        except Exception:
-            except Exception:
+            cur = pos
-                cur = pos
+        bridge = latched_position_break(cmd, cur, pos)
-            nudge = cur + (40 if pos >= cur else -40)
+        if bridge is not None:
-            nudge = int(max(0, min(1000, nudge)))
+            if not self.write_register(REG_POSITION, bridge):
-            if nudge == pos:
+                return False
-                nudge = 0 if pos != 0 else 40
+            if not self._wait_command_echo(bridge):
-            self.write_register(REG_POSITION, nudge)
+                return False
-            time.sleep(0.06)
+            # 读回只说明寄存器改了。再留一个控制周期，避免下一笔把中间值盖掉。
+            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:
+            if grip_motion_settled(st.grip_state, st.position, target, seen_motion):
-                return st
-            if st.grip_state == S_GRIP_CAUGHT:
                 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("正在初始化（手指回零）…")
+            print("正在初始化（手指回零，已初始化也会再做一次）…")
-            grip.initialize()
+            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)

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:

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,
         )

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 / "
+                "腕部主跟踪必须从 site.yaml 接入 lock_still_sec / wrist_align_sec / "
-                "wrist_handoff_conf / overhead_hold_s。收到哨兵 -1 = 配置没接线，"
+                "lock_still_move_m / wrist_handoff_conf / overhead_hold_s。"
-                "不许用代码默认 3 秒假装已经静止判据。"
+                "收到哨兵 -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 ==="
                 )

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():

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

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。"""

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"]

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

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

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.

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.

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

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

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

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

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

thirdparty/flexiv_rdk/.gitignore

@@ -0,0 +1,13 @@
+.vscode
+.vs
+.venv
+.DS_Store
+*.a
+*.so
+*.lib
+*.dll
+config.h
+build*/
+cloned/
+html/
+__pycache__

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()

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

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.

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()

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")

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)

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)

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()

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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;
+}

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())

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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()

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

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_ */

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_ */

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_ */

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_ */

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_ */

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_ */

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_ */

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_ */

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_ */

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_ */

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_ */

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_ */

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_ */

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() { }

thirdparty/flexiv_rdk/lib/flexiv_rdk.win_amd64.lib.sha256

@@ -0,0 +1 @@
+8159b1304cd25f4cbde0aafc37d8e41ea8edf77804b0680cee5abf9442ba7557

thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-linux-gnu.a.sha256

@@ -0,0 +1 @@
+d1267bfa2f54b30e044c92c5468bdfdeb80126fc99e22d1a9623e73315b070a7

thirdparty/flexiv_rdk/lib/libflexiv_rdk.aarch64-qnx.a.sha256

@@ -0,0 +1 @@
+temporarily_suspended

thirdparty/flexiv_rdk/lib/libflexiv_rdk.arm64-darwin.a.sha256

@@ -0,0 +1 @@
+24b0f511b6679d935c6d26e7dab512e534b80da32583699e4b1f2872f10eabde

thirdparty/flexiv_rdk/lib/libflexiv_rdk.x86_64-linux-gnu.a.sha256

@@ -0,0 +1 @@
+f5a0aab1fe2195ac28fd10764c6a3deb81d25789d7b04acb1e87e029f6924aea