Sophiar2/app/data/kuka_navigation.json

{
  "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": [
        135.945,
        39.1189,
        -267.07,
        0.536927,
        0.60565,
        -0.471815,
        -0.349698
      ]
    },
    {
      "name": "probe_in_model_ref",
      "type": "transform_obj"
    },
    {
      "name": "probe_in_model",
      "type": "transform_obj"
    },
    {
      "name": "tool_in_flange_const",
      "type": "transform_obj",
      "value": [
        193.183270697813,
        119.298696093691,
        1.38808397963203,
        0.341548509981856,
        -0.342944503560901,
        -0.618995814406514,
        0.618528790400057
      ]
    },
    {
      "name": "tool_in_model",
      "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": [
              249.608683668030,
              378.363630568367,
              -1999.86036620645,
              0.694408974678311,
              0.320622130095823,
              -0.539876883852255,
              -0.351469167724585
            ]
          },
          {
            "name": "robot_flange",
            "parent": "robot_base",
            "transform_var_name": "robot_flange_in_base"
          },
          {
            "name": "tool_real",
            "parent": "robot_flange",
            "transform_var_name": "tool_in_flange_const"
          },
          {
            "name": "tool",
            "parent": "tool_real",
            "transform": [
              0,
              0,
              -30,
              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"
          }
        ]
      }
    },
    {
      "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_flange_const",
        "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"
      ]
    }
  ]
}