Changed the servoj loop time for better stability

include/ur_modern_driver/ur_driver.h

@@ -39,13 +39,14 @@ private:
 	const unsigned int REVERSE_PORT_;
 	int incoming_sockfd_;
 	int new_sockfd_;
+	double servoj_time_;
 public:
 	UrRealtimeCommunication* rt_interface_;
 	UrCommunication* sec_interface_;
 
 	UrDriver(std::condition_variable& rt_msg_cond,
 			std::condition_variable& msg_cond, std::string host,
-			unsigned int reverse_port = 50007, unsigned int safety_count_max =
+			unsigned int reverse_port = 50007, double servoj_time = 0.016, unsigned int safety_count_max =
 					12, double max_time_step = 0.08, double min_payload = 0.,
 			double max_payload = 1.);
 	bool start();
@@ -53,14 +54,14 @@ public:
 
 	void setSpeed(double q0, double q1, double q2, double q3, double q4,
 			double q5, double acc = 100.);
-	void addTraj(
+	/* void addTraj(
 			std::vector<double> inp_timestamps, //DEPRECATED
 			std::vector<std::vector<double> > positions,
-			std::vector<std::vector<double> > velocities);
+			std::vector<std::vector<double> > velocities); */
 	void doTraj(std::vector<double> inp_timestamps,
 			std::vector<std::vector<double> > inp_positions,
 			std::vector<std::vector<double> > inp_velocities);
-	void servoj(std::vector<double> positions, double time, int keepalive);
+	void servoj(std::vector<double> positions, int keepalive = 1, double time = 0);
 
 	void stopTraj();
 
@@ -82,6 +83,7 @@ public:
 
 	void setMinPayload(double m);
 	void setMaxPayload(double m);
+	void setServojTime(double t);
 
 };
 

src/do_output.cpp

@@ -13,35 +13,35 @@
 
 void print_debug(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_DEBUG(inp.c_str());
+	ROS_DEBUG("%s", inp.c_str());
 #else
 	printf("DEBUG: %s\n", inp.c_str());
 #endif
 }
 void print_info(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_INFO(inp.c_str());
+	ROS_INFO("%s", inp.c_str());
 #else
 	printf("INFO: %s\n", inp.c_str());
 #endif
 }
 void print_warning(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_WARN(inp.c_str());
+	ROS_WARN("%s", inp.c_str());
 #else
 	printf("WARNING: %s\n", inp.c_str());
 #endif
 }
 void print_error(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_ERROR(inp.c_str());
+	ROS_ERROR("%s", inp.c_str());
 #else
 	printf("ERROR: %s\n", inp.c_str());
 #endif
 }
 void print_fatal(std::string inp) {
 #ifdef ROS_BUILD
-	ROS_FATAL(inp.c_str());
+	ROS_FATAL("%s", inp.c_str());
 	ros::shutdown();
 #else
 	printf("FATAL: %s\n", inp.c_str());

src/ur_driver.cpp

@@ -13,10 +13,10 @@
 
 UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
 		std::condition_variable& msg_cond, std::string host,
-		unsigned int reverse_port, unsigned int safety_count_max,
+		unsigned int reverse_port, double servoj_time, unsigned int safety_count_max,
 		double max_time_step, double min_payload, double max_payload) :
 		REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_(
-				min_payload), maximum_payload_(max_payload) {
+				min_payload), maximum_payload_(max_payload), servoj_time_(servoj_time) {
 	char buffer[256];
 	struct sockaddr_in serv_addr;
 	int n, flag;
@@ -63,11 +63,11 @@ std::vector<double> UrDriver::interp_cubic(double t, double T,
 	}
 	return positions;
 }
-
+ /*
 void UrDriver::addTraj(std::vector<double> inp_timestamps,
 		std::vector<std::vector<double> > inp_positions,
 		std::vector<std::vector<double> > inp_velocities) {
-	/* DEPRECATED */
+	// DEPRECATED
 	printf("!! addTraj is deprecated !!\n");
 	std::vector<double> timestamps;
 	std::vector<std::vector<double> > positions;
@@ -82,11 +82,11 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
 			timestamps.push_back(inp_timestamps[i - 1] + step_size * j);
 		}
 	}
