Fawkes API Fawkes Development Version
JacoBimanualInterface.cpp
1
2/***************************************************************************
3 * JacoBimanualInterface.cpp - Fawkes BlackBoard Interface - JacoBimanualInterface
4 *
5 * Templated created: Thu Oct 12 10:49:19 2006
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. A runtime exception applies to
14 * this software (see LICENSE.GPL_WRE file mentioned below for details).
15 *
16 * This program is distributed in the hope that it will be useful,
17 * but WITHOUT ANY WARRANTY; without even the implied warranty of
18 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
19 * GNU Library General Public License for more details.
20 *
21 * Read the full text in the LICENSE.GPL_WRE file in the doc directory.
22 */
23
24#include <interfaces/JacoBimanualInterface.h>
25
26#include <core/exceptions/software.h>
27
28#include <map>
29#include <string>
30#include <cstring>
31#include <cstdlib>
32
33namespace fawkes {
34
35/** @class JacoBimanualInterface <interfaces/JacoBimanualInterface.h>
36 * JacoBimanualInterface Fawkes BlackBoard Interface.
37 *
38 Interface for coordinate bimanual manipulation with a Kinova Jaco arm.
39
40 * @ingroup FawkesInterfaces
41 */
42
43
44/** ERROR_NONE constant */
45const uint32_t JacoBimanualInterface::ERROR_NONE = 0u;
46/** ERROR_UNSPECIFIC constant */
48/** ERROR_NO_IK constant */
49const uint32_t JacoBimanualInterface::ERROR_NO_IK = 2u;
50/** ERROR_PLANNING constant */
52
53/** Constructor */
54JacoBimanualInterface::JacoBimanualInterface() : Interface()
55{
56 data_size = sizeof(JacoBimanualInterface_data_t);
57 data_ptr = malloc(data_size);
58 data = (JacoBimanualInterface_data_t *)data_ptr;
59 data_ts = (interface_data_ts_t *)data_ptr;
60 memset(data_ptr, 0, data_size);
61 add_fieldinfo(IFT_UINT32, "msgid", 1, &data->msgid);
62 add_fieldinfo(IFT_BOOL, "final", 1, &data->final);
63 add_fieldinfo(IFT_UINT32, "error_code", 1, &data->error_code);
64 add_fieldinfo(IFT_BOOL, "constrained", 1, &data->constrained);
65 add_messageinfo("CartesianGotoMessage");
66 add_messageinfo("MoveGripperMessage");
67 add_messageinfo("SetPlannerParamsMessage");
68 add_messageinfo("SetConstrainedMessage");
69 unsigned char tmp_hash[] = {0x7c, 0x62, 0x7a, 0x5a, 0xc6, 0xc1, 0xb4, 0x12, 0x6f, 0xa4, 0x89, 0x89, 0xb8, 0xe5, 0x1, 0x66};
70 set_hash(tmp_hash);
71}
72
73/** Destructor */
74JacoBimanualInterface::~JacoBimanualInterface()
75{
76 free(data_ptr);
77}
78/* Methods */
79/** Get msgid value.
80 * The ID of the message that is currently being
81 processed, or 0 if no message is being processed.
82 * @return msgid value
83 */
84uint32_t
85JacoBimanualInterface::msgid() const
86{
87 return data->msgid;
88}
89
90/** Get maximum length of msgid value.
91 * @return length of msgid value, can be length of the array or number of
92 * maximum number of characters for a string
93 */
94size_t
95JacoBimanualInterface::maxlenof_msgid() const
96{
97 return 1;
98}
99
100/** Set msgid value.
101 * The ID of the message that is currently being
102 processed, or 0 if no message is being processed.
103 * @param new_msgid new msgid value
104 */
105void
106JacoBimanualInterface::set_msgid(const uint32_t new_msgid)
107{
108 set_field(data->msgid, new_msgid);
109}
110
111/** Get final value.
112 * True, if the last command has been finished,
113 false if it is still running
114 * @return final value
115 */
116bool
117JacoBimanualInterface::is_final() const
118{
119 return data->final;
120}
121
122/** Get maximum length of final value.
123 * @return length of final value, can be length of the array or number of
124 * maximum number of characters for a string
125 */
126size_t
127JacoBimanualInterface::maxlenof_final() const
128{
129 return 1;
130}
131
132/** Set final value.
133 * True, if the last command has been finished,
134 false if it is still running
135 * @param new_final new final value
136 */
137void
138JacoBimanualInterface::set_final(const bool new_final)
139{
140 set_field(data->final, new_final);
141}
142
143/** Get error_code value.
144 * Error code, set if
145 final is true. 0 if no error occured, an error code from ERROR_*
146 constants otherwise.
147 * @return error_code value
148 */
149uint32_t
150JacoBimanualInterface::error_code() const
151{
152 return data->error_code;
153}
154
155/** Get maximum length of error_code value.
156 * @return length of error_code value, can be length of the array or number of
157 * maximum number of characters for a string
158 */
159size_t
160JacoBimanualInterface::maxlenof_error_code() const
161{
162 return 1;
163}
164
165/** Set error_code value.
166 * Error code, set if
167 final is true. 0 if no error occured, an error code from ERROR_*
168 constants otherwise.
169 * @param new_error_code new error_code value
170 */
171void
172JacoBimanualInterface::set_error_code(const uint32_t new_error_code)
173{
174 set_field(data->error_code, new_error_code);
175}
176
177/** Get constrained value.
178 * Wheter planning is using constraint-function.
179 This is an OpenRAVE internal constraint function.
180 * @return constrained value
181 */
182bool
183JacoBimanualInterface::is_constrained() const
184{
185 return data->constrained;
186}
187
188/** Get maximum length of constrained value.
189 * @return length of constrained value, can be length of the array or number of
190 * maximum number of characters for a string
191 */
192size_t
193JacoBimanualInterface::maxlenof_constrained() const
194{
195 return 1;
196}
197
198/** Set constrained value.
199 * Wheter planning is using constraint-function.
200 This is an OpenRAVE internal constraint function.
201 * @param new_constrained new constrained value
202 */
203void
204JacoBimanualInterface::set_constrained(const bool new_constrained)
205{
206 set_field(data->constrained, new_constrained);
207}
208
209/* =========== message create =========== */
210Message *
211JacoBimanualInterface::create_message(const char *type) const
212{
213 if ( strncmp("CartesianGotoMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
214 return new CartesianGotoMessage();
215 } else if ( strncmp("MoveGripperMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
216 return new MoveGripperMessage();
217 } else if ( strncmp("SetPlannerParamsMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
218 return new SetPlannerParamsMessage();
219 } else if ( strncmp("SetConstrainedMessage", type, INTERFACE_MESSAGE_TYPE_SIZE_ - 1) == 0 ) {
220 return new SetConstrainedMessage();
221 } else {
222 throw UnknownTypeException("The given type '%s' does not match any known "
223 "message type for this interface type.", type);
224 }
225}
226
227
228/** Copy values from other interface.
229 * @param other other interface to copy values from
230 */
231void
232JacoBimanualInterface::copy_values(const Interface *other)
233{
234 const JacoBimanualInterface *oi = dynamic_cast<const JacoBimanualInterface *>(other);
235 if (oi == NULL) {
236 throw TypeMismatchException("Can only copy values from interface of same type (%s vs. %s)",
237 type(), other->type());
238 }
239 memcpy(data, oi->data, sizeof(JacoBimanualInterface_data_t));
240}
241
242const char *
243JacoBimanualInterface::enum_tostring(const char *enumtype, int val) const
244{
245 throw UnknownTypeException("Unknown enum type %s", enumtype);
246}
247
248/* =========== messages =========== */
249/** @class JacoBimanualInterface::CartesianGotoMessage <interfaces/JacoBimanualInterface.h>
250 * CartesianGotoMessage Fawkes BlackBoard Interface Message.
251 *
252
253 */
254
255
256/** Constructor with initial values.
257 * @param ini_l_x initial value for l_x
258 * @param ini_l_y initial value for l_y
259 * @param ini_l_z initial value for l_z
260 * @param ini_l_e1 initial value for l_e1
261 * @param ini_l_e2 initial value for l_e2
262 * @param ini_l_e3 initial value for l_e3
263 * @param ini_r_x initial value for r_x
264 * @param ini_r_y initial value for r_y
265 * @param ini_r_z initial value for r_z
266 * @param ini_r_e1 initial value for r_e1
267 * @param ini_r_e2 initial value for r_e2
268 * @param ini_r_e3 initial value for r_e3
269 */
270JacoBimanualInterface::CartesianGotoMessage::CartesianGotoMessage(const float ini_l_x, const float ini_l_y, const float ini_l_z, const float ini_l_e1, const float ini_l_e2, const float ini_l_e3, const float ini_r_x, const float ini_r_y, const float ini_r_z, const float ini_r_e1, const float ini_r_e2, const float ini_r_e3) : Message("CartesianGotoMessage")
271{
272 data_size = sizeof(CartesianGotoMessage_data_t);
273 data_ptr = malloc(data_size);
274 memset(data_ptr, 0, data_size);
275 data = (CartesianGotoMessage_data_t *)data_ptr;
277 data->l_x = ini_l_x;
278 data->l_y = ini_l_y;
279 data->l_z = ini_l_z;
280 data->l_e1 = ini_l_e1;
281 data->l_e2 = ini_l_e2;
282 data->l_e3 = ini_l_e3;
283 data->r_x = ini_r_x;
284 data->r_y = ini_r_y;
285 data->r_z = ini_r_z;
286 data->r_e1 = ini_r_e1;
287 data->r_e2 = ini_r_e2;
288 data->r_e3 = ini_r_e3;
289 add_fieldinfo(IFT_FLOAT, "l_x", 1, &data->l_x);
290 add_fieldinfo(IFT_FLOAT, "l_y", 1, &data->l_y);
291 add_fieldinfo(IFT_FLOAT, "l_z", 1, &data->l_z);
292 add_fieldinfo(IFT_FLOAT, "l_e1", 1, &data->l_e1);
293 add_fieldinfo(IFT_FLOAT, "l_e2", 1, &data->l_e2);
294 add_fieldinfo(IFT_FLOAT, "l_e3", 1, &data->l_e3);
295 add_fieldinfo(IFT_FLOAT, "r_x", 1, &data->r_x);
296 add_fieldinfo(IFT_FLOAT, "r_y", 1, &data->r_y);
297 add_fieldinfo(IFT_FLOAT, "r_z", 1, &data->r_z);
298 add_fieldinfo(IFT_FLOAT, "r_e1", 1, &data->r_e1);
299 add_fieldinfo(IFT_FLOAT, "r_e2", 1, &data->r_e2);
300 add_fieldinfo(IFT_FLOAT, "r_e3", 1, &data->r_e3);
301}
302/** Constructor */
304{
305 data_size = sizeof(CartesianGotoMessage_data_t);
306 data_ptr = malloc(data_size);
307 memset(data_ptr, 0, data_size);
308 data = (CartesianGotoMessage_data_t *)data_ptr;
310 add_fieldinfo(IFT_FLOAT, "l_x", 1, &data->l_x);
311 add_fieldinfo(IFT_FLOAT, "l_y", 1, &data->l_y);
312 add_fieldinfo(IFT_FLOAT, "l_z", 1, &data->l_z);
313 add_fieldinfo(IFT_FLOAT, "l_e1", 1, &data->l_e1);
314 add_fieldinfo(IFT_FLOAT, "l_e2", 1, &data->l_e2);
315 add_fieldinfo(IFT_FLOAT, "l_e3", 1, &data->l_e3);
316 add_fieldinfo(IFT_FLOAT, "r_x", 1, &data->r_x);
317 add_fieldinfo(IFT_FLOAT, "r_y", 1, &data->r_y);
318 add_fieldinfo(IFT_FLOAT, "r_z", 1, &data->r_z);
319 add_fieldinfo(IFT_FLOAT, "r_e1", 1, &data->r_e1);
320 add_fieldinfo(IFT_FLOAT, "r_e2", 1, &data->r_e2);
321 add_fieldinfo(IFT_FLOAT, "r_e3", 1, &data->r_e3);
322}
323
324/** Destructor */
326{
327 free(data_ptr);
328}
329
330/** Copy constructor.
331 * @param m message to copy from
332 */
334{
335 data_size = m->data_size;
336 data_ptr = malloc(data_size);
337 memcpy(data_ptr, m->data_ptr, data_size);
338 data = (CartesianGotoMessage_data_t *)data_ptr;
340}
341
342/* Methods */
343/** Get l_x value.
344 * X-coordinate of left manipulator
345 * @return l_x value
346 */
347float
349{
350 return data->l_x;
351}
352
353/** Get maximum length of l_x value.
354 * @return length of l_x value, can be length of the array or number of
355 * maximum number of characters for a string
356 */
357size_t
359{
360 return 1;
361}
362
363/** Set l_x value.
364 * X-coordinate of left manipulator
365 * @param new_l_x new l_x value
366 */
367void
369{
370 set_field(data->l_x, new_l_x);
371}
372
373/** Get l_y value.
374 * Y-coordinate of left manipulator
375 * @return l_y value
376 */
377float
379{
380 return data->l_y;
381}
382
383/** Get maximum length of l_y value.
384 * @return length of l_y value, can be length of the array or number of
385 * maximum number of characters for a string
386 */
387size_t
389{
390 return 1;
391}
392
393/** Set l_y value.
394 * Y-coordinate of left manipulator
395 * @param new_l_y new l_y value
396 */
397void
399{
400 set_field(data->l_y, new_l_y);
401}
402
403/** Get l_z value.
404 * Z-coordinate of left manipulator
405 * @return l_z value
406 */
407float
409{
410 return data->l_z;
411}
412
413/** Get maximum length of l_z value.
414 * @return length of l_z value, can be length of the array or number of
415 * maximum number of characters for a string
416 */
417size_t
419{
420 return 1;
421}
422
423/** Set l_z value.
424 * Z-coordinate of left manipulator
425 * @param new_l_z new l_z value
426 */
427void
429{
430 set_field(data->l_z, new_l_z);
431}
432
433/** Get l_e1 value.
434 * 1st Euler angle of left manipulator rotation
435 * @return l_e1 value
436 */
437float
439{
440 return data->l_e1;
441}
442
443/** Get maximum length of l_e1 value.
444 * @return length of l_e1 value, can be length of the array or number of
445 * maximum number of characters for a string
446 */
447size_t
449{
450 return 1;
451}
452
453/** Set l_e1 value.
454 * 1st Euler angle of left manipulator rotation
455 * @param new_l_e1 new l_e1 value
456 */
457void
459{
460 set_field(data->l_e1, new_l_e1);
461}
462
463/** Get l_e2 value.
464 * 2nd Euler angle of left manipulator rotation
465 * @return l_e2 value
466 */
467float
469{
470 return data->l_e2;
471}
472
473/** Get maximum length of l_e2 value.
474 * @return length of l_e2 value, can be length of the array or number of
475 * maximum number of characters for a string
476 */
477size_t
479{
480 return 1;
481}
482
483/** Set l_e2 value.
484 * 2nd Euler angle of left manipulator rotation
485 * @param new_l_e2 new l_e2 value
486 */
487void
489{
490 set_field(data->l_e2, new_l_e2);
491}
492
493/** Get l_e3 value.
494 * 3rd Euler angle of left manipulator rotation
495 * @return l_e3 value
496 */
497float
499{
500 return data->l_e3;
501}
502
503/** Get maximum length of l_e3 value.
504 * @return length of l_e3 value, can be length of the array or number of
505 * maximum number of characters for a string
506 */
507size_t
509{
510 return 1;
511}
512
513/** Set l_e3 value.
514 * 3rd Euler angle of left manipulator rotation
515 * @param new_l_e3 new l_e3 value
516 */
517void
519{
520 set_field(data->l_e3, new_l_e3);
521}
522
523/** Get r_x value.
524 * X-coordinate of right manipulator
525 * @return r_x value
526 */
527float
529{
530 return data->r_x;
531}
532
533/** Get maximum length of r_x value.
534 * @return length of r_x value, can be length of the array or number of
535 * maximum number of characters for a string
536 */
537size_t
539{
540 return 1;
541}
542
543/** Set r_x value.
544 * X-coordinate of right manipulator
545 * @param new_r_x new r_x value
546 */
547void
549{
550 set_field(data->r_x, new_r_x);
551}
552
553/** Get r_y value.
554 * Y-coordinate of right manipulator
555 * @return r_y value
556 */
557float
559{
560 return data->r_y;
561}
562
563/** Get maximum length of r_y value.
564 * @return length of r_y value, can be length of the array or number of
565 * maximum number of characters for a string
566 */
567size_t
569{
570 return 1;
571}
572
573/** Set r_y value.
574 * Y-coordinate of right manipulator
575 * @param new_r_y new r_y value
576 */
577void
579{
580 set_field(data->r_y, new_r_y);
581}
582
583/** Get r_z value.
584 * Z-coordinate of right manipulator
585 * @return r_z value
586 */
587float
589{
590 return data->r_z;
591}
592
593/** Get maximum length of r_z value.
594 * @return length of r_z value, can be length of the array or number of
595 * maximum number of characters for a string
596 */
597size_t
599{
600 return 1;
601}
602
603/** Set r_z value.
604 * Z-coordinate of right manipulator
605 * @param new_r_z new r_z value
606 */
607void
609{
610 set_field(data->r_z, new_r_z);
611}
612
613/** Get r_e1 value.
614 * 1st Euler angle of right manipulator rotation
615 * @return r_e1 value
616 */
617float
619{
620 return data->r_e1;
621}
622
623/** Get maximum length of r_e1 value.
624 * @return length of r_e1 value, can be length of the array or number of
625 * maximum number of characters for a string
626 */
627size_t
629{
630 return 1;
631}
632
633/** Set r_e1 value.
634 * 1st Euler angle of right manipulator rotation
635 * @param new_r_e1 new r_e1 value
636 */
637void
639{
640 set_field(data->r_e1, new_r_e1);
641}
642
643/** Get r_e2 value.
644 * 2nd Euler angle of right manipulator rotation
645 * @return r_e2 value
646 */
647float
649{
650 return data->r_e2;
651}
652
653/** Get maximum length of r_e2 value.
654 * @return length of r_e2 value, can be length of the array or number of
655 * maximum number of characters for a string
656 */
657size_t
659{
660 return 1;
661}
662
663/** Set r_e2 value.
664 * 2nd Euler angle of right manipulator rotation
665 * @param new_r_e2 new r_e2 value
666 */
667void
669{
670 set_field(data->r_e2, new_r_e2);
671}
672
673/** Get r_e3 value.
674 * 3rd Euler angle of right manipulator rotation
675 * @return r_e3 value
676 */
677float
679{
680 return data->r_e3;
681}
682
683/** Get maximum length of r_e3 value.
684 * @return length of r_e3 value, can be length of the array or number of
685 * maximum number of characters for a string
686 */
687size_t
689{
690 return 1;
691}
692
693/** Set r_e3 value.
694 * 3rd Euler angle of right manipulator rotation
695 * @param new_r_e3 new r_e3 value
696 */
697void
699{
700 set_field(data->r_e3, new_r_e3);
701}
702
703/** Clone this message.
704 * Produces a message of the same type as this message and copies the
705 * data to the new message.
706 * @return clone of this message
707 */
708Message *
710{
712}
713/** @class JacoBimanualInterface::MoveGripperMessage <interfaces/JacoBimanualInterface.h>
714 * MoveGripperMessage Fawkes BlackBoard Interface Message.
715 *
716
717 */
718
719
720/** Constructor with initial values.
721 * @param ini_l_finger1 initial value for l_finger1
722 * @param ini_l_finger2 initial value for l_finger2
723 * @param ini_l_finger3 initial value for l_finger3
724 * @param ini_r_finger1 initial value for r_finger1
725 * @param ini_r_finger2 initial value for r_finger2
726 * @param ini_r_finger3 initial value for r_finger3
727 */
728JacoBimanualInterface::MoveGripperMessage::MoveGripperMessage(const float ini_l_finger1, const float ini_l_finger2, const float ini_l_finger3, const float ini_r_finger1, const float ini_r_finger2, const float ini_r_finger3) : Message("MoveGripperMessage")
729{
730 data_size = sizeof(MoveGripperMessage_data_t);
731 data_ptr = malloc(data_size);
732 memset(data_ptr, 0, data_size);
733 data = (MoveGripperMessage_data_t *)data_ptr;
735 data->l_finger1 = ini_l_finger1;
736 data->l_finger2 = ini_l_finger2;
737 data->l_finger3 = ini_l_finger3;
738 data->r_finger1 = ini_r_finger1;
739 data->r_finger2 = ini_r_finger2;
740 data->r_finger3 = ini_r_finger3;
741 add_fieldinfo(IFT_FLOAT, "l_finger1", 1, &data->l_finger1);
742 add_fieldinfo(IFT_FLOAT, "l_finger2", 1, &data->l_finger2);
743 add_fieldinfo(IFT_FLOAT, "l_finger3", 1, &data->l_finger3);
744 add_fieldinfo(IFT_FLOAT, "r_finger1", 1, &data->r_finger1);
745 add_fieldinfo(IFT_FLOAT, "r_finger2", 1, &data->r_finger2);
746 add_fieldinfo(IFT_FLOAT, "r_finger3", 1, &data->r_finger3);
747}
748/** Constructor */
750{
751 data_size = sizeof(MoveGripperMessage_data_t);
752 data_ptr = malloc(data_size);
753 memset(data_ptr, 0, data_size);
754 data = (MoveGripperMessage_data_t *)data_ptr;
756 add_fieldinfo(IFT_FLOAT, "l_finger1", 1, &data->l_finger1);
757 add_fieldinfo(IFT_FLOAT, "l_finger2", 1, &data->l_finger2);
758 add_fieldinfo(IFT_FLOAT, "l_finger3", 1, &data->l_finger3);
759 add_fieldinfo(IFT_FLOAT, "r_finger1", 1, &data->r_finger1);
760 add_fieldinfo(IFT_FLOAT, "r_finger2", 1, &data->r_finger2);
761 add_fieldinfo(IFT_FLOAT, "r_finger3", 1, &data->r_finger3);
762}
763
764/** Destructor */
766{
767 free(data_ptr);
768}
769
770/** Copy constructor.
771 * @param m message to copy from
772 */
774{
775 data_size = m->data_size;
776 data_ptr = malloc(data_size);
777 memcpy(data_ptr, m->data_ptr, data_size);
778 data = (MoveGripperMessage_data_t *)data_ptr;
780}
781
782/* Methods */
783/** Get l_finger1 value.
784 * Value of finger 1 on left gripper. Range [0,60]
785 * @return l_finger1 value
786 */
787float
789{
790 return data->l_finger1;
791}
792
793/** Get maximum length of l_finger1 value.
794 * @return length of l_finger1 value, can be length of the array or number of
795 * maximum number of characters for a string
796 */
797size_t
799{
800 return 1;
801}
802
803/** Set l_finger1 value.
804 * Value of finger 1 on left gripper. Range [0,60]
805 * @param new_l_finger1 new l_finger1 value
806 */
807void
809{
810 set_field(data->l_finger1, new_l_finger1);
811}
812
813/** Get l_finger2 value.
814 * Value of finger 2 on left gripper. Range [0,60]
815 * @return l_finger2 value
816 */
817float
819{
820 return data->l_finger2;
821}
822
823/** Get maximum length of l_finger2 value.
824 * @return length of l_finger2 value, can be length of the array or number of
825 * maximum number of characters for a string
826 */
827size_t
829{
830 return 1;
831}
832
833/** Set l_finger2 value.
834 * Value of finger 2 on left gripper. Range [0,60]
835 * @param new_l_finger2 new l_finger2 value
836 */
837void
839{
840 set_field(data->l_finger2, new_l_finger2);
841}
842
843/** Get l_finger3 value.
844 * Value of finger 3 on left gripper. Range [0,60]
845 * @return l_finger3 value
846 */
847float
849{
850 return data->l_finger3;
851}
852
853/** Get maximum length of l_finger3 value.
854 * @return length of l_finger3 value, can be length of the array or number of
855 * maximum number of characters for a string
856 */
857size_t
859{
860 return 1;
861}
862
863/** Set l_finger3 value.
864 * Value of finger 3 on left gripper. Range [0,60]
865 * @param new_l_finger3 new l_finger3 value
866 */
867void
869{
870 set_field(data->l_finger3, new_l_finger3);
871}
872
873/** Get r_finger1 value.
874 * Value of finger 1 on right gripper. Range [0,60]
875 * @return r_finger1 value
876 */
877float
879{
880 return data->r_finger1;
881}
882
883/** Get maximum length of r_finger1 value.
884 * @return length of r_finger1 value, can be length of the array or number of
885 * maximum number of characters for a string
886 */
887size_t
889{
890 return 1;
891}
892
893/** Set r_finger1 value.
894 * Value of finger 1 on right gripper. Range [0,60]
895 * @param new_r_finger1 new r_finger1 value
896 */
897void
899{
900 set_field(data->r_finger1, new_r_finger1);
901}
902
903/** Get r_finger2 value.
904 * Value of finger 2 on right gripper. Range [0,60]
905 * @return r_finger2 value
906 */
907float
909{
910 return data->r_finger2;
911}
912
913/** Get maximum length of r_finger2 value.
914 * @return length of r_finger2 value, can be length of the array or number of
915 * maximum number of characters for a string
916 */
917size_t
919{
920 return 1;
921}
922
923/** Set r_finger2 value.
924 * Value of finger 2 on right gripper. Range [0,60]
925 * @param new_r_finger2 new r_finger2 value
926 */
927void
929{
930 set_field(data->r_finger2, new_r_finger2);
931}
932
933/** Get r_finger3 value.
934 * Value of finger 3 on right gripper. Range [0,60]
935 * @return r_finger3 value
936 */
937float
939{
940 return data->r_finger3;
941}
942
943/** Get maximum length of r_finger3 value.
944 * @return length of r_finger3 value, can be length of the array or number of
945 * maximum number of characters for a string
946 */
947size_t
949{
950 return 1;
951}
952
953/** Set r_finger3 value.
954 * Value of finger 3 on right gripper. Range [0,60]
955 * @param new_r_finger3 new r_finger3 value
956 */
957void
959{
960 set_field(data->r_finger3, new_r_finger3);
961}
962
963/** Clone this message.
964 * Produces a message of the same type as this message and copies the
965 * data to the new message.
966 * @return clone of this message
967 */
968Message *
970{
972}
973/** @class JacoBimanualInterface::SetPlannerParamsMessage <interfaces/JacoBimanualInterface.h>
974 * SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
975 *
976
977 */
978
979
980/** Constructor with initial values.
981 * @param ini_params initial value for params
982 */
983JacoBimanualInterface::SetPlannerParamsMessage::SetPlannerParamsMessage(const char * ini_params) : Message("SetPlannerParamsMessage")
984{
985 data_size = sizeof(SetPlannerParamsMessage_data_t);
986 data_ptr = malloc(data_size);
987 memset(data_ptr, 0, data_size);
988 data = (SetPlannerParamsMessage_data_t *)data_ptr;
990 strncpy(data->params, ini_params, 1024-1);
991 data->params[1024-1] = 0;
992 add_fieldinfo(IFT_STRING, "params", 1024, data->params);
993}
994/** Constructor */
996{
997 data_size = sizeof(SetPlannerParamsMessage_data_t);
998 data_ptr = malloc(data_size);
999 memset(data_ptr, 0, data_size);
1000 data = (SetPlannerParamsMessage_data_t *)data_ptr;
1002 add_fieldinfo(IFT_STRING, "params", 1024, data->params);
1003}
1004
1005/** Destructor */
1007{
1008 free(data_ptr);
1009}
1010
1011/** Copy constructor.
1012 * @param m message to copy from
1013 */
1015{
1016 data_size = m->data_size;
1017 data_ptr = malloc(data_size);
1018 memcpy(data_ptr, m->data_ptr, data_size);
1019 data = (SetPlannerParamsMessage_data_t *)data_ptr;
1021}
1022
1023/* Methods */
1024/** Get params value.
1025 * Planner parameters
1026 * @return params value
1027 */
1028char *
1030{
1031 return data->params;
1032}
1033
1034/** Get maximum length of params value.
1035 * @return length of params value, can be length of the array or number of
1036 * maximum number of characters for a string
1037 */
1038size_t
1040{
1041 return 1024;
1042}
1043
1044/** Set params value.
1045 * Planner parameters
1046 * @param new_params new params value
1047 */
1048void
1050{
1051 set_field(data->params, new_params);
1052}
1053
1054/** Clone this message.
1055 * Produces a message of the same type as this message and copies the
1056 * data to the new message.
1057 * @return clone of this message
1058 */
1059Message *
1061{
1063}
1064/** @class JacoBimanualInterface::SetConstrainedMessage <interfaces/JacoBimanualInterface.h>
1065 * SetConstrainedMessage Fawkes BlackBoard Interface Message.
1066 *
1067
1068 */
1069
1070
1071/** Constructor with initial values.
1072 * @param ini_constrained initial value for constrained
1073 */
1074JacoBimanualInterface::SetConstrainedMessage::SetConstrainedMessage(const bool ini_constrained) : Message("SetConstrainedMessage")
1075{
1076 data_size = sizeof(SetConstrainedMessage_data_t);
1077 data_ptr = malloc(data_size);
1078 memset(data_ptr, 0, data_size);
1079 data = (SetConstrainedMessage_data_t *)data_ptr;
1081 data->constrained = ini_constrained;
1082 add_fieldinfo(IFT_BOOL, "constrained", 1, &data->constrained);
1083}
1084/** Constructor */
1086{
1087 data_size = sizeof(SetConstrainedMessage_data_t);
1088 data_ptr = malloc(data_size);
1089 memset(data_ptr, 0, data_size);
1090 data = (SetConstrainedMessage_data_t *)data_ptr;
1092 add_fieldinfo(IFT_BOOL, "constrained", 1, &data->constrained);
1093}
1094
1095/** Destructor */
1097{
1098 free(data_ptr);
1099}
1100
1101/** Copy constructor.
1102 * @param m message to copy from
1103 */
1105{
1106 data_size = m->data_size;
1107 data_ptr = malloc(data_size);
1108 memcpy(data_ptr, m->data_ptr, data_size);
1109 data = (SetConstrainedMessage_data_t *)data_ptr;
1111}
1112
1113/* Methods */
1114/** Get constrained value.
1115 * Wheter planning is using constraint-function.
1116 This is an OpenRAVE internal constraint function.
1117 * @return constrained value
1118 */
1119bool
1121{
1122 return data->constrained;
1123}
1124
1125/** Get maximum length of constrained value.
1126 * @return length of constrained value, can be length of the array or number of
1127 * maximum number of characters for a string
1128 */
1129size_t
1131{
1132 return 1;
1133}
1134
1135/** Set constrained value.
1136 * Wheter planning is using constraint-function.
1137 This is an OpenRAVE internal constraint function.
1138 * @param new_constrained new constrained value
1139 */
1140void
1142{
1143 set_field(data->constrained, new_constrained);
1144}
1145
1146/** Clone this message.
1147 * Produces a message of the same type as this message and copies the
1148 * data to the new message.
1149 * @return clone of this message
1150 */
1151Message *
1153{
1155}
1156/** Check if message is valid and can be enqueued.
1157 * @param message Message to check
1158 * @return true if the message is valid, false otherwise.
1159 */
1160bool
1162{
1163 const CartesianGotoMessage *m0 = dynamic_cast<const CartesianGotoMessage *>(message);
1164 if ( m0 != NULL ) {
1165 return true;
1166 }
1167 const MoveGripperMessage *m1 = dynamic_cast<const MoveGripperMessage *>(message);
1168 if ( m1 != NULL ) {
1169 return true;
1170 }
1171 const SetPlannerParamsMessage *m2 = dynamic_cast<const SetPlannerParamsMessage *>(message);
1172 if ( m2 != NULL ) {
1173 return true;
1174 }
1175 const SetConstrainedMessage *m3 = dynamic_cast<const SetConstrainedMessage *>(message);
1176 if ( m3 != NULL ) {
1177 return true;
1178 }
1179 return false;
1180}
1181
1182/// @cond INTERNALS
1183EXPORT_INTERFACE(JacoBimanualInterface)
1184/// @endcond
1185
1186
1187} // end namespace fawkes
Base class for all Fawkes BlackBoard interfaces.
Definition: interface.h:80
const char * type() const
Get type of interface.
Definition: interface.cpp:652
void * data_ptr
Pointer to local memory storage.
Definition: interface.h:244
void set_field(FieldT &field, DataT &data)
Set a field, set data_changed to true and update data_changed accordingly.
Definition: interface.h:304
CartesianGotoMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_l_e1() const
Get maximum length of l_e1 value.
void set_l_e2(const float new_l_e2)
Set l_e2 value.
void set_r_e1(const float new_r_e1)
Set r_e1 value.
size_t maxlenof_l_e2() const
Get maximum length of l_e2 value.
size_t maxlenof_r_z() const
Get maximum length of r_z value.
void set_r_e3(const float new_r_e3)
Set r_e3 value.
void set_r_e2(const float new_r_e2)
Set r_e2 value.
size_t maxlenof_r_e3() const
Get maximum length of r_e3 value.
size_t maxlenof_r_x() const
Get maximum length of r_x value.
void set_l_e1(const float new_l_e1)
Set l_e1 value.
size_t maxlenof_r_e2() const
Get maximum length of r_e2 value.
virtual Message * clone() const
Clone this message.
void set_l_e3(const float new_l_e3)
Set l_e3 value.
size_t maxlenof_l_z() const
Get maximum length of l_z value.
size_t maxlenof_l_e3() const
Get maximum length of l_e3 value.
size_t maxlenof_r_e1() const
Get maximum length of r_e1 value.
size_t maxlenof_l_y() const
Get maximum length of l_y value.
size_t maxlenof_l_x() const
Get maximum length of l_x value.
size_t maxlenof_r_y() const
Get maximum length of r_y value.
MoveGripperMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_l_finger2() const
Get maximum length of l_finger2 value.
size_t maxlenof_l_finger1() const
Get maximum length of l_finger1 value.
void set_r_finger2(const float new_r_finger2)
Set r_finger2 value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_l_finger3() const
Get maximum length of l_finger3 value.
void set_l_finger1(const float new_l_finger1)
Set l_finger1 value.
size_t maxlenof_r_finger1() const
Get maximum length of r_finger1 value.
void set_l_finger3(const float new_l_finger3)
Set l_finger3 value.
size_t maxlenof_r_finger3() const
Get maximum length of r_finger3 value.
void set_r_finger3(const float new_r_finger3)
Set r_finger3 value.
void set_r_finger1(const float new_r_finger1)
Set r_finger1 value.
void set_l_finger2(const float new_l_finger2)
Set l_finger2 value.
size_t maxlenof_r_finger2() const
Get maximum length of r_finger2 value.
SetConstrainedMessage Fawkes BlackBoard Interface Message.
void set_constrained(const bool new_constrained)
Set constrained value.
virtual Message * clone() const
Clone this message.
size_t maxlenof_constrained() const
Get maximum length of constrained value.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
size_t maxlenof_params() const
Get maximum length of params value.
virtual Message * clone() const
Clone this message.
void set_params(const char *new_params)
Set params value.
JacoBimanualInterface Fawkes BlackBoard Interface.
virtual bool message_valid(const Message *message) const
Check if message is valid and can be enqueued.
static const uint32_t ERROR_PLANNING
ERROR_PLANNING constant.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
static const uint32_t ERROR_NO_IK
ERROR_NO_IK constant.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
void add_fieldinfo(interface_fieldtype_t type, const char *name, size_t length, void *value, const char *enumtype=0, const interface_enum_map_t *enum_map=0)
Add an entry to the info list.
Definition: message.cpp:435
void * data_ptr
Pointer to memory that contains local data.
Definition: message.h:146
message_data_ts_t * data_ts
data timestamp aliasing pointer
Definition: message.h:156
unsigned int data_size
Size of memory needed to hold all data.
Definition: message.h:147
Fawkes library namespace.
@ IFT_FLOAT
float field
Definition: types.h:46
@ IFT_STRING
string field
Definition: types.h:48
@ IFT_BOOL
boolean field
Definition: types.h:37
Timestamp data, must be present and first entries for each interface data structs!...
Definition: message.h:152