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, float l_y, float l_z, float l_e1, float l_e2, float l_e3,
46  float r_x, float r_y, float r_z, float r_e1, float r_e2, float r_e3);
47 
48  virtual void set_constrained(bool enable);
49 
50  protected:
51  /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
52  virtual void run() { Thread::run(); }
53 
54  private:
55  void _init();
56  void _load_robot();
57  void _init_dualmanipulation();
58 
59  void _set_trajec_state(fawkes::jaco_trajec_state_t state);
60  void _copy_env();
61  bool _solve_multi_ik(std::vector<float> &left,
62  std::vector<float> &right);
63  bool _plan_path();
64 
65  typedef struct arm_struct {
66  fawkes::jaco_arm_t *arm;
67 #ifdef HAVE_OPENRAVE
68  std::string manipname;
69  OpenRAVE::RobotBase::ManipulatorPtr manip;
71 #endif
72  } arm_struct_t;
73 
74  struct {
75  arm_struct_t left;
76  arm_struct_t right;
77  } __arms;
78 
79 #ifdef HAVE_OPENRAVE
80  fawkes::jaco_openrave_set_t __planner_env;
81  OpenRAVE::ModuleBasePtr __mod_dualmanip;
82 
83  std::set<OpenRAVE::KinBody::LinkPtr> links_left_;
84  std::set<OpenRAVE::KinBody::LinkPtr> links_right_;
85 #endif
86 
87  bool __constrained;
88 };
89 
90 
91 #endif
Jaco struct containing all components required for one arm.
Definition: types.h:95
Jaco Arm thread for dual-arm setup, integrating OpenRAVE.
JacoBimanualOpenraveThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
enum fawkes::jaco_trajec_state_enum jaco_trajec_state_t
The state of a trajectory.
virtual void finalize()
Finalize the thread.
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
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.
virtual void loop()
Code to execute in the thread.
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:49
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:112
virtual void run()
Stub to see name in backtrace for easier debugging.
virtual void plot_first()
Plot the first target of the target_queue, if it is a trajectory.
Base Jaco Arm thread, integrating OpenRAVE.