Fawkes API  Fawkes Development Version
bimanual_openrave_thread.h
1 
2 /***************************************************************************
3  * bimanual_openrave_thread.h - Jaco plugin OpenRAVE thread for bimanual manipulatin
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_BIMANUAL_OPENRAVE_THREAD_H_
24 #define _PLUGINS_JACO_BIMANUAL_OPENRAVE_THREAD_H_
25 
26 #include "openrave_base_thread.h"
27 
28 #ifdef HAVE_OPENRAVE
29 # include <openrave/openrave.h>
30 #endif
31 
32 #include <string>
33 
35 {
36 public:
38 
39  virtual void loop();
40  virtual void finalize();
41 
42  virtual void update_openrave();
43  virtual void plot_first();
44 
45  virtual bool add_target(float l_x,
46  float l_y,
47  float l_z,
48  float l_e1,
49  float l_e2,
50  float l_e3,
51  float r_x,
52  float r_y,
53  float r_z,
54  float r_e1,
55  float r_e2,
56  float r_e3);
57 
58  virtual void set_constrained(bool enable);
59 
60 protected:
61  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
62  virtual void
63  run()
64  {
65  Thread::run();
66  }
67 
68 private:
69  void _init();
70  void _load_robot();
71  void _init_dualmanipulation();
72 
73  void _set_trajec_state(fawkes::jaco_trajec_state_t state);
74  void _copy_env();
75  bool _solve_multi_ik(std::vector<float> &left, std::vector<float> &right);
76  bool _plan_path();
77 
78  typedef struct arm_struct
79  {
80  fawkes::jaco_arm_t *arm;
81 #ifdef HAVE_OPENRAVE
82  std::string manipname;
83  OpenRAVE::RobotBase::ManipulatorPtr manip;
85 #endif
86  } arm_struct_t;
87 
88  struct
89  {
90  arm_struct_t left;
91  arm_struct_t right;
92  } arms_;
93 
94 #ifdef HAVE_OPENRAVE
95  fawkes::jaco_openrave_set_t planner_env_;
96  OpenRAVE::ModuleBasePtr mod_dualmanip_;
97 
98  std::set<OpenRAVE::KinBody::LinkPtr> links_left_;
99  std::set<OpenRAVE::KinBody::LinkPtr> links_right_;
100 #endif
101 
102  bool __constrained;
103 };
104 
105 #endif
JacoBimanualOpenraveThread::update_openrave
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Definition: bimanual_openrave_thread.cpp:372
fawkes::RefPtr
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:55
JacoBimanualOpenraveThread::plot_first
virtual void plot_first()
Plot the first target of the target_queue, if it is a trajectory.
Definition: bimanual_openrave_thread.cpp:378
JacoBimanualOpenraveThread::JacoBimanualOpenraveThread
JacoBimanualOpenraveThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
Definition: bimanual_openrave_thread.cpp:56
JacoBimanualOpenraveThread::add_target
virtual bool add_target(float l_x, float l_y, float l_z, float l_e1, float l_e2, float l_e3, float r_x, float r_y, float r_z, float r_e1, float r_e2, float r_e3)
Add target for coordinated manipulation to the queue.
Definition: bimanual_openrave_thread.cpp:320
JacoBimanualOpenraveThread::set_constrained
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
Definition: bimanual_openrave_thread.cpp:294
JacoBimanualOpenraveThread::finalize
virtual void finalize()
Finalize the thread.
Definition: bimanual_openrave_thread.cpp:209
fawkes::jaco_dual_arm_struct
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:111
JacoOpenraveBaseThread
Definition: openrave_base_thread.h:54
JacoBimanualOpenraveThread
Definition: bimanual_openrave_thread.h:33
fawkes::jaco_arm_struct
Jaco struct containing all components required for one arm.
Definition: types.h:91
fawkes::jaco_trajec_state_t
enum fawkes::jaco_trajec_state_enum jaco_trajec_state_t
The state of a trajectory.
JacoBimanualOpenraveThread::loop
virtual void loop()
Code to execute in the thread.
Definition: bimanual_openrave_thread.cpp:226
JacoBimanualOpenraveThread::run
virtual void run()
Stub to see name in backtrace for easier debugging.
Definition: bimanual_openrave_thread.h:67