Improved ros_control performance and stability

README.md

@@ -28,9 +28,13 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
 * Added support for ros_control. 
   * As ros_control wants to have control over the robot at all times, ros_control compatability is set via a parameter at launch-time. 
   * With ros_control active, the driver doesn't open the action_lib interface nor publish joint_states or wrench msgs. This is handled by ros_control instead.
+  * Currently two controllers are available, both controlling the joint position of the robot, useable for trajectroy execution
+    * The velocity based controller sends joint speed commands to the robot, using the speej command
+    * The position based controller send joint position commands to the robot, using the servoj command
+    * I have so far only used the velocity based controller, but which one is optimal depends on the application.
   * As ros_control continuesly controls the robot, using the teach pendant while a controller is running will cause the controller **on the robot** to crash, as it obviously can't handle conflicting control input from two sources. Thus be sure to stop the running controller **before** moving the robot via the teach pendant:
     * A list of the loaded and running controllers can be found by a call to the controller_manager ```rosservice call /controller_manager/list_controllers {} ```
-    * The running position trajectory controller can be stopped with a call to  ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'position_based_position_trajectory_controller' strictness: 1" ``` (Remember you can use tab-completion for this)
+    * The running position trajectory controller can be stopped with a call to  ```rosservice call /universal_robot/controller_manager/switch_controller "start_controllers: - '' stop_controllers: - 'pos_based_pos_traj_controller' strictness: 1" ``` (Remember you can use tab-completion for this)
    
 
 ## Installation
@@ -61,12 +65,12 @@ Be sure to stop the currently running controller **either before or in the same
 
 The position based controller *should* stay closer to the commanded path, while the velocity based react faster (trajectory execution start within 50-70 ms, while it is in the 150-180ms range for the position_based. Usage without ros_control as well as the old driver is also in the 170ms range, as mentioned at my lightning talk @ ROSCon 2013).
 
