aubo_new_driver.h
Go to the documentation of this file.
00001 #ifndef AUBO_NEW_DRIVER_H_
00002 #define AUBO_NEW_DRIVER_H_
00003 
00004 #include <mutex>
00005 #include <string>
00006 #include <vector>
00007 #include <math.h>
00008 #include <chrono>
00009 
00010 #include "aubo_realtime_communication.h"
00011 #include "do_output.h"
00012 
00013 
00014 
00015 
00016 class AuboNewDriver {
00017 private:
00018         std::vector<std::string> joint_names_;
00019         std::string ip_addr_;
00020         bool reverse_connected_;
00021 
00022         int incoming_sockfd_;
00023         int new_sockfd_;
00024         const unsigned int REVERSE_PORT_;
00025         const int MULT_JOINTSTATE_ = 1000000;
00026         const int MULT_TIME_ = 1000000;
00027 
00028         double servoj_time_;
00029         bool executing_traj_;
00030 
00031         
00032 public:
00033         AuboRealtimeCommunication* rt_interface_;
00034         AuboNewDriver(std::condition_variable& rt_msg_cond,
00035                         std::condition_variable& msg_cond, 
00036                         std::string host,unsigned int reverse_port = 8899,double servoj_time = 0.020);
00037 
00038         bool start();
00039         void halt();
00040 
00041         bool doTraj(std::vector<double> inp_timestamps,
00042                         std::vector<std::vector<double> > inp_positions,
00043                         std::vector<std::vector<double> > inp_velocities);
00044 
00045 
00046         void setSpeed(double q0, double q1, double q2, double q3, double q4,
00047         double q5, double acc);
00048 
00049         void servoj(std::vector<double> positions, int keepalive = 1);
00050 
00051 
00052         void getRobotPos();
00053 
00054         void stopTraj();
00055 
00056         bool openServo();
00057         void closeServo(std::vector<double> positions);
00058 
00059         std::vector<double> interp_cubic(double t, double T,
00060                         std::vector<double> p0_pos, std::vector<double> p1_pos,
00061                         std::vector<double> p0_vel, std::vector<double> p1_vel);
00062 
00063         std::vector<std::string> getJointNames();
00064         void setJointNames(std::vector<std::string> jn);
00065 
00066         void setRobotIO(int type, int mode,int index,float state);
00067 
00068         void setServojTime(double t);
00069 
00070 
00071         /************************Aubo plan and move API*****************************/
00072         void initMoveProfile();
00073         void setBlock(bool flag);
00074         void setMaxSpeed(double speed);
00075         void setMaxAcc(double acc);
00076         void movej(std::vector<double> positions);
00077         void movel(std::vector<double> positions);
00078         void movelTo(std::vector<double> positions);
00079         void addWayPoint(std::vector<double> positions);
00080         void movep(double blendRadius,int trackMode);
00081         /************************Aubo plan and move API*****************************/  
00082 };
00083 
00084 #endif /* AUBO_NEW_DRIVER_H_ */


aubo_new_driver
Author(s): liuxin
autogenerated on Sat Jun 8 2019 19:06:02