Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019 #ifndef UR_DRIVER_H_
00020 #define UR_DRIVER_H_
00021
00022 #include <mutex>
00023 #include <condition_variable>
00024 #include "ur_realtime_communication.h"
00025 #include "ur_communication.h"
00026 #include "do_output.h"
00027 #include <vector>
00028 #include <math.h>
00029 #include <string>
00030 #include <sys/types.h>
00031 #include <sys/socket.h>
00032 #include <netinet/in.h>
00033
00034 #include <chrono>
00035
00036
00037 class UrDriver {
00038 private:
00039 double maximum_time_step_;
00040 double minimum_payload_;
00041 double maximum_payload_;
00042 std::vector<std::string> joint_names_;
00043 std::string ip_addr_;
00044 const int MULT_JOINTSTATE_ = 1000000;
00045 const int MULT_TIME_ = 1000000;
00046 const unsigned int REVERSE_PORT_;
00047 int incoming_sockfd_;
00048 int new_sockfd_;
00049 bool reverse_connected_;
00050 double servoj_time_;
00051 bool executing_traj_;
00052 double firmware_version_;
00053 double servoj_lookahead_time_;
00054 double servoj_gain_;
00055 public:
00056 UrRealtimeCommunication* rt_interface_;
00057 UrCommunication* sec_interface_;
00058
00059 UrDriver(std::condition_variable& rt_msg_cond,
00060 std::condition_variable& msg_cond, std::string host,
00061 unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max =
00062 12, double max_time_step = 0.08, double min_payload = 0.,
00063 double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.);
00064 bool start();
00065 void halt();
00066
00067 void setSpeed(double q0, double q1, double q2, double q3, double q4,
00068 double q5, double acc = 100.);
00069
00070 bool doTraj(std::vector<double> inp_timestamps,
00071 std::vector<std::vector<double> > inp_positions,
00072 std::vector<std::vector<double> > inp_velocities);
00073 void servoj(std::vector<double> positions, int keepalive = 1);
00074
00075 void stopTraj();
00076
00077 bool uploadProg();
00078 bool openServo();
00079 void closeServo(std::vector<double> positions);
00080
00081 std::vector<double> interp_cubic(double t, double T,
00082 std::vector<double> p0_pos, std::vector<double> p1_pos,
00083 std::vector<double> p0_vel, std::vector<double> p1_vel);
00084
00085 std::vector<std::string> getJointNames();
00086 void setJointNames(std::vector<std::string> jn);
00087 void setToolVoltage(unsigned int v);
00088 void setFlag(unsigned int n, bool b);
00089 void setDigitalOut(unsigned int n, bool b);
00090 void setAnalogOut(unsigned int n, double f);
00091 bool setPayload(double m);
00092
00093 void setMinPayload(double m);
00094 void setMaxPayload(double m);
00095 void setServojTime(double t);
00096 void setServojLookahead(double t);
00097 void setServojGain(double g);
00098
00099 };
00100
00101 #endif