Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-10 18:10:47 +02:00
Code Releases Activity

Changed servoj_time handling to minimize network transfers. Fixes #7

Browse Source
This commit is contained in:
Thomas Timm Andersen
2015-10-20 10:57:39 +02:00
parent 0c901bd7e3
commit 189ecc71ed
1 changed files with 11 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

32
src/ur_driver.cpp
View File

@@ -160,7 +160,7 @@ void UrDriver::servoj(std::vector<double> positions, int keepalive,
double time) { double time) {
unsigned int bytes_written; unsigned int bytes_written;
int tmp; int tmp;
unsigned char buf[32]; unsigned char buf[28];
if (time < 0.016) { if (time < 0.016) {
time = servoj_time_; time = servoj_time_;
} }
@@ -171,17 +171,12 @@ void UrDriver::servoj(std::vector<double> positions, int keepalive,
buf[i * 4 + 2] = (tmp >> 16) & 0xff; buf[i * 4 + 2] = (tmp >> 16) & 0xff;
buf[i * 4 + 3] = (tmp >> 24) & 0xff; buf[i * 4 + 3] = (tmp >> 24) & 0xff;
} }
tmp = htonl((int) (time * MULT_TIME_)); tmp = htonl((int) keepalive);
buf[6 * 4] = tmp & 0xff; buf[6 * 4] = tmp & 0xff;
buf[6 * 4 + 1] = (tmp >> 8) & 0xff; buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
buf[6 * 4 + 2] = (tmp >> 16) & 0xff; buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
buf[6 * 4 + 3] = (tmp >> 24) & 0xff; buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
tmp = htonl((int) keepalive); bytes_written = write(new_sockfd_, buf, 28);
buf[7 * 4] = tmp & 0xff;
buf[7 * 4 + 1] = (tmp >> 8) & 0xff;
buf[7 * 4 + 2] = (tmp >> 16) & 0xff;
buf[7 * 4 + 3] = (tmp >> 24) & 0xff;
bytes_written = write(new_sockfd_, buf, 32);
} }
void UrDriver::stopTraj() { void UrDriver::stopTraj() {
@@ -197,20 +192,14 @@ void UrDriver::uploadProg() {
sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_); sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
cmd_str += buf; cmd_str += buf;
sprintf(buf, "\tMULT_time = %i\n", MULT_TIME_);
cmd_str += buf;
cmd_str += "\tSERVO_IDLE = 0\n"; cmd_str += "\tSERVO_IDLE = 0\n";
cmd_str += "\tSERVO_RUNNING = 1\n"; cmd_str += "\tSERVO_RUNNING = 1\n";
cmd_str += "\tcmd_servo_state = SERVO_IDLE\n"; cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
cmd_str += "\tcmd_servo_id = 0 # 0 = idle, -1 = stop\n";
cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n"; cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
cmd_str += "\tcmd_servo_dt = 0.0\n"; cmd_str += "\tdef set_servo_setpoint(q):\n";
cmd_str += "\tdef set_servo_setpoint(q, dt):\n";
cmd_str += "\t\tenter_critical\n"; cmd_str += "\t\tenter_critical\n";
cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n"; cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
cmd_str += "\t\tcmd_servo_q = q\n"; cmd_str += "\t\tcmd_servo_q = q\n";
cmd_str += "\t\tcmd_servo_dt = dt\n";
cmd_str += "\t\texit_critical\n"; cmd_str += "\t\texit_critical\n";
cmd_str += "\tend\n"; cmd_str += "\tend\n";
cmd_str += "\tthread servoThread():\n"; cmd_str += "\tthread servoThread():\n";
@@ -218,7 +207,6 @@ void UrDriver::uploadProg() {
cmd_str += "\t\twhile True:\n"; cmd_str += "\t\twhile True:\n";
cmd_str += "\t\t\tenter_critical\n"; cmd_str += "\t\t\tenter_critical\n";
cmd_str += "\t\t\tq = cmd_servo_q\n"; cmd_str += "\t\t\tq = cmd_servo_q\n";
cmd_str += "\t\t\tdt = cmd_servo_dt\n";
cmd_str += "\t\t\tdo_brake = False\n"; cmd_str += "\t\t\tdo_brake = False\n";
cmd_str += "\t\t\tif (state == SERVO_RUNNING) and "; cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
cmd_str += "(cmd_servo_state == SERVO_IDLE):\n"; cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
@@ -231,7 +219,10 @@ void UrDriver::uploadProg() {
cmd_str += "\t\t\t\tstopj(1.0)\n"; cmd_str += "\t\t\t\tstopj(1.0)\n";
cmd_str += "\t\t\t\tsync()\n"; cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\telif state == SERVO_RUNNING:\n"; cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
cmd_str += "\t\t\t\tservoj(q, t=dt)\n";
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
cmd_str += buf;
cmd_str += "\t\t\telse:\n"; cmd_str += "\t\t\telse:\n";
cmd_str += "\t\t\t\tsync()\n"; cmd_str += "\t\t\t\tsync()\n";
cmd_str += "\t\t\tend\n"; cmd_str += "\t\t\tend\n";
@@ -245,7 +236,7 @@ void UrDriver::uploadProg() {
cmd_str += "\tthread_servo = run servoThread()\n"; cmd_str += "\tthread_servo = run servoThread()\n";
cmd_str += "\tkeepalive = 1\n"; cmd_str += "\tkeepalive = 1\n";
cmd_str += "\twhile keepalive > 0:\n"; cmd_str += "\twhile keepalive > 0:\n";
cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1+1)\n"; cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
cmd_str += "\t\tif params_mult[0] > 0:\n"; cmd_str += "\t\tif params_mult[0] > 0:\n";
cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, "; cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
cmd_str += "params_mult[2] / MULT_jointstate, "; cmd_str += "params_mult[2] / MULT_jointstate, ";
@@ -253,9 +244,8 @@ void UrDriver::uploadProg() {
cmd_str += "params_mult[4] / MULT_jointstate, "; cmd_str += "params_mult[4] / MULT_jointstate, ";
cmd_str += "params_mult[5] / MULT_jointstate, "; cmd_str += "params_mult[5] / MULT_jointstate, ";
cmd_str += "params_mult[6] / MULT_jointstate]\n"; cmd_str += "params_mult[6] / MULT_jointstate]\n";
cmd_str += "\t\t\tt = params_mult[7] / MULT_time\n"; cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
cmd_str += "\t\t\tkeepalive = params_mult[8]\n"; cmd_str += "\t\t\tset_servo_setpoint(q)\n";
cmd_str += "\t\t\tset_servo_setpoint(q, t)\n";
cmd_str += "\t\tend\n"; cmd_str += "\t\tend\n";
cmd_str += "\tend\n"; cmd_str += "\tend\n";
cmd_str += "\tsleep(.1)\n"; cmd_str += "\tsleep(.1)\n";