Fawkes API  Fawkes Development Version
goto_openrave_thread.h
1 
2 /***************************************************************************
3  * goto_openrave_thread.h - Katana goto one-time thread using openrave lib
4  *
5  * Created: Wed Jun 10 11:44:24 2009
6  * Copyright 2006-2009 Tim Niemueller [www.niemueller.de]
7  * 2011-2014 Bahram Maleki-Fard
8  *
9  ****************************************************************************/
10 
11 /* This program is free software; you can redistribute it and/or modify
12  * it under the terms of the GNU General Public License as published by
13  * the Free Software Foundation; either version 2 of the License, or
14  * (at your option) any later version.
15  *
16  * This program is distributed in the hope that it will be useful,
17  * but WITHOUT ANY WARRANTY; without even the implied warranty of
18  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19  * GNU Library General Public License for more details.
20  *
21  * Read the full text in the LICENSE.GPL file in the doc directory.
22  */
23 
24 #ifndef _PLUGINS_KATANA_GOTO_OPENRAVE_THREAD_H_
25 #define _PLUGINS_KATANA_GOTO_OPENRAVE_THREAD_H_
26 
27 #include "motion_thread.h"
28 
29 #ifdef HAVE_OPENRAVE
30 # include <plugins/openrave/types.h>
31 #endif
32 
33 #include <string>
34 #include <vector>
35 
36 namespace fawkes {
37 class OpenRaveConnector;
38 }
39 
40 /** class KatanaGotoOpenRaveThread */
42 {
43 #ifdef HAVE_OPENRAVE
44 public:
46  fawkes::Logger * logger,
47  fawkes::OpenRaveConnector * openrave,
48  unsigned int poll_interval_ms,
49  const std::string & robot_file,
50  const std::string & arm_model,
51  bool autoload_IK,
52  bool use_viewer);
53 
54  virtual void once();
55  virtual void init();
56  virtual void finalize();
57 
58  void set_target(float x, float y, float z, float phi, float theta, float psi);
59  void
60  set_target(float x, float y, float z, float quat_x, float quat_y, float quat_z, float quat_w);
61  void set_target(const std::string &object_name, float rot_x);
62  void set_theta_error(float error);
63  void set_move_straight(bool move_straight);
64  void set_arm_extension(bool arm_extension);
65  void set_plannerparams(std::string &params, bool straight = false);
66  void set_plannerparams(const char *params, bool straight = false);
67 
68  virtual bool plan_target();
69  virtual void update_openrave_data();
70  virtual bool update_motor_data();
71  virtual bool move_katana();
72 
73  static const std::string DEFAULT_PLANNERPARAMS;
74  static const std::string DEFAULT_PLANNERPARAMS_STRAIGHT;
75 
76 private:
77  fawkes::OpenRaveRobotPtr OR_robot_;
79 
80  std::string target_object_;
81  std::vector<std::vector<float>> * target_traj_;
82  std::vector<std::vector<float>>::iterator it_;
83 
84  std::vector<int> motor_encoders_;
85  std::vector<float> motor_angles_;
86 
87  const std::string cfg_robot_file_;
88  const std::string cfg_arm_model_;
89  bool cfg_autoload_IK_;
90  bool cfg_use_viewer_;
91 
92  bool is_target_object_;
93  bool has_target_quaternion_;
94  bool move_straight_;
95  bool is_arm_extension_;
96  std::string plannerparams_;
97  std::string plannerparams_straight_;
98 
99  fawkes::OpenRaveConnector *_openrave;
100 
101  float x_, y_, z_;
102  float phi_, theta_, psi_, theta_error_;
103  float quat_x_, quat_y_, quat_z_, quat_w_;
104  unsigned int poll_interval_usec_;
105 
106 #endif //HAVE_OPENRAVE
107 };
108 
109 #endif
fawkes::Thread::finalize
virtual void finalize()
Finalize the thread.
Definition: thread.cpp:467
KatanaGotoOpenRaveThread
class KatanaGotoOpenRaveThread
Definition: goto_openrave_thread.h:40
KatanaMotionThread
Definition: motion_thread.h:34
fawkes::RefPtr< fawkes::KatanaController >
fawkes::Thread::once
virtual void once()
Execute an action exactly once.
Definition: thread.cpp:1089
fawkes::Logger
Definition: logger.h:40
fawkes
fawkes::OpenRaveConnector
Definition: openrave_connector.h:52
fawkes::Thread::init
virtual void init()
Initialize the thread.
Definition: thread.cpp:347