r2_gazebo: mass_conversion.py Source File

Go to the documentation of this file.
00001 #!/usr/bin/env python
00002 
00003 # left arm
00004 # Mass (lbs)    x       y       z               Ixx     Ixy     Ixz     Iyy     Iyz     Izz
00005 #6.168856232    -0.00131572     0.10287         -0.0089916      0.0634150128    -0.0003950635           0.00012683              0.0198585269            -0.0060195977           0.0671900643
00006 #4.4270615312   -0.00028448     0.0077978       0.037084        0.0251962741    -5.2382497887E-005      2.17138622526E-005      0.02701064              0.001512947             0.0113836825
00007 #4.853438359    -0.041402       0.1016          -0.0074168      0.04214011      0.0065843922            0.0013754064            0.016007389             -0.00339462             0.0462370652
00008 #2.9710300235   0.033782        0.0154432       0.026162        0.0105350275    -0.001188117            0.0031605083            0.0120274897            0.0016417085            0.0088377175
00009 #6.939963261    -0.00122682     0.17653         0.0003556       0.0403842721    0.0005208986            -0.0001691457           0.0125249771            -0.0002446467           0.0395063532
00010 #0.1165732391   -0.004064       1.52908E-007    -0.00092202     0.00005765      4.3603308297E-011       1.75876431453E-006      1.69438359087E-005      5.4430975458E-008       4.4188587603E-005
00011 #1.1884120094   0.067564        -0.0253746      -0.00090932     0.001992876     0.000101546             .0001024239             0.0030141884            0.0003716524            0.0044481227
00012 
00013 # right arm
00014 #6.168856232    -0.00131572     -0.10287        0.0089916       0.0634150128    0.0003950635    -0.00012683     0.0198585269    -0.0060195977   0.0671900643
00015 #4.4270615312   -0.00028448     0.0077978       0.037084        0.0251962741    -5.2382497887E-005      2.17138622526E-005      0.02701064      0.001512947     0.0113836825
00016 #4.853438359    -0.041402       -0.1016 0.0074168       0.04214011      -0.0065843922   -0.0013754064   0.016007389     -0.00339462     0.0462370652
00017 #2.9710300235   0.033782        0.0154432       0.026162        0.0105350275    -0.001188117    0.0031605083    0.0120274897    0.0016417085    0.0088377175
00018 #6.939963261    -0.00122682     -0.17653        -0.0003556      0.0403842721    -0.0005208986   0.0001691457    0.0125249771    -0.0002446467   0.0395063532
00019 #0.1165732391   -0.004064       1.52908E-007    -0.00092202     0.00005765      4.3603308297E-011       1.75876431453E-006      1.69438359087E-005      5.4430975458E-008       4.4188587603E-005
00020 #1.1884120094   0.067564        0.0253746       0.00090932      0.001992876     -0.000101546    -0.0001024239   0.0030141884    0.0003716524    0.0044481227
00021 
00022 
00023 left_params = [[6.168856232,    -0.00131572,    0.10287,        -0.0089916,     0.0634150128,           -0.0003950635,          0.00012683,             0.0198585269,   -0.0060195977,  0.0671900643],
00024 [4.4270615312,  -0.00028448,    0.0077978,      0.037084,       0.0251962741,   -5.2382497887E-005,     2.17138622526E-005,     0.02701064,             0.001512947,            0.0113836825],
00025 [4.853438359,   -0.041402,      0.1016  ,       -0.0074168,     0.04214011,     0.0065843922,           0.0013754064,           0.016007389,            -0.00339462,            0.0462370652],
00026 [2.9710300235,  0.033782,       0.0154432,      0.026162,       0.0105350275,   -0.001188117,           0.0031605083,           0.0120274897,           0.0016417085,           0.0088377175],
00027 [6.939963261,   -0.00122682,    0.17653 ,       0.0003556,      0.0403842721,   0.0005208986,           -0.0001691457,          0.0125249771,           -0.0002446467,          0.0395063532],
00028 [0.1165732391,  -0.004064,      1.52908E-007,   -0.00092202,    0.00005765,     4.3603308297E-011,      1.75876431453E-006,     1.69438359087E-005,     5.4430975458E-008,      4.4188587603E-005],
00029 [1.1884120094,  0.067564,       -0.0253746,     -0.00090932,    0.001992876,    0.000101546,            .0001024239,            0.0030141884,           0.0003716524,           0.0044481227]]
00030 
00031 right_params = [[6.168856232,   -0.00131572,    -0.10287,       0.0089916,      0.0634150128,           0.0003950635,           -0.00012683,            0.0198585269,   -0.0060195977,  0.0671900643],
00032 [4.4270615312,  -0.00028448,    0.0077978,      0.037084,       0.0251962741,   -5.2382497887E-005,     2.17138622526E-005,     0.02701064,             0.001512947,            0.0113836825],
00033 [4.853438359,   -0.041402,      -0.1016,        0.0074168,      0.04214011,     -0.0065843922,          -0.0013754064,          0.016007389,            -0.00339462,            0.0462370652],
00034 [2.9710300235,  0.033782,       0.0154432,      0.026162,       0.0105350275,   -0.001188117,           0.0031605083,           0.0120274897,           0.0016417085,           0.0088377175],
00035 [6.939963261,   -0.00122682,    -0.17653,       -0.0003556,     0.0403842721,   -0.0005208986,          0.0001691457,           0.0125249771,           -0.0002446467,          0.0395063532],
00036 [0.1165732391,  -0.004064,      1.52908E-007,   -0.00092202,    0.00005765,     4.3603308297E-011,      1.75876431453E-006,     1.69438359087E-005,     5.4430975458E-008,      4.4188587603E-005],
00037 [1.1884120094,  0.067564,       0.0253746,      0.00090932,     0.001992876,    -0.000101546,           -0.0001024239,          0.0030141884,           0.0003716524,           0.0044481227]]
00038 
00039 
00040 ML = [0]*7
00041 PL = [[0, 0, 0]]*7
00042 IL = [[0, 0, 0, 0, 0, 0]]*7
00043 
00044 MR = [0]*7
00045 PR = [[0, 0, 0]]*7
00046 IR = [[0, 0, 0, 0, 0, 0]]*7
00047 
00048 for i in range(7) :
00049         ML[i] = left_params[i][0]
00050         MR[i] = right_params[i][0]
00051         PL[i] = [left_params[i][1], left_params[i][2], left_params[i][3]]
00052         PR[i] = [right_params[i][1], right_params[i][2], right_params[i][3]]
00053         IL[i] = [left_params[i][4], left_params[i][5], left_params[i][6], left_params[i][7], left_params[i][8], left_params[i][9]]
00054         IR[i] = [right_params[i][4], right_params[i][5], right_params[i][6], right_params[i][7], right_params[i][8], right_params[i][9]]
00055         
00056 
00057 #print "Left Mass (kg): ", ML
00058 #print "Left CoM (m): ", PL
00059 #print "Left I (kg*m^2): ", IL
00060 
00061 #print "Right Mass (kg): ", MR
00062 #print "Right CoM (m): ", PR
00063 #print "Right I (kg*m^2): ", IR
00064 
00065 for i in range(7) :
00066         print "ML[",i,"]", ML[i]
00067 print ""
00068 for i in range(7) :
00069         print "MR[",i,"]", MR[i]
00070 print ""
00071 for i in range(7) :
00072         print "PL[",i,"]", PL[i]
00073 print ""
00074 for i in range(7) :
00075         print "PR[",i,"]", PR[i]
00076 print ""
00077 for i in range(7) :
00078         print "IL[",i,"]", IL[i]
00079 print ""
00080 for i in range(7) :
00081         print "IR[",i,"]", IR[i]
00082 print " "
00083 
00084 for i in range(7) :
00085         print "Left[",i,"]"
00086         print "    <inertial>"
00087         print "        <origin xyz=\"", PL[i][0], " ", PL[i][1], " ", PL[i][2], "\" rpy=\"0 0 0\"/>"
00088         print "        <mass value=\"", ML[i], "\"/>"
00089         print "        <inertia ixx=\"", IL[i][0], "\"  ixy=\"", IL[i][1], "\"  ixz=\"", IL[i][2], "\"  iyy=\"", IL[i][3], "\"  iyz=\"", IL[i][4], "\"  izz=\"", IL[i][5], "\"/>"
00090         print "    </inertial>"
00091         print " "
00092 
00093 
00094 for i in range(7) :
00095         print "Right[",i,"]"
00096         print "    <inertial>"
00097         print "        <origin xyz=\"", PR[i][0], " ", PR[i][1], " ", PR[i][2], "\" rpy=\"0 0 0\"/>"
00098         print "        <mass value=\"", MR[i], "\"/>"
00099         print "        <inertia ixx=\"", IR[i][0], "\"  ixy=\"", IR[i][1], "\"  ixz=\"", IR[i][2], "\"  iyy=\"", IR[i][3], "\"  iyz=\"", IR[i][4], "\"  izz=\"", IR[i][5], "\"/>"
00100         print "    </inertial>"
00101         print " "
00102 
00103 
00104 
00105 
00106 
00107