Fawkes API  Fawkes Development Version
openrave_thread.h
1 
2 /***************************************************************************
3  * openrave_thread.h - Jaco plugin OpenRAVE thread for single arm
4  *
5  * Created: Mon Jul 28 19:43:20 2014
6  * Copyright 2014 Bahram Maleki-Fard
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #ifndef _PLUGINS_JACO_OPENRAVE_THREAD_H_
24 #define _PLUGINS_JACO_OPENRAVE_THREAD_H_
25 
26 #include "openrave_base_thread.h"
27 
28 #ifdef HAVE_OPENRAVE
29 # include <openrave/openrave.h>
30 # include <plugins/openrave/aspect/openrave.h>
31 #endif
32 
33 #include <string>
34 #include <vector>
35 
37 {
38 public:
39  JacoOpenraveThread(const char *name, fawkes::jaco_arm_t *arm, bool load_robot = true);
40 
41  virtual void loop();
42  virtual void finalize();
43 
44  virtual void update_openrave();
45  virtual void plot_first();
46 
47  virtual bool
48  add_target(float x, float y, float z, float e1, float e2, float e3, bool plan = true);
49  virtual bool
50  add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan = true);
51  virtual bool
52  set_target(float x, float y, float z, float e1, float e2, float e3, bool plan = true);
53  virtual bool
54  set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan = true);
55 
56 protected:
57  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
58  virtual void
59  run()
60  {
61  Thread::run();
62  }
63 
64 private:
65  void _init();
66  void _load_robot();
67  void _post_init();
68 
69  void _plan_path(fawkes::RefPtr<fawkes::jaco_target_t> &from,
71 
72  fawkes::jaco_arm_t *arm_;
73 
74  std::string cfg_manipname_;
75  bool load_robot_;
76 
77 #ifdef HAVE_OPENRAVE
78  fawkes::jaco_openrave_set_t planner_env_;
79 
80  OpenRAVE::RobotBasePtr robot_;
81  OpenRAVE::RobotBase::ManipulatorPtr manip_;
82  std::vector<OpenRAVE::dReal> joints_;
83 
84  std::vector<OpenRAVE::GraphHandlePtr> graph_handle_;
85  std::vector<OpenRAVE::GraphHandlePtr> graph_current_;
86 
87  std::vector<OpenRAVE::KinBody::LinkPtr> links_;
88 
89  bool plotted_current_; // keep track of plotting
90 #endif
91 };
92 
93 #endif
Jaco struct containing all components required for one arm.
Definition: types.h:92
virtual void update_openrave()
Update the openrave environment to represent the current situation.
virtual void plot_first()
Plot the first target of the queue in the viewer_env.
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void finalize()
Finalize the thread.
virtual bool set_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Flush the target_queue and add this one.
JacoOpenraveThread(const char *name, fawkes::jaco_arm_t *arm, bool load_robot=true)
Constructor.
virtual bool add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Add target joint values to the queue.
const char * name() const
Get name of thread.
Definition: thread.h:100
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
Jaco Arm thread for single-arm setup, integrating OpenRAVE.
virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Solve IK and add target to the queue.
virtual void loop()
Mani loop.
virtual bool set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Flush the target_queue and add this one.
Base Jaco Arm thread, integrating OpenRAVE.