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

Load the script from an external file

Browse Source
This commit is contained in:
Felix Mauch
2019-05-20 12:28:35 +02:00
parent ec31db35b2
commit 89ee1aae8a
7 changed files with 110 additions and 87 deletions
Download Patch File Download Diff File Expand all files Collapse all files

89
ur_rtde_driver/src/ur/ur_driver.cpp
View File

@@ -41,85 +41,8 @@ static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
static const std::string POSITION_PROGRAM = R"(
def myProg():
global steptime = get_steptime()
textmsg("steptime=", steptime)
MULT_jointstate = {{JOINT_STATE_REPLACE}}
SERVO_UNINITIALIZED = -1
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_UNINITIALIZED
cmd_servo_q = get_actual_joint_positions()
cmd_servo_q_last = get_actual_joint_positions()
def set_servo_setpoint(q):
enter_critical
cmd_servo_state = SERVO_RUNNING
cmd_servo_q_last = cmd_servo_q
cmd_servo_q = q
exit_critical
end
def extrapolate():
enter_critical
diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]]
cmd_servo_q_last = cmd_servo_q
cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]]
exit_critical
return cmd_servo_q
end
thread servoThread():
state = SERVO_IDLE
while True:
enter_critical
q = cmd_servo_q
do_extrapolate = False
if (cmd_servo_state == SERVO_IDLE):
do_extrapolate = True
end
state = cmd_servo_state
cmd_servo_state = SERVO_IDLE
exit_critical
if do_extrapolate:
textmsg("No new setpoint received. Extrapolating.")
q = extrapolate()
servoj(q, t=steptime, {{SERVO_J_REPLACE}})
elif state == SERVO_RUNNING:
servoj(q, t=steptime, {{SERVO_J_REPLACE}})
else:
textmsg("Should not be here")
sync()
end
end
stopj(0.1)
end
socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
thread_servo = run servoThread()
keepalive = -2
params_mult = socket_read_binary_integer(6+1, "reverse_socket")
keepalive = params_mult[7]
while keepalive > 0:
params_mult = socket_read_binary_integer(6+1, "reverse_socket", steptime)
if params_mult[0] > 0:
keepalive = params_mult[7]
q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate]
set_servo_setpoint(q)
else:
# TODO: Extrapolation goes here
keepalive = keepalive - 1
end
end
sleep(.1)
socket_close()
kill thread_servo
end
)";
ur_driver::UrDriver::UrDriver(const std::string& robot_ip)
ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file)
: servoj_time_(0.008), servoj_gain_(750), servoj_lookahead_time_(0.03)
{
LOG_INFO("Initializing RTDE client");
@@ -141,7 +64,7 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip)
uint32_t reverse_port = 50001; // TODO: Make this a parameter
std::string prog = POSITION_PROGRAM;
std::string prog = readScriptFile(script_file);
prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
std::ostringstream out;
out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
@@ -201,4 +124,12 @@ bool UrDriver::writeJointCommand(const vector6d_t& values)
return true;
}
std::string UrDriver::readScriptFile(const std::string& filename)
{
std::ifstream ifs(filename);
std::string content((std::istreambuf_iterator<char>(ifs)), (std::istreambuf_iterator<char>()));
return content;
}
} // namespace ur_driver