Added control switching

README.md

@@ -41,8 +41,6 @@ Besides this, the driver subscribes to two new topics:
 */joint\_speed : takes messages of type trajectory\_msgs/JointTrajectory, parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order.
 
 
-This driver is written in c++, which should make it possible to integrate it with ros_control. If you feel like undertaking this task, please dive right in. I don't have the posibility to do this.
-
 No script is sent to the robot. This means that the teach pendant can be used to move the robot around while the driver is running.
 
 ---

config/ur10_controllers.yaml

@@ -33,7 +33,7 @@ position_trajectory_controller:
      - wrist_1_joint
      - wrist_2_joint
      - wrist_3_joint
-  constraints:
+   constraints:
       goal_time: 0.6
       stopped_velocity_tolerance: 0.05
       shoulder_pan_joint: {trajectory: 0.1, goal: 0.1}
@@ -42,14 +42,19 @@ position_trajectory_controller:
       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
+   stop_trajectory_duration: 0.5
-  state_publish_rate:  125
+   state_publish_rate:  125
-  action_monitor_rate: 10
+   action_monitor_rate: 10
-   # gains:
+   gains:
-   #     joint1: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+      #!!These values have not been optimized!!
-   #     joint2: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+      shoulder_pan_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      elbow_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_1_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_2_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
+      wrist_3_joint: {p: 50.0,  i: 0.0, d: 0.1, i_clamp: 1}
 
-    # state_publish_rate:  50 # Defaults to 50
+   # state_publish_rate:  50 # Defaults to 50
-    # action_monitor_rate: 20 # Defaults to 20
+   # action_monitor_rate: 20 # Defaults to 20
-    #hold_trajectory_duration: 0 # Defaults to 0.5
+   #hold_trajectory_duration: 0 # Defaults to 0.5
 

config/ur5_controllers.yaml

@@ -47,12 +47,12 @@ position_trajectory_controller:
    action_monitor_rate: 10
    gains:
       #!!These values have not been optimized!!
-      shoulder_pan_joint: {p: 2.0,  i: 0.0, d: 0.01, i_clamp: 1}
+      shoulder_pan_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}
+      shoulder_lift_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}
+      elbow_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_1_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_2_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}
+      wrist_3_joint: {p: 50.0,  i: 0.0, d: 0.1, 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

@@ -86,6 +86,8 @@ public:
 	/// \brief write the command to the robot hardware.
 	virtual void write();
 
+	void doSwitch(const std::list<hardware_interface::ControllerInfo>&, const std::list<hardware_interface::ControllerInfo>&);
+
 protected:
 
 	// Startup and shutdown of the internal node inside a roscpp program
@@ -95,7 +97,7 @@ protected:
 	hardware_interface::JointStateInterface joint_state_interface_;
 	hardware_interface::ForceTorqueSensorInterface force_torque_interface_;
 	hardware_interface::VelocityJointInterface velocity_joint_interface_;
-
+	bool velocity_interface_running_;
 	// Shared memory
 	std::vector<std::string> joint_names_;
 	std::vector<double> joint_position_;

launch/ur5_ros_control.launch

@@ -28,8 +28,8 @@
     <rosparam file="$(find ur_modern_driver)/config/ur5_controllers.yaml" command="load"/>
 
     <!-- Load controller manager -->
-    <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
+    <!-- <node name="ros_control_controller_manager" pkg="controller_manager" type="controller_manager" respawn="false"
-	  output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_controller" />
+	  output="screen" args="spawn joint_state_controller force_torque_sensor_controller position_trajectory_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

@@ -106,6 +106,7 @@ void UrHardwareInterface::init() {
 	registerInterface(&joint_state_interface_); // From RobotHW base class.
 	registerInterface(&velocity_joint_interface_); // From RobotHW base class.
 	registerInterface(&force_torque_interface_); // From RobotHW base class.
+	velocity_interface_running_ = false;
 }
 
 void UrHardwareInterface::read() {
@@ -127,7 +128,51 @@ void UrHardwareInterface::read() {
 }
 
 void UrHardwareInterface::write() {
-	robot_->setSpeed(joint_velocity_command_[0],joint_velocity_command_[1],joint_velocity_command_[2],joint_velocity_command_[3],joint_velocity_command_[4],joint_velocity_command_[5],100);
+	if (velocity_interface_running_) {
+		robot_->setSpeed(joint_velocity_command_[0], joint_velocity_command_[1],
+				joint_velocity_command_[2], joint_velocity_command_[3],
+				joint_velocity_command_[4], joint_velocity_command_[5], 100);
+	}
+}
+
+void UrHardwareInterface::doSwitch(
+		const std::list<hardware_interface::ControllerInfo>& start_list,
+		const std::list<hardware_interface::ControllerInfo>& stop_list) {
+	for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
+			start_list.begin(); controller_it != start_list.end();
+			++controller_it) {
+		if (controller_it->hardware_interface
+				== "hardware_interface::VelocityJointInterface") {
+			velocity_interface_running_ = true;
+			ROS_DEBUG("Starting velocity interface");
+		}
+	}
+	for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
+			stop_list.begin(); controller_it != stop_list.end();
+			++controller_it) {
+		if (controller_it->hardware_interface
+				== "hardware_interface::VelocityJointInterface") {
+			velocity_interface_running_ = false;
+			ROS_DEBUG("Stopping velocity interface");
+		}
+	}
+	/*std::string outp;
+	 outp = "doSwitch called - Start list:";
+	 for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
+	 start_list.begin(); controller_it != start_list.end();
+	 ++controller_it) {
+	 outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")";
+	 }
+	 outp += " - Stop list:";
+	 for (std::list<hardware_interface::ControllerInfo>::const_iterator controller_it =
+	 stop_list.begin(); controller_it != stop_list.end();
+	 ++controller_it) {
+	 outp += " " + controller_it->name + "(" + controller_it->hardware_interface + ")";
+
+	 }
+	 ROS_INFO(outp.c_str()); */
+
 }
 
 } // namespace
+

src/ur_ros_wrapper.cpp

@@ -363,14 +363,12 @@ private:
 			last_time = current_time;
 			// Input
 			hardware_interface_->read();
-
+			robot_.rt_interface_->robot_state_->setControllerUpdated();
 			// Control
-			//controller_manager_->update(ros::Time::now(), elapsed_time);
 			controller_manager_->update(ros::Time(current_time.tv_sec, current_time.tv_nsec), elapsed_time);
 
 			// Output
 			hardware_interface_->write();
-			robot_.rt_interface_->robot_state_->setControllerUpdated();
 		}
 	}