; robot model meta-information
model_information {
  manipulatorModelType     0      // 0: Default-arm, 1: Wheel-based manipulator, 2: Floating-arm manipulator, 3: Fully actuated floating-arm manipulator
  
  ; motion joints in the URDF to consider fixed
  removeJoints {
    [0] "gripper"
    [1] "gripper_bar"
    [2] "ee_bar"
    [3] "left_finger"
    [4] "right_finger"
    [5] "ee_gripper"
    [6] "root_joint"
    
  }

  ; base frame of the robot (from URDF)
  baseFrame                       "root"
  ; end-effector frame of the robot (from URDF)
  eeFrame                         "wx250/gripper_prop_link"
}

model_settings
{
  usePreComputation               true
  recompileLibraries              true
}

; DDP settings
ddp
{
  algorithm                       SLQ

  nThreads                        3
  threadPriority                  50

  maxNumIterations                1
  minRelCost                      0.1
  constraintTolerance             1e-3

  displayInfo                     false
  displayShortSummary             false
  checkNumericalStability         false
  debugPrintRollout               false
  debugCaching                    false

  AbsTolODE                       1e-5
  RelTolODE                       1e-3
  maxNumStepsPerSecond            100000
  timeStep                        1e-3
  backwardPassIntegratorType      ODE45

  constraintPenaltyInitialValue   20.0
  constraintPenaltyIncreaseRate   2.0

  preComputeRiccatiTerms          true

  useFeedbackPolicy               false

  strategy                        LINE_SEARCH
  lineSearch
  {
    minStepLength                 1e-2
    maxStepLength                 1.0
    hessianCorrectionStrategy     DIAGONAL_SHIFT
    hessianCorrectionMultiple     1e-3
  }
}

; Rollout settings
rollout
{
  AbsTolODE                       1e-5
  RelTolODE                       1e-3
  timeStep                        1e-2
  integratorType                  ODE45
  maxNumStepsPerSecond            100000
  checkNumericalStability         false
}

; MPC settings
mpc
{
  timeHorizon                     1.0   ; [s]
  solutionTimeWindow              0.2   ; [s]
  coldStart                       false
  
  debugPrint                      false

  mpcDesiredFrequency             100   ; [Hz]
  mrtDesiredFrequency             400   ; [Hz]
}

; initial state
initialState
{
  ; initial state for the different types of arm base DOFs
  base
  {
    defaultManipulator
    {
    }

    floatingArmManipulator
    {
      (0,0)  0.0   ; position x
      (1,0)  0.0   ; position y
      (2,0)  0.0   ; position z
      (3,0)  0.0   ; euler angle z
      (4,0)  0.0   ; euler angle y
      (5,0)  0.0   ; euler angle x
    }

    fullyActuatedFloatingArmManipulator
    {
      (0,0)  0.0  ; position x
      (1,0)  0.0  ; position y
      (2,0)  0.0  ; position z
      (3,0)  0.0  ; euler angle z
      (4,0)  0.0  ; euler angle y
      (5,0)  0.0  ; euler angle x
    }

    wheelBasedMobileManipulator
    {
      (0,0)  0.0  ; position x
      (1,0)  0.0  ; position y
      (2,0)  0.0  ; heading
    }
  }

  ; initial state for the arm DOFs
  arm
  {
    (0,0)   -0.56    ; arm_1
    (1,0)    1.65    ; arm_2
    (2,0)   -1.71    ; arm_3
    (3,0)   -1.07    ; arm_4
    (4,0)    0.09    ; arm_5
    (5,0)    0.19    ; arm_6
  }
}

