Added the servoj gain and servoj lookahead time as a parameter at launch time. Resolves #47

include/ur_modern_driver/ur_driver.h

@@ -50,6 +50,8 @@ private:
 	double servoj_time_;
 	bool executing_traj_;
 	double firmware_version_;
+	double servoj_lookahead_time_;
+	double servoj_gain_;
 public:
 	UrRealtimeCommunication* rt_interface_;
 	UrCommunication* sec_interface_;
@@ -58,7 +60,7 @@ public:
 			std::condition_variable& msg_cond, std::string host,
 			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.);
+			double max_payload = 1., double servoj_lookahead_time=0.03, double servoj_gain=300.);
 	bool start();
 	void halt();
 
@@ -91,6 +93,8 @@ public:
 	void setMinPayload(double m);
 	void setMaxPayload(double m);
 	void setServojTime(double t);
+	void setServojLookahead(double t);
+	void setServojGain(double g);
 
 };
 

launch/ur10_ros_control.launch

@@ -19,12 +19,13 @@
 
 
   <!-- Load hardware interface -->
-  <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
+  <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="log" launch-prefix="$(arg launch_prefix)">
     <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
     <param name="min_payload" type="double" value="$(arg min_payload)"/>
     <param name="max_payload" type="double" value="$(arg max_payload)"/>
     <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
     <param name="use_ros_control" type="bool" value="True"/>
+    <param name="servoj_gain" type="double" value="750" />
     <arg name="prefix" value="$(arg prefix)" />
   </node>
 

launch/ur3_ros_control.launch

@@ -19,12 +19,13 @@
 
 
   <!-- Load hardware interface -->
-  <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
+  <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="log" launch-prefix="$(arg launch_prefix)">
     <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
     <param name="min_payload" type="double" value="$(arg min_payload)"/>
     <param name="max_payload" type="double" value="$(arg max_payload)"/>
     <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
     <param name="use_ros_control" type="bool" value="True"/>
+    <param name="servoj_gain" type="double" value="750" />
     <arg name="prefix" value="$(arg prefix)" />
   </node>
 

launch/ur5_ros_control.launch

@@ -19,12 +19,13 @@
 
 
   <!-- Load hardware interface -->
-  <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="screen" launch-prefix="$(arg launch_prefix)">
+  <node name="ur_hardware_interface" pkg="ur_modern_driver" type="ur_driver" output="log" launch-prefix="$(arg launch_prefix)">
     <param name="robot_ip_address" type="str" value="$(arg robot_ip)"/>
     <param name="min_payload" type="double" value="$(arg min_payload)"/>
     <param name="max_payload" type="double" value="$(arg max_payload)"/>
     <param name="max_velocity" type="double" value="$(arg max_velocity)"/>
     <param name="use_ros_control" type="bool" value="True"/>
+    <param name="servoj_gain" type="double" value="750" />
     <arg name="prefix" value="$(arg prefix)" />
   </node>
 

src/ur_driver.cpp

@@ -22,10 +22,10 @@ UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
 		std::condition_variable& msg_cond, std::string host,
 		unsigned int reverse_port, double servoj_time,
 		unsigned int safety_count_max, double max_time_step, double min_payload,
-		double max_payload) :
+		double max_payload, double servoj_lookahead_time, double servoj_gain) :
 		REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_(
 				min_payload), maximum_payload_(max_payload), servoj_time_(
-				servoj_time) {
+				servoj_time), servoj_lookahead_time_(servoj_lookahead_time), servoj_gain_(servoj_gain) {
 	char buffer[256];
 	struct sockaddr_in serv_addr;
 	int n, flag;
@@ -182,8 +182,8 @@ bool UrDriver::uploadProg() {
 	cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
 
 	if (sec_interface_->robot_state_->getVersion() >= 3.1)
-		sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n",
+		sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n",
-				servoj_time_);
+				servoj_time_, servoj_lookahead_time_, servoj_gain_);
 	else
 		sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
 	cmd_str += buf;
@@ -358,3 +358,25 @@ void UrDriver::setServojTime(double t) {
 		servoj_time_ = 0.008;
 	}
 }
+void UrDriver::setServojLookahead(double t){
+	if (t > 0.03) {
+		if (t < 0.2) {
+			servoj_lookahead_time_ = t;
+		} else {
+			servoj_lookahead_time_ = 0.2;
+		}
+	} else {
+		servoj_lookahead_time_ = 0.03;
+	}
+}
+void UrDriver::setServojGain(double g){
+	if (g > 100) {
+			if (g < 2000) {
+				servoj_gain_ = g;
+			} else {
+				servoj_gain_ = 2000;
+			}
+		} else {
+			servoj_gain_ = 100;
+		}
+}

src/ur_ros_wrapper.cpp

@@ -86,7 +86,7 @@ public:
 			as_(nh_, "follow_joint_trajectory",
 					boost::bind(&RosWrapper::goalCB, this, _1),
 					boost::bind(&RosWrapper::cancelCB, this, _1), false), robot_(
-					rt_msg_cond_, msg_cond_, host, reverse_port), io_flag_delay_(0.05), joint_offsets_(
+					rt_msg_cond_, msg_cond_, host, reverse_port, 0.03, 300), io_flag_delay_(0.05), joint_offsets_(
 					6, 0.0) {
 
 		std::string joint_prefix = "";
@@ -158,6 +158,20 @@ public:
 		}
 		robot_.setServojTime(servoj_time);
 
+		double servoj_lookahead_time = 0.03;
+		if (ros::param::get("~servoj_lookahead_time", servoj_lookahead_time)) {
+			sprintf(buf, "Servoj_lookahead_time set to: %f [sec]", servoj_lookahead_time);
+			print_debug(buf);
+		}
+		robot_.setServojLookahead(servoj_lookahead_time);
+
+		double servoj_gain = 300.;
+		if (ros::param::get("~servoj_gain", servoj_gain)) {
+			sprintf(buf, "Servoj_gain set to: %f [sec]", servoj_gain);
+			print_debug(buf);
+		}
+		robot_.setServojGain(servoj_gain);
+
         //Base and tool frames
         base_frame_ = joint_prefix + "base_link";
         tool_frame_ =  joint_prefix + "tool0_controller";