hardware_interface_adapter.h
Go to the documentation of this file.
00001 
00002 // Copyright (C) 2013, PAL Robotics S.L.
00003 //
00004 // Redistribution and use in source and binary forms, with or without
00005 // modification, are permitted provided that the following conditions are met:
00006 //   * Redistributions of source code must retain the above copyright notice,
00007 //     this list of conditions and the following disclaimer.
00008 //   * Redistributions in binary form must reproduce the above copyright
00009 //     notice, this list of conditions and the following disclaimer in the
00010 //     documentation and/or other materials provided with the distribution.
00011 //   * Neither the name of PAL Robotics S.L. nor the names of its
00012 //     contributors may be used to endorse or promote products derived from
00013 //     this software without specific prior written permission.
00014 //
00015 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00016 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00017 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00018 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00019 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00020 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00021 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00022 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00023 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00024 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00025 // POSSIBILITY OF SUCH DAMAGE.
00027 
00029 
00030 #ifndef JOINT_TRAJECTORY_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
00031 #define JOINT_TRAJECTORY_CONTROLLER_HARDWARE_INTERFACE_ADAPTER_H
00032 
00033 #include <cassert>
00034 #include <string>
00035 #include <vector>
00036 
00037 #include <boost/shared_ptr.hpp>
00038 
00039 #include <ros/node_handle.h>
00040 #include <ros/time.h>
00041 
00042 #include <control_toolbox/pid.h>
00043 #include <hardware_interface/joint_command_interface.h>
00044 
00054 template <class HardwareInterface, class State>
00055 class HardwareInterfaceAdapter
00056 {
00057 public:
00058   bool init(std::vector<typename HardwareInterface::ResourceHandleType>& /*joint_handles*/, ros::NodeHandle& /*controller_nh*/)
00059   {
00060     return false;
00061   }
00062 
00063   void starting(const ros::Time& /*time*/) {}
00064   void stopping(const ros::Time& /*time*/) {}
00065 
00066   void updateCommand(const ros::Time&     /*time*/,
00067                      const ros::Duration& /*period*/,
00068                      const State&         /*desired_state*/,
00069                      const State&         /*state_error*/) {}
00070 };
00071 
00092 template <class State>
00093 class HardwareInterfaceAdapter<hardware_interface::PositionJointInterface, State>
00094 {
00095 public:
00096   HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
00097 
00098   bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& /*controller_nh*/)
00099   {
00100     // Store pointer to joint handles
00101     joint_handles_ptr_ = &joint_handles;
00102 
00103     return true;
00104   }
00105 
00106   void starting(const ros::Time& /*time*/)
00107   {
00108     if (!joint_handles_ptr_) {return;}
00109 
00110     // Semantic zero for commands
00111     for (unsigned int i = 0; i < joint_handles_ptr_->size(); ++i)
00112     {
00113       (*joint_handles_ptr_)[i].setCommand((*joint_handles_ptr_)[i].getPosition());
00114     }
00115   }
00116 
00117   void stopping(const ros::Time& /*time*/) {}
00118 
00119   void updateCommand(const ros::Time&     /*time*/,
00120                      const ros::Duration& /*period*/,
00121                      const State&         desired_state,
00122                      const State&         /*state_error*/)
00123   {
00124     // Forward desired position to command
00125     const unsigned int n_joints = joint_handles_ptr_->size();
00126     for (unsigned int i = 0; i < n_joints; ++i) {(*joint_handles_ptr_)[i].setCommand(desired_state.position[i]);}
00127   }
00128 
00129 private:
00130   std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
00131 };
00132 
00156 template <class State>
00157 class HardwareInterfaceAdapter<hardware_interface::VelocityJointInterface, State>
00158 {
00159 public:
00160   HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
00161 
00162   bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& controller_nh)
00163   {
00164     // Store pointer to joint handles
00165     joint_handles_ptr_ = &joint_handles;
00166 
00167     // Initialize PIDs
00168     pids_.resize(joint_handles.size());
00169     for (unsigned int i = 0; i < pids_.size(); ++i)
00170     {
00171       // Node handle to PID gains
00172       ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handles[i].getName());
00173 
00174       // Init PID gains from ROS parameter server
00175       pids_[i].reset(new control_toolbox::Pid());
00176       if (!pids_[i]->init(joint_nh))
00177       {
00178         ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
00179         return false;
00180       }
00181     }
00182 
00183     return true;
00184   }
00185 
00186   void starting(const ros::Time& /*time*/)
00187   {
00188     if (!joint_handles_ptr_) {return;}
00189 
00190     // Reset PIDs, zero velocity commands
00191     for (unsigned int i = 0; i < pids_.size(); ++i)
00192     {
00193       pids_[i]->reset();
00194       (*joint_handles_ptr_)[i].setCommand(0.0);
00195     }
00196   }
00197 
00198   void stopping(const ros::Time& time) {}
00199 
00200   void updateCommand(const ros::Time&     /*time*/,
00201                      const ros::Duration& period,
00202                      const State&         /*desired_state*/,
00203                      const State&         state_error)
00204   {
00205     const unsigned int n_joints = joint_handles_ptr_->size();
00206 
00207     // Preconditions
00208     if (!joint_handles_ptr_)
00209       return;
00210     assert(n_joints == state_error.position.size());
00211     assert(n_joints == state_error.velocity.size());
00212 
00213     // Update PIDs
00214     for (unsigned int i = 0; i < n_joints; ++i)
00215     {
00216       const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
00217       (*joint_handles_ptr_)[i].setCommand(command);
00218     }
00219   }
00220 
00221 private:
00222   typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
00223   std::vector<PidPtr> pids_;
00224 
00225   std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
00226 };
00227 
00251 template <class State>
00252 class HardwareInterfaceAdapter<hardware_interface::EffortJointInterface, State>
00253 {
00254 public:
00255   HardwareInterfaceAdapter() : joint_handles_ptr_(0) {}
00256 
00257   bool init(std::vector<hardware_interface::JointHandle>& joint_handles, ros::NodeHandle& controller_nh)
00258   {
00259     // Store pointer to joint handles
00260     joint_handles_ptr_ = &joint_handles;
00261 
00262     // Initialize PIDs
00263     pids_.resize(joint_handles.size());
00264     for (unsigned int i = 0; i < pids_.size(); ++i)
00265     {
00266       // Node handle to PID gains
00267       ros::NodeHandle joint_nh(controller_nh, std::string("gains/") + joint_handles[i].getName());
00268 
00269       // Init PID gains from ROS parameter server
00270       pids_[i].reset(new control_toolbox::Pid());
00271       if (!pids_[i]->init(joint_nh))
00272       {
00273         ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
00274         return false;
00275       }
00276     }
00277 
00278     return true;
00279   }
00280 
00281   void starting(const ros::Time& /*time*/)
00282   {
00283     if (!joint_handles_ptr_) {return;}
00284 
00285     // Reset PIDs, zero effort commands
00286     for (unsigned int i = 0; i < pids_.size(); ++i)
00287     {
00288       pids_[i]->reset();
00289       (*joint_handles_ptr_)[i].setCommand(0.0);
00290     }
00291   }
00292 
00293   void stopping(const ros::Time& /*time*/) {}
00294 
00295   void updateCommand(const ros::Time&     /*time*/,
00296                      const ros::Duration& period,
00297                      const State&         /*desired_state*/,
00298                      const State&         state_error)
00299   {
00300     const unsigned int n_joints = joint_handles_ptr_->size();
00301 
00302     // Preconditions
00303     if (!joint_handles_ptr_) {return;}
00304     assert(n_joints == state_error.position.size());
00305     assert(n_joints == state_error.velocity.size());
00306 
00307     // Update PIDs
00308     for (unsigned int i = 0; i < n_joints; ++i)
00309     {
00310       const double command = pids_[i]->computeCommand(state_error.position[i], state_error.velocity[i], period);
00311       (*joint_handles_ptr_)[i].setCommand(command);
00312     }
00313   }
00314 
00315 private:
00316   typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
00317   std::vector<PidPtr> pids_;
00318 
00319   std::vector<hardware_interface::JointHandle>* joint_handles_ptr_;
00320 };
00321 
00322 #endif // header guard


joint_trajectory_controller
Author(s): Adolfo Rodriguez Tsouroukdissian
autogenerated on Sat Aug 13 2016 04:20:51