Fawkes API  Fawkes Development Version
controller_openrave.h
00001 
00002 /***************************************************************************
00003  *  controller_openrave.h - OpenRAVE Controller class for katana arm
00004  *
00005  *  Created: Sat Jan 07 16:10:54 2012
00006  *  Copyright  2012  Bahram Maleki-Fard, AllemaniACs RoboCup Team
00007  *
00008  ****************************************************************************/
00009 
00010 /*  This program is free software; you can redistribute it and/or modify
00011  *  it under the terms of the GNU General Public License as published by
00012  *  the Free Software Foundation; either version 2 of the License, or
00013  *  (at your option) any later version.
00014  *
00015  *  This program is distributed in the hope that it will be useful,
00016  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00017  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00018  *  GNU Library General Public License for more details.
00019  *
00020  *  Read the full text in the LICENSE.GPL file in the doc directory.
00021  */
00022 
00023 #ifndef __PLUGINS_KATANA_CONTROLLER_OPENRAVE_H_
00024 #define __PLUGINS_KATANA_CONTROLLER_OPENRAVE_H_
00025 
00026 #include "controller.h"
00027 
00028 #include <core/utils/refptr.h>
00029 #include <plugins/openrave/aspect/openrave.h>
00030 
00031 #ifdef HAVE_OPENRAVE
00032 #include <openrave/openrave.h>
00033 #endif
00034 
00035 #include <string>
00036 #include <memory>
00037 #include <vector>
00038 
00039 
00040 namespace fawkes {
00041 #if 0 /* just to make Emacs auto-indent happy */
00042 }
00043 #endif
00044 
00045 class KatanaControllerOpenrave : public KatanaController
00046 {
00047 #ifdef HAVE_OPENRAVE
00048  public:
00049   KatanaControllerOpenrave(fawkes::OpenRaveConnector* openrave);
00050   virtual ~KatanaControllerOpenrave();
00051 
00052   // setup
00053   virtual void init();
00054   virtual void set_max_velocity(unsigned int vel);
00055 
00056   // status checking
00057   virtual bool final();
00058   virtual bool joint_angles();
00059   virtual bool joint_encoders();
00060 
00061   // commands
00062   virtual void calibrate();
00063   virtual void stop();
00064   virtual void turn_on();
00065   virtual void turn_off();
00066   virtual void read_coordinates(bool refresh = false);
00067   virtual void read_motor_data();
00068   virtual void read_sensor_data();
00069   virtual void gripper_open(bool blocking = false);
00070   virtual void gripper_close(bool blocking = false);
00071   virtual void move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking = false);
00072   virtual void move_to(std::vector<int> encoders, bool blocking = false);
00073   virtual void move_to(std::vector<float> angles, bool blocking = false);
00074   virtual void move_motor_to(unsigned short id, int enc, bool blocking = false);
00075   virtual void move_motor_to(unsigned short id, float angle, bool blocking = false);
00076   virtual void move_motor_by(unsigned short id, int enc, bool blocking = false);
00077   virtual void move_motor_by(unsigned short id, float angle, bool blocking = false);
00078 
00079   // getters
00080   virtual double x();
00081   virtual double y();
00082   virtual double z();
00083   virtual double phi();
00084   virtual double theta();
00085   virtual double psi();
00086   virtual void get_sensors(std::vector<int>& to, bool refresh = false);
00087   virtual void get_encoders(std::vector<int>& to, bool refresh = false);
00088   virtual void get_angles(std::vector<float>& to, bool refresh = false);
00089 
00090  private:
00091   double __x, __y, __z;
00092   double __phi, __theta, __psi;
00093 
00094   fawkes::OpenRaveConnector*            __openrave;
00095   fawkes::OpenRaveEnvironment*          __OR_env;
00096   fawkes::OpenRaveRobot*                __OR_robot;
00097   fawkes::OpenRaveManipulator*          __OR_manip;
00098   OpenRAVE::EnvironmentBasePtr          __env;
00099   OpenRAVE::RobotBasePtr                __robot;
00100   OpenRAVE::RobotBase::ManipulatorPtr   __manip;
00101 
00102   bool __initialized;
00103 
00104   std::vector<short>             __active_motors;
00105 
00106   void update_manipulator();
00107   void wait_finished();
00108   void check_init();
00109 
00110   bool motor_oor(unsigned short id);
00111 #endif //HAVE_OPENRAVE
00112 };
00113 
00114 
00115 } // end of namespace fawkes
00116 
00117 #endif