00001 #include <gtest/gtest.h> 00002 #include "robodyn_controllers/KdlTreeId.h" 00003 #include "robodyn_controllers/JointDynamicsData.h" 00004 #include <ros/package.h> 00005 00006 class KdlTreeIdTest : public ::testing::Test 00007 { 00008 protected: 00009 virtual void SetUp() 00010 { 00011 } 00012 00013 virtual void TearDown() 00014 { 00015 } 00016 00017 KdlTreeId id; 00018 KDL::Tree tree; 00019 JointDynamicsData jd; 00020 }; 00021 00022 TEST_F(KdlTreeIdTest, FindChainFromNodeTest) 00023 { 00024 std::string packagePath = ros::package::getPath("robodyn_controllers"); 00025 id.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml"); 00026 tree = id.getTree(); 00027 jd.InitializeMaps(tree); 00028 00029 std::string toolFrame; 00030 00031 id.findChainFromNode("linkb", "child", toolFrame); 00032 EXPECT_EQ("link1", toolFrame); 00033 00034 id.findChainFromNode("link1", "parent", toolFrame); 00035 EXPECT_EQ("link1", toolFrame); 00036 00037 id.findChainFromNode("link2", "child", toolFrame); 00038 EXPECT_EQ("link2", toolFrame); 00039 00040 id.findChainFromNode("link3", "child", toolFrame); 00041 EXPECT_EQ("link4", toolFrame); 00042 00043 id.findChainFromNode("link4", "parent", toolFrame); 00044 EXPECT_EQ("link1", toolFrame); 00045 00046 id.findChainFromNode("link0", "parent", toolFrame); 00047 EXPECT_EQ("linkb", toolFrame); 00048 00049 } 00050 00051 00052 TEST_F(KdlTreeIdTest, FindBranchNodesTest) 00053 { 00054 std::string packagePath = ros::package::getPath("robodyn_controllers"); 00055 id.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml"); 00056 tree = id.getTree(); 00057 jd.InitializeMaps(tree); 00058 00059 std::vector<std::string> nL, dir; 00060 00061 id.findBranchNodes("linkb", "null", nL, dir); 00062 EXPECT_EQ(1, nL.size()); 00063 EXPECT_EQ("link0", nL[0]); 00064 EXPECT_EQ("child", dir[0]); 00065 00066 nL.clear(); 00067 dir.clear(); 00068 id.findBranchNodes("link1", "link2", nL, dir); 00069 EXPECT_EQ(2, nL.size()); 00070 EXPECT_EQ("link0", nL[0]); 00071 EXPECT_EQ("parent", dir[0]); 00072 EXPECT_EQ("link3", nL[1]); 00073 EXPECT_EQ("child", dir[1]); 00074 00075 nL.clear(); 00076 dir.clear(); 00077 id.findBranchNodes("link2", "link1", nL, dir); 00078 EXPECT_EQ(0, nL.size()); 00079 00080 nL.clear(); 00081 dir.clear(); 00082 id.findBranchNodes("link3", "link1", nL, dir); 00083 EXPECT_EQ(1, nL.size()); 00084 EXPECT_EQ("link4", nL[0]); 00085 EXPECT_EQ("child", dir[0]); 00086 00087 nL.clear(); 00088 dir.clear(); 00089 id.findBranchNodes("link3", "null", nL, dir); 00090 EXPECT_EQ(2, nL.size()); 00091 EXPECT_EQ("link1", nL[0]); 00092 EXPECT_EQ("parent", dir[0]); 00093 } 00094 00095 00096 TEST_F(KdlTreeIdTest, SumForcesTest) 00097 { 00098 std::string packagePath = ros::package::getPath("robodyn_controllers"); 00099 id.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml"); 00100 tree = id.getTree(); 00101 jd.InitializeMaps(tree); 00102 00103 KDL::Wrenches forceIn(2); 00104 forceIn[0] = KDL::Wrench(KDL::Vector(1, 0, 0), KDL::Vector::Zero()); 00105 forceIn[1] = KDL::Wrench(KDL::Vector(2, 1, 0), KDL::Vector(0, 0, 3)); 00106 00107 KDL::Wrench forceOut = id.sumForces(forceIn); 00108 KDL::Wrench fx(KDL::Vector(3, 1, 0), KDL::Vector(0, 0, 3)); 00109 EXPECT_EQ(fx, forceOut); 00110 00111 } 00112 00113 00114 TEST_F(KdlTreeIdTest, TreeRecursiveNewtonEulerTest) 00115 { 00116 std::string packagePath = ros::package::getPath("robodyn_controllers"); 00117 id.loadFromFile(packagePath + "/test/urdf/BranchingTestRobot.xml"); 00118 tree = id.getTree(); 00119 jd.InitializeMaps(tree); 00120 00121 KDL::Twist velIn, accelIn; 00122 KDL::Wrench forceOut; 00123 KDL::RigidBodyInertia inertiaOut; 00124 00125 id.treeRecursiveNewtonEuler(jd, "linkb", "null", velIn, accelIn, forceOut, inertiaOut); 00126 EXPECT_EQ(KDL::Wrench::Zero(), forceOut); 00127 EXPECT_NE(0.0, inertiaOut.getMass()); 00128 00129 id.treeRecursiveNewtonEuler(jd, "link1", "null", velIn, accelIn, forceOut, inertiaOut); 00130 EXPECT_EQ(KDL::Wrench::Zero(), forceOut); 00131 EXPECT_NE(0.0, inertiaOut.getMass()); 00132 00133 } 00134 00135 TEST_F(KdlTreeIdTest, R2LegsTest) 00136 { 00137 std::string packagePath = ros::package::getPath("robodyn_controllers"); 00138 id.loadFromFile(packagePath + "/test/urdf/r2c_legs_control.urdf.xml"); 00139 tree = id.getTree(); 00140 jd.InitializeMaps(tree); 00141 00142 KDL::Twist velIn, accelIn; 00143 KDL::Wrench forceOut; 00144 KDL::RigidBodyInertia inertiaOut; 00145 00146 JntDynData data; 00147 data.pos = -70.0 * 3.14159 / 180.0; 00148 data.vel = 0.0; 00149 data.acc = 0.0; 00150 jd.jointDataMap["r2/left_leg/joint0"] = data; 00151 data.pos = -data.pos; 00152 jd.jointDataMap["r2/right_leg/joint0"] = data; 00153 data.pos = -60.0 * 3.14159 / 180.0; 00154 jd.jointDataMap["r2/left_leg/joint1"] = data; 00155 jd.jointDataMap["r2/right_leg/joint1"] = data; 00156 00157 00158 id.getAccelInBaseFrame(KDL::Vector(0, 0, -9.8), "r2/robot_world", jd, "r2/robot_world", accelIn); 00159 00160 id.treeRecursiveNewtonEuler(jd, "r2/robot_world", "null", velIn, accelIn, forceOut, inertiaOut); 00161 00162 std::map<std::string, double>::const_iterator it = jd.jointTorqueCommandMap.begin(); 00163 std::cout << "Torque values: " << std::endl; 00164 00165 while (it != jd.jointTorqueCommandMap.end()) 00166 { 00167 std::cout << it->first << ": " << it->second << std::endl; 00168 it++; 00169 } 00170 00171 std::map<std::string, double>::const_iterator it2 = jd.jointInertiaMap.begin(); 00172 std::cout << "Inertia values: " << std::endl; 00173 00174 while (it2 != jd.jointInertiaMap.end()) 00175 { 00176 std::cout << it2->first << ": " << it2->second << std::endl; 00177 it2++; 00178 } 00179 00180 } 00181 00182 TEST_F(KdlTreeIdTest, R2LegswUpperTest) 00183 { 00184 std::string packagePath = ros::package::getPath("robodyn_controllers"); 00185 id.loadFromFile(packagePath + "/test/urdf/r2c_legs_control_wUpperMass.urdf.xml"); 00186 tree = id.getTree(); 00187 jd.InitializeMaps(tree); 00188 00189 KDL::Twist velIn, accelIn; 00190 KDL::Wrench forceOut; 00191 KDL::RigidBodyInertia inertiaOut; 00192 00193 JntDynData data; 00194 data.pos = 0.0 * 3.14159 / 180.0; 00195 data.vel = 0.0; 00196 data.acc = 0.0; 00197 jd.jointDataMap["r2/left_leg/joint0"] = data; 00198 data.pos = -data.pos; 00199 jd.jointDataMap["r2/right_leg/joint0"] = data; 00200 data.pos = 0.0 * 3.14159 / 180.0; 00201 jd.jointDataMap["r2/left_leg/joint1"] = data; 00202 jd.jointDataMap["r2/right_leg/joint1"] = data; 00203 00204 00205 id.getAccelInBaseFrame(KDL::Vector(0, 0, -9.8), "r2/robot_world", jd, "r2/robot_world", accelIn); 00206 EXPECT_EQ(KDL::Twist(KDL::Vector(0, 0, -9.8), KDL::Vector(0, 0, 0)), accelIn); 00207 00208 id.treeRecursiveNewtonEuler(jd, "r2/robot_world", "null", velIn, accelIn, forceOut, inertiaOut); 00209 00210 std::map<std::string, double>::const_iterator it = jd.jointTorqueCommandMap.begin(); 00211 std::cout << "Torque values: " << std::endl; 00212 00213 while (it != jd.jointTorqueCommandMap.end()) 00214 { 00215 std::cout << it->first << ": " << it->second << std::endl; 00216 it++; 00217 } 00218 00219 std::map<std::string, double>::const_iterator it2 = jd.jointInertiaMap.begin(); 00220 std::cout << "Inertia values: " << std::endl; 00221 00222 while (it2 != jd.jointInertiaMap.end()) 00223 { 00224 std::cout << it2->first << ": " << it2->second << std::endl; 00225 it2++; 00226 } 00227 00228 } 00229 00230 int main(int argc, char** argv) 00231 { 00232 ::testing::InitGoogleTest(&argc, argv); 00233 return RUN_ALL_TESTS(); 00234 }