Added scaled vel traj controller

Do it for all robots

ur_controllers/controller_plugins.xml

@@ -6,7 +6,12 @@
   </class>
   <class name="position_controllers/ScaledJointTrajectoryController" type="position_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
   <description>
-    Scaled joint trajectory controller
+    Scaled velocity-based joint trajectory controller
+  </description>
+  </class>
+  <class name="velocity_controllers/ScaledJointTrajectoryController" type="velocity_controllers::ScaledJointTrajectoryController" base_class_type="controller_interface::ControllerBase">
+  <description>
+    Scaled velocity-based joint trajectory controller
   </description>
   </class>
 </library>

ur_controllers/include/ur_controllers/hardware_interface_adapter.h

@@ -100,4 +100,132 @@ private:
   std::vector<ur_controllers::ScaledJointHandle>* joint_handles_ptr_;
 };
 
+namespace ur_controllers
+{
+/**
+ * \brief Helper base class template for closed loop HardwareInterfaceAdapter implementations.
+ *
+ * Adapters leveraging (specializing) this class will generate a command given the desired state and state error using a
+ * velocity feedforward term plus a corrective PID term.
+ *
+ * Use one of the available template specializations of this class (or create your own) to adapt the
+ * ScaledJointTrajectoryController to a specific hardware interface.
+ */
+template <class State>
+class ClosedLoopHardwareInterfaceAdapter
+{
+public:
+  ClosedLoopHardwareInterfaceAdapter() : joint_handles_ptr_(0)
+  {
+  }
+
+  bool init(std::vector<ur_controllers::ScaledJointHandle>& joint_handles, ros::NodeHandle& controller_nh)
+  {
+    // Store pointer to joint handles
+    joint_handles_ptr_ = &joint_handles;
+
+    // Initialize PIDs
+    pids_.resize(joint_handles.size());
+    for (unsigned int i = 0; i < pids_.size(); ++i)
+    {
+      // Node handle to PID gains
+      ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handles[i].getName());
+
+      // Init PID gains from ROS parameter server
+      pids_[i].reset(new control_toolbox::Pid());
+      if (!pids_[i]->init(joint_nh))
+      {
+        ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
+        return false;
+      }
+    }
+
+    // Load velocity feedforward gains from parameter server
+    velocity_ff_.resize(joint_handles.size());
+    for (unsigned int i = 0; i < velocity_ff_.size(); ++i)
+    {
+      controller_nh.param(std::string("velocity_ff/") + joint_handles[i].getName(), velocity_ff_[i], 0.0);
+    }
+
+    return true;
+  }
+
+  void starting(const ros::Time& /*time*/)
+  {
+    if (!joint_handles_ptr_)
+    {
+      return;
+    }
+
+    // Reset PIDs, zero commands
+    for (unsigned int i = 0; i < pids_.size(); ++i)
+    {
+      pids_[i]->reset();
+      (*joint_handles_ptr_)[i].setCommand(0.0);
+    }
+  }
+
+  void stopping(const ros::Time& /*time*/)
+  {
+  }
+
+  void updateCommand(const ros::Time& /*time*/, const ros::Duration& period, const State& desired_state,
+                     const State& state_error)
+  {
+    const unsigned int n_joints = joint_handles_ptr_->size();
+
+    // Preconditions
+    if (!joint_handles_ptr_)
+      return;
+    assert(n_joints == state_error.position.size());
+    assert(n_joints == state_error.velocity.size());
+
+    // Update PIDs
+    for (unsigned int i = 0; i < n_joints; ++i)
+    {
+      const double command = (desired_state.velocity[i] * velocity_ff_[i]) +
+                             pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
+      (*joint_handles_ptr_)[i].setCommand(command);
+    }
+  }
+
+private:
+  typedef std::shared_ptr<control_toolbox::Pid> PidPtr;
+  std::vector<PidPtr> pids_;
+
+  std::vector<double> velocity_ff_;
+
+  std::vector<ur_controllers::ScaledJointHandle>* joint_handles_ptr_;
+};
+}  // namespace ur_controllers
+
+/**
+ * \brief Adapter for an velocity-controlled hardware interface. Maps position and velocity errors to velocity commands
+ * through a velocity PID loop.
+ *
+ * The following is an example configuration of a controller that uses this adapter. Notice the \p gains and \p
+ * velocity_ff entries: \code head_controller: type: "velocity_controllers/ScaledJointTrajectoryController" joints:
+ *     - head_1_joint
+ *     - head_2_joint
+ *   gains:
+ *     head_1_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
+ *     head_2_joint: {p: 200, d: 1, i: 5, i_clamp: 1}
+ *   velocity_ff:
+ *     head_1_joint: 1.0
+ *     head_2_joint: 1.0
+ *   constraints:
+ *     goal_time: 0.6
+ *     stopped_velocity_tolerance: 0.02
+ *     head_1_joint: {trajectory: 0.05, goal: 0.02}
+ *     head_2_joint: {trajectory: 0.05, goal: 0.02}
+ *   stop_trajectory_duration: 0.5
+ *   state_publish_rate:  25
+ * \endcode
+ */
+template <class State>
+class HardwareInterfaceAdapter<ur_controllers::ScaledVelocityJointInterface, State>
+  : public ur_controllers::ClosedLoopHardwareInterfaceAdapter<State>
+{
+};
+
 #endif  // ifndef UR_CONTROLLERS_HARDWARE_INTERFACE_ADAPTER_H_INCLUDED

