{
  "controller_port": 5277,
  "variable_list": [
    {
      "name": "probe_in_tracker",
      "type": "transform_obj"
    },
    {
      "name": "model_ref_in_tracker",
      "type": "transform_obj"
    },
    {
      "name": "model_in_model_ref",
      "type": "transform_obj",
      "value": [
        134.705,
        42.6256,
        -267.533,
        0.535693,
        0.610751,
        -0.464744,
        -0.352178
      ]
    },
    {
      "name": "probe_in_model_ref",
      "type": "transform_obj"
    },
    {
      "name": "probe_in_model",
      "type": "transform_obj"
    },
    {
      "name": "tool_in_model",
      "type": "transform_obj"
    },
    {
      "name": "tool_in_robot_flange",
      "type": "transform_obj"
    },
    {
      "name": "robot_flange_in_base",
      "type": "transform_obj"
    },
    {
      "name": "tip_in_model_ref",
      "type": "scalarxyz_obj"
    },
    {
      "name": "picked_point_in_model_ref",
      "type": "scalarxyz_obj"
    },
    {
      "name": "model_in_model_ref_error",
      "type": "double_obj"
    },
    {
      "name": "tip_in_model",
      "type": "scalarxyz_obj"
    },
    {
      "name": "picked_point_in_model",
      "type": "scalarxyz_obj"
    },
    {
      "name": "target_tool_in_model",
      "type": "transform_obj"
    },
    {
      "name": "target_tool_offset",
      "type": "transform_obj"
    },
    {
      "name": "target_flange_offset",
      "type": "transform_obj"
    },
    {
      "name": "target_flange_pose_temp",
      "type": "transform_obj"
    },
    {
      "name": "flange_move_sig",
      "type": "bool_obj"
    },
    {
      "name": "target_flange_pose",
      "type": "transform_obj"
    },
    {
      "name": "robot_flange_speed_limit",
      "type": "double_obj",
      "value": 5
    },
    {
      "name": "kuka_joint_speed_limit",
      "type": "array7_obj",
      "value": [
        0.1,
        0.1,
        0.1,
        0.1,
        0.1,
        0.1,
        0.1
      ]
    },
    {
      "name": "robot_is_moving",
      "type": "bool_obj"
    },
    {
      "name": "kuka_control_mode",
      "type": "u64int_obj"
    }
  ],
  "object_list": [
    {
      "type": "transform_tree",
      "name": "transform_tree",
      "init_config": {
        "node_list": [
          {
            "name": "tracker"
          },
          {
            "name": "probe",
            "parent": "tracker",
            "transform_var_name": "probe_in_tracker"
          },
          {
            "name": "model_ref",
            "parent": "tracker",
            "transform_var_name": "model_ref_in_tracker"
          },
          {
            "name": "model",
            "parent": "model_ref",
            "transform_var_name": "model_in_model_ref"
          },
          {
            "name": "robot_base",
            "parent": "tracker",
            "transform": [
              227.964774380901,
              408.415090613534,
              -2083.90135385252,
              0.718310380441512,
              0.277543700520970,
              -0.563300905075462,
              -0.299485862758597
            ]
          },
          {
            "name": "robot_flange",
            "parent": "robot_base",
            "transform_var_name": "robot_flange_in_base"
          },
          {
            "name": "tool_real",
            "parent": "robot_flange",
            "transform": [
              193.674181012287,
              119.007940358412,
              0.975841097441376,
              0.340960180366781,
              -0.343553889200713,
              -0.619099078363195,
              0.618411846417222
            ]
          },
          {
            "name": "tool",
            "parent": "tool_real",
            "transform": [
              6,
              5,
              -25,
              1,
              0,
              0,
              0
            ]
          }
        ]
      },
      "start_config": {
        "watch_list": [
          {
            "target": "probe",
            "observer": "model_ref",
            "transform_var_name": "probe_in_model_ref"
          },
          {
            "target": "probe",
            "observer": "model",
            "transform_var_name": "probe_in_model"
          },
          {
            "target": "tool",
            "observer": "model",
            "transform_var_name": "tool_in_model"
          },
          {
            "target": "tool",
            "observer": "robot_flange",
            "transform_var_name": "tool_in_robot_flange"
          }
        ]
      }
    },
    {
      "type": "ndi_interface",
      "name": "ndi",
      "init_config": {
        "address_type": "ethernet",
        "ip": "10.0.0.5",
        "tcp_port": 8765,
        "com_port": "COM3",
        "tool_list": [
          {
            "rom_path": "/home/tpx/data/roms/Probe_Small_3Ball.rom",
            "outputs": {
              "transform": "probe_in_tracker"
            }
          },
          {
            "rom_path": "/home/tpx/data/roms/Drill_3Ball.rom",
            "outputs": {
              "transform": "model_ref_in_tracker"
            }
          }
        ]
      },
      "start_config": {
        "allow_unreliable": true,
        "prefer_stream_tracking": false
      }
    },
    {
      "type": "transform_obj_validity_watcher",
      "name": "probe_visibility_watcher",
      "start_config": {
        "variable_name": "probe_in_tracker"
      }
    },
    {
      "type": "transform_obj_validity_watcher",
      "name": "model_ref_visibility_watcher",
      "start_config": {
        "variable_name": "model_ref_in_tracker"
      }
    },
    {
      "type": "scalarxyz_transformer",
      "name": "probe_tip_transformer_for_reg",
      "start_config": {
        "transform_type": "point",
        "transform_var_name": "probe_in_model_ref",
        "target_value": [
          0,
          0,
          0
        ],
        "output_var_name": "tip_in_model_ref"
      },
      "dependencies": [
        "ndi",
        "transform_tree"
      ]
    },
    {
      "type": "transform_stabilizer",
      "name": "point_picker_for_reg",
      "start_config": {
        "stable_type": "point",
        "input_var_name": "tip_in_model_ref",
        "output_var_name": "picked_point_in_model_ref",
        "linear_tolerance_mm": 0.05,
        "temporal_interval_s": 3
      },
      "dependencies": [
        "probe_tip_transformer_for_reg"
      ]
    },
    {
      "type": "scalarxyz_obj_watcher",
      "name": "picked_point_watcher_for_reg",
      "start_config": {
        "variable_name": "picked_point_in_model_ref"
      }
    },
    {
      "type": "landmark_registration",
      "name": "model_registrator",
      "start_config": {
        "fiducial_points": [
          [
            9.16363,
            245.77,
            -35.5363
          ],
          [
            -38.2517,
            238.265,
            15.8886
          ],
          [
            -18.4721,
            228.506,
            -24.2903
          ]
        ],
        "point_var_name": "picked_point_in_model_ref",
        "transform_var_name": "model_in_model_ref",
        "error_var_name": "model_in_model_ref_error"
      },
      "dependencies": [
        "point_picker_for_reg"
      ]
    },
    {
      "type": "transform_obj_watcher",
      "name": "registration_result_watcher",
      "start_config": {
        "variable_name": "model_in_model_ref"
      }
    },
    {
      "type": "double_obj_watcher",
      "name": "registration_error_watcher",
      "start_config": {
        "variable_name": "model_in_model_ref_error"
      }
    },
    {
      "type": "scalarxyz_transformer",
      "name": "probe_tip_transformer_for_nav",
      "start_config": {
        "transform_type": "point",
        "transform_var_name": "probe_in_model",
        "target_value": [
          0,
          0,
          0
        ],
        "output_var_name": "tip_in_model"
      },
      "dependencies": [
        "ndi",
        "transform_tree"
      ]
    },
    {
      "type": "transform_stabilizer",
      "name": "point_picker_for_icp",
      "start_config": {
        "stable_type": "point",
        "input_var_name": "tip_in_model",
        "output_var_name": "picked_point_in_model",
        "linear_tolerance_mm": 0.05,
        "temporal_interval_s": 2
      },
      "dependencies": [
        "probe_tip_transformer_for_nav"
      ]
    },
    {
      "type": "scalarxyz_obj_watcher",
      "name": "picked_point_watcher_for_icp",
      "start_config": {
        "variable_name": "picked_point_in_model"
      }
    },
    {
      "type": "kuka_interface",
      "name": "kuka",
      "init_config": {
        "address": "10.0.0.10"
      },
      "start_config": {
        "output_config": {
          "report_frequency": 60,
          "flange_info": {
            "pose": "robot_flange_in_base"
          },
          "is_moving": "robot_is_moving",
          "current_control_mode": "kuka_control_mode"
        },
        "input_config": {
          "control_mode": "hard_flange_pose",
          "flange_pose": "target_flange_pose",
          "flange_speed_limit": "robot_flange_speed_limit",
          "joint_speed_limit": "kuka_joint_speed_limit"
        }
      }
    },
    {
      "type": "five_dof_offset_calculator",
      "name": "tcp_offset_calculator",
      "start_config": {
        "current_transform_var": "tool_in_model",
        "target_transform_var": "target_tool_in_model",
        "offset_transform_var": "target_tool_offset"
      },
      "dependencies": [
        "kuka",
        "ndi",
        "transform_tree"
      ]
    },
    {
      "type": "tcp_offset_to_flange_calculator",
      "name": "flange_offset_calculator",
      "start_config": {
        "tcp_offset_transform_var": "target_tool_offset",
        "tcp_in_flange_transform_var": "tool_in_robot_flange",
        "flange_offset_transform_var": "target_flange_offset"
      },
      "dependencies": [
        "kuka",
        "tcp_offset_calculator"
      ]
    },
    {
      "type": "simple_offset_to_pose_controller",
      "name": "kuka_controller",
      "start_config": {
        "current_transform_var": "robot_flange_in_base",
        "offset_transform_var": "target_flange_offset",
        "target_transform_var": "target_flange_pose_temp"
      },
      "dependencies": [
        "kuka",
        "flange_offset_calculator"
      ]
    },
    {
      "type": "transform_obj_watcher",
      "name": "robot_old_flange_watcher",
      "start_config": {
        "variable_name": "robot_flange_in_base",
        "minimum_interval_ms": 2000
      }
    },
    {
      "type": "transform_obj_watcher",
      "name": "robot_new_flange_watcher",
      "start_config": {
        "variable_name": "target_flange_pose_temp",
        "minimum_interval_ms": 2000
      }
    },
    {
      "type": "transform_obj_d_latch",
      "name": "kuka_controller_latch",
      "start_config": {
        "input_name": "target_flange_pose_temp",
        "output_name": "target_flange_pose",
        "signal_name": "flange_move_sig"
      },
      "dependencies": [
        "kuka",
        "kuka_controller"
      ]
    },
    {
      "type": "empty_object",
      "name": "reg_model",
      "dependencies": [
        "probe_visibility_watcher",
        "model_ref_visibility_watcher",
        "picked_point_watcher_for_reg",
        "model_registrator",
        "registration_result_watcher",
        "registration_error_watcher"
      ]
    },
    {
      "type": "empty_object",
      "name": "icp_model",
      "dependencies": [
        "probe_visibility_watcher",
        "model_ref_visibility_watcher",
        "picked_point_watcher_for_icp",
        "point_picker_for_icp",
        "registration_result_watcher"
      ]
    },
    {
      "type": "empty_object",
      "name": "robot",
      "dependencies": [
        "kuka_controller_latch",
        "robot_old_flange_watcher",
        "robot_new_flange_watcher"
      ]
    }
  ]
}