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

Fixed trajectory issue

Browse Source
This commit is contained in:
Simon Rasmussen
2017-07-07 16:39:34 +02:00
parent f35f40b45d
commit 93bf3487dd
1 changed files with 4 additions and 5 deletions
Download Patch File Download Diff File Expand all files Collapse all files

9
src/ros/trajectory_follower.cpp
View File

@@ -69,7 +69,7 @@ TrajectoryFollower::TrajectoryFollower(URCommander &commander, std::string& reve
: running_(false) : running_(false)
, commander_(commander) , commander_(commander)
, reverse_port_(reverse_port) , reverse_port_(reverse_port)
, servoj_time_(0.016/4) , servoj_time_(0.008)
, servoj_lookahead_time_(0.03) , servoj_lookahead_time_(0.03)
, servoj_gain_(300.) , servoj_gain_(300.)
, server_(reverse_port) , server_(reverse_port)
@@ -205,7 +205,6 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
return true; 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
for(size_t j = 0; j < positions.size(); j++) for(size_t j = 0; j < positions.size(); j++)
{ {
positions[j] = interpolate( positions[j] = interpolate(
@@ -221,7 +220,7 @@ bool TrajectoryFollower::execute(std::vector<TrajectoryPoint> &trajectory, std::
if(!execute(positions, true)) if(!execute(positions, true))
return false; return false;
std::this_thread::sleep_for(double_seconds(servoj_time_)); std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
} }
prev = point; prev = point;
@@ -239,8 +238,8 @@ void TrajectoryFollower::stop()
if(!running_) if(!running_)
return; return;
std::array<double, 6> empty; //std::array<double, 6> empty;
execute(empty, false); //execute(empty, false);
server_.disconnectClient(); server_.disconnectClient();
running_ = false; running_ = false;