YouBotArmTest.cpp
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   //    EthercatMaster::destroy();
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   // 1 sec no movement
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   // move to upstretched position
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   // move to folded position
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   // 1 sec no movement
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 }


youbot_driver
Author(s): Jan Paulus
autogenerated on Mon Oct 6 2014 09:08:01