inputCost
{
  ; control weight matrixarmarm
  
  R
  {
    ; input costs for the different types of arm base DOFs
    base
    {
      defaultManipulator
      {
      }

      floatingArmManipulator
      {
      }

      fullyActuatedFloatingArmManipulator
      {
        scaling 1e-2

        (0,0)  5.0  ; position x
        (1,1)  5.0  ; position y
        (2,2)  5.0  ; position z
        (3,3)  5.0  ; euler angle z
        (4,4)  5.0  ; euler angle y
        (5,5)  5.0  ; euler angle x
      }

      wheelBasedMobileManipulator
      {
        scaling 1e-2

        (0,0)  5.0  ; forward velocity
        (1,1)  5.0  ; turning velocity
      }
    }

    ; input costs for the arm DOFs
    arm
    {
      scaling 1e-2

      (0,0)  1.0  ; arm_1 velocity
      (1,1)  1.0  ; arm_2 velocity
      (2,2)  1.0  ; arm_3 velocity
      (3,3)  1.0  ; arm_4 velocity
      (4,4)  1.0  ; arm_5 velocity
      (5,5)  1.0  ; arm_6 velocity
    }
  }
}
endEffector
{
  ; end effector quadratic penalty scaling
  muPosition      10.0
  muOrientation    5.0
}

finalEndEffector
{
  muPosition      10.0
  muOrientation    5.0
}

selfCollision
{ 
  ; activate self-collision constraint
  activate  false

  ; TODO: Replace the collision meshes of the arm with primitive shapes.
}

; Only applied for arm joints: limits parsed from URDF
jointPositionLimits
{
  ; activate constraint
  activate  true

  ; relaxed log barrier mu
  mu      0.01

  ; relaxed log barrier delta
  delta   1e-3
}

jointVelocityLimits
{
  ; relaxed log barrier mu
  mu      0.01

  ; relaxed log barrier delta
  delta   1e-3
  
  lowerBound
  {
    ; velocity limits for the different types of arm base DOFs
    base
    {
      defaultManipulator
      {
      }

      floatingArmManipulator
      {
      }

      fullyActuatedFloatingArmManipulator
      {
        (0,0)  -0.1  ; linear velocity x
        (1,0)  -0.1  ; linear velocity y
        (2,0)  -0.1  ; linear velocity z
        (3,0)  -0.3  ; euler angle velocity z
        (4,0)  -0.3  ; euler angle velocity y
        (5,0)  -0.3  ; euler angle velocity x
      }

      wheelBasedMobileManipulator
      {
        (0,0)  -0.1 ; forward velocity
        (1,0)  -0.3 ; turning velocity
      }
    }

    ; velocity limits for the arm DOFs
    arm
    {
      (0,0)  -15.0     ; arm_1 [rad/s]
      (1,0)  -15.0	   ; arm_2 [rad/s]
      (2,0)  -15.0	   ; arm_3 [rad/s]
      (3,0)  -15.0	   ; arm_4 [rad/s]
      (4,0)  -15.0	   ; arm_5 [rad/s]
      (5,0)  -15.0	   ; arm_6 [rad/s]
    }
  }

  upperBound
  {
    ; velocity limits for the different types of arm base DOFs
    base
    {
      defaultManipulator
      {
      }

      floatingArmManipulator
      {
      }

      fullyActuatedFloatingArmManipulator
      {
        (0,0)  0.1  ; linear velocity x
        (1,0)  0.1  ; linear velocity y
        (2,0)  0.1  ; linear velocity z
        (3,0)  0.3  ; euler angle velocity z
        (4,0)  0.3  ; euler angle velocity y
        (5,0)  0.3  ; euler angle velocity x
      }

      wheelBasedMobileManipulator
      {
        (0,0)  0.1 ; forward velocity
        (1,0)  0.3 ; turning velocity
      }
    }

    ; velocity limits for the arm DOFs
    arm
    {
      (0,0)  15.0    ; arm_1 [rad/s]
      (1,0)  15.0	   ; arm_2 [rad/s]
      (2,0)  15.0	   ; arm_3 [rad/s]
      (3,0)  15.0	   ; arm_4 [rad/s]
      (4,0)  15.0	   ; arm_5 [rad/s]
      (5,0)  15.0	   ; arm_6 [rad/s]
    }
  }
}