-	//make sure we come to a smooth stop
-	/*while (timestamps.back() < inp_timestamps.back()) {
-	 timestamps.push_back(timestamps.back() + 0.008);
-	 }
-	 timestamps.pop_back();*/
+	// //make sure we come to a smooth stop
+	// while (timestamps.back() < inp_timestamps.back()) {
+	//   timestamps.push_back(timestamps.back() + 0.008);
+	// }
+	// timestamps.pop_back();
 
 	unsigned int j = 0;
 	for (unsigned int i = 0; i < timestamps.size(); i++) {
@@ -102,7 +102,7 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
 
 	timestamps.push_back(inp_timestamps[inp_timestamps.size() - 1]);
 	positions.push_back(inp_positions[inp_positions.size() - 1]);
-	/* This is actually faster than using a stringstream :-o */
+	/// This is actually faster than using a stringstream :-o
 	for (unsigned int i = 1; i < timestamps.size(); i++) {
 		char buf[128];
 		sprintf(buf,
@@ -117,7 +117,7 @@ void UrDriver::addTraj(std::vector<double> inp_timestamps,
 	rt_interface_->addCommandToQueue(command_string);
 
 }
-
+*/
 void UrDriver::doTraj(std::vector<double> inp_timestamps,
 		std::vector<std::vector<double> > inp_positions,
 		std::vector<std::vector<double> > inp_velocities) {
@@ -144,21 +144,24 @@ void UrDriver::doTraj(std::vector<double> inp_timestamps,
 				inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
 				inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
 
-		UrDriver::servoj(positions, 0.008, 1);
+		UrDriver::servoj(positions);
 
 		// oversample with 4 * sample_time
-		std::this_thread::sleep_for(std::chrono::milliseconds(2));
+		std::this_thread::sleep_for(std::chrono::milliseconds((int) ((servoj_time_*1000)/4.)));
 		t = std::chrono::high_resolution_clock::now();
 	}
 	//Signal robot to stop driverProg()
 	UrDriver::closeServo(positions);
 }
 
-void UrDriver::servoj(std::vector<double> positions, double time,
-		int keepalive) {
+void UrDriver::servoj(std::vector<double> positions,
+		int keepalive, double time) {
 	unsigned int bytes_written;
 	int tmp;
 	unsigned char buf[32];
+	if (time < 0.016) {
+		time = servoj_time_;
+	}
 	for (int i = 0; i < 6; i++) {
 		tmp = htonl((int) (positions[i] * MULT_JOINTSTATE_));
 		buf[i * 4] = tmp & 0xff;
@@ -272,9 +275,9 @@ void UrDriver::openServo() {
 }
 void UrDriver::closeServo(std::vector<double> positions) {
 	if (positions.size() != 6)
-		UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0.008, 0);
+		UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
 	else
-		UrDriver::servoj(positions, 0.008, 0);
+		UrDriver::servoj(positions, 0);
 	close(new_sockfd_);
 }
 
@@ -352,8 +355,20 @@ bool UrDriver::setPayload(double m) {
 }
 
 void UrDriver::setMinPayload(double m) {
+	if (m > 0) {
 		minimum_payload_ = m;
+	} else {
+		minimum_payload_ = 0;
+	}
+
 }
 void UrDriver::setMaxPayload(double m) {
 	maximum_payload_ = m;
 }
+void UrDriver::setServojTime(double t) {
+	if (t > 0.016) {
+		servoj_time_ = t;
+	} else {
+		servoj_time_ = 0.016;
+	}
+}

src/ur_hardware_interface.cpp

@@ -141,7 +141,7 @@ void UrHardwareInterface::write() {
 				joint_velocity_command_[2], joint_velocity_command_[3],
 				joint_velocity_command_[4], joint_velocity_command_[5], 100);
 	} else if (position_interface_running_) {
-		robot_->servoj(joint_position_command_, 0.008, 1);
+		robot_->servoj(joint_position_command_);
 	}
 }
 

src/ur_ros_wrapper.cpp

@@ -75,7 +75,7 @@ public:
 		char buf[256];
 
 		if (ros::param::get("~prefix", joint_prefix)) {
-			sprintf(buf,"Setting prefix to %s", joint_prefix.c_str());
+			sprintf(buf, "Setting prefix to %s", joint_prefix.c_str());
 			print_info(buf);
 		}
 		joint_names.push_back(joint_prefix + "shoulder_pan_joint");
@@ -122,6 +122,13 @@ public:
 				min_payload, max_payload);
 		print_debug(buf);
 
+		double servoj_time = 0.016;
+		if (ros::param::get("~servoj_time", servoj_time)) {
+			sprintf(buf, "Servoj_time set to: %f [sec]", servoj_time);
+			print_debug(buf);
+		}
+		robot_.setServojTime(servoj_time);
+
 		if (robot_.start()) {
 			if (use_ros_control_) {
 				ros_control_thread_ = new std::thread(
@@ -140,7 +147,8 @@ public:
 				//subscribe to the data topic of interest
 				rt_publish_thread_ = new std::thread(
 						boost::bind(&RosWrapper::publishRTMsg, this));
-				print_debug("The action server for this driver has been started");
+				print_debug(
+						"The action server for this driver has been started");
 			}
 			mb_publish_thread_ = new std::thread(
 					boost::bind(&RosWrapper::publishMbMsg, this));