Fawkes API Fawkes Development Version
jaco_plugin.cpp
1
2/***************************************************************************
3 * jaco_plugin.cpp - Fawkes Kinova Jaco Plugin
4 *
5 * Created: Tue Jun 04 13:13:20 2013
6 * Copyright 2013 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#include "jaco_plugin.h"
24
25#include "act_thread.h"
26#include "bimanual_act_thread.h"
27#include "bimanual_goto_thread.h"
28#include "bimanual_openrave_thread.h"
29#include "goto_thread.h"
30#include "info_thread.h"
31#include "openrave_thread.h"
32#include "types.h"
33
34using namespace fawkes;
35
36/** @class JacoPlugin <plugins/jaco/jaco_plugin.h>
37 * Kinova Jaco Arm plugin.
38 *
39 * @author Bahram Maleki-Fard
40 */
41
42/** Constructor.
43 * @param config Fawkes configuration
44 */
46{
47 // load different/multiple threads if using dual-arm setup
48 bool is_dual_arm = config->get_bool("/hardware/jaco/config/dual_arm");
49 if (!is_dual_arm) {
50 jaco_arm_t *arm = new jaco_arm_t();
51 arm->config = CONFIG_SINGLE;
52
53 JacoActThread * act_thread = new JacoActThread("JacoActThread", arm);
54 JacoInfoThread * info_thread = new JacoInfoThread("JacoInfoThread", arm);
55 JacoGotoThread * goto_thread = new JacoGotoThread("JacoGotoThread", arm);
56 JacoOpenraveThread *openrave_thread = NULL;
57#ifdef HAVE_OPENRAVE
58 openrave_thread = new JacoOpenraveThread("JacoOpenraveThread", arm);
59#endif
60
61 arm->goto_thread = goto_thread;
62 arm->openrave_thread = openrave_thread;
63
64 thread_list.push_back(act_thread);
65 thread_list.push_back(info_thread);
66 thread_list.push_back(goto_thread);
67#ifdef HAVE_OPENRAVE
68 thread_list.push_back(openrave_thread);
69#endif
70
71 } else {
72 jaco_arm_t *arm_l = new jaco_arm_t();
73 jaco_arm_t *arm_r = new jaco_arm_t();
74 arm_l->config = CONFIG_LEFT;
75 arm_r->config = CONFIG_RIGHT;
76
77 // each arm gets a separate set of threads for independent manipulation.
78 JacoActThread * act_thread_l = new JacoActThread("JacoActThreadLeft", arm_l);
79 JacoInfoThread *info_thread_l = new JacoInfoThread("JacoInfoThreadLeft", arm_l);
80 JacoGotoThread *goto_thread_l = new JacoGotoThread("JacoGotoThreadLeft", arm_l);
81
82 JacoActThread * act_thread_r = new JacoActThread("JacoActThreadRight", arm_r);
83 JacoInfoThread *info_thread_r = new JacoInfoThread("JacoInfoThreadRight", arm_r);
84 JacoGotoThread *goto_thread_r = new JacoGotoThread("JacoGotoThreadRight", arm_r);
85
86 JacoOpenraveThread *openrave_thread_l;
87 JacoOpenraveThread *openrave_thread_r;
88#ifdef HAVE_OPENRAVE
89 // each arm gets 1 openrave-thread, providing planning and updating the openrave-model of that arm.
90 // additionally we need a separate openrave thread for planning symmetric bimanual manipulation.
91 openrave_thread_l =
92 new JacoOpenraveThread("JacoOpenraveThreadLeft", arm_l, /*load_robot=*/false);
93 openrave_thread_r =
94 new JacoOpenraveThread("JacoOpenraveThreadRight", arm_r, /*load_robot=*/false);
95#else
96 openrave_thread_l = NULL;
97 openrave_thread_r = NULL;
98#endif
99
100 arm_l->goto_thread = goto_thread_l;
101 arm_l->openrave_thread = openrave_thread_l;
102
103 arm_r->goto_thread = goto_thread_r;
104 arm_r->openrave_thread = openrave_thread_r;
105
106 thread_list.push_back(act_thread_l);
107 thread_list.push_back(info_thread_l);
108 thread_list.push_back(goto_thread_l);
109
110 thread_list.push_back(act_thread_r);
111 thread_list.push_back(info_thread_r);
112 thread_list.push_back(goto_thread_r);
113
114 // we also need a set of threads for coordinated bimanual manipulation
115 jaco_dual_arm_t *arms = new jaco_dual_arm_t();
116 arms->left = arm_l;
117 arms->right = arm_r;
118
119 JacoBimanualActThread * act_thread = new JacoBimanualActThread(arms);
120 JacoBimanualGotoThread *goto_thread = new JacoBimanualGotoThread(arms);
121
122 JacoBimanualOpenraveThread *openrave_thread = NULL;
123#ifdef HAVE_OPENRAVE
124 openrave_thread = new JacoBimanualOpenraveThread(arms);
125#endif
126
127 arms->goto_thread = goto_thread;
128 arms->openrave_thread = openrave_thread;
129
130 thread_list.push_back(act_thread);
131 thread_list.push_back(goto_thread);
132#ifdef HAVE_OPENRAVE
133 thread_list.push_back(openrave_thread);
134 thread_list.push_back(openrave_thread_l);
135 thread_list.push_back(openrave_thread_r);
136#endif
137 }
138}
139
140PLUGIN_DESCRIPTION("Jaco Plugin")
141EXPORT_PLUGIN(JacoPlugin)
Jaco Arm control thread.
Definition: act_thread.h:41
Jaco Arm act-thread for coordinate bimanual manipulation.
Jaco Arm movement thread.
Jaco Arm thread for dual-arm setup, integrating OpenRAVE.
Jaco Arm movement thread.
Definition: goto_thread.h:44
Jaco Arm information thread.
Definition: info_thread.h:43
Jaco Arm thread for single-arm setup, integrating OpenRAVE.
Kinova Jaco Arm plugin.
Definition: jaco_plugin.h:29
JacoPlugin(fawkes::Configuration *config)
Constructor.
Definition: jaco_plugin.cpp:45
Interface for configuration handling.
Definition: config.h:68
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
Plugin interface class.
Definition: plugin.h:34
ThreadList thread_list
Thread list member.
Definition: plugin.h:53
Configuration * config
Fawkes configuration.
Definition: plugin.h:58
void push_back(Thread *thread)
Add thread to the end.
Fawkes library namespace.
@ CONFIG_SINGLE
we only have one arm.
Definition: types.h:52
@ CONFIG_LEFT
this arm is the left one out of two.
Definition: types.h:53
@ CONFIG_RIGHT
this arm is the right one out of two.
Definition: types.h:54
Jaco struct containing all components required for one arm.
Definition: types.h:93
jaco_arm_config_t config
configuration for this arm
Definition: types.h:94
JacoGotoThread * goto_thread
the GotoThread of this arm.
Definition: types.h:98
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Definition: types.h:99
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:113
JacoBimanualGotoThread * goto_thread
GotoThread for coordinated manipulation.
Definition: types.h:117
jaco_arm_t * right
the struct with all the data for the right arm.
Definition: types.h:115
JacoBimanualOpenraveThread * openrave_thread
OpenraveThread for coordinated manipulation.
Definition: types.h:118
jaco_arm_t * left
the struct with all the data for the left arm.
Definition: types.h:114