cob_trajectory_controller: cob_trajectory_controller: genericArmCtrl.cpp Source File

00001 #include <cob_trajectory_controller/genericArmCtrl.h>
00002 #include <fstream>
00003 #include <cob_trajectory_controller/RefValJS_PTP_Trajectory.h>
00004 #include <cob_trajectory_controller/RefValJS_PTP.h>
00005 
00006 
00007 /********************************************************************
00008  *          Some macros for error checking and handling             *
00009  ********************************************************************/
00010 
00011 
00012 
00013 #define STD_CHECK_PROCEDURE()                                                                                   \
00014 if ( isMoving )                                                                                                         \
00015 {                                                                                                                                               \
00016         ROS_WARN("Manipulator still in motion, call stop first!");              \
00017         return false;                                                                                                           \
00018 }                                                                                                                                               \
00019 if (m_pRefVals != NULL) {                                                                                               \
00020         delete m_pRefVals;                                                                                                      \
00021         m_pRefVals = NULL;                                                                                                      \
00022 }
00023 
00024 
00025 #define START_CTRL_JOB(info)                                                                                    \
00026 if ( m_cThread.startJob(m_pRefVals) == false ) {                                                \
00027         m_ErrorMsg.assign("Internal Error: Unable to start movement, other control Job still active."); \
00028         return false;                                                                                                           \
00029 } else {                                                                                                                                \s                                                                                              \
00030         return true;                                                                                                            \
00031 }
00032 
00033 
00034 
00035 
00036 /********************************************************************
00037  *                  Initialisierungsfunktionen                      *
00038  ********************************************************************/
00039 
00040 
00041 genericArmCtrl::genericArmCtrl(int DOF, double PTPvel, double PTPacc, double maxError) 
00042 {
00043         
00044         m_DOF = DOF;
00045         
00046         m_pRefVals = NULL;
00047         
00048         isMoving = false;
00049 
00050         //TODO: make configurable
00051         SetPTPvel(PTPvel);
00052         SetPTPacc(PTPacc);
00053 
00054         //m_P = 2.5;
00055         m_P = 4.0;
00056         m_Vorsteuer = 0.9;
00057         m_AllowedError = maxError;//0.5;//0.25; // rad
00058         m_CurrentError = 0.0; // rad
00059         m_TargetError = 0.02; // rad;
00060 
00061         m_ExtraTime = 3;        // s
00062         
00063 }
00064 
00065 
00066 genericArmCtrl::~genericArmCtrl()
00067 {
00068         if (m_pRefVals != NULL)
00069                 delete m_pRefVals;      
00070 }
00071 
00072 /********************************************************************
00073  *                      Parameter Abfragen                          *
00074  ********************************************************************/
00075 
00076 std::vector<double> genericArmCtrl::GetPTPvel() const
00077         { return m_vel_js; }
00078 
00079 std::vector<double> genericArmCtrl::GetPTPacc() const
00080         { return m_acc_js; }
00081 
00082 
00083 
00084 
00085 /********************************************************************
00086  *                       Parameter Setzen                           *
00087  ********************************************************************/
00088 
00089 void genericArmCtrl::SetPTPvel(double vel)
00090 {
00091         m_vel_js.resize(m_DOF);
00092         for (int i = 0; i < m_DOF; i++)
00093                 m_vel_js.at(i) = vel;
00094 }
00095 
00096 void genericArmCtrl::SetPTPacc(double acc)
00097 {
00098         m_acc_js.resize(m_DOF);
00099         for (int i = 0; i < m_DOF; i++)
00100                 m_acc_js.at(i) = acc;
00101 }
00102 
00103 /********************************************************************
00104  *                   PTP (Joint Space) Motion:                      *
00105  ********************************************************************/
00106 
00108 bool genericArmCtrl::moveThetas(std::vector<double> conf_goal, std::vector<double> conf_current)
00109 {
00110         /* Prüfen ob Arm noch in Bewegung & Prüfen ob alte Sollwerte gelöscht werden müssen: */
00111         STD_CHECK_PROCEDURE()
00112         
00113         /* Sollwerte generieren: */
00114         double vel = 10000;
00115         for     (unsigned int i = 0; i<m_vel_js.size(); i++)
00116         {
00117                 if(m_vel_js.at(i) < vel)
00118                         vel = m_vel_js.at(i);
00119         }
00120         double acc = 10000;
00121         for     (unsigned int i = 0; i<m_acc_js.size(); i++)
00122         {
00123                 if(m_acc_js.at(i) < acc)
00124                         acc = m_acc_js.at(i);
00125         }
00126 
00127                                         ;
00128         m_pRefVals = new RefValJS_PTP(conf_current, conf_goal, vel, acc);
00129         startTime_.SetNow();
00130         isMoving = true;
00131         TotalTime_ = m_pRefVals->getTotalTime();
00132         ROS_INFO("Starting control of trajectory: %f s long", TotalTime_);
00133 
00134         return true; //TODO when to return false?
00135 }
00136         
00137 bool genericArmCtrl::moveTrajectory(trajectory_msgs::JointTrajectory pfad, std::vector<double> conf_current)
00138 {
00139         /* Prüfen, ob erster Punkt nah genug an momentaner Position ist: */
00140         if (pfad.points.size() == 2)
00141         {
00142                 return moveThetas(pfad.points[1].positions, conf_current);
00143         }
00144         for(unsigned int i = 0; i < pfad.points.front().positions.size(); i++ )
00145         {
00146                 if((pfad.points.front().positions.at(i) - conf_current.at(i)) > 0.15)
00147                 {
00148                         ROS_ERROR("Cannot start trajectory motion, manipulator not in trajectory start position.");
00149                         return false;
00150                 }
00151         }
00152         
00153         /* Prüfen ob Arm noch in Bewegung & Prüfen ob alte Sollwerte gelöscht werden müssen: */
00154         STD_CHECK_PROCEDURE()
00155         
00156         /* Sollwerte generieren: */
00157         double vel = m_vel_js.at(0);
00158         for     (unsigned int i = 0; i<m_vel_js.size(); i++)
00159         {
00160                 if(m_vel_js.at(i) < vel)
00161                         vel = m_vel_js.at(i);
00162         }
00163         double acc = m_acc_js.at(0);
00164         for     (unsigned int i = 0; i<m_acc_js.size(); i++)
00165         {
00166                 if(m_acc_js.at(i) < acc)
00167                         acc = m_acc_js.at(i);
00168         }
00169         m_pRefVals = new RefValJS_PTP_Trajectory(pfad, vel, acc, true);
00170         
00171         /* Regeljob starten: */
00172         startTime_.SetNow();
00173         isMoving = true;
00174         TotalTime_ = m_pRefVals->getTotalTime();
00175         ROS_INFO("Starting control of trajectory: %f s long", TotalTime_);
00176 
00177         //START_CTRL_JOB(Trajectory)
00178         return true;
00179 }
00180 
00181 //bool genericArmCtrl::movePos(AbsPos position)
00182 //{
00183 //      /* Prüfen ob Arm noch in Bewegung & Prüfen ob alte Sollwerte gelöscht werden müssen: */
00184 //      STD_CHECK_PROCEDURE()
00185 //
00186 //      Jointd start = getCurrentAngles();
00187 //
00188 //      // attempt to move along straight line:
00189 //      RefValJS_CartesianStraight* rv = new RefValJS_CartesianStraight(m_pKin, start);
00190 //      m_pRefVals = rv;
00191 //      if ( rv->calculate(position, m_vel_cartesian, m_acc_cartesian) == false )
00192 //      {
00193 //              m_ErrorMsg.assign("MovePos: Target Position not reachable.");
00194 //              #ifdef GENERIC_ARM_CTRL_LOG
00195 //              m_log << m_ErrorMsg << "\n";
00196 //              #endif
00197 //              return false;
00198 //      }
00199 //
00200 //      /* Regeljob starten: */
00201 //      START_CTRL_JOB(Cartesian)
00202 //}
00203 
00204 bool genericArmCtrl::step(std::vector<double> current_pos, std::vector<double> & desired_vel)
00205 {
00206         if(isMoving)
00207         {
00208                 TimeStamp timeNow_;
00209                 timeNow_.SetNow();
00210 
00211                 if ( m_pRefVals == NULL )
00212                                 return false;
00213                 double t = timeNow_ - startTime_;
00214                 std::vector<double> m_qsoll = m_pRefVals->r_t( t );
00215                 std::vector<double> m_vsoll = m_pRefVals->dr_dt(t);
00216 
00217                 double len = 0;
00218                 for(int i = 0; i < m_DOF; i++)
00219                 {
00220                          len +=  (m_qsoll.at(i) - current_pos.at(i)) * (m_qsoll.at(i) - current_pos.at(i));
00221                 }
00222                 m_CurrentError = sqrt(len);
00223                 if ( m_pRefVals != NULL && t < TotalTime_ + m_ExtraTime && (m_CurrentError > m_TargetError || t < TotalTime_) )
00224                 {
00225                                 if ( m_CurrentError >= m_AllowedError )
00226                                 {
00227                                         ROS_ERROR("Current control error exceeds limit: %f >= %f", m_CurrentError, m_AllowedError);
00228                                         //TODO: make generic to avoid out of bound exception
00229                                         //ROS_ERROR("Current Soll: %f %f %f %f %f %f %f ", m_qsoll.at(0), m_qsoll.at(1), m_qsoll.at(2), m_qsoll.at(3), m_qsoll.at(4), m_qsoll.at(5), m_qsoll.at(6));
00230                                         //ROS_ERROR("Current Ist: %f %f %f %f %f %f %f ", current_pos.at(0), current_pos.at(1), current_pos.at(2), current_pos.at(3), current_pos.at(4), current_pos.at(5), current_pos.at(6));
00231                                         isMoving = false;
00232                                         return false;
00233                                 }
00234                                 desired_vel.resize(m_DOF);
00235                                 /* Vorsteuerung + P-Lageregler: */
00236                                 for(int i = 0; i < m_DOF; i++)
00237                                 {
00238                                         desired_vel.at(i) = m_vsoll.at(i) * m_Vorsteuer + ( m_qsoll.at(i) - current_pos.at(i) ) * m_P;
00239                                 }
00240 
00241                                 return true;
00242                 }
00243                 else /* entweder sind keine Sollwerte vorhanden, oder Zeit ist abgelaufen */
00244                 {
00245                         ROS_INFO("Probably finished trajectory");
00246                         isMoving = false;
00247                         desired_vel.resize(m_DOF);
00248                         for(int i = 0; i < m_DOF; i++)
00249                         {
00250                                 desired_vel.at(i) = 0.0;
00251                         }
00252                         return true;
00253                 }
00254         }
00255         return false;
00256 }
00257 
00258 
00259 
00260