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/JointCommandSeriesElastic.h"
00007 #include <ros/package.h>
00008
00009 using namespace std;
00010
00011 std::map<std::string, float> testFloats;
00012 std::map<std::string, int32_t> testInt32s;
00013 std::map<std::string, uint16_t> testUInt16s;
00014 CoeffMapPtr motorCoeffMap;
00015 CoeffMapPtr brainstemCoeffMap;
00016 RobotInstancePtr instance;
00017
00018 float getFloat(std::string item)
00019 {
00020 return testFloats[item];
00021 }
00022
00023 void setFloat(std::string item, float value)
00024 {
00025 testFloats[item] = value;
00026 }
00027
00028 float getUInt16(std::string item)
00029 {
00030 return testUInt16s[item];
00031 }
00032
00033 void setUInt16(std::string item, uint16_t value)
00034 {
00035 testUInt16s[item] = value;
00036 }
00037
00038 int32_t getInt32(std::string item)
00039 {
00040 return testInt32s[item];
00041 }
00042
00043 void setInt32(std::string item, int32_t value)
00044 {
00045 testInt32s[item] = value;
00046 }
00047
00048 float getMotorCoeff(std::string item)
00049 {
00050 return motorCoeffMap->operator[](item);
00051 }
00052
00053 bool hasBrainstemCoeff(std::string item)
00054 {
00055 if (brainstemCoeffMap->find(item) != brainstemCoeffMap->end())
00056 {
00057 return true;
00058 }
00059
00060 return false;
00061 }
00062
00063 float getBrainstemCoeff(std::string item)
00064 {
00065 return brainstemCoeffMap->operator[](item);
00066 }
00067
00068 std::string getCommandFile(std::string mechanism)
00069 {
00070 return instance->configurationSafetyBasePath + instance->APIMAP_PATH + instance->mechanismCommandMap[mechanism];
00071 }
00072
00073 class JointCommandSeriesElasticTest : public ::testing::Test
00074 {
00075 protected:
00076 virtual void SetUp()
00077 {
00078 packagePath = ros::package::getPath("robot_instance");
00079 configPath = packagePath + "/test/config";
00080 configSafetyPath = packagePath + "/test/configSafety";
00081 fileName = "TestRobotInstance.xml";
00082 instance = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName);
00083
00084 motorCoeffMap = boost::make_shared<CoeffMap>();
00085 brainstemCoeffMap = boost::make_shared<CoeffMap>();
00086 CoeffMapLoader::loadElements(instance, motorCoeffMap, brainstemCoeffMap);
00087
00088 testFloats.clear();
00089 testInt32s.clear();
00090
00091 io.getFloat = getFloat;
00092 io.setFloat = setFloat;
00093 io.setUInt16 = setUInt16;
00094 io.getInt32 = getInt32;
00095 io.getMotorCoeff = getMotorCoeff;
00096 io.hasBrainstemCoeff = hasBrainstemCoeff;
00097 io.getBrainstemCoeff = getBrainstemCoeff;
00098 io.getCommandFile = getCommandFile;
00099
00100 ros::Time::init();
00101 }
00102
00103 virtual void TearDown()
00104 {
00105 }
00106
00107 string packagePath, configPath, configSafetyPath, fileName;
00108 JointCommandInterface::IoFunctions io;
00109 };
00110
00111 TEST_F(JointCommandSeriesElasticTest, Constructor)
00112 {
00113 ASSERT_NO_THROW(JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io));
00114
00116 io.getFloat = 0;
00117 ASSERT_THROW(JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io), std::invalid_argument);
00118 }
00119
00120 TEST_F(JointCommandSeriesElasticTest, LoadCoeffs)
00121 {
00122 JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io);
00123
00124 EXPECT_NO_THROW(seriesElasticJoint.loadCoeffs());
00125
00126 r2_msgs::JointControlData controlMsg;
00127 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00128 EXPECT_NO_THROW(seriesElasticJoint.updateMeasuredState(controlMsg));
00129
00130 r2_msgs::JointCommand msg;
00131 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00132
00133 msg.header.stamp = ros::Time::now();
00134
00135 msg.name.resize(1);
00136 msg.desiredPosition.resize(1);
00137 msg.desiredPositionVelocityLimit.resize(1);
00138 msg.feedForwardTorque.resize(1);
00139 msg.proportionalGain.resize(1);
00140 msg.derivativeGain.resize(1);
00141 msg.integralGain.resize(1);
00142 msg.positionLoopTorqueLimit.resize(1);
00143 msg.positionLoopWindupLimit.resize(1);
00144 msg.torqueLoopVelocityLimit.resize(1);
00145
00146 msg.name[0] = "r2/left_leg/joint0";
00147 msg.desiredPosition[0] = 1;
00148 msg.desiredPositionVelocityLimit[0] = 2;
00149 msg.feedForwardTorque[0] = 3;
00150 msg.proportionalGain[0] = 4;
00151 msg.derivativeGain[0] = 5;
00152 msg.integralGain[0] = 6;
00153 msg.positionLoopTorqueLimit[0] = 7;
00154 msg.positionLoopWindupLimit[0] = 8;
00155 msg.torqueLoopVelocityLimit[0] = 9;
00156
00157 EXPECT_NO_THROW(seriesElasticJoint.setCommand(msg, controlMsg));
00158
00159 EXPECT_NO_THROW(seriesElasticJoint.getCommandedState());
00160
00161 EXPECT_NO_THROW(seriesElasticJoint.getCapability());
00162 }
00163
00164 TEST_F(JointCommandSeriesElasticTest, getSimpleMeasuredState)
00165 {
00166 JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io);
00167
00168 setFloat("r2/left_leg/joint0/APS2Unfiltered", 1);
00169 setFloat("r2/left_leg/joint0/JointVelocity", 2);
00170 setFloat("r2/left_leg/joint0/APS1Unfiltered", 3);
00171 setFloat("r2/left_leg/joint0/APS1Vel", 4);
00172 setFloat("r2/left_leg/joint0/EncPos", 5);
00173 setFloat("r2/left_leg/joint0/EncVel", 6);
00174 setInt32("r2/left_leg/joint0/IncEnc", 123);
00175 setFloat("r2/left_leg/joint0/JointTorque", 10);
00176
00177 r2_msgs::JointControlData controlMsg;
00178 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00179 seriesElasticJoint.updateMeasuredState(controlMsg);
00180 sensor_msgs::JointState msg = seriesElasticJoint.getSimpleMeasuredState();
00181
00182 EXPECT_EQ(1, msg.name.size());
00183 EXPECT_EQ(1, msg.position.size());
00184 EXPECT_EQ(1, msg.velocity.size());
00185 EXPECT_EQ(1, msg.effort.size());
00186
00187 EXPECT_STREQ("r2/left_leg/joint0", msg.name[0].c_str());
00188 EXPECT_FLOAT_EQ(1, msg.position[0]);
00189 EXPECT_FLOAT_EQ(2, msg.velocity[0]);
00190 EXPECT_FLOAT_EQ(10, msg.effort[0]);
00191 }
00192
00193 TEST_F(JointCommandSeriesElasticTest, getCompleteMeasuredState)
00194 {
00195 JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io);
00196
00197 setFloat("r2/left_leg/joint0/APS2Unfiltered", 1);
00198 setFloat("r2/left_leg/joint0/JointVelocity", 2);
00199 setFloat("r2/left_leg/joint0/APS1Unfiltered", 3);
00200 setFloat("r2/left_leg/joint0/APS1Vel", 4);
00201 setFloat("r2/left_leg/joint0/EncPos", 5);
00202 setFloat("r2/left_leg/joint0/EncVel", 6);
00203 setFloat("r2/left_leg/joint0/APS2", 1.1);
00204 setFloat("r2/left_leg/joint0/APS1", 3.1);
00205 setInt32("r2/left_leg/joint0/IncEnc", 123);
00206 setFloat("r2/left_leg/joint0/JointTorque", 10);
00207 setFloat("r2/left_leg/joint0/PosComSmoothed", 12);
00208 setFloat("r2/left_leg/joint0/VelSmoothedFiltered", 13);
00209 setFloat("r2/left_leg/joint0/TorqueDes", 14);
00210
00211 r2_msgs::JointControlData controlMsg;
00212 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00213 seriesElasticJoint.updateMeasuredState(controlMsg);
00214 sensor_msgs::JointState msg = seriesElasticJoint.getCompleteMeasuredState();
00215
00216 EXPECT_EQ(7, msg.name.size());
00217 EXPECT_EQ(7, msg.position.size());
00218 EXPECT_EQ(7, msg.velocity.size());
00219 EXPECT_EQ(7, msg.effort.size());
00220
00221 EXPECT_STREQ("r2/left_leg/joint0", msg.name[0].c_str());
00222 EXPECT_STREQ("r2/left_leg/motor0", msg.name[1].c_str());
00223 EXPECT_STREQ("r2/left_leg/encoder0", msg.name[2].c_str());
00224 EXPECT_STREQ("r2/left_leg/jointCalculated0", msg.name[3].c_str());
00225 EXPECT_STREQ("r2/left_leg/encoderCalculated0", msg.name[4].c_str());
00226 EXPECT_STREQ("r2/left_leg/hallsCalculated0", msg.name[5].c_str());
00227 EXPECT_STREQ("r2/left_leg/embeddedCommand0", msg.name[6].c_str());
00228
00230 EXPECT_FLOAT_EQ(1, msg.position[0]);
00231 EXPECT_FLOAT_EQ(2, msg.velocity[0]);
00232 EXPECT_FLOAT_EQ(10, msg.effort[0]);
00233
00235 EXPECT_FLOAT_EQ(3, msg.position[1]);
00236 EXPECT_FLOAT_EQ(4, msg.velocity[1]);
00237
00238
00240 EXPECT_FLOAT_EQ(5, msg.position[2]);
00241 EXPECT_FLOAT_EQ(6.0 / 160.0, msg.velocity[2]);
00242 EXPECT_FLOAT_EQ(0, msg.effort[2]);
00243
00245 EXPECT_FLOAT_EQ(0, msg.position[3]);
00246 EXPECT_FLOAT_EQ(0, msg.velocity[3]);
00247 EXPECT_FLOAT_EQ((1.1 - 3.1) * -100.0, msg.effort[3]);
00248
00250
00251
00252
00253
00255
00256
00257 EXPECT_FLOAT_EQ(0, msg.effort[5]);
00258
00260 EXPECT_FLOAT_EQ(12, msg.position[6]);
00261 EXPECT_FLOAT_EQ(13, msg.velocity[6]);
00262 EXPECT_FLOAT_EQ(14, msg.effort[6]);
00263 }
00264
00265 TEST_F(JointCommandSeriesElasticTest, getCommandedState)
00266 {
00267 JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io);
00268
00269 setFloat("r2/left_leg/joint0/PosCom", 1);
00270 setFloat("r2/left_leg/joint0/PosComVelocity", 2);
00271 setFloat("r2/left_leg/joint0/TorqueCom", 3);
00272 setFloat("r2/left_leg/joint0/JointStiffness", 4);
00273 setFloat("r2/left_leg/joint0/JointDamping", 5);
00274 setFloat("r2/left_leg/joint0/PLKi", 6);
00275 setFloat("r2/left_leg/joint0/TorqueLim", 7);
00276 setFloat("r2/left_leg/joint0/PLWindupLim", 8);
00277 setFloat("r2/left_leg/joint0/TLVelLimit", 9);
00278
00279 r2_msgs::JointCommand msg = seriesElasticJoint.getCommandedState();
00280
00281 EXPECT_EQ(1, msg.name.size());
00282 EXPECT_EQ(1, msg.desiredPosition.size());
00283 EXPECT_EQ(1, msg.desiredPositionVelocityLimit.size());
00284 EXPECT_EQ(1, msg.feedForwardTorque.size());
00285 EXPECT_EQ(1, msg.proportionalGain.size());
00286 EXPECT_EQ(1, msg.derivativeGain.size());
00287 EXPECT_EQ(1, msg.integralGain.size());
00288 EXPECT_EQ(1, msg.positionLoopTorqueLimit.size());
00289 EXPECT_EQ(1, msg.positionLoopWindupLimit.size());
00290 EXPECT_EQ(1, msg.torqueLoopVelocityLimit.size());
00291
00292 EXPECT_STREQ("r2/left_leg/joint0", msg.name[0].c_str());
00293 EXPECT_FLOAT_EQ(1, msg.desiredPosition[0]);
00294 EXPECT_FLOAT_EQ(2, msg.desiredPositionVelocityLimit[0]);
00295 EXPECT_FLOAT_EQ(3, msg.feedForwardTorque[0]);
00296 EXPECT_FLOAT_EQ(4, msg.proportionalGain[0]);
00297 EXPECT_FLOAT_EQ(5, msg.derivativeGain[0]);
00298 EXPECT_FLOAT_EQ(6, msg.integralGain[0]);
00299 EXPECT_FLOAT_EQ(7, msg.positionLoopTorqueLimit[0]);
00300 EXPECT_FLOAT_EQ(8, msg.positionLoopWindupLimit[0]);
00301
00302 }
00303
00304 TEST_F(JointCommandSeriesElasticTest, setCommand)
00305 {
00306 JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io);
00307
00308 r2_msgs::JointCommand msg;
00309 r2_msgs::JointControlData controlMsg;
00310 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00311
00312 msg.header.stamp = ros::Time::now();
00313
00314 msg.name.resize(1);
00315 msg.desiredPosition.resize(1);
00316 msg.desiredPositionVelocityLimit.resize(1);
00317 msg.feedForwardTorque.resize(1);
00318 msg.proportionalGain.resize(1);
00319 msg.derivativeGain.resize(1);
00320 msg.integralGain.resize(1);
00321 msg.positionLoopTorqueLimit.resize(1);
00322 msg.positionLoopWindupLimit.resize(1);
00323 msg.torqueLoopVelocityLimit.resize(1);
00324
00325 msg.name[0] = "r2/left_leg/joint0";
00326 msg.desiredPosition[0] = 1;
00327 msg.desiredPositionVelocityLimit[0] = 2;
00328 msg.feedForwardTorque[0] = 3;
00329 msg.proportionalGain[0] = 4;
00330 msg.derivativeGain[0] = 5;
00331 msg.integralGain[0] = 6;
00332 msg.positionLoopTorqueLimit[0] = 7;
00333 msg.positionLoopWindupLimit[0] = 8;
00334 msg.torqueLoopVelocityLimit[0] = 9;
00335
00336 seriesElasticJoint.setCommand(msg, controlMsg);
00337
00338 EXPECT_FLOAT_EQ(1, getFloat("r2/left_leg/joint0/PosCom"));
00339 EXPECT_FLOAT_EQ(2, getFloat("r2/left_leg/joint0/PosComVelocity"));
00340 EXPECT_FLOAT_EQ(3, getFloat("r2/left_leg/joint0/TorqueCom"));
00341 EXPECT_FLOAT_EQ(4, getFloat("r2/left_leg/joint0/JointStiffness"));
00342 EXPECT_FLOAT_EQ(5, getFloat("r2/left_leg/joint0/JointDamping"));
00343 EXPECT_FLOAT_EQ(6, getFloat("r2/left_leg/joint0/PLKi"));
00344 EXPECT_FLOAT_EQ(7, getFloat("r2/left_leg/joint0/TorqueLim"));
00345 EXPECT_FLOAT_EQ(8, getFloat("r2/left_leg/joint0/PLWindupLim"));
00346
00347
00348 msg.desiredPositionVelocityLimit.resize(0);
00349 msg.proportionalGain.resize(0);
00350 msg.integralGain.resize(0);
00351 msg.positionLoopWindupLimit.resize(0);
00352
00353 msg.desiredPosition[0] = 10;
00354 msg.feedForwardTorque[0] = 30;
00355 msg.derivativeGain[0] = 50;
00356 msg.positionLoopTorqueLimit[0] = 70;
00357 msg.torqueLoopVelocityLimit[0] = 90;
00358
00359 seriesElasticJoint.setCommand(msg, controlMsg);
00360
00361 EXPECT_FLOAT_EQ(10, getFloat("r2/left_leg/joint0/PosCom"));
00362 EXPECT_FLOAT_EQ(2, getFloat("r2/left_leg/joint0/PosComVelocity"));
00363 EXPECT_FLOAT_EQ(30, getFloat("r2/left_leg/joint0/TorqueCom"));
00364 EXPECT_FLOAT_EQ(4, getFloat("r2/left_leg/joint0/JointStiffness"));
00365 EXPECT_FLOAT_EQ(50, getFloat("r2/left_leg/joint0/JointDamping"));
00366 EXPECT_FLOAT_EQ(6, getFloat("r2/left_leg/joint0/PLKi"));
00367 EXPECT_FLOAT_EQ(70, getFloat("r2/left_leg/joint0/TorqueLim"));
00368 EXPECT_FLOAT_EQ(8, getFloat("r2/left_leg/joint0/PLWindupLim"));
00369 }
00370
00371 TEST_F(JointCommandSeriesElasticTest, setCommand_BadMsg)
00372 {
00373 JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io);
00374
00375 r2_msgs::JointCommand msg;
00376 r2_msgs::JointControlData controlMsg;
00377 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00378
00380 EXPECT_THROW(seriesElasticJoint.setCommand(msg, controlMsg), std::runtime_error);
00381
00382 msg.header.stamp = ros::Time::now();
00383
00384 msg.name.resize(1);
00385 msg.desiredPosition.resize(1);
00386 msg.desiredPositionVelocityLimit.resize(1);
00387 msg.feedForwardTorque.resize(1);
00388 msg.proportionalGain.resize(1);
00389 msg.derivativeGain.resize(1);
00390 msg.integralGain.resize(1);
00391 msg.positionLoopTorqueLimit.resize(1);
00392 msg.positionLoopWindupLimit.resize(1);
00393 msg.torqueLoopVelocityLimit.resize(1);
00394
00395 msg.name[0] = "r2/left_leg/joint1";
00396 msg.desiredPosition[0] = 1;
00397 msg.desiredPositionVelocityLimit[0] = 2;
00398 msg.feedForwardTorque[0] = 3;
00399 msg.proportionalGain[0] = 4;
00400 msg.derivativeGain[0] = 5;
00401 msg.integralGain[0] = 6;
00402 msg.positionLoopTorqueLimit[0] = 7;
00403 msg.positionLoopWindupLimit[0] = 8;
00404 msg.torqueLoopVelocityLimit[0] = 9;
00405
00407 EXPECT_THROW(seriesElasticJoint.setCommand(msg, controlMsg), std::runtime_error);
00408
00409 msg.name[0] = "r2/left_leg/joint0";
00410 msg.desiredPosition.push_back(11);
00411
00413 EXPECT_THROW(seriesElasticJoint.setCommand(msg, controlMsg), std::runtime_error);
00414 }
00415
00416 TEST_F(JointCommandSeriesElasticTest, getCapability)
00417 {
00418 JointCommandSeriesElastic seriesElasticJoint("r2/left_leg/joint0", io);
00419
00420 r2_msgs::JointCapability msg = seriesElasticJoint.getCapability();
00421
00422 EXPECT_EQ(1, msg.name.size());
00423 EXPECT_EQ(1, msg.positionLimitMax.size());
00424 EXPECT_EQ(1, msg.positionLimitMin.size());
00425 EXPECT_EQ(1, msg.torqueLimit.size());
00426
00427 EXPECT_STREQ("r2/left_leg/joint0", msg.name[0].c_str());
00428 EXPECT_FLOAT_EQ(3.14159265358979, msg.positionLimitMax[0]);
00429 EXPECT_FLOAT_EQ(-3.14159265358979, msg.positionLimitMin[0]);
00430 EXPECT_FLOAT_EQ(271.1, msg.torqueLimit[0]);
00431 }
00432
00433 int main(int argc, char** argv)
00434 {
00435 testing::InitGoogleTest(&argc, argv);
00436 return RUN_ALL_TESTS();
00437 }