00001 #include <gtest/gtest.h> 00002 #include <ros/package.h> 00003 #include "robodyn_controllers/RosMsgGainCalculator.h" 00004 00005 class RosMsgGainCalculatorTest : public ::testing::Test 00006 { 00007 protected: 00008 virtual void SetUp() 00009 { 00010 dd.joint_names.push_back("joint1"); 00011 dd.joint_names.push_back("joint2"); 00012 00013 dd.naturalFreq.push_back(2.0); 00014 dd.naturalFreq.push_back(3.0); 00015 00016 dd.dampingRatio.push_back(1.0); 00017 dd.dampingRatio.push_back(1.5); 00018 00019 dd.windupLimit.push_back(10); 00020 dd.windupLimit.push_back(0); 00021 00022 yankLim = 1.0; 00023 yankLimWindup = 0.05; 00024 00025 std::string packagePath = ros::package::getPath("robodyn_controllers"); 00026 rmg.loadFromFile(packagePath + "/test/urdf/SimpleTestRobot.xml"); 00027 00028 } 00029 00030 virtual void TearDown() 00031 { 00032 } 00033 00034 RosMsgGainCalculator rmg; 00035 r2_msgs::Gains dd, dg; 00036 double scaleI, minD, yankLim, yankLimWindup; 00037 }; 00038 00039 TEST_F(RosMsgGainCalculatorTest, initializeTest) 00040 { 00041 rmg.initialize(); 00042 ASSERT_FALSE( rmg.jointGainsData.empty()); 00043 EXPECT_EQ (1, rmg.jointGainsData.count("joint1")); 00044 EXPECT_EQ (0, rmg.jointGainsData.count("link3")); 00045 } 00046 00047 TEST_F(RosMsgGainCalculatorTest, StoreGainInformationTest) 00048 { 00049 rmg.initialize(); 00050 00051 rmg.jointGainsData["joint1"].maxWn = 5000; 00052 rmg.jointGainsData["joint2"].maxWn = 5000; 00053 rmg.jointGainsData["joint1"].maxZeta = 20; 00054 rmg.jointGainsData["joint2"].maxZeta = 20; 00055 00056 rmg.storeGainInformation(dd); 00057 00058 EXPECT_EQ(2.0, rmg.jointGainsData["joint1"].naturalFreq); 00059 EXPECT_EQ(1.5, rmg.jointGainsData["joint2"].dampingRatio); 00060 EXPECT_EQ(10.0, rmg.jointGainsData["joint1"].windupLim); 00061 EXPECT_EQ(0.0, rmg.jointGainsData["joint2"].windupLim); 00062 } 00063 00064 TEST_F(RosMsgGainCalculatorTest, FindDesiredDynTest) 00065 { 00066 rmg.jointGainsData["joint1"].maxWn = 5000; 00067 rmg.jointGainsData["joint2"].maxWn = 5000; 00068 rmg.jointGainsData["joint1"].maxZeta = 20; 00069 rmg.jointGainsData["joint2"].maxZeta = 20; 00070 00071 sensor_msgs::JointState inertia; 00072 inertia.name.push_back("joint1"); 00073 inertia.position.push_back(-5.0); 00074 ASSERT_ANY_THROW(rmg.findDesiredDynamics(inertia, dg)); 00075 00076 inertia.position[0] = 5.0; 00077 00078 rmg.storeGainInformation(dd); 00079 rmg.yl.setRateLimit(yankLim); 00080 rmg.ylWindup.setRateLimit(yankLimWindup); 00081 EXPECT_EQ(0, rmg.jointGainsData["joint1"].stiffness); 00082 00083 rmg.jointGainsData["joint1"].maxK = 30; 00084 rmg.jointGainsData["joint2"].maxK = 30; 00085 rmg.jointGainsData["joint1"].maxD = 30; 00086 rmg.jointGainsData["joint2"].maxD = 30; 00087 rmg.jointGainsData["joint1"].scaleI = 0.01; 00088 rmg.jointGainsData["joint2"].scaleI = 0.01; 00089 rmg.jointGainsData["joint1"].minD = 5.0; 00090 rmg.jointGainsData["joint2"].minD = 5.0; 00091 00092 rmg.findDesiredDynamics(inertia, dg); 00093 EXPECT_EQ("joint1", dg.joint_names[0]); 00094 EXPECT_EQ(1, dg.joint_names.size()); 00095 EXPECT_EQ(1.25, dg.K[0]); 00096 EXPECT_EQ(5.0, dg.D[0]); 00097 EXPECT_EQ(0.0125, dg.I[0]); 00098 EXPECT_EQ(0.05, dg.windupLimit[0]); 00099 00100 yankLim = 20.0; 00101 rmg.yl.setRateLimit(yankLim); 00102 rmg.findDesiredDynamics(inertia, dg); 00103 EXPECT_EQ("joint1", dg.joint_names[0]); 00104 EXPECT_EQ(1, dg.joint_names.size()); 00105 EXPECT_EQ(20.0, dg.K[0]); 00106 EXPECT_EQ(20.0, dg.D[0]); 00107 EXPECT_EQ(0.2, dg.I[0]); 00108 EXPECT_EQ(0.1, dg.windupLimit[0]); 00109 00110 yankLimWindup = 2.0; 00111 rmg.ylWindup.setRateLimit(yankLimWindup); 00112 rmg.findDesiredDynamics(inertia, dg); 00113 EXPECT_EQ("joint1", dg.joint_names[0]); 00114 EXPECT_EQ(1, dg.joint_names.size()); 00115 EXPECT_EQ(20.0, dg.K[0]); 00116 EXPECT_EQ(20.0, dg.D[0]); 00117 EXPECT_EQ(0.2, dg.I[0]); 00118 EXPECT_EQ(2.1, dg.windupLimit[0]); 00119 00120 inertia.name.clear(); 00121 inertia.position.clear(); 00122 inertia.name.push_back("joint2"); 00123 inertia.name.push_back("joint1"); 00124 inertia.position.push_back(4.0); 00125 inertia.position.push_back(7.0); 00126 00127 rmg.findDesiredDynamics(inertia, dg); 00128 EXPECT_EQ(2, dg.joint_names.size()); 00129 EXPECT_EQ("joint2", dg.joint_names[0]); 00130 EXPECT_EQ(20.0, dg.K[0]); 00131 EXPECT_EQ(3.0 * sqrt(80.0), dg.D[0]); 00132 EXPECT_EQ(0.2, dg.I[0]); 00133 EXPECT_EQ(28.0, dg.K[1]); 00134 EXPECT_EQ(28.0, dg.D[1]); 00135 EXPECT_EQ(0.28, dg.I[1]); 00136 EXPECT_EQ(4.1, dg.windupLimit[1]); 00137 00138 00139 rmg.findDesiredDynamics(inertia, dg); 00140 EXPECT_EQ(25.0, dg.K[0]); 00141 EXPECT_EQ(30.0, dg.D[0]); 00142 EXPECT_EQ(0.25, dg.I[0]); 00143 EXPECT_EQ(6.1, dg.windupLimit[1]); 00144 EXPECT_EQ(0.0, dg.windupLimit[0]); 00145 } 00146 00147 int main(int argc, char** argv) 00148 { 00149 ::testing::InitGoogleTest(&argc, argv); 00150 return RUN_ALL_TESTS(); 00151 } 00152