00001 #ifndef __GENERIC_ARM_CTRL_V4_H_ 00002 #define __GENERIC_ARM_CTRL_V4_H_ 00003 00004 #include "ros/ros.h" 00005 //#include "Manipulation/ManipUtil/Joint.h" 00006 //#include "ManipUtil/datastructsManipulator.h" 00007 //#include "ManipUtil/ManipulatorXML.h" 00008 //#include "Interfaces/KinematicsInterface.h" 00009 //#include "Interfaces/armInterface_ctrl.h" 00010 00011 #include <trajectory_msgs/JointTrajectory.h> 00012 00013 #include <cob_trajectory_controller/RefVal_JS.h> 00014 #include <cob_trajectory_controller/TimeStamp.h> 00015 00016 class genericArmCtrl 00017 { 00018 public: 00019 00020 genericArmCtrl(int DOF, double PTPvel = 0.7, double PTPacc = 0.2, double maxError = 0.7); 00021 ~genericArmCtrl(); 00022 00023 std::vector<double> GetPTPvel() const; 00024 std::vector<double> GetPTPacc() const; 00025 void SetPTPvel(double vel); 00026 void SetPTPacc(double acc); 00027 00028 double overlap_time; 00029 00030 // void stop(); --> TODO: better reset 00031 00032 bool step(std::vector<double> current_pos, std::vector<double> & desired_vel); 00033 00034 bool moveThetas(std::vector<double> conf_goal, std::vector<double> conf_current); 00035 bool moveTrajectory(trajectory_msgs::JointTrajectory pfad, std::vector<double> conf_current); 00036 00037 00038 // bool movePos(AbsPos position); 00039 00040 int m_DOF; 00041 00042 RefVal_JS* m_pRefVals; 00043 00044 std::vector<double> m_vel_js; 00045 std::vector<double> last_q; 00046 std::vector<double> last_q1; 00047 std::vector<double> last_q2; 00048 std::vector<double> last_q3; 00049 std::vector<double> m_acc_js; 00050 bool isMoving; 00051 00052 TimeStamp startTime_; 00053 double TotalTime_; 00054 double m_P; 00055 double m_Vorsteuer; 00056 double m_AllowedError; 00057 double m_CurrentError; 00058 double m_TargetError; 00059 double m_ExtraTime; // Zusätzliche Zeit, um evtl. verbleibende Regelfehler auszuregeln 00060 }; 00061 00062 00063 00064 #endif