From 273236f7c1dff2d5dd6fbd3f9caf963fe209eb4b Mon Sep 17 00:00:00 2001
From: Thomas Timm Andersen <thomas.timm.dk@gmail.com>
Date: Tue, 15 Sep 2015 17:12:52 +0200
Subject: [PATCH] Set speed on all joints to 0 before closing RT socket. Set
 short timeout on non-RT socket

---
 src/ur_communication.cpp          | 2 ++
 src/ur_realtime_communication.cpp | 2 ++
 2 files changed, 4 insertions(+)

diff --git a/src/ur_communication.cpp b/src/ur_communication.cpp
index e007392..52e9cff 100644
--- a/src/ur_communication.cpp
+++ b/src/ur_communication.cpp
@@ -104,6 +104,8 @@ void UrCommunication::run() {
 		command_string_lock_.unlock();
 
 	}
+	//wait for some traffic so the UR socket doesn't die in version 3.1.
+	std::this_thread::sleep_for(std::chrono::milliseconds(500));
 	close(sockfd_);
 }
 
diff --git a/src/ur_realtime_communication.cpp b/src/ur_realtime_communication.cpp
index 9692b75..99b1348 100644
--- a/src/ur_realtime_communication.cpp
+++ b/src/ur_realtime_communication.cpp
@@ -99,6 +99,8 @@ void UrRealtimeCommunication::run() {
 		command_string_lock_.unlock();
 
 	}
+	setSpeed(0., 0., 0., 0., 0., 0.);
+	write(sockfd_, command_.c_str(), command_.length());
 	close(sockfd_);
 }