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/JointCommandWrist.h"
00007 #include <ros/package.h>
00008 #include "r2_msgs/JointControlData.h"
00009
00010 using namespace std;
00011 using namespace Eigen;
00012
00013 std::map<std::string, int16_t> testInt16;
00014 std::map<std::string, uint16_t> testUint16;
00015 std::map<std::string, float> testLiveCoeffs;
00016 ParamMap testParamMap;
00017 CoeffMapPtr motorCoeffMap;
00018 CoeffMapPtr brainstemCoeffMap;
00019 RobotInstancePtr instance;
00020
00021
00022 float getInt16(std::string item)
00023 {
00024 return testInt16[item];
00025 }
00026
00027 float getUint16(std::string item)
00028 {
00029 return testUint16[item];
00030 }
00031
00032 void setInt16(std::string item, int16_t value)
00033 {
00034 testInt16[item] = value;
00035 }
00036
00037 void setUint16(std::string item, uint16_t value)
00038 {
00039 testUint16[item] = value;
00040 }
00041
00042 float getBrainstemCoeff(std::string item)
00043 {
00044 return brainstemCoeffMap->operator[](item);
00045 }
00046
00047 float getMotorCoeff(std::string item)
00048 {
00049 return motorCoeffMap->operator[](item);
00050 }
00051
00052 bool hasLiveCoeff(std::string coeff)
00053 {
00054 return testLiveCoeffs.count(coeff) > 0;
00055 }
00056
00057 float getLiveCoeff(std::string coeff)
00058 {
00059 return testLiveCoeffs[coeff];
00060 }
00061
00062 void setLiveCoeff(std::string coeff, float value)
00063 {
00064 testLiveCoeffs[coeff] = value;
00065 }
00066
00067 bool hasParam(std::string paramName)
00068 {
00069 return (testParamMap.find(paramName) != testParamMap.end());
00070 }
00071
00072 bool getBoolParam(std::string paramName, bool& value)
00073 {
00074 value = boost::get<bool>(testParamMap[paramName]);
00075 return true;
00076 }
00077
00078 bool getStringParam(std::string paramName, std::string& value)
00079 {
00080 value = boost::get<std::string>(testParamMap[paramName]);
00081 return true;
00082 }
00083
00084 bool getDoubleParam(std::string paramName, double& value)
00085 {
00086 value = boost::get<double>(testParamMap[paramName]);
00087 return true;
00088 }
00089
00090 bool getIntParam(std::string paramName, int& value)
00091 {
00092 value = boost::get<int>(testParamMap[paramName]);
00093 return true;
00094 }
00095
00096 void setBoolParam(std::string paramName, bool value)
00097 {
00098 testParamMap[paramName] = value;
00099 }
00100
00101 void setStringParam(std::string paramName, std::string value)
00102 {
00103 testParamMap[paramName] = value;
00104 }
00105
00106 void setDoubleParam(std::string paramName, double value)
00107 {
00108 testParamMap[paramName] = value;
00109 }
00110
00111 void setIntParam(std::string paramName, int value)
00112 {
00113 testParamMap[paramName] = value;
00114 }
00115
00116 std::vector<std::string> getJointNames(std::string mechanism)
00117 {
00118 return instance->mechanismJointsMap[mechanism];
00119 }
00120
00121 std::vector<std::string> getActuatorNames(std::string mechanism)
00122 {
00123 return instance->mechanismActuatorsMap[mechanism];
00124 }
00125
00126 std::string getCommandFile(std::string mechanism)
00127 {
00128 return instance->configurationSafetyBasePath + instance->APIMAP_PATH + instance->mechanismCommandMap[mechanism];
00129 }
00130
00131 class JointCommandWristTest : public ::testing::Test
00132 {
00133 protected:
00134 virtual void SetUp()
00135 {
00136 packagePath = ros::package::getPath("robot_instance");
00137 configPath = packagePath + "/test/config";
00138 configSafetyPath = packagePath + "/test/configSafety";
00139 fileName = "TestRobotInstanceWrist.xml";
00140 instance = RobotInstanceFactory::createRobotInstanceFromFile(configPath, configSafetyPath, fileName);
00141
00142 motorCoeffMap = boost::make_shared<CoeffMap>();
00143 brainstemCoeffMap = boost::make_shared<CoeffMap>();
00144 CoeffMapLoader::loadElements(instance, motorCoeffMap, brainstemCoeffMap);
00145
00146 testInt16.clear();
00147 testUint16.clear();
00148 testLiveCoeffs.clear();
00149
00150 io.setInt16 = setInt16;
00151 io.getInt16 = getInt16;
00152 io.getUInt16 = getUint16;
00153 io.getBrainstemCoeff = getBrainstemCoeff;
00154 io.getMotorCoeff = getMotorCoeff;
00155 io.getJointNames = getJointNames;
00156 io.getActuatorNames = getActuatorNames;
00157 io.getCommandFile = getCommandFile;
00158 io.hasLiveCoeff = hasLiveCoeff;
00159 io.getLiveCoeff = getLiveCoeff;
00160 io.setLiveCoeff = setLiveCoeff;
00161 io.hasParam = hasParam;
00162 io.getBoolParam = getBoolParam;
00163 io.getIntParam = getIntParam;
00164 io.getDoubleParam = getDoubleParam;
00165 io.getStringParam = getStringParam;
00166 io.setBoolParam = setBoolParam;
00167 io.setIntParam = setIntParam;
00168 io.setDoubleParam = setDoubleParam;
00169 io.setStringParam = setStringParam;
00170
00171 ros::Time::init();
00172 mechanism = "r2/right_arm/wrist";
00173 calEncoder = Vector2f(0, 0);
00174 calAngle(0) = io.getMotorCoeff("r2/right_arm/wrist/ManualCalPitch");
00175 calAngle(1) = io.getMotorCoeff("r2/right_arm/wrist/ManualCalYaw");
00176 calSlider(0) = io.getMotorCoeff("r2/right_arm/wrist/ManualCalThumbSlider");
00177 calSlider(1) = io.getMotorCoeff("r2/right_arm/wrist/ManualCalLittleSlider");
00178 calibrationModeName = "r2/right_arm/wrist/CalibrationMode";
00179 io.setLiveCoeff(calibrationModeName, r2_msgs::JointControlCalibrationMode::UNCALIBRATED);
00180 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00181 }
00182
00183 virtual void TearDown()
00184 {
00185 }
00186
00187 string packagePath, configPath, configSafetyPath, fileName;
00188 string mechanism;
00189 JointCommandInterface::IoFunctions io;
00190 Vector2f calEncoder, calAngle, calSlider;
00191 r2_msgs::JointControlData controlMsg;
00192 r2_msgs::JointCommand commandMsg;
00193 string calibrationModeName;
00194 };
00195
00196 TEST_F(JointCommandWristTest, Constructor)
00197 {
00198 ASSERT_NO_THROW(JointCommandWrist wristJoint(mechanism, io));
00200 io.getInt16 = 0;
00201 ASSERT_THROW(JointCommandWrist wristJoint(mechanism, io), std::invalid_argument);
00202 }
00203
00204 TEST_F(JointCommandWristTest, LoadCoeffs)
00205 {
00206 JointCommandWrist wristJoint(mechanism, io);
00207
00208 EXPECT_NO_THROW(wristJoint.loadCoeffs());
00209
00210 r2_msgs::JointControlData controlMsg;
00211 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00212 EXPECT_NO_THROW(wristJoint.updateMeasuredState(controlMsg));
00213
00214 r2_msgs::JointCommand msg;
00215 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00216
00217 msg.header.stamp = ros::Time::now();
00218
00219 msg.name.resize(1);
00220 msg.desiredPosition.resize(1);
00221 msg.desiredPositionVelocityLimit.resize(1);
00222 msg.feedForwardTorque.resize(1);
00223 msg.proportionalGain.resize(1);
00224 msg.derivativeGain.resize(1);
00225 msg.integralGain.resize(1);
00226 msg.positionLoopTorqueLimit.resize(1);
00227 msg.positionLoopWindupLimit.resize(1);
00228 msg.torqueLoopVelocityLimit.resize(1);
00229
00230 msg.name[0] = mechanism;
00231 msg.desiredPosition[0] = 1;
00232 msg.desiredPositionVelocityLimit[0] = 2;
00233 msg.feedForwardTorque[0] = 3;
00234 msg.proportionalGain[0] = 4;
00235 msg.derivativeGain[0] = 5;
00236 msg.integralGain[0] = 6;
00237 msg.positionLoopTorqueLimit[0] = 7;
00238 msg.positionLoopWindupLimit[0] = 8;
00239 msg.torqueLoopVelocityLimit[0] = 9;
00240
00241 EXPECT_NO_THROW(wristJoint.setCommand(msg, controlMsg));
00242
00243 EXPECT_NO_THROW(wristJoint.getCommandedState());
00244
00245 EXPECT_NO_THROW(wristJoint.getCapability());
00246 }
00247
00249 TEST_F(JointCommandWristTest, getSimpleMeasuredState)
00250 {
00251 JointCommandWrist wristJoint(mechanism, io);
00252
00253 io.setBoolParam("r2/right_arm/wrist/UseHalls", false);
00254
00255 setInt16("r2/right_arm/wrist/thumbside/IncEnc", calEncoder(0));
00256 setInt16("r2/right_arm/wrist/thumbside/MotCom", 5);
00257 setInt16("r2/right_arm/wrist/littleside/IncEnc", calEncoder(1));
00258 setInt16("r2/right_arm/wrist/littleside/MotCom", 5);
00259
00260 wristJoint.updateMeasuredState(controlMsg);
00261 sensor_msgs::JointState msg = wristJoint.getSimpleMeasuredState();
00262
00263 ASSERT_EQ(2, msg.name.size());
00264 ASSERT_EQ(2, msg.position.size());
00265 ASSERT_EQ(2, msg.velocity.size());
00266 ASSERT_EQ(2, msg.effort.size());
00267
00268 EXPECT_STREQ("r2/right_arm/wrist/pitch", msg.name[0].c_str());
00269 EXPECT_STREQ("r2/right_arm/wrist/yaw", msg.name[1].c_str());
00270
00271 EXPECT_NEAR(0, msg.position[0], 0.00174532925);
00272 EXPECT_NEAR(0, msg.position[1], 0.00174532925);
00273
00274 controlMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00275 wristJoint.setCommand(commandMsg, controlMsg);
00276 wristJoint.updateMeasuredState(controlMsg);
00277 wristJoint.updateMeasuredState(controlMsg);
00278 msg = wristJoint.getSimpleMeasuredState();
00279
00280 EXPECT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, io.getLiveCoeff(calibrationModeName));
00281
00282 EXPECT_NEAR(calAngle(0), msg.position[0], 0.00174532925);
00283 EXPECT_NEAR(calAngle(1), msg.position[1], 0.00174532925);
00284
00285 wristJoint.updateMeasuredState(controlMsg);
00286 msg = wristJoint.getSimpleMeasuredState();
00287
00288 EXPECT_NEAR(calAngle(0), msg.position[0], 0.005);
00289 EXPECT_NEAR(calAngle(1), msg.position[1], 0.005);
00290
00291 EXPECT_FLOAT_EQ(0, msg.velocity[0]);
00292 EXPECT_FLOAT_EQ(0, msg.velocity[1]);
00293 }
00294
00295 TEST_F(JointCommandWristTest, getCompleteMeasuredState)
00296 {
00297 JointCommandWrist wristJoint(mechanism, io);
00298
00299 io.setBoolParam("r2/right_arm/wrist/UseHalls", false);
00300
00301 setInt16("r2/right_arm/wrist/thumbside/IncEnc", calEncoder(0));
00302 setInt16("r2/right_arm/wrist/thumbside/MotCom", 5);
00303 setInt16("r2/right_arm/wrist/littleside/IncEnc", calEncoder(1));
00304 setInt16("r2/right_arm/wrist/littleside/MotCom", 5);
00305 setUint16("r2/right_arm/wrist/pitch/Hall", 2);
00306 setUint16("r2/right_arm/wrist/yaw/Hall", 3);
00307
00308 wristJoint.updateMeasuredState(controlMsg);
00309 sensor_msgs::JointState msg = wristJoint.getCompleteMeasuredState();
00310
00311 ASSERT_EQ(12, msg.name.size());
00312 ASSERT_EQ(12, msg.position.size());
00313 ASSERT_EQ(12, msg.velocity.size());
00314 ASSERT_EQ(12, msg.effort.size());
00315
00316 EXPECT_STREQ("r2/right_arm/wrist/pitch", msg.name[0].c_str());
00317 EXPECT_STREQ("r2/right_arm/wrist/yaw", msg.name[1].c_str());
00318 EXPECT_STREQ("r2/right_arm/wrist/thumbside", msg.name[2].c_str());
00319 EXPECT_STREQ("r2/right_arm/wrist/littleside", msg.name[3].c_str());
00320 EXPECT_STREQ("r2/right_arm/wrist/pitch/halls", msg.name[4].c_str());
00321 EXPECT_STREQ("r2/right_arm/wrist/yaw/halls", msg.name[5].c_str());
00322 EXPECT_STREQ("r2/right_arm/wrist/thumbside/encoder", msg.name[6].c_str());
00323 EXPECT_STREQ("r2/right_arm/wrist/littleside/encoder", msg.name[7].c_str());
00324 EXPECT_STREQ("r2/right_arm/wrist/pitch/embeddedCommand", msg.name[8].c_str());
00325 EXPECT_STREQ("r2/right_arm/wrist/yaw/embeddedCommand", msg.name[9].c_str());
00326 EXPECT_STREQ("r2/right_arm/wrist/thumbside/embeddedCommand", msg.name[10].c_str());
00327 EXPECT_STREQ("r2/right_arm/wrist/littleside/embeddedCommand", msg.name[11].c_str());
00328
00329 EXPECT_NEAR(0, msg.position[0], 0.00174532925);
00330 EXPECT_NEAR(0, msg.position[1], 0.00174532925);
00331 EXPECT_NEAR(0, msg.position[2], 1);
00332 EXPECT_NEAR(0, msg.position[3], 1);
00333 EXPECT_NEAR(2, msg.position[4], 0.1);
00334 EXPECT_NEAR(3, msg.position[5], 0.1);
00335 EXPECT_FLOAT_EQ(calEncoder(0), msg.position[6]);
00336 EXPECT_FLOAT_EQ(calEncoder(1), msg.position[7]);
00337
00338 controlMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00339 wristJoint.setCommand(commandMsg, controlMsg);
00340 wristJoint.updateMeasuredState(controlMsg);
00341 wristJoint.updateMeasuredState(controlMsg);
00342 msg = wristJoint.getCompleteMeasuredState();
00343
00344 EXPECT_NEAR(calAngle(0), msg.position[0], 0.00174532925);
00345 EXPECT_NEAR(calAngle(1), msg.position[1], 0.00174532925);
00346 EXPECT_NEAR(calSlider(0), msg.position[2], 1);
00347 EXPECT_NEAR(calSlider(1), msg.position[3], 1);
00348 EXPECT_NEAR(2, msg.position[4], 0.1);
00349 EXPECT_NEAR(3, msg.position[5], 0.1);
00350 EXPECT_FLOAT_EQ(calEncoder(0), msg.position[6]);
00351 EXPECT_FLOAT_EQ(calEncoder(1), msg.position[7]);
00352
00353 wristJoint.updateMeasuredState(controlMsg);
00354 msg = wristJoint.getCompleteMeasuredState();
00355
00356 EXPECT_NEAR(calAngle(0), msg.position[0], 0.005);
00357 EXPECT_NEAR(calAngle(1), msg.position[1], 0.005);
00358 EXPECT_NEAR(calSlider(0), msg.position[2], 1);
00359 EXPECT_NEAR(calSlider(1), msg.position[3], 1);
00360 EXPECT_NEAR(2, msg.position[4], 0.1);
00361 EXPECT_NEAR(3, msg.position[5], 0.1);
00362 EXPECT_NEAR(calEncoder(0), msg.position[6], 1);
00363 EXPECT_NEAR(calEncoder(1), msg.position[7], 1);
00364
00365 EXPECT_FLOAT_EQ(0, msg.velocity[0]);
00366 EXPECT_FLOAT_EQ(0, msg.velocity[1]);
00367 EXPECT_FLOAT_EQ(0, msg.velocity[2]);
00368 EXPECT_FLOAT_EQ(0, msg.velocity[3]);
00369 EXPECT_FLOAT_EQ(0, msg.velocity[4]);
00370 EXPECT_FLOAT_EQ(0, msg.velocity[5]);
00371 EXPECT_FLOAT_EQ(0, msg.velocity[6]);
00372 EXPECT_FLOAT_EQ(0, msg.velocity[7]);
00373 }
00374
00375 TEST_F(JointCommandWristTest, LiveCoeff)
00376 {
00377 JointCommandWrist wristJoint("r2/right_arm/wrist", io);
00378 io.setBoolParam("r2/right_arm/wrist/UseHalls", false);
00379
00380 setInt16("r2/right_arm/wrist/thumbside/IncEnc", calEncoder(0));
00381 setInt16("r2/right_arm/wrist/thumbside/MotCom", 5);
00382 setInt16("r2/right_arm/wrist/littleside/IncEnc", calEncoder(1));
00383 setInt16("r2/right_arm/wrist/littleside/MotCom", 5);
00384 setUint16("r2/right_arm/wrist/pitch/Hall", calAngle(0));
00385 setUint16("r2/right_arm/wrist/yaw/Hall", calAngle(1));
00386
00387 controlMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::UNCALIBRATED;
00388 wristJoint.setCommand(commandMsg, controlMsg);
00389 wristJoint.updateMeasuredState(controlMsg);
00390 wristJoint.getSimpleMeasuredState();
00391 EXPECT_FLOAT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, io.getLiveCoeff(calibrationModeName));
00392
00393 io.setLiveCoeff(calibrationModeName, r2_msgs::JointControlCalibrationMode::ENABLE);
00394 controlMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00395 wristJoint.setCommand(commandMsg, controlMsg);
00396
00397 EXPECT_FLOAT_EQ(r2_msgs::JointControlCalibrationMode::ENABLE, io.getLiveCoeff(calibrationModeName) );
00398
00399 wristJoint.updateMeasuredState(controlMsg);
00400 wristJoint.getSimpleMeasuredState();
00401
00402 EXPECT_FLOAT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, io.getLiveCoeff(calibrationModeName));
00403
00404 }
00405
00406 TEST_F(JointCommandWristTest, Commands)
00407 {
00408 JointCommandWrist wristJoint("r2/right_arm/wrist", io);
00409 io.setBoolParam("r2/right_arm/wrist/UseHalls", false);
00410
00411 r2_msgs::JointCommand outMsg;
00412
00413 commandMsg.header.stamp = ros::Time::now();
00414
00415 commandMsg.name.resize(2);
00416 commandMsg.desiredPosition.resize(2);
00417
00418 std::cout << "testing bootloader" << std::endl;
00419 controlMsg.controlMode.state = r2_msgs::JointControlMode::BOOTLOADER;
00420 controlMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00421
00422 commandMsg.name[0] = "r2/right_arm/wrist/pitch";
00423 commandMsg.name[1] = "r2/right_arm/wrist/yaw";
00424
00425 commandMsg.desiredPosition[0] = 0;
00426 commandMsg.desiredPosition[1] = 0;
00427
00428 wristJoint.setCommand(commandMsg, controlMsg);
00429 wristJoint.updateMeasuredState(controlMsg);
00430 wristJoint.getSimpleMeasuredState();
00431
00432 EXPECT_FLOAT_EQ(0, getInt16("r2/right_arm/wrist/thumbside/MotCom"));
00433 EXPECT_FLOAT_EQ(0, getInt16("r2/right_arm/wrist/littleside/MotCom"));
00434
00435 std::cout << "testing park" << std::endl;
00436 controlMsg.controlMode.state = r2_msgs::JointControlMode::PARK;
00437
00438 commandMsg.name[0] = "r2/right_arm/wrist/pitch";
00439 commandMsg.name[1] = "r2/right_arm/wrist/yaw";
00440
00441 commandMsg.desiredPosition[0] = 0;
00442 commandMsg.desiredPosition[1] = 0;
00443
00444 wristJoint.setCommand(commandMsg, controlMsg);
00445
00446 EXPECT_FLOAT_EQ(0, getInt16("r2/right_arm/wrist/thumbside/MotCom"));
00447 EXPECT_FLOAT_EQ(0, getInt16("r2/right_arm/wrist/littleside/MotCom"));
00448
00449 std::cout << "testing drive" << std::endl;
00450 controlMsg.controlMode.state = r2_msgs::JointControlMode::DRIVE;
00451
00452
00453 std::cout << "testing pitch/yaw commands" << std::endl;
00454 commandMsg.name[0] = "r2/right_arm/wrist/pitch";
00455 commandMsg.name[1] = "r2/right_arm/wrist/yaw";
00456
00457 commandMsg.desiredPosition[0] = .1;
00458 commandMsg.desiredPosition[1] = .1;
00459
00460 wristJoint.setCommand(commandMsg, controlMsg);
00461
00462 EXPECT_NE(0, getInt16("r2/right_arm/wrist/thumbside/MotCom"));
00463 EXPECT_NE(0, getInt16("r2/right_arm/wrist/littleside/MotCom"));
00464
00465 outMsg = wristJoint.getCommandedState();
00466
00467 ASSERT_EQ(2, outMsg.name.size());
00468 ASSERT_EQ(2, outMsg.desiredPosition.size());
00469
00470 EXPECT_STREQ("r2/right_arm/wrist/pitch", outMsg.name[0].c_str());
00471 EXPECT_STREQ("r2/right_arm/wrist/yaw", outMsg.name[1].c_str());
00472
00473 EXPECT_NEAR(.1, outMsg.desiredPosition[0], 0.0005);
00474 EXPECT_NEAR(.1, outMsg.desiredPosition[1], 0.0005);
00475
00476 std::cout << "testing actuator commands" << std::endl;
00477 controlMsg.commandMode.state = r2_msgs::JointControlCommandMode::ACTUATOR;
00478
00479 commandMsg.name[0] = "r2/right_arm/wrist/thumbside";
00480 commandMsg.name[1] = "r2/right_arm/wrist/littleside";
00481
00482 commandMsg.desiredPosition[0] = calSlider(0);
00483 commandMsg.desiredPosition[1] = calSlider(1);
00484
00485 wristJoint.setCommand(commandMsg, controlMsg);
00486
00487 EXPECT_NE(0, getInt16("r2/right_arm/wrist/thumbside/MotCom"));
00488 EXPECT_NE(0, getInt16("r2/right_arm/wrist/littleside/MotCom"));
00489
00490 outMsg = wristJoint.getCommandedState();
00491
00492 ASSERT_EQ(2, outMsg.name.size());
00493 ASSERT_EQ(2, outMsg.desiredPosition.size());
00494
00495 EXPECT_STREQ("r2/right_arm/wrist/pitch", outMsg.name[0].c_str());
00496 EXPECT_STREQ("r2/right_arm/wrist/yaw", outMsg.name[1].c_str());
00497
00498 EXPECT_NEAR(calAngle(0), outMsg.desiredPosition[0], 0.005);
00499 EXPECT_NEAR(calAngle(1), outMsg.desiredPosition[1], 0.005);
00500 }
00501
00502
00503 TEST_F(JointCommandWristTest, getCapability)
00504 {
00505 JointCommandWrist wristJoint("r2/right_arm/wrist", io);
00506
00507 r2_msgs::JointCapability msg = wristJoint.getCapability();
00508
00509 ASSERT_EQ(2, msg.name.size());
00510 ASSERT_EQ(2, msg.positionLimitMax.size());
00511 ASSERT_EQ(2, msg.positionLimitMin.size());
00512
00513 EXPECT_STREQ("r2/right_arm/wrist/pitch", msg.name[0].c_str());
00514 EXPECT_STREQ("r2/right_arm/wrist/yaw", msg.name[1].c_str());
00515 EXPECT_GE(getMotorCoeff("r2/right_arm/wrist/UpperPitchMax"), msg.positionLimitMax[0]);
00516 EXPECT_FLOAT_EQ(getMotorCoeff("r2/right_arm/wrist/LowerPitchMin"), msg.positionLimitMin[0]);
00517 EXPECT_FLOAT_EQ(getMotorCoeff("r2/right_arm/wrist/UpperYawMax"), msg.positionLimitMax[1]);
00518 EXPECT_FLOAT_EQ(getMotorCoeff("r2/right_arm/wrist/LowerYawMin"), msg.positionLimitMin[1]);
00519
00520 }
00521
00523 TEST_F(JointCommandWristTest, setFault)
00524 {
00525
00526 std::string nameIsCalibrated = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "CalibrationMode");
00527 std::string namePitchLimit = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "PitchLimit");
00528 std::string nameYawLimit = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "YawLimit");
00529 std::string nameThumbsideSliderLimit = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "ThumbsideSliderLimit");
00530 std::string nameLittlesideSliderLimit = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "LittlesideSliderLimit");
00531 std::string nameSensorError = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "SensorError");
00532 std::string nameSliderDiffError = StringUtilities::makeFullyQualifiedRoboDynElement(mechanism, "SliderDiffError");
00533
00534 JointCommandWrist wristJoint(mechanism, io);
00535 io.setBoolParam("r2/right_arm/wrist/UseHalls", false);
00536
00537 EXPECT_TRUE(io.hasLiveCoeff(nameIsCalibrated));
00538 EXPECT_TRUE(io.hasLiveCoeff(namePitchLimit));
00539 EXPECT_TRUE(io.hasLiveCoeff(nameYawLimit));
00540 EXPECT_TRUE(io.hasLiveCoeff(nameLittlesideSliderLimit));
00541 EXPECT_TRUE(io.hasLiveCoeff(nameThumbsideSliderLimit));
00542 EXPECT_TRUE(io.hasLiveCoeff(nameSensorError));
00543 EXPECT_TRUE(io.hasLiveCoeff(nameSliderDiffError));
00544
00545 EXPECT_FLOAT_EQ(r2_msgs::JointControlCalibrationMode::UNCALIBRATED, io.getLiveCoeff(nameIsCalibrated));
00546 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(namePitchLimit));
00547 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameYawLimit));
00548 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameLittlesideSliderLimit));
00549 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameThumbsideSliderLimit));
00550 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameSensorError));
00551 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameSliderDiffError));
00552
00553 setInt16("r2/right_arm/wrist/thumbside/IncEnc", calEncoder(0));
00554 setInt16("r2/right_arm/wrist/thumbside/MotCom", 5);
00555 setInt16("r2/right_arm/wrist/littleside/IncEnc", calEncoder(1));
00556 setInt16("r2/right_arm/wrist/littleside/MotCom", 5);
00557 setUint16("r2/right_arm/wrist/pitch/Hall", calAngle(0));
00558 setUint16("r2/right_arm/wrist/yaw/Hall", calAngle(1));
00559
00560 r2_msgs::JointControlData controlMsg;
00561 controlMsg.coeffState.state = r2_msgs::JointControlCoeffState::LOADED;
00562 controlMsg.calibrationMode.state = r2_msgs::JointControlCalibrationMode::ENABLE;
00563
00564 commandMsg.name.resize(2);
00565 commandMsg.desiredPosition.resize(2);
00566
00567 commandMsg.name[0] = "r2/right_arm/wrist/pitch";
00568 commandMsg.name[1] = "r2/right_arm/wrist/yaw";
00569
00570 commandMsg.desiredPosition[0] = 0;
00571 commandMsg.desiredPosition[1] = 0;
00572
00573 wristJoint.setCommand(commandMsg, controlMsg);
00574 wristJoint.updateMeasuredState(controlMsg);
00575
00576 EXPECT_FLOAT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, io.getLiveCoeff(nameIsCalibrated));
00577 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(namePitchLimit));
00578 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameYawLimit));
00579 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameLittlesideSliderLimit));
00580 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameThumbsideSliderLimit));
00581 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameSensorError));
00582 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameSliderDiffError));
00583
00584
00585
00586 setInt16("r2/right_arm/wrist/littleside/IncEnc", 11780);
00587 setInt16("r2/right_arm/wrist/littleside/MotCom", 2);
00588 setInt16("r2/right_arm/wrist/thumbside/IncEnc", 11780);
00589 setInt16("r2/right_arm/wrist/thumbside/MotCom", 4);
00590 setInt16("r2/right_arm/wrist/pitch/Hall", 0);
00591 setInt16("r2/right_arm/wrist/yaw/Hall", 0);
00592
00593 wristJoint.updateMeasuredState(controlMsg);
00594
00595 EXPECT_FLOAT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, io.getLiveCoeff(nameIsCalibrated));
00596 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(namePitchLimit));
00597 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameYawLimit));
00598 EXPECT_FLOAT_EQ(1, io.getLiveCoeff(nameLittlesideSliderLimit));
00599 EXPECT_FLOAT_EQ(1, io.getLiveCoeff(nameThumbsideSliderLimit));
00600 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameSensorError));
00601 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameSliderDiffError));
00602
00603
00604 setInt16("r2/right_arm/wrist/littleside/IncEnc", 0);
00605 setInt16("r2/right_arm/wrist/littleside/MotCom", 2);
00606 setInt16("r2/right_arm/wrist/thumbside/IncEnc", 11780);
00607 setInt16("r2/right_arm/wrist/thumbside/MotCom", 4);
00608 setInt16("r2/right_arm/wrist/pitch/Hall", 0);
00609 setInt16("r2/right_arm/wrist/yaw/Hall", 0);
00610
00611 wristJoint.updateMeasuredState(controlMsg);
00612
00613 EXPECT_FLOAT_EQ(r2_msgs::JointControlCalibrationMode::DISABLE, io.getLiveCoeff(nameIsCalibrated));
00614 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(namePitchLimit));
00615 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameYawLimit));
00616 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameLittlesideSliderLimit));
00617 EXPECT_FLOAT_EQ(1, io.getLiveCoeff(nameThumbsideSliderLimit));
00618 EXPECT_FLOAT_EQ(0, io.getLiveCoeff(nameSensorError));
00619 EXPECT_FLOAT_EQ(1, io.getLiveCoeff(nameSliderDiffError));
00620 }
00621
00622 int main(int argc, char** argv)
00623 {
00624 testing::InitGoogleTest(&argc, argv);
00625 return RUN_ALL_TESTS();
00626 }