cartesian_controller.hpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  *
00003  *  Software License Agreement (BSD License)
00004  *
00005  *  Copyright (c) 2014, Johannes Maurer
00006  *                      Institute for Software Technology,
00007  *                      Graz University of Technology
00008  *  All rights reserved.
00009  *
00010  *  Redistribution and use in source and binary forms, with or without
00011  *  modification, are permitted provided that the following conditions
00012  *  are met:
00013  *
00014  *   * Redistributions of source code must retain the above copyright
00015  *     notice, this list of conditions and the following disclaimer.
00016  *   * Redistributions in binary form must reproduce the above
00017  *     copyright notice, this list of conditions and the following
00018  *     disclaimer in the documentation and/or other materials provided
00019  *     with the distribution.
00020  *   * Neither the name of Graz University of Technology nor the names of
00021  *     its contributors may be used to endorse or promote products derived
00022  *     from this software without specific prior written permission.
00023  *
00024  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00025  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00026  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00027  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00028  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00029  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00030  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00031  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00032  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00033  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00034  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00035  *  POSSIBILITY OF SUCH DAMAGE.
00036  *
00037  *********************************************************************/
00038 
00039 #ifndef cartesian_controller_hpp___
00040 #define cartesian_controller_hpp___
00041 
00042 #include <controller_interface/controller.h>
00043 #include <hardware_interface/joint_command_interface.h>
00044 #include <geometry_msgs/TwistStamped.h>
00045 
00046 #include <boost/scoped_ptr.hpp>
00047 #include <kdl/chain.hpp>
00048 #include <kdl/tree.hpp>
00049 #include <kdl/chainjnttojacsolver.hpp>
00050 #include <kdl/chainfksolverpos_recursive.hpp>
00051 #include <kdl/frames.hpp>
00052 #include <kdl/jacobian.hpp>
00053 #include <kdl/jntarray.hpp>
00054 #include <kdl/chainiksolver.hpp>
00055 #include <kdl/chainiksolvervel_pinv.hpp>
00056 #include <kdl/chainfksolver.hpp>
00057 #include <kdl/chainfksolverpos_recursive.hpp>
00058 
00059 
00060 namespace velocity_controllers
00061 {
00062 
00063 class CartesianController : public controller_interface::Controller<hardware_interface::VelocityJointInterface>
00064 {
00065 public:
00066     bool init(hardware_interface::VelocityJointInterface* hw, ros::NodeHandle &nh);
00067 
00068     void update(const ros::Time& time, const ros::Duration& period);
00069 
00070     void starting(const ros::Time& time);
00071     void stopping(const ros::Time& time);
00072 
00073 private:
00074     hardware_interface::JointHandle joint_;
00075     ros::Subscriber vel_cmd_sub_;
00076 
00077     KDL::Tree kdl_tree_;
00078     KDL::Chain kdl_chain_;
00079     std::vector<hardware_interface::JointHandle> joint_handles_;
00080 
00081     boost::scoped_ptr<KDL::ChainIkSolverVel> chain_ik_solver_vel_;
00082     boost::scoped_ptr<KDL::ChainFkSolverPos> chain_fk_solver_;
00083 
00084     KDL::Twist cmd_linear_twist_;
00085     KDL::Twist cmd_angular_twist_;
00086 
00087     std::string root_name_, tip_name_;
00088 
00089     bool got_msg_;
00090 
00091     ros::Time last_msg_;
00092     ros::Duration dead_man_timeout_;
00093 
00094     double joint_vel_limit_;
00095 
00096     void velCmdCB(const geometry_msgs::TwistStampedConstPtr& msg);
00097 };
00098 
00099 }
00100 
00101 #endif // cartesian_controller_hpp___


tedusar_cartesian_controller
Author(s): Johannes Maurer , Alexander Buchegger
autogenerated on Wed Aug 26 2015 16:30:39