lwr_controller.h
Go to the documentation of this file.
00001 /*
00002  *  Gazebo - Outdoor Multi-Robot Simulator
00003  *  Copyright (C) 2003  
00004  *     Nate Koenig & Andrew Howard
00005  *
00006  *  This program is free software; you can redistribute it and/or modify
00007  *  it under the terms of the GNU General Public License as published by
00008  *  the Free Software Foundation; either version 2 of the License, or
00009  *  (at your option) any later version.
00010  *
00011  *  This program is distributed in the hope that it will be useful,
00012  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00014  *  GNU General Public License for more details.
00015  *
00016  *  You should have received a copy of the GNU General Public License
00017  *  along with this program; if not, write to the Free Software
00018  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00019  *
00020  */
00021 /*
00022  * Desc: 3D position interface.
00023  * Author: Sachin Chitta and John Hsu
00024  * Date: 10 June 2008
00025  * SVN: $Id$
00026  */
00027 #ifndef LWR_CONTROLLER_HH
00028 #define LWR_CONTROLLER_HH
00029 
00030 #include <ros/callback_queue.h>
00031 #include <ros/advertise_options.h>
00032 
00033 #include "physics/physics.h"
00034 #include "transport/TransportTypes.hh"
00035 #include "common/Time.hh"
00036 #include "common/Plugin.hh"
00037 #include "common/Events.hh"
00038 #include <ros/ros.h>
00039 #include <kdl/chain.hpp>
00040 #include <kdl/chaindynparam.hpp>
00041 #include <kdl/chainfksolverpos_recursive.hpp>
00042 #include <kdl/chainjnttojacsolver.hpp>
00043 #include "Eigen/Dense"
00044 #include <kuka_lwr_fri/friComm.h>
00045 
00046 #include <sys/socket.h> /* for bind socket accept */
00047 #include <unistd.h> /* for close() */
00048 #include <arpa/inet.h>/* for inet_Addr etc*/
00049 
00050 namespace gazebo
00051 {
00052 
00053    class LWRController : public ModelPlugin
00054    {
00055       public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00057       public: LWRController();
00058 
00060       public: virtual ~LWRController();
00061 
00063       public: void Load( physics::ModelPtr _parent, sdf::ElementPtr _sdf );
00064 
00066       protected: virtual void UpdateChild();
00067       
00068       void GetRobotChain();
00069       
00070       private:
00071         /*
00072          *  \brief pointer to ros node
00073          */
00074         ros::NodeHandle* rosnode_;
00075         
00076         gazebo::physics::ModelPtr parent_model_;
00077         std::string robotPrefix;
00078         std::vector<gazebo::physics::JointPtr>  joints_;
00079         
00080          // Pointer to the model
00081         physics::WorldPtr world;
00082 
00083         // Pointer to the update event connection
00084         event::ConnectionPtr updateConnection;
00085         
00086         KDL::Chain chain_;
00087         KDL::ChainDynParam *dyn;
00088         KDL::ChainFkSolverPos_recursive *fk;
00089         KDL::ChainJntToJacSolver *jc;
00090 
00091         std::string base_frame_;
00092         
00093         int cnt;
00094         
00095         Eigen::Matrix<double, 7, 1> joint_pos;
00096         Eigen::Matrix<double, 7, 1> joint_pos_cmd;
00097         Eigen::Matrix<double, 7, 1> joint_vel;
00098         Eigen::Matrix<double, 7, 1> stiffness_;
00099         Eigen::Matrix<double, 7, 1> damping_;
00100         Eigen::Matrix<double, 7, 1> trq_cmd_;
00101         Eigen::Matrix<double, 7, 1> trq;
00102         
00103         int remote_port;
00104         std::string remote;
00105         
00106         int socketFd;
00107         struct sockaddr_in localAddr, remoteAddr;
00108 
00109         tFriMsrData m_msr_data;
00110         tFriCmdData m_cmd_data;
00111 
00112    };
00113 
00114 }
00115 
00116 #endif
00117 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


lwr_simulation
Author(s): Konrad Banachowicz
autogenerated on Thu Nov 14 2013 11:57:28