Fawkes API Fawkes Development Version
arm_kindrv.cpp
1
2/***************************************************************************
3 * arm_kindrv.cpp - Class for a Kinova Jaco arm, using libkindrv
4 *
5 * Created: Tue Jul 29 14:58:32 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#include "arm_kindrv.h"
24
25#include <core/exception.h>
26#include <libkindrv/kindrv.h>
27
28#include <cstdio>
29#include <vector>
30
31using namespace KinDrv;
32
33namespace fawkes {
34
35/** @class JacoArmKindrv <plugins/jaco/arm_kindrv.h>
36 * Class for commanding a Kinova Jaco Arm, using libkindrv.
37 * @author Bahram Maleki-Fard
38 */
39
40/** Constructor.
41 * @param name The name of the arm we want to connect to.
42 */
44{
45 // take the first arm we can connect to
46 arm_.reset(new KinDrv::JacoArm);
47 name_ = arm_->get_client_config(true).name;
48 // trim tailing whitespaces
49 name_.erase(name_.find_last_not_of(" ") + 1);
50
51 std::string found_names = "'" + name_ + "'";
52
53 if (name != NULL) {
54 // Check all connected arms until the right one is found.
55 std::vector<std::unique_ptr<KinDrv::JacoArm>> arms;
56 while (name_.compare(name) != 0) {
57 arms.push_back(std::move(arm_));
58 try {
59 arm_.reset(new KinDrv::JacoArm);
60 name_ = arm_->get_client_config(true).name;
61 name_.erase(name_.find_last_not_of(" ") + 1);
62 found_names += ", '" + name_ + "'";
63 } catch (KinDrvException &e) {
64 // don't throw yet, print helpful error below
65 arm_.reset();
66 break;
67 }
68 }
69 arms.clear();
70 }
71
72 if (!arm_) {
73 throw fawkes::Exception("Could not connect to Jaco arm '%s' with libkindrv. "
74 "But I found the following arms: %s",
75 name,
76 found_names.c_str());
77 }
78
79 initialized_ = false;
80 final_ = true;
81 ctrl_ang_ = true;
82}
83
84/** Destructor. */
86{
87}
88
89void
91{
92 goto_ready();
93}
94
95bool
97{
98 if (final_)
99 return true;
100
101 switch (target_type_) {
102 case TARGET_READY: {
103 jaco_retract_mode_t mode = arm_->get_status();
104 final_ = (mode == MODE_READY_STANDBY);
105
106 if (final_) {
107 arm_->release_joystick();
108 } else if (mode == MODE_READY_TO_RETRACT) {
109 // is moving in wrong direction
110 arm_->release_joystick();
111 arm_->push_joystick_button(2);
112 }
113 } break;
114
115 case TARGET_RETRACT: {
116 jaco_retract_mode_t mode = arm_->get_status();
117 final_ = (mode == MODE_RETRACT_STANDBY);
118 }
119 if (final_)
120 arm_->release_joystick();
121 break;
122
123 default: //TARGET_ANGULAR, TARGET_CARTESIAN
124 final_ = true;
125 {
126 jaco_position_t vel = arm_->get_ang_vel();
127 for (unsigned int i = 0; i < 6; ++i) {
128 final_ &= std::abs(vel.joints[i]) < 0.01;
129 }
130 for (unsigned int i = 0; i < 3; ++i) {
131 final_ &= std::abs(vel.finger_position[i]) < 0.01;
132 }
133 }
134 break;
135 }
136
137 return final_;
138}
139
140bool
142{
143 if (!initialized_) {
144 jaco_retract_mode_t mode = arm_->get_status();
145 initialized_ = (mode != MODE_NOINIT);
146 }
147
148 return initialized_;
149}
150
151void
152JacoArmKindrv::get_coords(std::vector<float> &to)
153{
154 if (ctrl_ang_) {
155 // nedd to set control to cart, otherwise we will not get updated data
156 arm_->set_control_cart();
157 ctrl_ang_ = false;
158 }
159 jaco_position_t pos = arm_->get_cart_pos();
160
161 to.clear();
162 to.push_back(-pos.position[1]);
163 to.push_back(pos.position[0]);
164 to.push_back(pos.position[2]);
165 to.push_back(pos.rotation[0]);
166 to.push_back(pos.rotation[1]);
167 to.push_back(pos.rotation[2]);
168}
169
170void
171JacoArmKindrv::get_joints(std::vector<float> &to) const
172{
173 jaco_position_t pos = arm_->get_ang_pos();
174
175 to.clear();
176 to.push_back(pos.joints[0]);
177 to.push_back(pos.joints[1]);
178 to.push_back(pos.joints[2]);
179 to.push_back(pos.joints[3]);
180 to.push_back(pos.joints[4]);
181 to.push_back(pos.joints[5]);
182}
183
184void
185JacoArmKindrv::get_fingers(std::vector<float> &to) const
186{
187 jaco_position_t pos = arm_->get_cart_pos();
188
189 to.clear();
190 to.push_back(pos.finger_position[0]);
191 to.push_back(pos.finger_position[1]);
192 to.push_back(pos.finger_position[2]);
193}
194
195void
197{
198 arm_->release_joystick();
199 final_ = true;
200}
201
202void
204{
205 arm_->start_api_ctrl();
206 arm_->push_joystick_button(button);
207 final_ = false;
208}
209
210void
212{
213 arm_->start_api_ctrl();
214 arm_->release_joystick();
215 final_ = true;
216}
217
218void
219JacoArmKindrv::goto_trajec(std::vector<std::vector<float>> *trajec, std::vector<float> &fingers)
220{
221 arm_->start_api_ctrl();
222 arm_->set_control_ang();
223 ctrl_ang_ = true;
224 usleep(500);
225 for (unsigned int i = 0; i < trajec->size(); ++i) {
226 arm_->set_target_ang(trajec->at(i).at(0),
227 trajec->at(i).at(1),
228 trajec->at(i).at(2),
229 trajec->at(i).at(3),
230 trajec->at(i).at(4),
231 trajec->at(i).at(5),
232 fingers.at(0),
233 fingers.at(1),
234 fingers.at(2));
235 }
236}
237
238void
239JacoArmKindrv::goto_joints(std::vector<float> &joints, std::vector<float> &fingers, bool followup)
240{
241 target_type_ = TARGET_ANGULAR;
242 final_ = false;
243
244 if (!followup) {
245 arm_->start_api_ctrl();
246 arm_->set_control_ang();
247 ctrl_ang_ = true;
248 usleep(500);
249 }
250
251 arm_->set_target_ang(joints.at(0),
252 joints.at(1),
253 joints.at(2),
254 joints.at(3),
255 joints.at(4),
256 joints.at(5),
257 fingers.at(0),
258 fingers.at(1),
259 fingers.at(2));
260}
261
262void
263JacoArmKindrv::goto_coords(std::vector<float> &coords, std::vector<float> &fingers)
264{
265 target_type_ = TARGET_CARTESIAN;
266 final_ = false;
267
268 arm_->start_api_ctrl();
269 arm_->set_control_cart();
270 ctrl_ang_ = false;
271 usleep(500);
272 //arm_->arm->set_target_cart(y_, -x_, z_, e1_, e2_, e3_, f1_, f2_, f3_);
273 arm_->set_target_cart(coords.at(1),
274 -coords.at(0),
275 coords.at(2),
276 coords.at(3),
277 coords.at(4),
278 coords.at(5),
279 fingers.at(0),
280 fingers.at(1),
281 fingers.at(2));
282}
283
284void
286{
287 target_type_ = TARGET_READY;
288 final_ = false;
289
290 arm_->start_api_ctrl();
291 jaco_retract_mode_t mode = arm_->get_status();
292 switch (mode) {
293 case MODE_RETRACT_TO_READY:
294 //2 buttons needed
295 arm_->push_joystick_button(2);
296 arm_->release_joystick();
297 arm_->push_joystick_button(2);
298 break;
299
300 case MODE_NORMAL_TO_READY:
301 case MODE_READY_TO_RETRACT:
302 case MODE_RETRACT_STANDBY:
303 case MODE_NORMAL:
304 case MODE_NOINIT:
305 //1 button needed
306 arm_->push_joystick_button(2);
307 break;
308
309 case MODE_ERROR:
310 // error: some error occured
311 // TODO: return something?
312 break;
313
314 case MODE_READY_STANDBY:
315 // no action. error?
316 // final_ = true;
317 break;
318 }
319}
320
321void
323{
324 target_type_ = TARGET_RETRACT;
325 final_ = false;
326
327 arm_->start_api_ctrl();
328 jaco_retract_mode_t mode = arm_->get_status();
329 switch (mode) {
330 case MODE_READY_TO_RETRACT:
331 // 2 buttons needed
332 arm_->push_joystick_button(2);
333 arm_->release_joystick();
334 arm_->push_joystick_button(2);
335 break;
336
337 case MODE_READY_STANDBY:
338 case MODE_RETRACT_TO_READY:
339 // 1 button needed
340 arm_->push_joystick_button(2);
341 break;
342
343 case MODE_NORMAL_TO_READY:
344 case MODE_NORMAL:
345 case MODE_NOINIT:
346 // warn: cannot go from NORMAL/NOINIT to RETRACT");
347 //final_ = true;
348 break;
349
350 case MODE_ERROR:
351 // error: some error occured!!
352 // TODO: return something?
353 break;
354
355 case MODE_RETRACT_STANDBY:
356 // no action. error?
357 //final_ = true;
358 break;
359 }
360}
361
362} // end of namespace fawkes
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual void get_fingers(std::vector< float > &to) const
Get the position values of the fingers.
Definition: arm_kindrv.cpp:185
virtual void goto_trajec(std::vector< std::vector< float > > *trajec, std::vector< float > &fingers)
Move the arm along the given trajectory.
Definition: arm_kindrv.cpp:219
virtual bool final()
Check if movement is final.
Definition: arm_kindrv.cpp:96
virtual void release_joystick()
Simulate releasing the joystick of the Kinova Jaco arm.
Definition: arm_kindrv.cpp:211
virtual void goto_ready()
Move the arm to READY position.
Definition: arm_kindrv.cpp:285
virtual void stop()
Stop the current movement.
Definition: arm_kindrv.cpp:196
JacoArmKindrv(const char *name=NULL)
Constructor.
Definition: arm_kindrv.cpp:43
virtual ~JacoArmKindrv()
Destructor.
Definition: arm_kindrv.cpp:85
virtual void initialize()
Initialize the arm.
Definition: arm_kindrv.cpp:90
virtual void push_joystick(unsigned int button)
Simulate a push of a button on the joystick of the Kinova Jaco arm.
Definition: arm_kindrv.cpp:203
virtual void goto_retract()
Move the arm to RETRACT position.
Definition: arm_kindrv.cpp:322
virtual bool initialized()
Check if arm is initialized.
Definition: arm_kindrv.cpp:141
virtual void goto_coords(std::vector< float > &coords, std::vector< float > &fingers)
Move the arm to given configuration.
Definition: arm_kindrv.cpp:263
virtual void get_coords(std::vector< float > &to)
Get the cartesian coordinates of the arm.
Definition: arm_kindrv.cpp:152
virtual void get_joints(std::vector< float > &to) const
Get the joint angles of the arm.
Definition: arm_kindrv.cpp:171
virtual void goto_joints(std::vector< float > &joints, std::vector< float > &fingers, bool followup=false)
Move the arm to given configuration.
Definition: arm_kindrv.cpp:239
std::string name_
the name of this arm
Definition: arm.h:121
bool initialized_
track if the arm has been initialized or not
Definition: arm.h:122
Fawkes library namespace.
@ TARGET_READY
target is the READY position of the Jaco arm.
Definition: types.h:62
@ TARGET_CARTESIAN
target with cartesian coordinates.
Definition: types.h:59
@ TARGET_RETRACT
target is the RETRACT position of the Jaco arm.
Definition: types.h:63
@ TARGET_ANGULAR
target with angular coordinates.
Definition: types.h:60