Add velocity feedforward term to velocity HardwareInterfaceAdapter

joint_trajectory_controller/CMakeLists.txt

@@ -51,7 +51,7 @@ add_library(${PROJECT_NAME} src/joint_trajectory_controller.cpp
 target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 
 if(CATKIN_ENABLE_TESTING)
-  find_package(catkin 
+  find_package(catkin
     REQUIRED COMPONENTS
       actionlib
       controller_manager
@@ -100,6 +100,7 @@ if(CATKIN_ENABLE_TESTING)
                     test/joint_trajectory_controller_vel.test
                     test/joint_trajectory_controller_test.cpp)
   target_link_libraries(joint_trajectory_controller_vel_test ${catkin_LIBRARIES})
+  target_compile_definitions(joint_trajectory_controller_vel_test PRIVATE TEST_VELOCITY_FF=1)
 
   add_rostest_gtest(joint_trajectory_controller_wrapping_test
                     test/joint_trajectory_controller_wrapping.test

joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h

@@ -81,7 +81,7 @@ class HardwareInterfaceAdapter
  *   joints:
  *     - head_1_joint
  *     - head_2_joint
- *   
+ *
  *   constraints:
  *     goal_time: 0.6
  *     stopped_velocity_tolerance: 0.02
@@ -133,33 +133,19 @@ class HardwareInterfaceAdapter<hardware_interface::PositionJointInterface, State
 };
 
 /**
- * \brief Adapter for an velocity-controlled hardware interface. Maps position and velocity errors to velocity commands
- * through a velocity PID loop.
+ * \brief Helper base class template for closed loop HardwareInterfaceAdapter implementations.
  *
- * The following is an example configuration of a controller that uses this adapter. Notice the \p gains entry:
- * \code
- * head_controller:
- *   type: "velocity_controllers/JointTrajectoryController"
- *   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}
- *   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
+ * 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
+ * JointTrajectoryController to a specidfic hardware interface.
  */
 template <class State>
-class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State>
+class ClosedLoopHardwareInterfaceAdapter
 {
 public:
-  HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
+  ClosedLoopHardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
 
   bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& controller_nh)
   {
@@ -182,26 +168,33 @@ class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State
       }
     }
 
+    // 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 velocity commands
+    // 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 stopping(const ros::Time& /*time*/) {}
 
   void updateCommand(const ros::Time&     /*time*/,
                      const ros::Duration& period,
-                     const State&         /*desired_state*/,
+                     const State&         desired_state,
                      const State&         state_error)
   {
     const unsigned int n_joints = joint_handles_ptr_->size();
@@ -215,7 +208,7 @@ class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State
     // Update PIDs
     for (unsigned int i = 0; i < n_joints; ++i)
     {
-      const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
+      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);
     }
   }
@@ -224,14 +217,48 @@ class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State
   typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
   std::vector<PidPtr> pids_;
 
+  std::vector<double> velocity_ff_;
+
   std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
 };
 
+/**
+ * \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/JointTrajectoryController"
+ *   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<hardware_interface::VelocityJointInterface, State> : public ClosedLoopHardwareInterfaceAdapter<State>
+{};
+
 /**
  * \brief Adapter for an effort-controlled hardware interface. Maps position and velocity errors to effort commands
  * through a position PID loop.
  *
- * The following is an example configuration of a controller that uses this adapter. Notice the \p gains entry:
+ * 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: "effort_controllers/JointTrajectoryController"
@@ -241,6 +268,9 @@ class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State
  *   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
@@ -251,75 +281,8 @@ class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State
  * \endcode
  */
 template <class State>
