#This is an example WAM configuration file

system{
    busCount = 1
    bus[]{
        type = "CAN"                # Type of bus
        device = "/dev/can0"        # Communication device
        address = 0                 # Address of robot
        name = "WAM7"               # Name of the robot
    }
    bus[]{
        type = "Ethernet"           # Type of bus
        device = "eth0"             # Communication device
        address = "192.168.139.65"  # Address of robot
        name = "EtherWAM"           # Name of the robot
    }
}

WAM7{
    name = "4 DOF WAM with 3 DOF Wrist" # Cool name
    dof = 7                         # Degrees of freedom
    home = <0.0, -1.9, 0.0, +3.14, 0.0, 0.0, 0.0>  # Home position of each joint (rad)
    tlimit = <15.0, 25.0, 8.0, 8.0, 2.0, 3.0, 2.0>
    
    world = <<1,0,0>,<0,1,0>,<0,0,1>> # World -> Base frame transform
    # world = <<0,0,1>,<0,1,0>,<1,0,0>> # World -> Base frame transform
    #world = <<0.5,0,0.5><0,1,0><-0.5,0,0.5>>
    tool = <<1,2,3,3>,<1,2,3,3>,<1,2,3,3>> # Lastframe -> Toolframe transform

    safety{
        tipFaultVel = 2.0   # Endtip velocity fault threshold (m/s)
        tipWarnVel = 0.5    # Endtip velocity warning threshold (m/s)
        faultTrq = 8.4      # Motor torque fault threshold (Nm)
        warnTrq = 5.0       # Motor torque warning threshold (Nm)
    }

    # Transmission ratios
    N = <42, 28.25, 28.25, 18, 9.7, 9.7, 14.93>
    n = <0, 0, 1.68, 0, 0, 1, 0>
    
    # Motor to Joint transform matrices (pre-2007)
#m2jp = <<-.0238095,0,0,0,0,0,0>,<0,.017699,-.017699,0,0,0,0>,<0,-.0297345,-.0297345,0,0,0,0>,<0,0,0,.0555556,0,0,0>,<0,0,0,0,.0486855,.0486855,0>,<0,0,0,0,-.0486855,.0486855,0>,<0,0,0,0,0,0,-.0669792>>
#j2mt = <<-.0238095,0,0,0,0,0,0>,<0,.017699,-.0297345,0,0,0,0>,<0,-.017699,-.0297345,0,0,0,0>,<0,0,0,.0555556,0,0,0>,<0,0,0,0,.0486855,-.0486855,0>,<0,0,0,0,.0486855,.0486855,0>,<0,0,0,0,0,0,-.0669792>>

    # Motor to Joint transform matrices (2007, M4 reversed)
M4_2007 = 1 # Motor reversed?
m2jp = <<-.0238095,0,0,0,0,0,0>,<0,.017699,-.017699,0,0,0,0>,<0,-.0297345,-.0297345,0,0,0,0>,<0,0,0,-.0555556,0,0,0>,<0,0,0,0,0.0515464,0.0515464,0>,<0,0,0,0,-0.0515464,0.0515464,0>,<0,0,0,0,0,0,-.0669792>>
j2mt = <<-.0238095,0,0,0,0,0,0>,<0,.017699,-.0297345,0,0,0,0>,<0,-.017699,-.0297345,0,0,0,0>,<0,0,0,-.0555556,0,0,0>,<0,0,0,0,0.0515464,-0.0515464,0>,<0,0,0,0,0.0515464,0.0515464,0>,<0,0,0,0,0,0,-.0669792>>

    link[]{
        name = "Base"
        
        # Denavit-Hartenberg parameters (Spong)
        # Defines the transformation from the previous frame to this link's frame
        dh{ 
            theta = 0.0 # (multiplied by pi)
            d = 0.0
            a = 0.0
            alpha = -0.5 # (multiplied by pi)
        }
        
        # Mass of this link (kg)
        mass = 8.3936
        
        # Center of mass (m), xyz translation from this link's frame
        com = <0.3506e-3, 132.6795e-3, 0.6286e-3>
        
        # Inertial matrix, chosen at this link's frame
        #I = <<242919.8516E-6,636.5542E-6,-93.1687E-6>,<636.5542E-6,92036.7003E-6,-262.6549E-6>,<-93.1687E-6,-262.6542E-6,207050.7372E-6>>
        
        # Inertial matrix, chosen at this link's center of mass
        I = <<95157.4294e-6,246.1404e-6,-95.0183e-6>,<246.1404e-6,92032.3524e-6,-962.6725e-6>,<-95.0183e-6,-962.6725e-6,59290.5997e-6>>
        
        # Rotor inertia <Lxx, Lyy, Lzz>
        rotorI = <0.0, 0.201, 0.0>
        
        vel = 0.5                       # Default velocity
        acc = 0.5                       # Default acceleration
        
        pid{ # PID control parameters
            kp = 900
            ki = 2.5
            kd = 10
            max = 8191 # abs(jointTorque) is bounded to this value
        }
    }
    link[]{
        name = "Pitch"

        # Denavit-Hartenberg parameters (Spong)
        # Defines the transformation from the previous frame to this link's frame
        dh{
            theta = 0.0 # (multiplied by pi)
            d = 0.0
            a = 0.0
            alpha = 0.5 # (multiplied by pi)
        }

        # Mass of this link (kg)
        mass = 4.8487
        
        # Center of mass (m), xyz translation from this link's frame
        com = <-0.2230e-3, -21.3924e-3, 13.3754e-3>
        
        # Inertial matrix, chosen at this link's frame
        #I = <<32413.1852E-6,-20.2663E-6,-143.7579E-6>,<-20.2663E-6,21649.2574E-6,-38.6737E-6>,<-143.7579E-6,-38.6737E-6,25026.5101E-6>>
        
        # Inertial matrix, chosen at this link's center of mass
        I = <<29326.8098e-6,-43.3994e-6,-129.2942e-6>,<-43.3994e-6,20781.5826e-6,1348.6924e-6>,<-129.2942e-6,1348.6924e-6,22807.3271e-6>>

        # Rotor inertia <Lxx, Lyy, Lzz>
        rotorI = <0.0, 0.182, 0.0>
        
        vel = 0.5                       # Default velocity
        acc = 0.5                       # Default acceleration
        
        pid{ # PID control parameters
            kp = 2500
            ki = 5
            kd = 20
            max = 8191 # abs(jointTorque) is bounded to this value
        }
    }
    link[]{
        name = "Twist"

        # Denavit-Hartenberg parameters (Spong)
        # Defines the transformation from the previous frame to this link's frame
        dh{
            theta = 0.0 # (multiplied by pi)
            d = 0.550
            a = 0.045
            alpha = -0.5 # (multiplied by pi)
        }

        # Mass of this link (kg)
        mass = 1.7251

        # Center of mass (m), xyz translation from this link's frame
        com = <-38.7565e-3, 217.9078e-3, 0.0252e-3>
        
        # Inertial matrix, chosen at this link's frame
        #I = <<138575.0448E-6,-16890.4832E-6,6.5293E-6>,<-16890.4832E-6,5749.2209E-6,-7.1669E-6>,<6.5293E-6,-7.1669E-6,141310.5180E-6>>
        
        # Inertial matrix, chosen at this link's center of mass
        I = <<56662.2970e-6,-2321.6892e-6,8.2125e-6>,<-2321.6892e-6,3158.0509e-6,-16.6307e-6>,<8.2125e-6,-16.6307e-6,56806.6024e-6>>
       
        # Rotor inertia <Lxx, Lyy, Lzz>
        rotorI = <0.0, 0.067, 0.0>
        
        vel = 0.5                       # Default velocity
        acc = 0.5                       # Default acceleration
        
        pid{ # PID control parameters
            kp = 600
            ki = 2.5
            kd = 10
            max = 8191 # abs(jointTorque) is bounded to this value
        }
    }
    link[]{
	     name = "Elbow + Wrist body"

        # Denavit-Hartenberg parameters (Spong)
        # Defines the transformation from the previous frame to this link's frame
        dh{
            theta = 0.0 # (multiplied by pi)
            d = 0.0
            a = -0.045
            alpha = 0.5 # (multiplied by pi)
        }
        
        # Mass of this link (kg)
        #mass = 2.0824 # Old wrist
        mass = 2.17266212 # New wrist
        
        # Center of mass (m), xyz translation from this link's frame
        #com = <6.2895e-3, -0.0010e-3, 111.0633e-3> # Old wrist
        com = <0.00553408,0.00006822,0.11927695>
        
        # Inertial matrix, chosen at this link's frame (old wrist)
        #I = <<35752.4193E-6,14.5876E-6,62.1491E-6>,<14.5876E-6,35769.9341E-6,29.2508E-6>,<62.1491E-6,29.2508E-6,2921.3314E-6>>
        
        # Inertial matrix, chosen at this link's center of mass (old wrist)
        #I = <<10065.3990e-6,14.6007e-6,-1392.4965e-6><14.6007e-6,10000.5377e-6,29.4814e-6><-1392.4965e-6,29.4814e-6,2838.9554e-6>>
        
        # Inertial matrix, chosen at this link's center of mass (new wrist)
        I = <<0.01067491,0.00004503,-0.00135557>,<0.00004503,0.01058659,-0.00011002>,<-0.00135557,-0.00011002,0.00282036>>
        
        # Rotor inertia <Lxx, Lyy, Lzz>
        rotorI = <0.0, 0.034, 0.0>
        
        vel = 0.5                       # Default velocity
        acc = 0.5                       # Default acceleration
        
        pid{ # PID control parameters
            kp = 500
            ki = 0.5
            kd = 2.5
            max = 8191 # abs(jointTorque) is bounded to this value
        }
    }
    link[]{
	     name = "Wrist Yaw"
        
        # Denavit-Hartenberg parameters (Spong)
        # Defines the transformation from the previous frame to this link's frame
        dh{
            theta = 0.0 # (multiplied by pi)
            d = 0.3
            a = 0.0
            alpha = -0.5 # (multiplied by pi)
        }
        
        # Mass of this link (kg)
        #mass = 0.3067 # Old wrist
        mass = 0.35655692 # New wrist
        
        # Center of mass (m), xyz translation from this link's frame
        #com = <0.0584e-3, 28.3754e-3, 0.1902e-3> # Old wrist
        com = <0.00005483,0.02886286,0.00148493> # New wrist
        
        # Inertial matrix, chosen at this link's frame (old wrist)
        #I = <<567.9724E-6,0.4416E-6,-0.0045E-6>,<0.4416E-6,172.3759E-6,-0.8172E-6>,<-0.0045E-6,-0.8172E-6,597.6265E-6>>
        
        # Inertial matrix, chosen at this link's center of mass (old wrist)
        #I = <<321.0141e-6,-0.0667e-6,-0.0079e-6>,<-0.0667e-6,172.3637e-6,-2.4724e-6>,<-0.0079e-6,-2.4724e-6,350.6782e-6>>
        
        #Inertial matrix, chosen at this link's center of mass (new wrist)
        I = <<0.00037112,-0.00000008,-0.00000003>,<-0.00000008,0.00019434,-0.00001613>,<-0.00000003,-0.00001613,0.00038209>>
        
        # Rotor inertia <Lxx, Lyy, Lzz>
        rotorI = <0.0, 0.0033224, 0.0>
        
        vel = 0.5                       # Default velocity
        acc = 0.5                       # Default acceleration
        
        pid{ # PID control parameters
            kp = 50
            kd = .5
            ki = .5
            max = 8191 # abs(jointTorque) is bounded to this value
        }
    }
    link[]{
	     name = "Wrist Pitch"
        
        # Denavit-Hartenberg parameters (Spong)
        # Defines the transformation from the previous frame to this link's frame
        dh{ 
            theta = 0.0 # (multiplied by pi)
            d = 0.0
            a = 0.0
            alpha = 0.5 # (multiplied by pi)
        }
        
        # Mass of this link (kg)
        #mass = 0.4278 # Old wrist
        mass = 0.40915886
        
        # Center of mass (m), xyz translation from this link's frame
        #com = <-0.0311e-3, -14.8635e-3, 25.6326e-3>  # Old wrist
        com = <-0.00005923,-0.01686123,0.02419052>
        
        # Inertial matrix, chosen at this link's frame
        #I = <<979.8079E-6,0.2804E-6,-0.5308E-6>,<0.2804E-6,550.3998E-6,-225.3322E-6>,<-0.5308E-6,-225.3322E-6,602.4224E-6>>
        
        # Inertial matrix, chosen at this link's center of mass (old wrist)
        #I = <<604.1921e-6,0.0825e-6,-0.1896e-6>,<0.0825e-6,269.3020e-6,-62.3326e-6>,<-0.1896e-6,-62.3326e-6,507.9036e-6>>

        # Inertial matrix, chosen at this link's center of mass (new wrist)
        I = <<0.00054889,0.00000019,-0.00000010>,<0.00000019,0.00023846,-0.00004430>,<-0.00000010,-0.00004430,0.00045133>>
        
        # Rotor inertia <Lxx, Lyy, Lzz>
        rotorI = <0.0, 0.0033224, 0.0>
        
        vel = 0.5                       # Default velocity
        acc = 0.5                       # Default acceleration
        
        pid{ # PID control parameters
            kp = 50
            kd = .5
            ki = .5
            max = 8191 # abs(jointTorque) is bounded to this value
        }
        
    }
    link[]{
	     name = "Wrist Roll"
        
        # Denavit-Hartenberg parameters (Spong)
        # Defines the transformation from the previous frame to this link's frame
        dh{ # Denavit-Hartenberg parameters (Spong)
            theta = 0.0 # (multiplied by pi)
            d = 0.0609
            a = 0.0
            alpha = 0.0 # (multiplied by pi)
        }
        
        # Mass of this link (kg)
        #mass = 0.0557 # Old wrist
        mass = 0.07548270
        
        # Center of mass (m), xyz translation from this link's frame
        #com = <-0.0001e-3, -0.1823e-3, -284.7431e-3> # Old wrist
        com = <0.00014836,0.00007252,-0.00335185>
        
        # Inertial matrix, chosen at this link's frame
        #I = <<4536.8120E-6,-0.0001E-6,0.0011E-6>,<-0.0001E-6,4537.0156E-6,2.8944E-6>,<0.0011E-6,2.8944E-6,42.1994E-6>>
        
        # Inertial matrix, chosen at this link's center of mass (old wrist)
        #I = <<21.4102e-6,-0.0001e-6,0.0000e-6>,<-0.0001e-6,21.6157e-6,0.0040e-6>,<0.0000e-6,0.0040e-6,42.1975e-6>>

        # Inertial matrix, chosen at this link's center of mass (new wrist)
        I = <<0.00003911,0.00000019,0.00000000>,<0.00000019,0.00003877,0.00000000>,<0.00000000,0.00000000,0.00007614>>
        
        # Rotor + idle gear inertia  <Lxx, Lyy, Lzz>
        rotorI = <0.0, 0.0, 0.000466939>
        
        vel = 0.5                       # Default velocity
        acc = 0.5                       # Default acceleration
        
        pid{ # PID control parameters
            kp = 10 #40
            kd = .1
            ki = .1
            max = 8191 # abs(jointTorque) is bounded to this value
        }
    }
    link[]{
        name = "Tool"
        
        # Denavit-Hartenberg parameters (Spong)
        # Defines the transformation from the previous frame to this link's frame
        dh{
            theta = 0.0 # (multiplied by pi)
            #d = 0.10 # BarrettHand
            #d = 0.095 # Haptic ball
            #d = 0.380 # Trocar tool
            d = 0.1 # Nothing attached (J7 plate only)
            a = 0.0
            alpha = 0.0 # (multiplied by pi)
        }
        
        # Mass of the tool (kg)
        #mass = 0.3460 # Haptic ball
        #mass = 1.16477 # Trocar tool?
        #mass = 1.2 # BarrettHand
        mass = 0.0
        
        # Center of mass (m), xyz translation from the tool frame
        #com = <0.0, 0.0, -0.270> # Trocar tool
        #com = <0.0, 0.0, -0.05> # Haptic Ball
        #com = <0.0, 0.0, -0.04> # BarrettHand
        com = <0.0, 0.0, 0.00>
        
        # Inertial matrix, chosen at this link's frame
        #I = <<0.0, 0.0, 0.0>,<0.0, 0.0, 0.0>,<0.0, 0.0, 0.0>>
        
        # Inertial matrix, chosen at this link's center of mass
        I = <<0.0, 0.0, 0.0>,<0.0, 0.0, 0.0>,<0.0, 0.0, 0.0>>
    }
}

