00001 #include <gtest/gtest.h>
00002 #include "robodyn_mechanisms/FingerController.h"
00003
00004 class FingerControllerTest : public ::testing::Test
00005 {
00006 protected:
00007 virtual void SetUp()
00008 {
00009 }
00010
00011 virtual void TearDown()
00012 {
00013 }
00014
00015 FingerController<3> finger;
00016 FingerKinematics<3> fingerKin;
00017 };
00018
00019 TEST_F(FingerControllerTest, Controller)
00020 {
00021 double DEG2RAD = 3.14159 / 180.;
00022
00023 FingerController<3>::ReferenceMatrixType refMat;
00024 refMat(0, 0) = -3.78381327904;
00025 refMat(0, 1) = -3.78381327904;
00026 refMat(0, 2) = 3.78381327904;
00027 refMat(0, 3) = 3.78381327904;
00028 refMat(1, 0) = -4.94806351875;
00029 refMat(1, 1) = 6.7308216983;
00030 refMat(1, 2) = -4.94806351875;
00031 refMat(1, 3) = 6.7308216983;
00032 refMat(2, 0) = 0;
00033 refMat(2, 1) = 0;
00034 refMat(2, 2) = -4.94806351875;
00035 refMat(2, 3) = 4.94806351875;
00036 finger.setReferenceMatrix(refMat);
00037
00038 double mmPerCount = 0.0174427;
00039 finger.setMillimetersPerCount(mmPerCount);
00040 double hallScale = 0.000076295;
00041 FingerController<3>::JointVectorType hallCoeffs0, hallCoeffs1, hallCoeffs2, hallCoeffs3;
00042 hallCoeffs0[0] = 1.3025;
00043 hallCoeffs0[1] = 3.4872;
00044 hallCoeffs0[2] = 12.4459;
00045 hallCoeffs1[0] = -0.6444;
00046 hallCoeffs1[1] = -1.7247;
00047 hallCoeffs1[2] = -10.2433;
00048 hallCoeffs2[0] = 0.0528;
00049 hallCoeffs2[1] = 0.3821;
00050 hallCoeffs2[2] = 3.3336;
00051 hallCoeffs3[0] = -0.0039;
00052 hallCoeffs3[1] = -0.0463;
00053 hallCoeffs3[2] = -0.4100;
00054 finger.setHallAngleParameters(hallScale, hallCoeffs0, hallCoeffs1, hallCoeffs2, hallCoeffs3);
00055
00056 FingerController<3>::SliderVectorType aGains, bGains, tensionOffsets, calstrain;
00057 aGains[0] = 0.01;
00058 aGains[1] = -0.01;
00059 aGains[2] = 0.01;
00060 aGains[3] = 0.01;
00061 bGains[0] = -0.01;
00062 bGains[1] = 0.01;
00063 bGains[2] = -0.01;
00064 bGains[3] = -0.01;
00065 tensionOffsets[0] = 0.969;
00066 tensionOffsets[1] = -1.132;
00067 tensionOffsets[2] = 3.743;
00068 tensionOffsets[3] = -4.896;
00069 calstrain[0] = 1.;
00070 calstrain[1] = 1.;
00071 calstrain[2] = 1.;
00072 calstrain[3] = 1.;
00073 finger.setTensionParameters(aGains, bGains, tensionOffsets, calstrain);
00074
00075 FingerController<3>::SliderVectorType encoders, sliders, sliderPos, tensions;
00076 encoders = FingerController<3>::SliderVectorType::Zero();
00077 sliders[0] = -112;
00078 sliders[1] = 65;
00079 sliders[2] = -213;
00080 sliders[3] = 10;
00081 sliderPos[0] = -31.73 * mmPerCount;
00082 sliderPos[1] = 111.5 * mmPerCount;
00083 sliderPos[2] = -74.37 * mmPerCount;
00084 sliderPos[3] = 38.32 * mmPerCount;
00085 tensions[0] = 0.09;
00086 tensions[1] = 2.837;
00087 tensions[2] = 7.84;
00088 tensions[3] = -1.265;
00089 finger.setTubeTareParameters(sliders, sliderPos, tensions);
00090
00091 MultiLoopController mlc;
00092 mlc.setLoopRate(350);
00093 mlc.setPositionLoopParameters(0, 0, 0);
00094 mlc.setVelocityLoopParameters(0, 0, 0, 0, 0);
00095 mlc.setCurrentLoopParameters(-.25, .25, 200);
00096 mlc.setHardwareParameters(0, 0, .00742, 15.1, .007419811, 0, 1, 50000, .000002, .0000002);
00097 finger.setMultiLoopController(mlc, 0);
00098 finger.setMultiLoopController(mlc, 1);
00099 finger.setMultiLoopController(mlc, 2);
00100 finger.setMultiLoopController(mlc, 3);
00101 finger.setBusVoltage(48);
00102
00103 encoders[0] = -42;
00104 encoders[1] = -42;
00105 encoders[2] = -37;
00106 encoders[3] = -36;
00107 finger.getSlidersFromEncoders(encoders, sliders);
00108 EXPECT_NEAR(38.27 * mmPerCount, sliders[0], 0.1);
00109 EXPECT_NEAR(4.514 * mmPerCount, sliders[1], 0.1);
00110 EXPECT_NEAR(101.6 * mmPerCount, sliders[2], 0.1);
00111 EXPECT_NEAR(-7.68 * mmPerCount, sliders[3], 0.1);
00112
00113 FingerController<3>::JointVectorType joints;
00114 finger.getJointsFromSliders(sliders, joints);
00115 EXPECT_NEAR(3.378 * DEG2RAD, joints[0], 0.01);
00116 EXPECT_NEAR(-1.054 * DEG2RAD, joints[1], 0.01);
00117 EXPECT_NEAR(-9.795 * DEG2RAD, joints[2], 0.01);
00118
00119 FingerController<3>::JointVectorType halls;
00120 halls[0] = 33220;
00121 halls[1] = 47300;
00122 halls[2] = 44530;
00123 finger.getHallAngles(halls, joints);
00124 EXPECT_NEAR(-0.0538, joints[0], 0.017);
00125 EXPECT_NEAR(0.0602, joints[1], 0.017);
00126 EXPECT_NEAR(0.0591, joints[2], 0.017);
00127
00128 joints[0] = 0 * DEG2RAD;
00129 joints[1] = 5 * DEG2RAD;
00130 joints[2] = 2 * DEG2RAD;
00131 finger.getSlidersFromJoints(joints, sliders);
00132 EXPECT_NEAR(-0.017 * 25.4, sliders[0], 0.001);
00133 EXPECT_NEAR(0.02313 * 25.4, sliders[1], 0.001);
00134 EXPECT_NEAR(-0.0238 * 25.4, sliders[2], 0.001);
00135 EXPECT_NEAR(0.02992 * 25.4, sliders[3], 0.001);
00136
00137 FingerController<3>::SliderVectorType tensionA, tensionB, tensionOut;
00138 tensionA[0] = 32670;
00139 tensionA[1] = 32640;
00140 tensionA[2] = 33030;
00141 tensionA[3] = 32850;
00142 tensionB[0] = 32690;
00143 tensionB[1] = 33150;
00144 tensionB[2] = 32590;
00145 tensionB[3] = 32150;
00146 finger.getCalibratedTensions(tensionA, tensionB, 0.0, tensionOut);
00147 EXPECT_NEAR(0.719, tensionOut[0], 0.1);
00148 EXPECT_NEAR(1.061, tensionOut[1], 0.1);
00149 EXPECT_NEAR(0.243, tensionOut[2], 0.1);
00150 EXPECT_NEAR(3.289, tensionOut[3], 0.1);
00151 }
00152
00153 TEST_F(FingerControllerTest, RangeSpace)
00154 {
00155
00156 FingerController<3>::ReferenceMatrixType refMat;
00157 refMat(0, 0) = -3.78381327904;
00158 refMat(0, 1) = -3.78381327904;
00159 refMat(0, 2) = 3.78381327904;
00160 refMat(0, 3) = 3.78381327904;
00161 refMat(1, 0) = -4.94806351875;
00162 refMat(1, 1) = 6.7308216983;
00163 refMat(1, 2) = -4.94806351875;
00164 refMat(1, 3) = 6.7308216983;
00165 refMat(2, 0) = 0;
00166 refMat(2, 1) = 0;
00167 refMat(2, 2) = -4.94806351875;
00168 refMat(2, 3) = 4.94806351875;
00169 fingerKin.setReferenceMatrix(refMat);
00170
00171 FingerController<3>::SliderVectorType sliders;
00172 sliders[0] = -0.01837;
00173 sliders[1] = -0.02344;
00174 sliders[2] = -0.0483;
00175 sliders[3] = -0.006295;
00176 fingerKin.projectToRangeSpace(sliders, sliders);
00177 EXPECT_NEAR(0.0112, sliders[0], 0.015);
00178 EXPECT_NEAR(-0.01411, sliders[1], 0.015);
00179 EXPECT_NEAR(-0.03641, sliders[2], 0.015);
00180 EXPECT_NEAR(0.002753, sliders[3], 0.015);
00181
00182 sliders[0] = -0.3396;
00183 sliders[1] = 0.4174;
00184 sliders[2] = -0.7117;
00185 sliders[3] = 0.7709;
00186 fingerKin.projectToRangeSpace(sliders, sliders);
00187 EXPECT_NEAR(-0.3103, sliders[0], 0.015);
00188 EXPECT_NEAR(0.4267, sliders[1], 0.015);
00189 EXPECT_NEAR(-0.6999, sliders[2], 0.015);
00190 EXPECT_NEAR(0.7799, sliders[3], 0.015);
00191 }
00192
00193 TEST_F(FingerControllerTest, CheckHWNumbers)
00194 {
00195
00196 FingerController<3>::ReferenceMatrixType refMat;
00197 refMat(0, 0) = 3.78381327904;
00198 refMat(0, 1) = 3.78381327904;
00199 refMat(0, 2) = -3.78381327904;
00200 refMat(0, 3) = -3.78381327904;
00201 refMat(1, 0) = 4.94806351875;
00202 refMat(1, 1) = -6.7308216983;
00203 refMat(1, 2) = 4.94806351875;
00204 refMat(1, 3) = -6.7308216983;
00205 refMat(2, 0) = 0;
00206 refMat(2, 1) = 0;
00207 refMat(2, 2) = 4.94806351875;
00208 refMat(2, 3) = -4.94806351875;
00209 finger.setReferenceMatrix(refMat);
00210
00211 double mmPerCount = 0.0174427;
00212 finger.setMillimetersPerCount(mmPerCount);
00213 FingerController<3>::SliderVectorType encoders, sliders, sliderPos, tensions;
00214 encoders = FingerController<3>::SliderVectorType::Zero();
00215 sliders[0] = -78;
00216 sliders[1] = -4;
00217 sliders[2] = -52;
00218 sliders[3] = -6;
00219 sliderPos[0] = 5.1545;
00220 sliderPos[1] = -10.6512;
00221 sliderPos[2] = 8.861;
00222 sliderPos[3] = -8.1897;
00223 finger.setTubeTareParameters(sliders, sliderPos, tensions);
00224
00225 encoders[0] = -78;
00226 encoders[1] = -4;
00227 encoders[2] = -52;
00228 encoders[3] = -6;
00229 finger.getSlidersFromEncoders(encoders, sliders);
00230 EXPECT_NEAR(5.1545, sliders[0], 0.1);
00231 EXPECT_NEAR(-10.6512, sliders[1], 0.1);
00232 EXPECT_NEAR(8.861, sliders[2], 0.1);
00233 EXPECT_NEAR(-8.1897, sliders[3], 0.1);
00234
00235 FingerController<3>::JointVectorType joints;
00236 finger.getJointsFromSliders(sliders, joints);
00237 EXPECT_NEAR(-0.411, joints[0], 0.02);
00238 EXPECT_NEAR(1.3423, joints[1], 0.02);
00239 EXPECT_NEAR(0.141, joints[2], 0.02);
00240
00241 joints[0] = -0.411;
00242 joints[1] = 1.3423;
00243 joints[2] = 0.141;
00244 finger.getSlidersFromJoints(joints, sliders);
00245 EXPECT_NEAR(5.1545, sliders[0], 0.1);
00246 EXPECT_NEAR(-10.6512, sliders[1], 0.1);
00247 EXPECT_NEAR(8.861, sliders[2], 0.1);
00248 EXPECT_NEAR(-8.1897, sliders[3], 0.1);
00249
00250 joints[0] = -0.411;
00251 joints[1] = .5;
00252 joints[2] = 0.141;
00253 finger.getSlidersFromJoints(joints, sliders);
00254 EXPECT_GE(5.1545, sliders[0]);
00255 EXPECT_LE(-10.6512, sliders[1]);
00256 EXPECT_GE(8.861, sliders[2]);
00257 EXPECT_LE(-8.1897, sliders[3]);
00258 }
00259
00260 int main(int argc, char** argv)
00261 {
00262 testing::InitGoogleTest(&argc, argv);
00263 return RUN_ALL_TESTS();
00264 }