Go to the documentation of this file.00001 #include <youbot_driver/testing/YouBotArmTest.hpp>
00002 #include <stdlib.h>
00003 #include <stdexcept>
00004
00005 using namespace youbot;
00006
00007 YouBotArmTest::YouBotArmTest() :
00008 dof(5)
00009 {
00010 char* location = getenv("YOUBOT_CONFIG_FOLDER_LOCATION");
00011 if (location == NULL)
00012 throw std::runtime_error("YouBotArmTest.cpp: Could not find environment variable YOUBOT_CONFIG_FOLDER_LOCATION");
00013
00014 EthercatMaster::getInstance("youbot-ethercat.cfg", location, true);
00015 }
00016
00017 YouBotArmTest::~YouBotArmTest()
00018 {
00019 }
00020
00021 void YouBotArmTest::setUp()
00022 {
00023 Logger::logginLevel = trace;
00024 updateCycle = 2000;
00025
00026 }
00027
00028 void YouBotArmTest::tearDown()
00029 {
00030
00031 }
00032
00033 void YouBotArmTest::youBotArmTest()
00034 {
00035 char* configLocation = getenv("YOUBOT_CONFIG_FOLDER_LOCATION");
00036 if (configLocation == NULL)
00037 throw std::runtime_error("YouBotArmTest.cpp: Could not find environment variable YOUBOT_CONFIG_FOLDER_LOCATION");
00038
00039 LOG(info) << __func__ << "\n";
00040 YouBotManipulator myArm("youbot-manipulator", configLocation);
00041 myArm.doJointCommutation();
00042 myArm.calibrateManipulator();
00043
00044 std::stringstream jointNameStream;
00045 boost::ptr_vector<DataTrace> myTrace;
00046 std::vector<JointAngleSetpoint> upstretchedpose;
00047 std::vector<JointAngleSetpoint> foldedpose;
00048 JointAngleSetpoint desiredJointAngle;
00049
00050 desiredJointAngle.angle = 2.56244 * radian;
00051 upstretchedpose.push_back(desiredJointAngle);
00052 desiredJointAngle.angle = 1.04883 * radian;
00053 upstretchedpose.push_back(desiredJointAngle);
00054 desiredJointAngle.angle = -2.43523 * radian;
00055 upstretchedpose.push_back(desiredJointAngle);
00056 desiredJointAngle.angle = 1.73184 * radian;
00057 upstretchedpose.push_back(desiredJointAngle);
00058 desiredJointAngle.angle = 1.5 * radian;
00059 upstretchedpose.push_back(desiredJointAngle);
00060
00061 desiredJointAngle.angle = 0.11 * radian;
00062 foldedpose.push_back(desiredJointAngle);
00063 desiredJointAngle.angle = 0.11 * radian;
00064 foldedpose.push_back(desiredJointAngle);
00065 desiredJointAngle.angle = -0.11 * radian;
00066 foldedpose.push_back(desiredJointAngle);
00067 desiredJointAngle.angle = 0.11 * radian;
00068 foldedpose.push_back(desiredJointAngle);
00069 desiredJointAngle.angle = 0.2 * radian;
00070 foldedpose.push_back(desiredJointAngle);
00071
00072 for (int i = 1; i <= dof; i++)
00073 {
00074 jointNameStream << "Joint_" << i << "_" << __func__;
00075 myTrace.push_back(new DataTrace(myArm.getArmJoint(i), jointNameStream.str(), true));
00076 jointNameStream.str("");
00077 }
00078
00079 for (int i = 0; i < dof; i++)
00080 {
00081 myTrace[i].startTrace();
00082 }
00083
00084
00085 startTime = myTrace[0].getTimeDurationMilliSec();
00086 overallTime = startTime + 1000;
00087 while (myTrace[0].getTimeDurationMilliSec() < overallTime)
00088 {
00089 for (int i = 0; i < dof; i++)
00090 {
00091 myTrace[i].updateTrace();
00092 }
00093 SLEEP_MICROSEC(updateCycle);
00094 }
00095
00096
00097 startTime = myTrace[0].getTimeDurationMilliSec();
00098 overallTime = startTime + 5000;
00099 myArm.setJointData(upstretchedpose);
00100 while (myTrace[0].getTimeDurationMilliSec() < overallTime)
00101 {
00102 for (int i = 0; i < dof; i++)
00103 {
00104 myTrace[i].updateTrace();
00105 }
00106 SLEEP_MICROSEC(updateCycle);
00107 }
00108
00109
00110 startTime = myTrace[0].getTimeDurationMilliSec();
00111 overallTime = startTime + 5000;
00112 myArm.setJointData(foldedpose);
00113 while (myTrace[0].getTimeDurationMilliSec() < overallTime)
00114 {
00115 for (int i = 0; i < dof; i++)
00116 {
00117 myTrace[i].updateTrace();
00118 }
00119 SLEEP_MICROSEC(updateCycle);
00120 }
00121
00122
00123 startTime = myTrace[0].getTimeDurationMilliSec();
00124 overallTime = startTime + 1000;
00125 while (myTrace[0].getTimeDurationMilliSec() < overallTime)
00126 {
00127 for (int i = 0; i < dof; i++)
00128 {
00129 myTrace[i].updateTrace();
00130 }
00131 SLEEP_MICROSEC(updateCycle);
00132 }
00133
00134 for (int i = 0; i < dof; i++)
00135 {
00136 myTrace[i].stopTrace();
00137 myTrace[i].plotTrace();
00138 }
00139
00140 }