00001 #include <gtest/gtest.h> 00002 #include "robodyn_controllers/Cartesian_HybCntrl.h" 00003 00004 class Cartesian_HybCntrlTest : public ::testing::Test, public Cartesian_HybCntrl 00005 { 00006 protected: 00007 virtual void SetUp() 00008 { 00009 } 00010 00011 virtual void TearDown() 00012 { 00013 } 00014 }; 00015 00016 00017 TEST_F(Cartesian_HybCntrlTest, setGainsTest) 00018 { 00019 setGain(1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0, 8.0, 0.02, 0.02); 00020 00021 EXPECT_EQ(fKp, 1.0); 00022 EXPECT_EQ(tKp, 2.0); 00023 EXPECT_EQ(fKi, 3.0); 00024 EXPECT_EQ(tKi, 4.0); 00025 EXPECT_EQ(fKd, 5.0); 00026 EXPECT_EQ(tKd, 6.0); 00027 EXPECT_EQ(forceIntegralWindupLimit, 7.0); 00028 EXPECT_EQ(torqueIntegralWindupLimit, 8.0); 00029 00030 } 00031 00032 TEST_F(Cartesian_HybCntrlTest, poseSettingsTest) 00033 { 00035 setPoseSettings(1.0, 2.0, 3.0, 4.0); 00036 00037 EXPECT_EQ(maxLinearVelocity, 1.0); 00038 EXPECT_EQ(maxAngularVelocity, 2.0); 00039 00041 KDL::Twist v1 = KDL::Twist::Zero(); 00042 KDL::Twist v2 = KDL::Twist(KDL::Vector(1, 1, 1), KDL::Vector(1, 1, 1)); 00043 00045 limitVelocity(v1, v2); 00046 EXPECT_EQ(KDL::Twist(KDL::Vector(1, 1, 1), KDL::Vector(1, 1, 1)), v2); 00047 00048 KDL::Twist v3 = KDL::Twist(KDL::Vector(2, 2, 2), KDL::Vector(4, 4, 4)); 00049 limitVelocity(v2, v3); 00051 EXPECT_EQ(KDL::Vector(1, 1, 1), v3.vel); 00053 EXPECT_EQ(KDL::Vector(2, 2, 2), v3.rot); 00054 00055 KDL::Twist v4 = KDL::Twist(KDL::Vector(4, 4, 4), KDL::Vector(4, 4, 4)); 00056 setPoseSettings(5.0, 5.0, 1.0, 1.0); 00057 limitVelocity(v3, v4); 00059 EXPECT_EQ(KDL::Vector(2, 2, 2), v4.vel); 00061 EXPECT_EQ(KDL::Vector(3, 3, 3), v4.rot); 00062 00063 } 00064 00065 TEST_F(Cartesian_HybCntrlTest, forceControlMapTest) 00066 { 00067 std::map<std::string, std::pair<std::vector<int>, std::vector<double> > > forceControlInput; 00068 createForceControlMap(forceControlInput); 00069 EXPECT_TRUE(forceCntrlMap.empty()); 00070 00071 std::vector<int> axes; 00072 std::vector<double> mag; 00073 forceControlInput["testNode"] = std::make_pair(axes, mag); 00074 00075 createForceControlMap(forceControlInput); 00076 EXPECT_EQ(1, forceCntrlMap.size()); 00077 EXPECT_FALSE(forceCntrlMap["testNode"].x); 00078 EXPECT_FALSE(forceCntrlMap["testNode"].y); 00079 EXPECT_FALSE(forceCntrlMap["testNode"].z); 00080 EXPECT_FALSE(forceCntrlMap["testNode"].roll); 00081 EXPECT_FALSE(forceCntrlMap["testNode"].pitch); 00082 EXPECT_FALSE(forceCntrlMap["testNode"].yaw); 00083 00084 axes.push_back(2); 00085 forceControlInput["testNode"] = std::make_pair(axes, mag); 00086 createForceControlMap(forceControlInput); 00087 EXPECT_TRUE(forceCntrlMap.empty()); 00088 00089 mag.push_back(1); 00090 forceControlInput["testNode"] = std::make_pair(axes, mag); 00091 createForceControlMap(forceControlInput); 00092 00093 EXPECT_EQ(1, forceCntrlMap.size()); 00094 EXPECT_FALSE(forceCntrlMap["testNode"].x); 00095 EXPECT_FALSE(forceCntrlMap["testNode"].y); 00096 EXPECT_TRUE(forceCntrlMap["testNode"].z); 00097 EXPECT_FALSE(forceCntrlMap["testNode"].roll); 00098 EXPECT_FALSE(forceCntrlMap["testNode"].pitch); 00099 EXPECT_FALSE(forceCntrlMap["testNode"].yaw); 00100 00101 EXPECT_EQ(1, forceCntrlMap["testNode"].desiredForce.force.z()); 00102 00103 } 00104 00105 TEST_F(Cartesian_HybCntrlTest, sensorForceTest) 00106 { 00107 std::map<std::string, std::string> sensorNameMap; 00108 std::map<std::string, KDL::Wrench> sensorForces; 00109 std::map<std::string, std::pair<std::vector<int>, std::vector<double> > > forceControlInput; 00110 std::vector<int> axis; 00111 std::vector<double> mag; 00112 00113 setFilter(0.5); 00114 00115 sensorNameMap["left_arm"] = "/r2/left_arm/forearm/jr3"; 00116 sensorNameMap["right_arm"] = "/r2/right_arm/forearm/jr3"; 00117 sensorNameMap["left_leg"] = "/r2/left_leg/ati"; 00118 sensorNameMap["right_leg"] = "/r2/right_leg/ati"; 00119 setSensorNameMap(sensorNameMap); 00120 sensorForces["/r2/right_leg/ati"] = KDL::Wrench::Zero(); 00121 axis.push_back(0); 00122 mag.push_back(0); 00123 forceControlInput["/r2/right_leg/gripper/tip"] = std::make_pair(axis, mag); 00124 createForceControlMap(forceControlInput); 00125 00126 updateSensorForces(sensorForces); 00127 00128 EXPECT_EQ("/r2/right_leg/ati", forceCntrlMap["/r2/right_leg/gripper/tip"].sensorName); 00129 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["/r2/right_leg/gripper/tip"].sensorForce); 00130 00131 sensorForces["/r2/right_leg/ati"] = KDL::Wrench(KDL::Vector(1, 0, 0), KDL::Vector(0, 0, 0)); 00132 updateSensorForces(sensorForces); 00133 EXPECT_EQ(KDL::Wrench(KDL::Vector(0.5, 0, 0), KDL::Vector(0, 0, 0)), sensorForceMap["/r2/right_leg/ati"]); 00134 00135 sensorForces["/r2/right_leg/ati"] = KDL::Wrench::Zero(); 00136 updateSensorForces(sensorForces); 00137 EXPECT_EQ(KDL::Wrench(KDL::Vector(0.25, 0, 0), KDL::Vector(0, 0, 0)), sensorForceMap["/r2/right_leg/ati"]); 00138 00139 updateSensorForces(sensorForces); 00140 EXPECT_EQ(KDL::Wrench(KDL::Vector(0.125, 0, 0), KDL::Vector(0, 0, 0)), sensorForceMap["/r2/right_leg/ati"]); 00141 00142 updateSensorForces(sensorForces); 00143 EXPECT_EQ(KDL::Wrench(KDL::Vector(0.0625, 0, 0), KDL::Vector(0, 0, 0)), sensorForceMap["/r2/right_leg/ati"]); 00144 00145 updateSensorForces(sensorForces); 00146 EXPECT_EQ(KDL::Wrench(KDL::Vector(0.03125, 0, 0), KDL::Vector(0, 0, 0)), sensorForceMap["/r2/right_leg/ati"]); 00147 00148 } 00149 00150 TEST_F(Cartesian_HybCntrlTest, updateTest) 00151 { 00152 std::map<std::string, std::string> sensorNameMap; 00153 std::map<std::string, KDL::Wrench> sensorForces; 00154 std::map<std::string, std::pair<std::vector<int>, std::vector<double> > > forceControlInput; 00155 std::vector<int> axis; 00156 std::vector<double> mag; 00157 00158 std::vector<std::string> nodeNames; 00159 std::map<std::string, KDL::Frame> refFrames; 00160 std::vector<KDL::Frame> nodeFrames; 00161 double dt = 1; 00162 00164 setGain(1, 1, 0.0, 0.0, 0.0, 0.0, 0.0001, 0.0001, 10.0, 10.0); 00165 setPoseSettings(5.0, 5.0, 3.0, 3.0); 00166 setFilter(0.0); 00167 sensorNameMap["testNode"] = "testSensor"; 00168 setSensorNameMap(sensorNameMap); 00169 doFeedForward = false; 00170 00172 axis.push_back(0); 00173 mag.push_back(0); 00174 forceControlInput["testNode"] = std::make_pair(axis, mag); 00175 createForceControlMap(forceControlInput); 00176 setInitialPose("testNode", KDL::Frame::Identity()); 00177 00179 sensorForces["testSensor"] = KDL::Wrench::Zero(); 00180 updateSensorForces(sensorForces); 00181 nodeNames.push_back("testNode"); 00182 EXPECT_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt), std::runtime_error); 00183 nodeFrames.push_back(KDL::Frame::Identity()); 00184 EXPECT_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt), std::runtime_error); 00185 refFrames["testNode"] = KDL::Frame::Identity(); 00186 EXPECT_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt), std::runtime_error); 00187 refFrames["testSensor"] = KDL::Frame::Identity(); 00188 EXPECT_NO_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt)); 00189 00190 EXPECT_EQ(KDL::Frame::Identity(), nodeFrames[0]); 00191 00193 sensorForces["testSensor"] = KDL::Wrench(KDL::Vector(1, 0, 0), KDL::Vector(0, 0, 0)); 00194 updateSensorForces(sensorForces); 00195 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].desiredForce); 00196 EXPECT_EQ(KDL::Twist::Zero(), forceCntrlMap["testNode"].prevVelocity); 00197 EXPECT_NO_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt)); 00198 EXPECT_EQ(KDL::Wrench(KDL::Vector(1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].sensorForce); 00199 EXPECT_EQ(KDL::Wrench(KDL::Vector(-1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].error); 00200 EXPECT_EQ(KDL::Wrench(KDL::Vector(-1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].gain); 00201 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].integrator); 00202 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].derivative); 00203 EXPECT_EQ(KDL::Twist(KDL::Vector(-1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].prevVelocity); 00204 00205 EXPECT_EQ(-1, forceCntrlMap["testNode"].prevVelocity.vel.x()); 00206 EXPECT_EQ(0, forceCntrlMap["testNode"].prevVelocity.vel.y()); 00207 EXPECT_EQ(0, forceCntrlMap["testNode"].prevVelocity.vel.z()); 00208 EXPECT_EQ(0, forceCntrlMap["testNode"].prevVelocity.rot.x()); 00209 EXPECT_EQ(0, forceCntrlMap["testNode"].prevVelocity.rot.y()); 00210 EXPECT_EQ(0, forceCntrlMap["testNode"].prevVelocity.rot.z()); 00211 00212 EXPECT_EQ(-1, nodeFrames[0].p.x()); 00213 00214 sensorForces["testSensor"] = KDL::Wrench::Zero(); 00215 updateSensorForces(sensorForces); 00216 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].desiredForce); 00217 EXPECT_EQ(KDL::Twist(KDL::Vector(-1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].prevVelocity); 00218 EXPECT_NO_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt)); 00219 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].sensorForce); 00220 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].error); 00221 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].gain); 00222 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].integrator); 00223 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].derivative); 00224 EXPECT_EQ(KDL::Twist::Zero(), forceCntrlMap["testNode"].prevVelocity); 00225 EXPECT_FLOAT_EQ(-1, nodeFrames[0].p.x()); 00226 00228 sensorForces["testSensor"] = KDL::Wrench(KDL::Vector(1, 0, 0), KDL::Vector(0, 0, 0)); 00229 updateSensorForces(sensorForces); 00230 setGain(1, 1, 0.01, 0.01, 0.0, 0.0, 0.0001, 0.0001, 10.0, 10.0); 00231 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].desiredForce); 00232 EXPECT_EQ(KDL::Twist::Zero(), forceCntrlMap["testNode"].prevVelocity); 00233 EXPECT_NO_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt)); 00234 EXPECT_EQ(KDL::Wrench(KDL::Vector(1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].sensorForce); 00235 EXPECT_EQ(KDL::Wrench(KDL::Vector(-1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].error); 00236 EXPECT_EQ(KDL::Wrench(KDL::Vector(-1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].gain); 00237 EXPECT_EQ(KDL::Wrench(KDL::Vector(-0.0001, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].integrator); 00238 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].derivative); 00239 EXPECT_EQ(KDL::Twist(KDL::Vector(-1.0001, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].prevVelocity); 00240 EXPECT_FLOAT_EQ(-2.0001, nodeFrames[0].p.x()); 00241 00242 sensorForces["testSensor"] = KDL::Wrench::Zero(); 00243 updateSensorForces(sensorForces); 00244 EXPECT_NO_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt)); 00245 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].sensorForce); 00246 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].error); 00247 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].gain); 00248 EXPECT_EQ(KDL::Wrench(KDL::Vector(-0.0001, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].integrator); 00249 EXPECT_EQ(KDL::Wrench::Zero(), forceCntrlMap["testNode"].derivative); 00250 EXPECT_EQ(KDL::Twist(KDL::Vector(-0.0001, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].prevVelocity); 00251 EXPECT_FLOAT_EQ(-2.0002, nodeFrames[0].p.x()); 00252 00254 sensorForces["testSensor"] = KDL::Wrench(KDL::Vector(-1, 0, 0), KDL::Vector(0, 0, 0)); 00255 updateSensorForces(sensorForces); 00256 setGain(1, 1, 0.01, 0.01, 0.001, 0.001, 0.0001, 0.0001, 10.0, 10.0); 00257 EXPECT_NO_THROW(updateFrames(nodeNames, refFrames, nodeFrames, dt)); 00258 EXPECT_EQ(KDL::Wrench(KDL::Vector(-1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].sensorForce); 00259 EXPECT_EQ(KDL::Wrench(KDL::Vector(1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].error); 00260 EXPECT_EQ(KDL::Wrench(KDL::Vector(1, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].gain); 00261 EXPECT_EQ(KDL::Wrench(KDL::Vector(0.0001, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].integrator); 00262 EXPECT_EQ(KDL::Wrench(KDL::Vector(0.001, 0, 0), KDL::Vector(0, 0, 0)), forceCntrlMap["testNode"].derivative); 00263 EXPECT_FLOAT_EQ(-0.99910003, nodeFrames[0].p.x()); 00264 00265 } 00266 00267 00268 00269 int main(int argc, char** argv) 00270 { 00271 ::testing::InitGoogleTest(&argc, argv); 00272 return RUN_ALL_TESTS(); 00273 }