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 <plugins/openrave/aspect/openrave.h>
30  #include <openrave/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 add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true);
48  virtual bool add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true);
49  virtual bool set_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true);
50  virtual bool set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true);
51 
52  protected:
53  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
54  virtual void run() { Thread::run(); }
55 
56  private:
57  void _init();
58  void _load_robot();
59  void _post_init();
60 
62 
63  fawkes::jaco_arm_t *__arm;
64 
65  std::string __cfg_manipname;
66  bool __load_robot;
67 
68 #ifdef HAVE_OPENRAVE
69  fawkes::jaco_openrave_set_t __planner_env;
70 
71  OpenRAVE::RobotBasePtr __robot;
72  OpenRAVE::RobotBase::ManipulatorPtr __manip;
73  std::vector<OpenRAVE::dReal> __joints;
74 
75  std::vector<OpenRAVE::GraphHandlePtr> __graph_handle;
76  std::vector<OpenRAVE::GraphHandlePtr> __graph_current;
77 
78  std::vector<OpenRAVE::KinBody::LinkPtr> __links;
79 
80  bool __plotted_current; // keep track of plotting
81 #endif
82 };
83 
84 
85 #endif
Jaco struct containing all components required for one arm.
Definition: types.h:95
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:95
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.