Fawkes API
Fawkes Development Version
|
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