Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-07-10 03:51:11 +02:00
Code Releases Activity

Major refactor

Browse Source
This commit is contained in:
Simon Rasmussen
2017-04-27 06:40:03 +02:00
parent 46f4e493cf
commit c59bfc78cc
22 changed files with 825 additions and 423 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/ros/trajectory_follower.cpp
+165 -61
View File
@@ -1,83 +1,96 @@
#include <endian.h>
#include "ur_modern_driver/ros/trajectory_follower.h"
TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3)
: running_(false)
, commander_(commander)
, server_(reverse_port)
, program_(buildProgram(version_3))
{
}
static const int32_t MULT_JOINTSTATE_ = 1000000;
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 driverProg():
MULT_jointstate = {{JOINT_STATE_REPLACE}}
MULT_jointstate = {{JOINT_STATE_REPLACE}}
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_IDLE
cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
def set_servo_setpoint(q):
enter_critical
cmd_servo_state = SERVO_RUNNING
cmd_servo_q = q
exit_critical
end
thread servoThread():
state = SERVO_IDLE
while True:
enter_critical
q = cmd_servo_q
do_brake = False
if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
do_brake = True
end
state = cmd_servo_state
cmd_servo_state = SERVO_IDLE
exit_critical
if do_brake:
stopj(1.0)
sync()
elif state == SERVO_RUNNING:
servoj(q, {{SERVO_J_REPLACE}})
else:
sync()
end
end
end
SERVO_IDLE = 0
SERVO_RUNNING = 1
cmd_servo_state = SERVO_IDLE
cmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
def set_servo_setpoint(q):
enter_critical
cmd_servo_state = SERVO_RUNNING
cmd_servo_q = q
exit_critical
end
thread servoThread():
state = SERVO_IDLE
while True:
enter_critical
q = cmd_servo_q
do_brake = False
if (state == SERVO_RUNNING) and (cmd_servo_state == SERVO_IDLE):
do_brake = True
end
state = cmd_servo_state
cmd_servo_state = SERVO_IDLE
exit_critical
if do_brake:
stopj(1.0)
sync()
elif state == SERVO_RUNNING:
servoj(q, {{SERVO_J_REPLACE}})
else:
sync()
end
end
end
socket_open(\"{{SERVER_IP_REPLACE}}\", {{SERVER_PORT_REPLACE}})
thread_servo = run servoThread()
keepalive = 1
while keepalive > 0:
params_mult = socket_read_binary_integer(6+1)
if params_mult[0] > 0:
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]
keepalive = params_mult[7]
set_servo_setpoint(q)
end
params_mult = socket_read_binary_integer(6+1)
if params_mult[0] > 0:
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]
keepalive = params_mult[7]
set_servo_setpoint(q)
end
end
sleep(.1)
socket_close()
kill thread_servo
end
)";
std::string TrajectoryFollower::buildProgram(bool version_3)
TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port, bool version_3)
: running_(false)
, commander_(commander)
, reverse_port_(reverse_port)
, server_(reverse_port)
{
std::string res(POSITION_PROGRAM);
size_t js_idx = POSITION_PROGRAM.find(JOINT_STATE_REPLACE);
size_t sj_idx = POSITION_PROGRAM.find(SERVO_J_REPLACE);
res.replace(res.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
std::ostringstream out;
out << "t=" << std::fixed << std::setprecision(4) << servoj_time_;
if(version_3)
out << ", lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
res.replace(js_idx, JOINT_STATE_REPLACE.length(), std::to_string(MULT_JOINTSTATE_));
res.replace(sj_idx, SERVO_J_REPLACE.length(), out.str());
res.replace(res.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
program_ = res;
}
std::string TrajectoryFollower::buildProgram()
{
std::string res(program_);
std::string IP(server_.getIP());
LOG_INFO("Local IP: %s ", IP.c_str());
res.replace(res.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), "127.0.0.1");
res.replace(res.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port_));
return res;
}
@@ -86,12 +99,31 @@ bool TrajectoryFollower::start()
if(running_)
return true; //not sure
//TODO
std::string prog(""); // buildProg();
if(!commander_.uploadProg(prog))
if(!server_.bind())
{
LOG_ERROR("Failed to bind server");
return false;
}
stream_ = std::move(server_.accept()); //todo: pointer instead?
LOG_INFO("Uploading trajectory program to robot");
std::string prog(buildProgram());
//std::string prog = "socket_open(\"127.0.0.1\", 50001)\n";
if(!commander_.uploadProg(prog))
{
LOG_ERROR("Program upload failed!");
return false;
}
LOG_INFO("Awaiting incomming robot connection");
if(!server_.accept())
{
LOG_ERROR("Failed to accept incomming robot connection");
return false;
}
LOG_INFO("Robot successfully connected");
return (running_ = true);
}
@@ -115,8 +147,18 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
int32_t val = htobe32(static_cast<int32_t>(keep_alive));
append(idx, val);
ssize_t res = stream_.send(buf, sizeof(buf));
return res > 0 && res == sizeof(buf);
size_t written;
return server_.write(buf, sizeof(buf), written);
}
double TrajectoryFollower::interpolate(double t, double T, double p0_pos, double p1_pos, double p0_vel, double p1_vel)
{
using std::pow;
double a = p0_pos;
double b = p0_vel;
double c = (-3 * a + 3 * p1_pos - 2 * T * b - T * p1_vel) / pow(T, 2);
double d = (2 * a - 2 * p1_pos + T * b + T * p1_vel) / pow(T, 3);
return a + b * t + c * pow(t, 2) + d * pow(t, 3);
}
bool TrajectoryFollower::execute(std::array<double, 6> &positions)
@@ -124,6 +166,68 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions)
return execute(positions, true);
}
bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::atomic<bool> &interrupt)
{
if(!running_)
return false;
using namespace std::chrono;
typedef duration<double> double_seconds;
typedef high_resolution_clock Clock;
typedef Clock::time_point Time;
auto const& last = trajectory[trajectory.size()-1];
auto& prev = trajectory[0];
Time t0 = Clock::now();
Time latest = t0;
std::array<double, 6> positions;
for(auto const& point : trajectory)
{
//skip t0
if(&point == &prev)
continue;
auto duration = point.time_from_start - prev.time_from_start;
double d_s = duration_cast<double_seconds>(duration).count();
//interpolation loop
while(!interrupt)
{
latest = Clock::now();
auto elapsed = latest - t0;
if(point.time_from_start <= elapsed || last.time_from_start >= elapsed)
break;
double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
//double prev_seconds
for(size_t j = 0; j < positions.size(); j++)
{
positions[j] = interpolate(
elapsed_s,
d_s,
prev.positions[j],
point.positions[j],
prev.velocities[j],
point.velocities[j]
);
}
if(!execute(positions, true))
return false;
std::this_thread::sleep_for(double_seconds(servoj_time_));
}
prev = point;
}
return true;
}
void TrajectoryFollower::stop()
{
if(!running_)
@@ -132,6 +236,6 @@ void TrajectoryFollower::stop()
std::array<double, 6> empty;
execute(empty, false);
stream_.disconnect();
//server_.disconnect();
running_ = false;
}