-class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State>
-{
-public:
-  HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
-
-  bool init(std::vector<hardware_interface::JointHandle>& 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;
-      }
-    }
-
-    return true;
-  }
-
-  void starting(const ros::Time& /*time*/)
-  {
-    if (!joint_handles_ptr_) {return;}
-
-    // Reset PIDs, zero effort 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 = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
-      (*joint_handles_ptr_)[i].setCommand(command);
-    }
-  }
-
-private:
-  typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
-  std::vector<PidPtr> pids_;
-
-  std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
-};
+class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State> : public ClosedLoopHardwareInterfaceAdapter<State>
+{};
 
 /**
  * \brief Adapter for a pos-vel hardware interface. Forwards desired positions with velcities as commands.

joint_trajectory_controller/test/joint_trajectory_controller_test.cpp

@@ -43,6 +43,10 @@
 #include <control_msgs/JointTrajectoryControllerState.h>
 #include <control_msgs/QueryTrajectoryState.h>
 
+#include <controller_manager_msgs/LoadController.h>
+#include <controller_manager_msgs/UnloadController.h>
+#include <controller_manager_msgs/SwitchController.h>
+
 // Floating-point value comparison threshold
 const double EPS = 0.01;
 
@@ -108,6 +112,11 @@ class JointTrajectoryControllerTest : public ::testing::Test
     // Query state service client
     query_state_service = nh.serviceClient<control_msgs::QueryTrajectoryState>("query_state");
 
+    // Controller management services
+    load_controller_service = nh.serviceClient<controller_manager_msgs::LoadController>("/controller_manager/load_controller");
+    unload_controller_service = nh.serviceClient<controller_manager_msgs::UnloadController>("/controller_manager/unload_controller");
+    switch_controller_service = nh.serviceClient<controller_manager_msgs::SwitchController>("/controller_manager/switch_controller");
+
     // Action client
     const std::string action_server_name = nh.getNamespace() + "/follow_joint_trajectory";
     action_client.reset(new ActionClient(action_server_name));
@@ -144,6 +153,9 @@ class JointTrajectoryControllerTest : public ::testing::Test
   ros::Publisher     traj_pub;
   ros::Subscriber    state_sub;
   ros::ServiceClient query_state_service;
+  ros::ServiceClient load_controller_service;
+  ros::ServiceClient unload_controller_service;
+  ros::ServiceClient switch_controller_service;
   ActionClientPtr    action_client;
   ActionClientPtr    action_client2;
 
@@ -192,6 +204,31 @@ class JointTrajectoryControllerTest : public ::testing::Test
     }
     return true;
   }
+
+  bool reloadController(const std::string& name)
+  {
+    controller_manager_msgs::SwitchController stop_controller;
+    stop_controller.request.stop_controllers.push_back(name);
+    stop_controller.request.strictness = stop_controller.request.STRICT;
+    if(!switch_controller_service.call(stop_controller)) return false;
+    if(!stop_controller.response.ok) return false;
+
+    controller_manager_msgs::UnloadController unload_controller;
+    unload_controller.request.name = name;
+    if(!unload_controller_service.call(unload_controller)) return false;
+    if(!unload_controller.response.ok) return false;
+
+    controller_manager_msgs::LoadController load_controller;
+    load_controller.request.name = name;
+    if(!load_controller_service.call(load_controller)) return false;
+    if(!load_controller.response.ok) return false;
+
+    controller_manager_msgs::SwitchController start_controller;
+    start_controller.request.start_controllers.push_back(name);
+    start_controller.request.strictness = start_controller.request.STRICT;
+    if(!switch_controller_service.call(start_controller)) return false;
+    if(!start_controller.response.ok) return false;
+  }
 };
 
 // Controller state ROS API ////////////////////////////////////////////////////////////////////////////////////////////
@@ -943,6 +980,58 @@ TEST_F(JointTrajectoryControllerTest, ignorePartiallyOldActionTraj)
   }
 }
 
+// Velocity FF parameter ///////////////////////////////////////////////////////////////////////////////////////////////
+// This test will only be built and run for the VelocityJointInterface-based version of the JointTrajectoryController
+
+#if TEST_VELOCITY_FF
+
+TEST_F(JointTrajectoryControllerTest, jointVelocityFeedForward)
+{
+  ASSERT_TRUE(initState());
+  ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+  // Go to home configuration, we need known initial conditions
+  traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+  action_client->sendGoal(traj_home_goal);
+  ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+  // Send trajectory
+  traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+  action_client->sendGoal(traj_goal);
+  EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+  // Wait until done
+  EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+  // Go to home configuration, we need known initial conditions
+  traj_home_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+  action_client->sendGoal(traj_home_goal);
+  ASSERT_TRUE(waitForState(action_client, SimpleClientGoalState::SUCCEEDED, long_timeout));
+
+  // Disable velocity feedforward
+  ros::param::set("/rrbot_controller/velocity_ff/joint1", 0.0);
+  ros::param::set("/rrbot_controller/velocity_ff/joint2", 0.0);
+  ASSERT_TRUE(reloadController("rrbot_controller"));
+  ASSERT_TRUE(action_client->waitForServer(long_timeout));
+
+  // Send trajectory
+  traj_goal.trajectory.header.stamp = ros::Time(0); // Start immediately
+  action_client->sendGoal(traj_goal);
+  EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ACTIVE, short_timeout));
+
+  // Wait until done
+  EXPECT_TRUE(waitForState(action_client, SimpleClientGoalState::ABORTED, long_timeout));
+  EXPECT_EQ(action_client->getResult()->error_code, control_msgs::FollowJointTrajectoryResult::PATH_TOLERANCE_VIOLATED);
+
+  // Re-enable velocity feedforward
+  ros::param::set("/rrbot_controller/velocity_ff/joint1", 1.0);
+  ros::param::set("/rrbot_controller/velocity_ff/joint2", 1.0);
+  ASSERT_TRUE(reloadController("rrbot_controller"));
+  ASSERT_TRUE(action_client->waitForServer(long_timeout));
+}
+
+#endif // TEST_VELOCITY_FF
+
 // Tolerance checking //////////////////////////////////////////////////////////////////////////////////////////////////
 
 TEST_F(JointTrajectoryControllerTest, pathToleranceViolation)

joint_trajectory_controller/test/joint_trajectory_controller_vel.test

@@ -34,6 +34,6 @@
   <!-- Controller test -->
   <test test-name="joint_trajectory_controller_vel_test"
         pkg="joint_trajectory_controller"
-        type="joint_trajectory_controller_test"
-        time-limit="85.0"/>
+        type="joint_trajectory_controller_vel_test"
+        time-limit="120.0"/>
 </launch>

joint_trajectory_controller/test/rrbot_vel_controllers.yaml

@@ -14,5 +14,9 @@ rrbot_controller:
       trajectory: 0.05
 
   gains:
-    joint1: {p: 60.0,  i: 0.0, d: 0.0, i_clamp: 1}
-    joint2: {p: 60.0,  i: 0.0, d: 0.0, i_clamp: 1}
+    joint1: {p: 1.0,  i: 0.0, d: 0.0, i_clamp: 1}
+    joint2: {p: 1.0,  i: 0.0, d: 0.0, i_clamp: 1}
+
+  velocity_ff:
+    joint1: 1.0
+    joint2: 1.0