ur_controllers/src/scaled_joint_trajectory_controller.cpp

@@ -38,4 +38,12 @@ typedef ur_controllers::ScaledJointTrajectoryController<trajectory_interface::Qu
     ScaledJointTrajectoryController;
 }
 
+namespace velocity_controllers
+{
+typedef ur_controllers::ScaledJointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,
+                                                        ur_controllers::ScaledVelocityJointInterface>
+    ScaledJointTrajectoryController;
+}
+
 PLUGINLIB_EXPORT_CLASS(position_controllers::ScaledJointTrajectoryController, controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(velocity_controllers::ScaledJointTrajectoryController, controller_interface::ControllerBase)

ur_robot_driver/config/ur10e_controllers.yaml

@@ -61,7 +61,44 @@ pos_traj_controller:
    state_publish_rate: *loop_hz
    action_monitor_rate: 10
 
-vel_based_pos_traj_controller:
+scaled_vel_traj_controller:
+   type: velocity_controllers/ScaledJointTrajectoryController
+   joints: *robot_joints
+   constraints:
+      goal_time: 0.6
+      stopped_velocity_tolerance: 0.05
+      shoulder_pan_joint: {trajectory: 10.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 have not been optimized!!
+      shoulder_pan_joint:  {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      shoulder_lift_joint: {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      elbow_joint:         {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_1_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_2_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+      wrist_3_joint:       {p: 5.0,  i: 0.05, d: 0.1, i_clamp: 1}
+
+   # Use a feedforward term to reduce the size of PID gains
+   velocity_ff:
+      shoulder_pan_joint: 1.0
+      shoulder_lift_joint: 1.0
+      elbow_joint: 1.0
+      wrist_1_joint: 1.0
+      wrist_2_joint: 1.0
+      wrist_3_joint: 1.0
+
+   # state_publish_rate:  50 # Defaults to 50
+   # action_monitor_rate: 20 # Defaults to 20
+   #stop_trajectory_duration: 0 # Defaults to 0.0
+
+vel_traj_controller:
    type: velocity_controllers/JointTrajectoryController
    joints: *robot_joints
    constraints:

ur_robot_driver/include/ur_robot_driver/ros/hardware_interface.h

@@ -204,6 +204,7 @@ protected:
   hardware_interface::PositionJointInterface pj_interface_;
   ur_controllers::SpeedScalingInterface speedsc_interface_;
   hardware_interface::VelocityJointInterface vj_interface_;
+  ur_controllers::ScaledVelocityJointInterface svj_interface_;
   hardware_interface::ForceTorqueSensorInterface fts_interface_;
 
   vector6d_t joint_position_command_;

ur_robot_driver/launch/2xbringup.launch

+<?xml version="1.0"?>
+<launch>
+
+ <group ns="frank">
+   <arg name="robot_ip" default="169.254.198.38"/>
+   <param name="robot_ip" value="$(arg robot_ip)"/>
+ </group>
+
+ <group ns="klaus">
+   <arg name="robot_ip" default="169.254.198.35"/>
+   <param name="robot_ip" value="$(arg robot_ip)"/>
+ </group>
+
+</launch>

ur_robot_driver/resources/ros_control.urscript

@@ -97,18 +97,19 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket")
 
 control_mode = MODE_UNINITIALIZED
 thread_move = 0
-keepalive = -2
+global keepalive = -2
 textmsg("External control active")
 params_mult = socket_read_binary_integer(1+6+1, "reverse_socket")
 keepalive = params_mult[1]
 while keepalive > 0 and control_mode > MODE_STOPPED:
+  enter_critical
   socket_send_line(1, "reverse_socket")
   params_mult = socket_read_binary_integer(1+6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation
-  enter_critical
   if params_mult[0] > 0:
     keepalive = params_mult[1]
     if control_mode != params_mult[8]:
       control_mode = params_mult[8]
+      join thread_move
       if control_mode == MODE_SERVOJ:
         thread_move = run servoThread()
       elif control_mode == MODE_SPEEDJ:

ur_robot_driver/src/ros/hardware_interface.cpp

@@ -287,6 +287,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
         hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i]));
     spj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
         js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_));
+    svj_interface_.registerHandle(ur_controllers::ScaledJointHandle(
+        js_interface_.getHandle(joint_names_[i]), &joint_velocity_command_[i], &speed_scaling_combined_));
   }
 
   speedsc_interface_.registerHandle(
@@ -300,6 +302,7 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
   registerInterface(&spj_interface_);
   registerInterface(&pj_interface_);
   registerInterface(&vj_interface_);
+  registerInterface(&svj_interface_);
   registerInterface(&speedsc_interface_);
   registerInterface(&fts_interface_);
 
@@ -488,10 +491,6 @@ void HardwareInterface::write(const ros::Time& time, const ros::Duration& period
     }
     packet_read_ = false;
   }
-  else
-  {
-    ur_driver_->stopControl();
-  }
 }
 
 bool HardwareInterface::prepareSwitch(const std::list<hardware_interface::ControllerInfo>& start_list,
@@ -533,6 +532,10 @@ void HardwareInterface::doSwitch(const std::list<hardware_interface::ControllerI
       {
         position_controller_running_ = true;
       }
+      if (resource_it.hardware_interface == "ur_controllers::ScaledVelocityJointInterface")
+      {
+        velocity_controller_running_ = true;
+      }
       if (resource_it.hardware_interface == "hardware_interface::VelocityJointInterface")
       {
         velocity_controller_running_ = true;