Fawkes API Fawkes Development Version
bimanual_act_thread.cpp
1
2/***************************************************************************
3 * bimanual_act_thread.cpp - Jaco plugin act-thread for coordinated bimanual manipulation
4 *
5 * Created: Mon Sep 29 03:13: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#include "bimanual_act_thread.h"
24
25#include "bimanual_goto_thread.h"
26#include "bimanual_openrave_thread.h"
27
28#include <interfaces/JacoBimanualInterface.h>
29
30using namespace fawkes;
31
32/** @class JacoBimanualActThread "bimanual_act_thread.h"
33 * Jaco Arm act-thread for coordinate bimanual manipulation.
34 *
35 * @author Bahram Maleki-Fard
36 */
37
38/** Constructor.
39 * @param arms pointer to jaco_dual_arm_t struct, to be used in this thread
40 */
42: Thread("JacoBimanualActThread", Thread::OPMODE_WAITFORWAKEUP),
44 arms_(arms)
45{
46}
47
48/** Destructor. */
50{
51}
52
53/** Initialize. */
54void
56{
57 // open interface for writing
58 arms_->iface = blackboard->open_for_writing<JacoBimanualInterface>("JacoArm Bimanual");
59}
60
61/** Finalize. */
62void
64{
65 arms_->goto_thread = NULL;
66 arms_->openrave_thread = NULL;
67
68 try {
69 blackboard->close(arms_->iface);
70 } catch (fawkes::Exception &e) {
72 "Could not close JacoBimanualInterface interface. Er:%s",
74 }
75}
76
77/** Main loop. */
78void
80{
81 if (arms_->openrave_thread == NULL || arms_->goto_thread == NULL)
82 return;
83
84 while (!arms_->iface->msgq_empty()) {
85 Message *m = arms_->iface->msgq_first(m);
86 arms_->iface->set_msgid(m->id());
87 arms_->iface->set_final(false);
88 arms_->iface->set_error_code(JacoBimanualInterface::ERROR_NONE);
89 //~ arms_->iface->write();
90
93 logger->log_debug(name(), "SetPlannerParamsMessage rcvd. params:%s", msg->params());
94
95#ifdef HAVE_OPENRAVE
97#endif
98
101 logger->log_debug(name(), "SetConstrainedMessage rcvd. Enabled:%u", msg->is_constrained());
102
103#ifdef HAVE_OPENRAVE
105#endif
106
110 "CartesianGotoMessage rcvd. left: x:%f y:%f z:%f e1:%f e2:%f e3:%f",
111 msg->l_x(),
112 msg->l_y(),
113 msg->l_z(),
114 msg->l_e1(),
115 msg->l_e2(),
116 msg->l_e3());
118 "CartesianGotoMessage right: x:%f y:%f z:%f e1:%f e2:%f e3:%f",
119 msg->r_x(),
120 msg->r_y(),
121 msg->r_z(),
122 msg->r_e1(),
123 msg->r_e2(),
124 msg->r_e3());
125#ifdef HAVE_OPENRAVE
127 "CartesianGotoMessage is being passed to openrave",
128 arms_->iface->id());
129 // add target to OpenRAVE queue for planning
130 bool s = arms_->openrave_thread->add_target(msg->l_x(),
131 msg->l_y(),
132 msg->l_z(),
133 msg->l_e1(),
134 msg->l_e2(),
135 msg->l_e3(),
136 msg->r_x(),
137 msg->r_y(),
138 msg->r_z(),
139 msg->r_e1(),
140 msg->r_e2(),
141 msg->r_e3());
142 if (!s) {
143 arms_->iface->set_error_code(JacoBimanualInterface::ERROR_NO_IK);
145 "Failed executing CartesianGotoMessage, could not find IK solution");
146 }
147#else
149 "OpenRAVE not found. Cannot plan coordinated trajectories. Skipping!");
150#endif
151
155 "MoveGripperMessage rcvd. left: f1:%f f2:%f f3:%f",
156 msg->l_finger1(),
157 msg->l_finger2(),
158 msg->l_finger3());
160 "MoveGripperMessage right: f1:%f f2:%f f3:%f",
161 msg->r_finger1(),
162 msg->r_finger2(),
163 msg->r_finger3());
164
165 arms_->goto_thread->move_gripper(msg->l_finger1(),
166 msg->l_finger2(),
167 msg->l_finger3(),
168 msg->r_finger2(),
169 msg->r_finger2(),
170 msg->r_finger3());
171
172 } else {
173 logger->log_warn(name(), "Unknown message received. Skipping");
174 }
175
176 arms_->iface->msgq_pop();
177 }
178
179 arms_->iface->set_final(arms_->goto_thread->final());
180 arms_->iface->write();
181}
virtual void init()
Initialize.
virtual void finalize()
Finalize.
virtual void loop()
Main loop.
JacoBimanualActThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
virtual ~JacoBimanualActThread()
Destructor.
virtual bool final()
Check if arm is final.
virtual void move_gripper(float l_f1, float l_f2, float l_f3, float r_f1, float r_f2, float r_f3)
Moves only the gripper of both arms.
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 set_constrained(bool enable)
Enable/Disable constrained planning.
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
virtual void close(Interface *interface)=0
Close interface.
Thread aspect to use blocked timing.
Base class for exceptions in Fawkes.
Definition: exception.h:36
virtual const char * what_no_backtrace() const noexcept
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:351
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1215
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1200
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:501
const char * id() const
Get identifier of interface.
Definition: interface.cpp:661
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1062
CartesianGotoMessage Fawkes BlackBoard Interface Message.
MoveGripperMessage Fawkes BlackBoard Interface Message.
SetConstrainedMessage Fawkes BlackBoard Interface Message.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
JacoBimanualInterface Fawkes BlackBoard Interface.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
void set_final(const bool new_final)
Set final value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:44
unsigned int id() const
Get message ID.
Definition: message.cpp:181
Thread class encapsulation of pthreads.
Definition: thread.h:46
const char * name() const
Get name of thread.
Definition: thread.h:100
Fawkes library namespace.
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:113
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Definition: types.h:116
JacoBimanualGotoThread * goto_thread
GotoThread for coordinated manipulation.
Definition: types.h:117
JacoBimanualOpenraveThread * openrave_thread
OpenraveThread for coordinated manipulation.
Definition: types.h:118