aubo_new_driver.cpp
Go to the documentation of this file.
00001 #include "aubo_new_driver/aubo_new_driver.h"
00002 
00003 
00004 AuboNewDriver::AuboNewDriver(std::condition_variable& rt_msg_cond,
00005                 std::condition_variable& msg_cond, std::string host,
00006         unsigned int reverse_port,double servoj_time):REVERSE_PORT_(reverse_port),servoj_time_(servoj_time)
00007 {
00008 
00009         char buffer[256];
00010         struct sockaddr_in serv_addr;
00011         int n, flag;
00012 
00013         reverse_connected_ = false;
00014         executing_traj_ = false;
00015     rt_interface_ = new AuboRealtimeCommunication(rt_msg_cond, host);
00016 }
00017 
00018 
00019 std::vector<double> AuboNewDriver::interp_cubic(double t, double T,
00020                 std::vector<double> p0_pos, std::vector<double> p1_pos,
00021                 std::vector<double> p0_vel, std::vector<double> p1_vel) {
00022         /*Returns positions of the joints at time 't' */
00023         std::vector<double> positions;
00024         for (unsigned int i = 0; i < p0_pos.size(); i++) {
00025                 double a = p0_pos[i];
00026                 double b = p0_vel[i];
00027                 double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
00028                                 - T * p1_vel[i]) / pow(T, 2);
00029                 double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
00030                                 + T * p1_vel[i]) / pow(T, 3);
00031                 positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
00032         }
00033         return positions;
00034 }
00035 
00036 bool AuboNewDriver::doTraj(std::vector<double> inp_timestamps,
00037                 std::vector<std::vector<double> > inp_positions,
00038                 std::vector<std::vector<double> > inp_velocities) {
00039         std::chrono::high_resolution_clock::time_point t0, t;
00040         std::vector<double> positions;
00041         unsigned int j;
00042 
00043     if (!AuboNewDriver::openServo()) {
00044         return false;
00045     }
00046         
00047         executing_traj_ = true;
00048         t0 = std::chrono::high_resolution_clock::now();
00049         t = t0;
00050         j = 0;
00051         while ((inp_timestamps[inp_timestamps.size() - 1]
00052                         >= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count())
00053                         and executing_traj_) {
00054                 while (inp_timestamps[j]
00055                                 <= std::chrono::duration_cast<std::chrono::duration<double>>(
00056                                                 t - t0).count() && j < inp_timestamps.size() - 1) {
00057                         j += 1;
00058                 }
00059                 positions = AuboNewDriver::interp_cubic(
00060                                 std::chrono::duration_cast<std::chrono::duration<double>>(
00061                                                 t - t0).count() - inp_timestamps[j - 1],
00062                                 inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
00063                                 inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
00064 
00065         AuboNewDriver::servoj(positions);
00066 
00067                 // oversample with 4 * sample_time
00068         std::this_thread::sleep_for(std::chrono::milliseconds((int) ((servoj_time_ * 1000) / 4.)));
00069                 t = std::chrono::high_resolution_clock::now();
00070         }
00071         executing_traj_ = false;
00072 
00073         //Signal robot to stop driverProg()
00074         AuboNewDriver::closeServo(positions);
00075         return true;
00076 }
00077 
00078 void AuboNewDriver::servoj(std::vector<double> positions, int keepalive) {
00079         if (!reverse_connected_) {
00080                 print_error("AuboNewDriver::servoj called without a reverse connection present. Keepalive: "
00081                                                 + std::to_string(keepalive));
00082                 return;
00083         }
00084 
00085     char buf[1024];
00086 
00087     sprintf(buf,"{\"command\":\"servoj\",\"data\":{\"joint1\":%f,\"joint2\":%f,\"joint3\":%f,\"joint4\":%f,\"joint5\":%f,\"joint6\":%f}}\n",positions[0],positions[1],positions[2],positions[3],positions[4],positions[5]);
00088     print_info((std::string)buf);
00089     rt_interface_->addCommandToQueue((std::string)buf);
00090 }
00091 
00092 
00093 void AuboNewDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
00094         double q5, double acc) {
00095     rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
00096 }
00097 
00098 
00099 
00100 
00101 /************************Aubo plan and move API*****************************/
00102 void AuboNewDriver::initMoveProfile() {
00103     rt_interface_->addCommandToQueue("{\"command\":\"initMoveProfile\"}\n");
00104 }
00105 
00106 void AuboNewDriver::setBlock(bool flag) {
00107     char buf[128];
00108     sprintf(buf, "{\"command\":\"enableBlock\",\"data\":{\"value\":%s}}\n",flag ? "true" : "false");
00109     //print_info((std::string)buf);
00110     rt_interface_->addCommandToQueue(buf);
00111 }
00112 
00113 void AuboNewDriver::setMaxSpeed(double speed){
00114 
00115     char cmd[1024];
00116     sprintf(cmd,"{\"command\":\"setSpeed\",\"data\":{\"value\":%f}}\n",speed);
00117     rt_interface_->addCommandToQueue((std::string) (cmd));
00118 }
00119 
00120 void AuboNewDriver::setMaxAcc(double acc){
00121 
00122     char cmd[1024];
00123     sprintf(cmd,"{\"command\":\"setAcce\",\"data\":{\"value\":%f}}\n",acc);
00124     rt_interface_->addCommandToQueue((std::string) (cmd));
00125 }
00126 
00127 void AuboNewDriver::movej(std::vector<double> positions) {
00128 
00129     char buf[1024];
00130     sprintf(buf,"{\"command\":\"movej\",\"data\":{\"joint1\":%f,\"joint2\":%f,\"joint3\":%f,\"joint4\":%f,\"joint5\":%f,\"joint6\":%f}}\n",positions[0],positions[1],positions[2],positions[3],positions[4],positions[5]);
00131     //print_info((std::string)buf);
00132     rt_interface_->addCommandToQueue((std::string)buf);
00133 }
00134 
00135 void AuboNewDriver::movel(std::vector<double> positions) {
00136 
00137     char buf[1024];
00138 
00139     sprintf(buf,"{\"command\":\"movel\",\"data\":{\"joint1\":%f,\"joint2\":%f,\"joint3\":%f,\"joint4\":%f,\"joint5\":%f,\"joint6\":%f}}\n",positions[0],positions[1],positions[2],positions[3],positions[4],positions[5]);
00140     //print_info((std::string)buf);
00141     rt_interface_->addCommandToQueue((std::string)buf);
00142 }
00143 
00144 void AuboNewDriver::movelTo(std::vector<double> positions) {
00145 
00146     char buf[1024];
00147 
00148     sprintf(buf,"{\"command\":\"movelTo\",\"data\":{\"x\":%f,\"y\":%f,\"z\":%f}}\n",positions[0],positions[1],positions[2]);
00149     //print_info((std::string)buf);
00150     rt_interface_->addCommandToQueue((std::string)buf);
00151 }
00152 
00153 void AuboNewDriver::addWayPoint(std::vector<double> positions) {
00154 
00155     char buf[1024];
00156 
00157     sprintf(buf,"{\"command\":\"addWaypoint\",\"data\":{\"joint1\":%f,\"joint2\":%f,\"joint3\":%f,\"joint4\":%f,\"joint5\":%f,\"joint6\":%f}}\n",positions[0],positions[1],positions[2],positions[3],positions[4],positions[5]);
00158     //print_info((std::string)buf);
00159     rt_interface_->addCommandToQueue((std::string)buf);
00160 }
00161 
00162 void AuboNewDriver::movep(double blendRadius,int trackMode){
00163     if (!reverse_connected_) {
00164         print_error("AuboNewDriver::movep called without a reverse connection present.");
00165         return;
00166     }
00167     char cmd[1024];
00168     sprintf(cmd,"{\"command\":\"movep\",\"data\":{\"blend_radius\":%f,\"track_mode\":%d}}\n",blendRadius,trackMode);
00169     rt_interface_->addCommandToQueue((std::string) (cmd));
00170 }
00171 
00172 void AuboNewDriver::getRobotPos() {
00173     rt_interface_->addCommandToQueue("{\"command\":\"getRobotPos\"}\n");
00174 }
00175 /************************Aubo plan and move API end *****************************/
00176 
00177 
00178 
00179 bool AuboNewDriver::openServo() {
00180     reverse_connected_ = true;
00181     return true;
00182 }
00183 
00184 void AuboNewDriver::closeServo(std::vector<double> positions) {
00185         if (positions.size() != 6)
00186         AuboNewDriver::servoj(rt_interface_->robot_state_->getJonitPosition(), 0);
00187         else
00188                 AuboNewDriver::servoj(positions, 0);
00189 
00190         reverse_connected_ = false;
00191 }
00192 
00193 bool AuboNewDriver::start() {
00194         if (!rt_interface_->start())
00195                 return false;
00196         ip_addr_ = rt_interface_->getLocalIp();
00197         print_debug("Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_)
00198                                         + "\n");
00199         return true;
00200 
00201 }
00202 
00203 void AuboNewDriver::halt() {
00204         if (executing_traj_) {
00205                 AuboNewDriver::stopTraj();
00206         }
00207         rt_interface_->halt();
00208 }
00209 
00210 void AuboNewDriver::stopTraj() {
00211         executing_traj_ = false;
00212     rt_interface_->addCommandToQueue("{\"command\":\"robotStop\"}\n");
00213 }
00214 
00215 std::vector<std::string> AuboNewDriver::getJointNames() {
00216         return joint_names_;
00217 }
00218 
00219 void AuboNewDriver::setJointNames(std::vector<std::string> jn) {
00220         joint_names_ = jn;
00221 }
00222 
00223 
00224 void AuboNewDriver::setRobotIO(int type, int mode,int index,float state) {
00225     char buf[256];
00226     sprintf(buf, "set_digital_out(%d, %d, %d, %f)\n", type, mode, index, state);
00227 
00228     //not implement yet
00229     //rt_interface_->addCommandToQueue(buf);
00230     //print_debug(buf);
00231 }
00232 
00233 
00234 void AuboNewDriver::setServojTime(double t) {
00235     if (t > 0.020) {
00236         servoj_time_ = t;
00237     } else {
00238         servoj_time_ = 0.020;
00239     }
00240 }
00241 
00242 
00243 


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