00001 #include <gtest/gtest.h> 00002 #include <boost/shared_ptr.hpp> 00003 #include <boost/make_shared.hpp> 00004 #include "robot_instance/RobotInstanceFactory.h" 00005 #include "robot_instance/CoeffMapLoader.h" 00006 #include "robodyn_mechanisms/JointCommandGripper.h" 00007 #include <ros/package.h> 00008 00009 using namespace std; 00010 00011 std::map<std::string, float> testFloats; 00012 CoeffMapPtr motorCoeffMap; 00013 CoeffMapPtr brainstemCoeffMap; 00014 RobotInstancePtr instance; 00015 00016 float getFloat(std::string item) 00017 { 00018 return testFloats[item]; 00019 } 00020 00021 void setFloat(std::string item, float value) 00022 { 00023 testFloats[item] = value; 00024 } 00025 00026 float getMotorCoeff(std::string item) 00027 { 00028 return motorCoeffMap->operator[](item); 00029 } 00030 00031 bool hasBrainstemCoeff(std::string item) 00032 { 00033 if (brainstemCoeffMap->find(item) != brainstemCoeffMap->end()) 00034 { 00035 return true; 00036 } 00037 00038 return false; 00039 } 00040 00041 float getBrainstemCoeff(std::string item) 00042 { 00043 return brainstemCoeffMap->operator[](item); 00044 } 00045 00046 std::string getCommandFile(std::string mechanism) 00047 { 00048 return instance->configurationSafetyBasePath + instance->APIMAP_PATH + instance->mechanismCommandMap[mechanism]; 00049 } 00050 00051 class JointCommandGripperTest : public ::testing::Test 00052 { 00053 protected: 00054 virtual void SetUp() 00055 { 00056 packagePath = ros::package::getPath("robot_instance"); 00057 configPath = packagePath + "/test/config"; 00058 configSafetyPath = packagePath + "/test/configSafety"; 00059 fileName = "TestRobotInstance.xml"; 00060 instance = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName); 00061 00062 motorCoeffMap = boost::make_shared<CoeffMap>(); 00063 brainstemCoeffMap = boost::make_shared<CoeffMap>(); 00064 CoeffMapLoader::loadElements(instance, motorCoeffMap, brainstemCoeffMap); 00065 00066 testFloats.clear(); 00067 00068 io.getFloat = getFloat; 00069 io.setFloat = setFloat; 00070 io.getMotorCoeff = getMotorCoeff; 00071 io.getCommandFile = getCommandFile; 00072 io.hasBrainstemCoeff = hasBrainstemCoeff; 00073 io.getBrainstemCoeff = getBrainstemCoeff; 00074 00075 ros::Time::init(); 00076 } 00077 00078 virtual void TearDown() 00079 { 00080 } 00081 00082 string packagePath, configPath, configSafetyPath, fileName; 00083 JointCommandInterface::IoFunctions io; 00084 }; 00085 00086 TEST_F(JointCommandGripperTest, Constructor) 00087 { 00088 ASSERT_NO_THROW(JointCommandGripper gripperJoint("r2/right_leg/gripper/joint0", io)); 00089 00091 io.getFloat = 0; 00092 ASSERT_THROW(JointCommandGripper gripperJoint("r2/right_leg/gripper/joint0", io), std::invalid_argument); 00093 } 00094 00095 TEST_F(JointCommandGripperTest, LoadCoeffs) 00096 { 00097 JointCommandGripper gripperJoint("r2/right_leg/gripper/joint0", io); 00098 00099 EXPECT_NO_THROW(gripperJoint.loadCoeffs()); 00100 00101 r2_msgs::JointControlData controlMsg; 00102 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED; 00103 EXPECT_NO_THROW(gripperJoint.updateMeasuredState(controlMsg)); 00104 00105 r2_msgs::JointCommand msg; 00106 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK; 00107 00108 msg.header.stamp = ros::Time::now(); 00109 00110 msg.name.resize(1); 00111 msg.desiredPosition.resize(1); 00112 msg.desiredPositionVelocityLimit.resize(1); 00113 msg.feedForwardTorque.resize(1); 00114 msg.proportionalGain.resize(1); 00115 msg.derivativeGain.resize(1); 00116 msg.integralGain.resize(1); 00117 msg.positionLoopTorqueLimit.resize(1); 00118 msg.positionLoopWindupLimit.resize(1); 00119 msg.torqueLoopVelocityLimit.resize(1); 00120 00121 msg.name[0] = "r2/right_leg/gripper/joint0"; 00122 msg.desiredPosition[0] = 1; 00123 msg.desiredPositionVelocityLimit[0] = 2; 00124 msg.feedForwardTorque[0] = 3; 00125 msg.proportionalGain[0] = 4; 00126 msg.derivativeGain[0] = 5; 00127 msg.integralGain[0] = 6; 00128 msg.positionLoopTorqueLimit[0] = 7; 00129 msg.positionLoopWindupLimit[0] = 8; 00130 msg.torqueLoopVelocityLimit[0] = 9; 00131 00132 EXPECT_NO_THROW(gripperJoint.setCommand(msg, controlMsg)); 00133 00134 EXPECT_NO_THROW(gripperJoint.getCommandedState()); 00135 00136 EXPECT_NO_THROW(gripperJoint.getCapability()); 00137 } 00138 00139 TEST_F(JointCommandGripperTest, getSimpleMeasuredState) 00140 { 00141 JointCommandGripper gripperJoint("r2/right_leg/gripper/joint0", io); 00142 00143 setFloat("r2/right_leg/gripper/joint0/EncPos", 1); 00144 setFloat("r2/right_leg/gripper/joint0/EncVel", 2); 00145 setFloat("r2/right_leg/gripper/joint0/APS1", 3); 00146 setFloat("r2/right_leg/gripper/joint0/APS2", 5); 00147 setFloat("r2/right_leg/gripper/joint0/JawLoad1", 7); 00148 setFloat("r2/right_leg/gripper/joint0/JawLoad2", 8); 00149 00150 r2_msgs::JointControlData controlMsg; 00151 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED; 00152 gripperJoint.updateMeasuredState(controlMsg); 00153 sensor_msgs::JointState msg = gripperJoint.getSimpleMeasuredState(); 00154 00155 EXPECT_EQ(3, msg.name.size()); 00156 EXPECT_EQ(3, msg.position.size()); 00157 EXPECT_EQ(3, msg.velocity.size()); 00158 EXPECT_EQ(3, msg.effort.size()); 00159 00160 EXPECT_STREQ("r2/right_leg/gripper/joint0", msg.name[0].c_str()); 00161 EXPECT_STREQ("r2/right_leg/gripper/jawLeft", msg.name[1].c_str()); 00162 EXPECT_STREQ("r2/right_leg/gripper/jawRight", msg.name[2].c_str()); 00163 00164 EXPECT_FLOAT_EQ(1, msg.position[0]); 00165 EXPECT_FLOAT_EQ(3, msg.position[1]); 00166 EXPECT_FLOAT_EQ(5, msg.position[2]); 00167 00168 EXPECT_FLOAT_EQ(2, msg.velocity[0]); 00169 EXPECT_FLOAT_EQ(0, msg.velocity[1]); 00170 EXPECT_FLOAT_EQ(0, msg.velocity[2]); 00171 00172 EXPECT_FLOAT_EQ(0, msg.effort[0]); 00173 EXPECT_FLOAT_EQ(7, msg.effort[1]); 00174 EXPECT_FLOAT_EQ(8, msg.effort[2]); 00175 } 00176 00177 TEST_F(JointCommandGripperTest, getCommandedState) 00178 { 00179 JointCommandGripper gripperJoint("r2/right_leg/gripper/joint0", io); 00180 00181 setFloat("r2/right_leg/gripper/joint0/PosCom", 1); 00182 setFloat("r2/right_leg/gripper/joint0/PosComVelocity", 2); 00183 setFloat("r2/right_leg/gripper/joint0/TorqueCom", 3); 00184 setFloat("r2/right_leg/gripper/joint0/JointStiffness", 4); 00185 setFloat("r2/right_leg/gripper/joint0/JointDamping", 5); 00186 setFloat("r2/right_leg/gripper/joint0/PLKi", 6); 00187 setFloat("r2/right_leg/gripper/joint0/TorqueLim", 7); 00188 setFloat("r2/right_leg/gripper/joint0/PLWindupLim", 8); 00189 setFloat("r2/right_leg/gripper/joint0/TLVelLimit", 9); 00190 00191 r2_msgs::JointCommand msg = gripperJoint.getCommandedState(); 00192 00193 EXPECT_EQ(0, msg.name.size()); 00194 EXPECT_EQ(0, msg.desiredPosition.size()); 00195 EXPECT_EQ(0, msg.desiredPositionVelocityLimit.size()); 00196 EXPECT_EQ(0, msg.feedForwardTorque.size()); 00197 EXPECT_EQ(0, msg.proportionalGain.size()); 00198 EXPECT_EQ(0, msg.derivativeGain.size()); 00199 EXPECT_EQ(0, msg.integralGain.size()); 00200 EXPECT_EQ(0, msg.positionLoopTorqueLimit.size()); 00201 EXPECT_EQ(0, msg.positionLoopWindupLimit.size()); 00202 EXPECT_EQ(0, msg.torqueLoopVelocityLimit.size()); 00203 } 00204 00205 TEST_F(JointCommandGripperTest, setCommand) 00206 { 00207 JointCommandGripper gripperJoint("r2/right_leg/gripper/joint0", io); 00208 00209 r2_msgs::JointCommand msg; 00210 r2_msgs::JointControlData controlMsg; 00211 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK; 00212 00213 msg.header.stamp = ros::Time::now(); 00214 00215 msg.name.resize(1); 00216 msg.desiredPosition.resize(1); 00217 msg.desiredPositionVelocityLimit.resize(1); 00218 msg.feedForwardTorque.resize(1); 00219 msg.proportionalGain.resize(1); 00220 msg.derivativeGain.resize(1); 00221 msg.integralGain.resize(1); 00222 msg.positionLoopTorqueLimit.resize(1); 00223 msg.positionLoopWindupLimit.resize(1); 00224 msg.torqueLoopVelocityLimit.resize(1); 00225 00226 msg.name[0] = "r2/right_leg/gripper/joint0"; 00227 msg.desiredPosition[0] = 1; 00228 msg.desiredPositionVelocityLimit[0] = 2; 00229 msg.feedForwardTorque[0] = 3; 00230 msg.proportionalGain[0] = 4; 00231 msg.derivativeGain[0] = 5; 00232 msg.integralGain[0] = 6; 00233 msg.positionLoopTorqueLimit[0] = 7; 00234 msg.positionLoopWindupLimit[0] = 8; 00235 msg.torqueLoopVelocityLimit[0] = 9; 00236 00237 gripperJoint.setCommand(msg, controlMsg); 00238 00239 EXPECT_FLOAT_EQ(1, getFloat("r2/right_leg/gripper/joint0/PosCom")); 00240 EXPECT_FLOAT_EQ(2, getFloat("r2/right_leg/gripper/joint0/PosComVelocity")); 00241 EXPECT_FLOAT_EQ(3, getFloat("r2/right_leg/gripper/joint0/TorqueCom")); 00242 EXPECT_FLOAT_EQ(4, getFloat("r2/right_leg/gripper/joint0/JointStiffness")); 00243 EXPECT_FLOAT_EQ(5, getFloat("r2/right_leg/gripper/joint0/JointDamping")); 00244 EXPECT_FLOAT_EQ(6, getFloat("r2/right_leg/gripper/joint0/PLKi")); 00245 EXPECT_FLOAT_EQ(7, getFloat("r2/right_leg/gripper/joint0/TorqueLim")); 00246 EXPECT_FLOAT_EQ(8, getFloat("r2/right_leg/gripper/joint0/PLWindupLim")); 00247 00248 msg.desiredPositionVelocityLimit.resize(0); 00249 msg.proportionalGain.resize(0); 00250 msg.integralGain.resize(0); 00251 msg.positionLoopWindupLimit.resize(0); 00252 00253 msg.desiredPosition[0] = 10; 00254 msg.feedForwardTorque[0] = 30; 00255 msg.derivativeGain[0] = 50; 00256 msg.positionLoopTorqueLimit[0] = 70; 00257 msg.torqueLoopVelocityLimit[0] = 90; 00258 00259 gripperJoint.setCommand(msg, controlMsg); 00260 00261 EXPECT_FLOAT_EQ(10, getFloat("r2/right_leg/gripper/joint0/PosCom")); 00262 EXPECT_FLOAT_EQ(2, getFloat("r2/right_leg/gripper/joint0/PosComVelocity")); 00263 EXPECT_FLOAT_EQ(30, getFloat("r2/right_leg/gripper/joint0/TorqueCom")); 00264 EXPECT_FLOAT_EQ(4, getFloat("r2/right_leg/gripper/joint0/JointStiffness")); 00265 EXPECT_FLOAT_EQ(50, getFloat("r2/right_leg/gripper/joint0/JointDamping")); 00266 EXPECT_FLOAT_EQ(6, getFloat("r2/right_leg/gripper/joint0/PLKi")); 00267 EXPECT_FLOAT_EQ(70, getFloat("r2/right_leg/gripper/joint0/TorqueLim")); 00268 EXPECT_FLOAT_EQ(8, getFloat("r2/right_leg/gripper/joint0/PLWindupLim")); 00269 } 00270 00271 TEST_F(JointCommandGripperTest, setCommand_BadMsg) 00272 { 00273 JointCommandGripper gripperJoint("r2/right_leg/gripper/joint0", io); 00274 00275 r2_msgs::JointCommand msg; 00276 r2_msgs::JointControlData controlMsg; 00277 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK; 00278 00280 EXPECT_THROW(gripperJoint.setCommand(msg, controlMsg), std::runtime_error); 00281 00282 msg.header.stamp = ros::Time::now(); 00283 00284 msg.name.resize(1); 00285 msg.desiredPosition.resize(1); 00286 msg.desiredPositionVelocityLimit.resize(1); 00287 msg.feedForwardTorque.resize(1); 00288 msg.proportionalGain.resize(1); 00289 msg.derivativeGain.resize(1); 00290 msg.integralGain.resize(1); 00291 msg.positionLoopTorqueLimit.resize(1); 00292 msg.positionLoopWindupLimit.resize(1); 00293 msg.torqueLoopVelocityLimit.resize(1); 00294 00295 msg.name[0] = "r2/right_leg/gripper/joint1"; 00296 msg.desiredPosition[0] = 1; 00297 msg.desiredPositionVelocityLimit[0] = 2; 00298 msg.feedForwardTorque[0] = 3; 00299 msg.proportionalGain[0] = 4; 00300 msg.derivativeGain[0] = 5; 00301 msg.integralGain[0] = 6; 00302 msg.positionLoopTorqueLimit[0] = 7; 00303 msg.positionLoopWindupLimit[0] = 8; 00304 msg.torqueLoopVelocityLimit[0] = 9; 00305 00307 EXPECT_THROW(gripperJoint.setCommand(msg, controlMsg), std::runtime_error); 00308 00309 msg.name[0] = "r2/right_leg/gripper/joint0"; 00310 msg.desiredPosition.push_back(11); 00311 00313 EXPECT_THROW(gripperJoint.setCommand(msg, controlMsg), std::runtime_error); 00314 } 00315 00316 TEST_F(JointCommandGripperTest, getCapability) 00317 { 00318 JointCommandGripper gripperJoint("r2/right_leg/gripper/joint0", io); 00319 00320 r2_msgs::JointCapability msg = gripperJoint.getCapability(); 00321 00322 EXPECT_EQ(1, msg.name.size()); 00323 EXPECT_EQ(1, msg.positionLimitMax.size()); 00324 EXPECT_EQ(1, msg.positionLimitMin.size()); 00325 EXPECT_EQ(1, msg.torqueLimit.size()); 00326 00327 EXPECT_STREQ("r2/right_leg/gripper/joint0", msg.name[0].c_str()); 00328 EXPECT_FLOAT_EQ(3.14159265358979, msg.positionLimitMax[0]); 00329 EXPECT_FLOAT_EQ(-3.14159265358979, msg.positionLimitMin[0]); 00330 EXPECT_FLOAT_EQ(0., msg.torqueLimit[0]); 00331 } 00332 00333 int main(int argc, char** argv) 00334 { 00335 testing::InitGoogleTest(&argc, argv); 00336 return RUN_ALL_TESTS(); 00337 }