-**Note** that the PID values are not tweaked as of this moment.
+**Note** that the PID values are not optimally tweaked as of this moment.
 
 To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following 
 ```
 controller_list:
- - name: velocity_based_position_trajectory_controller #or position_based_position_trajectory_controller
+ - name: vel_based_pos_traj_controller #or pos_based_pos_traj_controller
    action_ns: follow_joint_trajectory
    type: FollowJointTrajectory
    default: true

config/ur10_controllers.yaml

@@ -22,10 +22,10 @@ force_torque_sensor_controller:
    type:         force_torque_sensor_controller/ForceTorqueSensorController
    publish_rate: 125
 
-# Joint Trajectory Controller -------------------------------
+# Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-velocity_based_position_trajectory_controller:
+pos_based_pos_traj_controller:
-   type: velocity_controllers/JointTrajectoryController
+   type: position_controllers/JointTrajectoryController
    joints:
      - shoulder_pan_joint
      - shoulder_lift_joint
@@ -47,12 +47,49 @@ velocity_based_position_trajectory_controller:
    action_monitor_rate: 10
    gains:
       #!!These values have not been optimized!!
-      shoulder_pan_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      shoulder_pan_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
-      shoulder_lift_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
-      elbow_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      elbow_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
-      wrist_1_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_1_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
-      wrist_2_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_2_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
-      wrist_3_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_3_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #hold_trajectory_duration: 0 # Defaults to 0.5
+   
+# Joint Trajectory Controller -------------------------------
+# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
+vel_based_pos_traj_controller:
+   type: velocity_controllers/JointTrajectoryController
+   joints:
+     - shoulder_pan_joint
+     - shoulder_lift_joint
+     - elbow_joint
+     - wrist_1_joint
+     - wrist_2_joint
+     - wrist_3_joint
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
+      shoulder_lift_joint: {trajectory: 0.1, goal: 0.1}
+      elbow_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_1_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_2_joint: {trajectory: 0.1, goal: 0.1}
+      wrist_3_joint: {trajectory: 0.1, goal: 0.1}
+   stop_trajectory_duration: 0.5
+   state_publish_rate:  125
+   action_monitor_rate: 10
+   gains:
+       #!!These values are useable, but maybe not optimal
+      shoulder_pan_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
+      shoulder_lift_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
+      elbow_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
+      wrist_1_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
+      wrist_2_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
+      wrist_3_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
+
 
    # state_publish_rate:  50 # Defaults to 50
    # action_monitor_rate: 20 # Defaults to 20

config/ur5_controllers.yaml

@@ -24,7 +24,7 @@ force_torque_sensor_controller:
 
 # Joint Trajectory Controller - position based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-position_based_position_trajectory_controller:
+pos_based_pos_traj_controller:
    type: position_controllers/JointTrajectoryController
    joints:
      - shoulder_pan_joint
@@ -60,7 +60,7 @@ position_based_position_trajectory_controller:
    
 # Joint Trajectory Controller - velocity based -------------------------------
 # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
-velocity_based_position_trajectory_controller:
+vel_based_pos_traj_controller:
    type: velocity_controllers/JointTrajectoryController
    joints:
      - shoulder_pan_joint
@@ -82,13 +82,13 @@ velocity_based_position_trajectory_controller:
    state_publish_rate:  125
    action_monitor_rate: 10
    gains:
-      #!!These values have not been optimized!!
+      #!!These values are useable, but maybe not optimal
-      shoulder_pan_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      shoulder_pan_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
-      shoulder_lift_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
-      elbow_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      elbow_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
-      wrist_1_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_1_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
-      wrist_2_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_2_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
-      wrist_3_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_3_joint: {p: 20.0,  i: 0.0, d: 0.7, i_clamp: 1}
 
    # state_publish_rate:  50 # Defaults to 50
    # action_monitor_rate: 20 # Defaults to 20

include/ur_modern_driver/ur_hardware_interface.h

@@ -58,6 +58,7 @@
 #include <controller_manager/controller_manager.h>
 #include <boost/scoped_ptr.hpp>
 #include <ros/ros.h>
+#include <math.h>
 #include "do_output.h"
 #include "ur_driver.h"
 
@@ -86,6 +87,8 @@ public:
 	/// \brief write the command to the robot hardware.
 	virtual void write();
 
+	void setMaxVelChange(double inp);
+
 	bool canSwitch(
 			const std::list<hardware_interface::ControllerInfo> &start_list,
 			const std::list<hardware_interface::ControllerInfo> &stop_list) const;
@@ -111,10 +114,13 @@ protected:
 	std::vector<double> joint_effort_;
 	std::vector<double> joint_position_command_;
 	std::vector<double> joint_velocity_command_;
+	std::vector<double> prev_joint_velocity_command_;
 		std::size_t num_joints_;
 	double robot_force_[3] = { 0., 0., 0. };
 	double robot_torque_[3] = { 0., 0., 0. };
 
+	double max_vel_change_;
+
 	// Robot API
 	UrDriver* robot_;
 

launch/ur10_ros_control.launch

@@ -29,11 +29,11 @@
 
   <!-- spawn controller manager -->
   <node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
+    output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" />
 
   <!-- load other controller --> 
   <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
-    output="screen" args="load position_based_position_trajectory_controller" /> 
+    output="screen" args="load pos_based_pos_traj_controller" /> 
 
   <!-- Convert joint states to /tf tranforms -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

launch/ur5_ros_control.launch

@@ -30,11 +30,11 @@
 
   <!-- spawn controller manager -->
   <node name="ros_control_controller_spawner" pkg="controller_manager" type="spawner" respawn="false"
-    output="screen" args="joint_state_controller force_torque_sensor_controller velocity_based_position_trajectory_controller" />
+    output="screen" args="joint_state_controller force_torque_sensor_controller vel_based_pos_traj_controller" />
 
   <!-- load other controller --> 
   <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
-    output="screen" args="load position_based_position_trajectory_controller" /> 
+    output="screen" args="load pos_based_pos_traj_controller" /> 
 
   <!-- Convert joint states to /tf tranforms -->
   <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher"/>

src/ur_hardware_interface.cpp

@@ -57,6 +57,8 @@ UrHardwareInterface::UrHardwareInterface(ros::NodeHandle& nh, UrDriver* robot) :
 	// Initialize shared memory and interfaces here
 	init(); // this implementation loads from rosparam
 
+	max_vel_change_ = 0.12; // equivalent of an acceleration of 15 rad/sec^2
+
 	ROS_INFO_NAMED("ur_hardware_interface", "Loaded ur_hardware_interface.");
 }
 
@@ -79,6 +81,7 @@ void UrHardwareInterface::init() {
 	joint_effort_.resize(num_joints_);
 	joint_position_command_.resize(num_joints_);
 	joint_velocity_command_.resize(num_joints_);
+	prev_joint_velocity_command_.resize(num_joints_);
 
 	// Initialize controller
 	for (std::size_t i = 0; i < num_joints_; ++i) {
@@ -102,6 +105,7 @@ void UrHardwareInterface::init() {
 				hardware_interface::JointHandle(
 						joint_state_interface_.getHandle(joint_names_[i]),
 						&joint_velocity_command_[i]));
+		prev_joint_velocity_command_[i] = 0.;
 	}
 
 	// Create force torque interface
@@ -135,11 +139,30 @@ void UrHardwareInterface::read() {
 
 }
 
+void UrHardwareInterface::setMaxVelChange(double inp) {
+	max_vel_change_ = inp;
+}
+
 void UrHardwareInterface::write() {
 	if (velocity_interface_running_) {
-		robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1],
+		std::vector<double> cmd;
-				joint_velocity_command_[2], joint_velocity_command_[3],
+		//do some rate limiting
-				joint_velocity_command_[4], joint_velocity_command_[5], 100);
+		cmd.resize(joint_velocity_command_.size());
+		for (unsigned int i = 0; i < joint_velocity_command_.size(); i++) {
+			cmd[i] = joint_velocity_command_[i];
+			if (cmd[i] > prev_joint_velocity_command_[i] + max_vel_change_) {
+				cmd[i] = prev_joint_velocity_command_[i] + max_vel_change_;
+			} else if (cmd[i]
+					< prev_joint_velocity_command_[i] - max_vel_change_) {
+				cmd[i] = prev_joint_velocity_command_[i] - max_vel_change_;
+			}
+			if (cmd[i] > M_PI/2.)
+				cmd[i] = M_PI/2.;
+			else if (cmd[i] < -M_PI/2.)
+				cmd[i] = -M_PI/2.;
+			prev_joint_velocity_command_[i] = cmd[i];
+		}
+		robot_->setSpeed(cmd[0], cmd[1], cmd[2], cmd[3], cmd[4], cmd[5],  max_vel_change_*125);
 	} else if (position_interface_running_) {
 		robot_->servoj(joint_position_command_);
 	}

src/ur_ros_wrapper.cpp

@@ -129,6 +129,14 @@ public:
 		}
 		robot_.setServojTime(servoj_time);
 
+		double max_vel_change = 0.12; // equivalent of an acceleration of 15 rad/sec^2
+		if (ros::param::get("~max_acceleration", max_vel_change)) {
+			max_vel_change = max_vel_change/125;
+		}
+		sprintf(buf, "Max acceleration set to: %f [rad/sec²]", max_vel_change*125);
+		print_debug(buf);
+		hardware_interface_->setMaxVelChange(max_vel_change);
+
 		if (robot_.start()) {
 			if (use_ros_control_) {
 				ros_control_thread_ = new std::thread(