Merge pull request #116 from ipa-fxm/feature/velocity_controllers/Joi…

…ntTrajectoryController_indigo feature/velocity_controllers/joint_trajectory_controller rebased to indigo-devel
ros-controls · Sep 4, 2014 · 47a66f1 · 47a66f1
2 parents 1d2d36b + 4e812d8
commit 47a66f1
Show file tree

Hide file tree

Showing 4 changed files with 145 additions and 14 deletions.
diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h b/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h
@@ -45,10 +45,10 @@
 /**
  * \brief Helper class to simplify integrating the JointTrajectoryController with different hardware interfaces.
  *
- * The JointTrajectoryController outputs position, velocity, command triplets, while the more common hardware interfaces
- * it is supposed to work with accept either position or effort commands.
+ * The JointTrajectoryController outputs position, velocity and acceleration command triplets, while the more common hardware
+ * interfaces accept position, velocity or effort commands.
  *
- * Use one of the avaialble template specializations of this class (or create your own) to adapt the
+ * 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 HardwareInterface, class State>
@@ -71,6 +71,23 @@ class HardwareInterfaceAdapter
 
 /**
  * \brief Adapter for a position-controlled hardware interface. Forwards desired positions as commands.
+ *
+ * The following is an example configuration of a controller that uses this adapter.
+ * \code
+ * head_controller:
+ *   type: "position_controllers/JointTrajectoryController"
+ *   joints:
+ *     - head_1_joint
+ *     - head_2_joint
+ *   
+ *   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::PositionJointInterface, State>
@@ -104,29 +121,121 @@ class HardwareInterfaceAdapter<hardware_interface::PositionJointInterface, State
 };
 
 /**
- * \brief Adapter for an effort-controlled hardware interface. Maps position and velocity errors to effort commands
- * through a position PID loop.
+ * \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 entry:
  * \code
  * head_controller:
- *   type: "position_controllers/JointTrajectoryController"
+ *   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:
- *       goal: 0.01
+ *     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:
+  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 velocity 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_;
+};
+
+/**
+ * \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:
+ * \code
+ * head_controller:
+ *   type: "effort_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
  */
 template <class State>

diff --git a/...t_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h b/...t_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.h
@@ -118,8 +118,9 @@ namespace joint_trajectory_controller
  * };
  * \endcode
  *
- * \tparam HardwareInterface Controller hardware interface. Currently \p hardware_interface::PositionJointInterface and
- * \p hardware_interface::EffortJointInterface are supported out-of-the-box.
+ * \tparam HardwareInterface Controller hardware interface. Currently \p hardware_interface::PositionJointInterface,
+ * \p hardware_interface::VelocityJointInterface, and \p hardware_interface::EffortJointInterface are supported 
+ * out-of-the-box.
  */
 template <class SegmentImpl, class HardwareInterface>
 class JointTrajectoryController : public controller_interface::Controller<HardwareInterface>

diff --git a/joint_trajectory_controller/ros_control_plugins.xml b/joint_trajectory_controller/ros_control_plugins.xml
@@ -9,6 +9,15 @@
     </description>
   </class>
 
+  <class name="velocity_controllers/JointTrajectoryController"
+         type="velocity_controllers::JointTrajectoryController"
+         base_class_type="controller_interface::ControllerBase">
+    <description>
+      The JointTrajectoryController executes joint-space trajectories on a set of joints.
+      This variant represents trajectory segments as quintic splines and sends commands to a velocity interface.
+    </description>
+  </class>
+
   <class name="effort_controllers/JointTrajectoryController"
          type="effort_controllers::JointTrajectoryController"
          base_class_type="controller_interface::ControllerBase">

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -44,6 +44,17 @@ namespace position_controllers
           JointTrajectoryController;
 }
 
+namespace velocity_controllers
+{
+  /**
+   * \brief Joint trajectory controller that represents trajectory segments as <b>quintic splines</b> and sends
+   * commands to a \b velocity interface.
+   */
+  typedef joint_trajectory_controller::JointTrajectoryController<trajectory_interface::QuinticSplineSegment<double>,
+                                                                 hardware_interface::VelocityJointInterface>
+          JointTrajectoryController;
+}
+
 namespace effort_controllers
 {
   /**
@@ -56,4 +67,5 @@ namespace effort_controllers
 }
 
 PLUGINLIB_EXPORT_CLASS(position_controllers::JointTrajectoryController, controller_interface::ControllerBase)
+PLUGINLIB_EXPORT_CLASS(velocity_controllers::JointTrajectoryController, controller_interface::ControllerBase)
 PLUGINLIB_EXPORT_CLASS(effort_controllers::JointTrajectoryController,   controller_interface::ControllerBase)