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)
00042 {
00043         
00044         m_DOF = DOF;
00045         
00046         m_pRefVals = NULL;
00047         
00048         isMoving = false;
00049 
00050         //TODO: make configurable
00051         SetPTPvel(0.7);
00052         SetPTPacc(0.2);
00053 
00054         //m_P = 2.5;
00055         m_P = 4.0;
00056         m_Vorsteuer = 0.9;
00057         m_AllowedError = 0.75;//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 }
00135         
00136 bool genericArmCtrl::moveTrajectory(trajectory_msgs::JointTrajectory pfad, std::vector<double> conf_current)
00137 {
00138         /* Prüfen, ob erster Punkt nah genug an momentaner Position ist: */
00139         if (pfad.points.size() == 2)
00140         {
00141                 return moveThetas(pfad.points[1].positions, conf_current);
00142         }
00143         for(unsigned int i = 0; i < pfad.points.front().positions.size(); i++ )
00144         {
00145                 if((pfad.points.front().positions.at(i) - conf_current.at(i)) > 0.15)
00146                 {
00147                         ROS_ERROR("Cannot start trajectory motion, manipulator not in trajectory start position.");
00148                         return false;
00149                 }
00150         }
00151         
00152         /* Prüfen ob Arm noch in Bewegung & Prüfen ob alte Sollwerte gelöscht werden müssen: */
00153         STD_CHECK_PROCEDURE()
00154         
00155         /* Sollwerte generieren: */
00156         double vel = m_vel_js.at(0);
00157         for     (unsigned int i = 0; i<m_vel_js.size(); i++)
00158         {
00159                 if(m_vel_js.at(i) < vel)
00160                         vel = m_vel_js.at(i);
00161         }
00162         double acc = m_acc_js.at(0);
00163         for     (unsigned int i = 0; i<m_acc_js.size(); i++)
00164         {
00165                 if(m_acc_js.at(i) < acc)
00166                         acc = m_acc_js.at(i);
00167         }
00168         m_pRefVals = new RefValJS_PTP_Trajectory(pfad, vel, acc, true);
00169         
00170         /* Regeljob starten: */
00171         startTime_.SetNow();
00172         isMoving = true;
00173         TotalTime_ = m_pRefVals->getTotalTime();
00174         ROS_INFO("Starting control of trajectory: %f s long", TotalTime_);
00175 
00176         //START_CTRL_JOB(Trajectory)
00177         return true;
00178 }
00179 
00180 //bool genericArmCtrl::movePos(AbsPos position)
00181 //{
00182 //      /* Prüfen ob Arm noch in Bewegung & Prüfen ob alte Sollwerte gelöscht werden müssen: */
00183 //      STD_CHECK_PROCEDURE()
00184 //
00185 //      Jointd start = getCurrentAngles();
00186 //
00187 //      // attempt to move along straight line:
00188 //      RefValJS_CartesianStraight* rv = new RefValJS_CartesianStraight(m_pKin, start);
00189 //      m_pRefVals = rv;
00190 //      if ( rv->calculate(position, m_vel_cartesian, m_acc_cartesian) == false )
00191 //      {
00192 //              m_ErrorMsg.assign("MovePos: Target Position not reachable.");
00193 //              #ifdef GENERIC_ARM_CTRL_LOG
00194 //              m_log << m_ErrorMsg << "\n";
00195 //              #endif
00196 //              return false;
00197 //      }
00198 //
00199 //      /* Regeljob starten: */
00200 //      START_CTRL_JOB(Cartesian)
00201 //}
00202 
00203 bool genericArmCtrl::step(std::vector<double> current_pos, std::vector<double> & desired_vel)
00204 {
00205         if(isMoving)
00206         {
00207                 TimeStamp timeNow_;
00208                 timeNow_.SetNow();
00209 
00210                 if ( m_pRefVals == NULL )
00211                                 return false;
00212                 double t = timeNow_ - startTime_;
00213                 std::vector<double> m_qsoll = m_pRefVals->r_t( t );
00214                 std::vector<double> m_vsoll = m_pRefVals->dr_dt(t);
00215 
00216                 double len = 0;
00217                 for(unsigned int i = 0; i < m_DOF; i++)
00218                 {
00219                          len +=  (m_qsoll.at(i) - current_pos.at(i)) * (m_qsoll.at(i) - current_pos.at(i));
00220                 }
00221                 m_CurrentError = sqrt(len);
00222                 if ( m_pRefVals != NULL && t < TotalTime_ + m_ExtraTime && (m_CurrentError > m_TargetError || t < TotalTime_) )
00223                 {
00224                                 if ( m_CurrentError >= m_AllowedError )
00225                                 {
00226                                         ROS_ERROR("Current control error exceeds limit: %f >= %f", m_CurrentError, m_AllowedError);
00227                                         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));
00228                                         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));
00229                                         isMoving = false;
00230                                         return false;
00231                                 }
00232                                 desired_vel.resize(m_DOF);
00233                                 /* Vorsteuerung + P-Lageregler: */
00234                                 for(int i = 0; i < m_DOF; i++)
00235                                 {
00236                                         desired_vel.at(i) = m_vsoll.at(i) * m_Vorsteuer + ( m_qsoll.at(i) - current_pos.at(i) ) * m_P;
00237                                 }
00238 
00239                                 return true;
00240                 }
00241                 else /* entweder sind keine Sollwerte vorhanden, oder Zeit ist abgelaufen */
00242                 {
00243                         ROS_INFO("Probably finished trajectory");
00244                         isMoving = false;
00245                         desired_vel.resize(m_DOF);
00246                         for(unsigned int i = 0; i < m_DOF; i++)
00247                         {
00248                                 desired_vel.at(i) = 0.0;
00249                         }
00250                         return true;
00251                 }
00252         }
00253         return false;
00254 }
00255 
00256 
00257 
00258