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{
36public:
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
60protected:
61 /** Stub to see name in backtrace for easier debugging. @see Thread::run() */
62 virtual void
64 {
65 Thread::run();
66 }
67
68private:
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 {
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
Jaco Arm thread for dual-arm setup, integrating OpenRAVE.
virtual void finalize()
Finalize the thread.
virtual void run()
Stub to see name in backtrace for easier debugging.
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.
JacoBimanualOpenraveThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
virtual void plot_first()
Plot the first target of the target_queue, if it is a trajectory.
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
virtual void loop()
Code to execute in the thread.
Base Jaco Arm thread, integrating OpenRAVE.
RefPtr<> is a reference-counting shared smartpointer.
Definition: refptr.h:50
enum fawkes::jaco_trajec_state_enum jaco_trajec_state_t
The state of a trajectory.
Jaco struct containing all components required for one arm.
Definition: types.h:93
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:113