Fawkes API  Fawkes Development Version
goto_openrave_thread.h
00001 
00002 /***************************************************************************
00003  *  goto_openrave_thread.h - Katana goto one-time thread using openrave lib
00004  *
00005  *  Created: Wed Jun 10 11:44:24 2009
00006  *  Copyright  2006-2009  Tim Niemueller [www.niemueller.de]
00007  *                  2010  Bahram Maleki-Fard
00008  *
00009  ****************************************************************************/
00010 
00011 /*  This program is free software; you can redistribute it and/or modify
00012  *  it under the terms of the GNU General Public License as published by
00013  *  the Free Software Foundation; either version 2 of the License, or
00014  *  (at your option) any later version.
00015  *
00016  *  This program is distributed in the hope that it will be useful,
00017  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00018  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00019  *  GNU Library General Public License for more details.
00020  *
00021  *  Read the full text in the LICENSE.GPL file in the doc directory.
00022  */
00023 
00024 #ifndef __PLUGINS_KATANA_GOTO_OPENRAVE_THREAD_H_
00025 #define __PLUGINS_KATANA_GOTO_OPENRAVE_THREAD_H_
00026 
00027 #include "motion_thread.h"
00028 
00029 #include <vector>
00030 #include <string>
00031 
00032 #include <plugins/openrave/aspect/openrave.h>
00033 
00034 #ifdef HAVE_OPENRAVE
00035 namespace fawkes {
00036   class OpenRaveRobot;
00037   class OpenRaveManipulator;
00038 }
00039 #endif
00040 
00041 /** class KatanaGotoOpenRaveThread */
00042 class KatanaGotoOpenRaveThread : public KatanaMotionThread
00043 {
00044 #ifdef HAVE_OPENRAVE
00045 
00046  public:
00047   KatanaGotoOpenRaveThread(fawkes::RefPtr<fawkes::KatanaController> katana, fawkes::Logger *logger, fawkes::OpenRaveConnector* openrave,
00048                    unsigned int poll_interval_ms,
00049                    std::string robot_file,
00050                    std::string arm_model,
00051                    bool autoload_IK,
00052                    bool use_viewer);
00053 
00054   virtual void once();
00055   virtual void init();
00056   virtual void finalize();
00057 
00058   void set_target(float x, float y, float z, float phi, float theta, float psi);
00059   void set_target(float x, float y, float z, float quat_x, float quat_y, float quat_z, float quat_w);
00060   void set_target(const std::string& object_name, float rot_x);
00061   void set_theta_error(float error);
00062   void set_move_straight(bool move_straight);
00063   void set_arm_extension(bool arm_extension);
00064   void set_plannerparams(std::string& params, bool straight=false);
00065   void set_plannerparams(const char* params, bool straight=false);
00066 
00067   virtual bool plan_target();
00068   virtual void update_openrave_data();
00069   virtual bool update_motor_data();
00070   virtual bool move_katana();
00071 
00072   static const std::string DEFAULT_PLANNERPARAMS;
00073   static const std::string DEFAULT_PLANNERPARAMS_STRAIGHT;
00074 
00075  private:
00076   fawkes::OpenRaveRobot*        __OR_robot;
00077   fawkes::OpenRaveManipulator*  __OR_manip;
00078 
00079   std::string                                   __target_object;
00080   std::vector< std::vector<float> >*            __target_traj;
00081   std::vector< std::vector<float> >::iterator   __it;
00082 
00083   std::vector< int >    __motor_encoders;
00084   std::vector< float >  __motor_angles;
00085 
00086   const std::string     __cfg_robot_file;
00087   const std::string     __cfg_arm_model;
00088   bool                  __cfg_autoload_IK;
00089   bool                  __cfg_use_viewer;
00090 
00091   bool                  __is_target_object;
00092   bool                  __has_target_quaternion;
00093   bool                  __move_straight;
00094   bool                  __is_arm_extension;
00095   std::string           __plannerparams;
00096   std::string           __plannerparams_straight;
00097 
00098   fawkes::OpenRaveConnector*    _openrave;
00099 
00100   float __x, __y, __z;
00101   float __phi, __theta, __psi, __theta_error;
00102   float __quat_x, __quat_y, __quat_z, __quat_w;
00103   unsigned int __poll_interval_usec;
00104 
00105 #endif //HAVE_OPENRAVE
00106 };
00107 
00108 #endif