syntax = "proto3";
package cmvr.config;

import "cmvr/common/geometry.proto";
import "cmvr/config/joint_limits_config.proto";
import "cmvr/config/lawba_ik_config.proto";
import "cmvr/config/pinocchio_dls_ik_config.proto";
import "cmvr/config/pinocchio_qp_ik_config.proto";
import "cmvr/config/srs_ik_config.proto";

enum ToppraPathType {
  TOPPRA_PATH_TYPE_UNKNOWN = 0;
  TOPPRA_PATH_TYPE_LINEAR = 1;
  TOPPRA_PATH_TYPE_CUBIC_HERMITE = 2;
  TOPPRA_PATH_TYPE_QUINTIC = 3;
  TOPPRA_PATH_TYPE_NATURAL = 4;
}

message MotorRobotArmBackendConfig {
  string motor_system_id = 2;
  int32 dof = 3;
  repeated string joint_names = 4;
  int32 upd_freq = 5;
  int32 buffer_size = 6;
  double default_vel = 7;
  double default_acc = 8;
  repeated string motor_group_ids = 9;
}

enum VendorRobotArmBrand {
  VENDOR_ROBOT_ARM_BRAND_UNKNOWN = 0;
  VENDOR_ROBOT_ARM_BRAND_AUBO_ARM = 1;
}

message VendorRobotArmBackendConfig {
  VendorRobotArmBrand brand = 1;
  string ip = 2;
  int32 port = 3;
  string model = 4;
  int32 dof = 5;
  repeated string joint_names = 6;
  string base_frame = 7;
  string tool_frame = 8;
  string username = 9;
  string password = 10;
}

message QpJointLimitAvoidanceConfig {
  bool enable = 1;
  double margin_ratio = 2;
  double gain = 3;
  double max_push = 4;
  double weight = 5;
}

message CartesianVelocityQpConfig {
  JointLimitsConfig joint_limits = 1;
  .cmvr.common.Vec6 twist_tracking_weight = 2;
  double qdot_regularization = 3;
  double prev_qdot_regularization = 4;
  double solver_eps = 5;
  QpJointLimitAvoidanceConfig joint_limit_avoidance = 6;
}

message SpeedLPlannerConfig {
  double linear_velocity_max = 1;
  double linear_acceleration_max = 2;
  double linear_jerk_max = 3;
  double angular_velocity_max = 4;
  double angular_acceleration_max = 5;
  double angular_jerk_max = 6;
  repeated double joint_acceleration_max = 7;
  double linear_target_replan_threshold = 8;
  double angular_target_replan_threshold = 9;
  double linear_reverse_cos_threshold = 10;
  double linear_reverse_switch_speed_threshold = 11;
  double normal_direction_deviation_deg = 12;
  double mild_direction_deviation_deg = 13;
  double severe_direction_deviation_deg = 14;
  double linear_min_speed_ratio = 15;
  CartesianVelocityQpConfig qp = 16;
}

message CartesianVelocityControllerConfig {
  double control_period_s = 1;
  double stop_twist_norm = 2;
  double stop_command_velocity_norm = 3;
  double stop_measured_velocity_norm = 4;
}

message ToppraJointMotionPlannerConfig {
  ToppraPathType path_type = 1;
  double sample_period_s = 2;
  int32 grid_size = 3;
  int32 high_grid_size = 4;
}

message MoveJConfig {
  oneof algorithm {
    ToppraJointMotionPlannerConfig toppra_joint_motion_planner = 1;
  }
}

message MoveLPlannerConfig {
  double sample_period_s = 1;
  double position_gain = 2;
  double rotation_gain = 3;
  CartesianVelocityQpConfig qp = 4;
}

message MoveLConfig {
  oneof algorithm {
    MoveLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1;
    MoveLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2;
  }
}

message SpeedLControllerConfig {
  oneof algorithm {
    CartesianVelocityControllerConfig cartesian_velocity_controller = 1;
  }
}

message SpeedLConfig {
  oneof algorithm {
    SpeedLPlannerConfig pinocchio_dls_cartesian_motion_planner = 1;
    SpeedLPlannerConfig pinocchio_qp_cartesian_motion_planner = 2;
  }

  SpeedLControllerConfig speed_l_controller = 10;
}

message ArmKinematicsConfig {
  oneof algorithm {
    PinocchioDlsIKConfig pinocchio_dls_ik_solver = 10;
    PinocchioQpIKConfig pinocchio_qp_ik_solver = 11;
    SrsIKConfig srs_ik_solver = 12;
    LawbaIKConfig lawba_ik_solver = 13;
  }
}

message ArmMotionConfig {
  MoveJConfig move_j = 1;
  MoveLConfig move_l = 2;
  reserved 3;
  SpeedLConfig speed_l = 4;
}

message RobotArmConfig {
  string id = 1;
  reserved 2;

  oneof backend {
    MotorRobotArmBackendConfig motor = 10;
    VendorRobotArmBackendConfig vendor = 11;
  }

  ArmKinematicsConfig kinematics = 20;
  ArmMotionConfig motion = 21;
}

message ArmConfig {
  repeated RobotArmConfig robot_arms = 1;
}

message ArmRootConfig {
  ArmConfig arm = 1;
}