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

Trjaectory follower improvements

Browse Source
This commit is contained in:
Simon Rasmussen
2017-05-19 01:29:22 +02:00
parent 502506e7fc
commit 0479c047d3
2 changed files with 27 additions and 2 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
include/ur_modern_driver/ros/trajectory_follower.h
View File

@@ -1,6 +1,7 @@
#pragma once #pragma once
#include <array> #include <array>
#include <vector>
#include <atomic> #include <atomic>
#include <cstddef> #include <cstddef>
#include <cstring> #include <cstring>

28
src/ros/trajectory_follower.cpp
View File

@@ -1,3 +1,4 @@
#include <cmath>
#include <endian.h> #include <endian.h>
#include "ur_modern_driver/ros/trajectory_follower.h" #include "ur_modern_driver/ros/trajectory_follower.h"
@@ -46,7 +47,7 @@ def driverProg():
end end
end end
socket_open(\"{{SERVER_IP_REPLACE}}\", {{SERVER_PORT_REPLACE}}) socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}})
thread_servo = run servoThread() thread_servo = run servoThread()
keepalive = 1 keepalive = 1
@@ -68,6 +69,9 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, int reverse_port,
: running_(false) : running_(false)
, commander_(commander) , commander_(commander)
, reverse_port_(reverse_port) , reverse_port_(reverse_port)
, servoj_time_(0.016/4)
, servoj_lookahead_time_(0.03)
, servoj_gain_(300.)
, server_(reverse_port) , server_(reverse_port)
{ {
std::string res(POSITION_PROGRAM); std::string res(POSITION_PROGRAM);
@@ -132,6 +136,8 @@ bool TrajectoryFollower::execute(std::array<double, 6> &positions, bool keep_ali
if(!running_) if(!running_)
return false; return false;
// LOG_INFO("servoj([%f,%f,%f,%f,%f,%f])", positions[0], positions[1], positions[2], positions[3], positions[4], positions[5]);
last_positions_ = positions; last_positions_ = positions;
uint8_t buf[sizeof(uint32_t)*7]; uint8_t buf[sizeof(uint32_t)*7];
@@ -190,18 +196,26 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
if(&point == &prev) if(&point == &prev)
continue; continue;
if(interrupt)
break;
auto duration = point.time_from_start - prev.time_from_start; auto duration = point.time_from_start - prev.time_from_start;
double d_s = duration_cast<double_seconds>(duration).count(); double d_s = duration_cast<double_seconds>(duration).count();
LOG_INFO("Point duration: %f", d_s);
//interpolation loop //interpolation loop
while(!interrupt) while(!interrupt)
{ {
latest = Clock::now(); latest = Clock::now();
auto elapsed = latest - t0; auto elapsed = latest - t0;
if(point.time_from_start <= elapsed || last.time_from_start >= elapsed) if(point.time_from_start <= elapsed)
break; break;
if(last.time_from_start <= elapsed)
return true;
double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count(); double elapsed_s = duration_cast<double_seconds>(elapsed - prev.time_from_start).count();
//double prev_seconds //double prev_seconds
for(size_t j = 0; j < positions.size(); j++) for(size_t j = 0; j < positions.size(); j++)
@@ -225,6 +239,16 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
prev = point; prev = point;
} }
//In theory it's possible the last position won't be sent by
//the interpolation loop above but rather some position between
//t[N-1] and t[N] where N is the number of trajectory points.
//To make sure this does not happen the last position is sent
//if not interrupted.
if(!interrupt)
{
return execute(last.positions, true);
}
return true; return true;
} }