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 
30 using 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),
43  BlockedTimingAspect(BlockedTimingAspect::WAKEUP_HOOK_ACT),
44  arms_(arms)
45 {
46 }
47 
48 /** Destructor. */
50 {
51 }
52 
53 /** Initialize. */
54 void
56 {
57  // open interface for writing
58  arms_->iface = blackboard->open_for_writing<JacoBimanualInterface>("JacoArm Bimanual");
59 }
60 
61 /** Finalize. */
62 void
64 {
65  arms_->goto_thread = NULL;
66  arms_->openrave_thread = NULL;
67 
68  try {
69  blackboard->close(arms_->iface);
70  } catch (fawkes::Exception &e) {
71  logger->log_warn(name(),
72  "Could not close JacoBimanualInterface interface. Er:%s",
73  e.what_no_backtrace());
74  }
75 }
76 
77 /** Main loop. */
78 void
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 
109  logger->log_debug(name(),
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());
117  logger->log_debug(name(),
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
126  logger->log_debug(name(),
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);
144  logger->log_warn(name(),
145  "Failed executing CartesianGotoMessage, could not find IK solution");
146  }
147 #else
148  logger->log_warn(name(),
149  "OpenRAVE not found. Cannot plan coordinated trajectories. Skipping!");
150 #endif
151 
154  logger->log_debug(name(),
155  "MoveGripperMessage rcvd. left: f1:%f f2:%f f3:%f",
156  msg->l_finger1(),
157  msg->l_finger2(),
158  msg->l_finger3());
159  logger->log_debug(name(),
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 }
fawkes::JacoBimanualInterface::MoveGripperMessage
MoveGripperMessage Fawkes BlackBoard Interface Message.
Definition: JacoBimanualInterface.h:135
JacoBimanualGotoThread::move_gripper
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.
Definition: bimanual_goto_thread.cpp:274
fawkes::JacoBimanualInterface::SetConstrainedMessage
SetConstrainedMessage Fawkes BlackBoard Interface Message.
Definition: JacoBimanualInterface.h:205
fawkes::JacoBimanualInterface::set_final
void set_final(const bool new_final)
Set final value.
Definition: JacoBimanualInterface.cpp:138
fawkes::Interface::msgq_first_is
bool msgq_first_is()
Check if first message has desired type.
Definition: interface.h:314
fawkes::Interface::msgq_pop
void msgq_pop()
Erase first message from queue.
Definition: interface.cpp:1182
fawkes::JacoBimanualInterface::SetConstrainedMessage::is_constrained
bool is_constrained() const
Get constrained value.
Definition: JacoBimanualInterface.cpp:1120
fawkes::Interface::msgq_empty
bool msgq_empty()
Check if queue is empty.
Definition: interface.cpp:1029
fawkes::JacoBimanualInterface::SetPlannerParamsMessage
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
Definition: JacoBimanualInterface.h:180
fawkes::JacoBimanualInterface::MoveGripperMessage::r_finger3
float r_finger3() const
Get r_finger3 value.
Definition: JacoBimanualInterface.cpp:938
fawkes::JacoBimanualInterface::set_error_code
void set_error_code(const uint32_t new_error_code)
Set error_code value.
Definition: JacoBimanualInterface.cpp:172
fawkes::Message
Base class for all messages passed through interfaces in Fawkes BlackBoard.
Definition: message.h:45
fawkes::JacoBimanualInterface::MoveGripperMessage::r_finger2
float r_finger2() const
Get r_finger2 value.
Definition: JacoBimanualInterface.cpp:908
fawkes::JacoBimanualInterface::CartesianGotoMessage::r_e1
float r_e1() const
Get r_e1 value.
Definition: JacoBimanualInterface.cpp:618
fawkes::JacoBimanualInterface::CartesianGotoMessage::l_e2
float l_e2() const
Get l_e2 value.
Definition: JacoBimanualInterface.cpp:468
JacoBimanualGotoThread::final
virtual bool final()
Check if arm is final.
Definition: bimanual_goto_thread.cpp:223
JacoBimanualActThread::init
virtual void init()
Initialize.
Definition: bimanual_act_thread.cpp:55
fawkes::JacoBimanualInterface::SetPlannerParamsMessage::params
char * params() const
Get params value.
Definition: JacoBimanualInterface.cpp:1029
fawkes::JacoBimanualInterface::CartesianGotoMessage::r_x
float r_x() const
Get r_x value.
Definition: JacoBimanualInterface.cpp:528
fawkes::JacoBimanualInterface::CartesianGotoMessage::r_e2
float r_e2() const
Get r_e2 value.
Definition: JacoBimanualInterface.cpp:648
fawkes::JacoBimanualInterface::CartesianGotoMessage::l_e1
float l_e1() const
Get l_e1 value.
Definition: JacoBimanualInterface.cpp:438
fawkes::JacoBimanualInterface::MoveGripperMessage::l_finger1
float l_finger1() const
Get l_finger1 value.
Definition: JacoBimanualInterface.cpp:788
fawkes::BlockedTimingAspect
Thread aspect to use blocked timing.
Definition: blocked_timing.h:51
fawkes::JacoBimanualInterface::CartesianGotoMessage::r_z
float r_z() const
Get r_z value.
Definition: JacoBimanualInterface.cpp:588
fawkes::JacoBimanualInterface::set_msgid
void set_msgid(const uint32_t new_msgid)
Set msgid value.
Definition: JacoBimanualInterface.cpp:106
fawkes::Thread::name
const char * name() const
Get name of thread.
Definition: thread.h:100
fawkes::Interface::id
const char * id() const
Get identifier of interface.
Definition: interface.cpp:652
fawkes::JacoBimanualInterface
JacoBimanualInterface Fawkes BlackBoard Interface.
Definition: JacoBimanualInterface.h:34
JacoBimanualOpenraveThread::add_target
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.
Definition: bimanual_openrave_thread.cpp:321
fawkes::JacoBimanualInterface::CartesianGotoMessage::l_x
float l_x() const
Get l_x value.
Definition: JacoBimanualInterface.cpp:348
fawkes::JacoBimanualInterface::CartesianGotoMessage::r_e3
float r_e3() const
Get r_e3 value.
Definition: JacoBimanualInterface.cpp:678
fawkes::JacoBimanualInterface::CartesianGotoMessage::r_y
float r_y() const
Get r_y value.
Definition: JacoBimanualInterface.cpp:558
JacoBimanualOpenraveThread::set_constrained
virtual void set_constrained(bool enable)
Enable/Disable constrained planning.
Definition: bimanual_openrave_thread.cpp:295
fawkes::JacoBimanualInterface::MoveGripperMessage::l_finger2
float l_finger2() const
Get l_finger2 value.
Definition: JacoBimanualInterface.cpp:818
fawkes::LoggingAspect::logger
Logger * logger
This is the Logger member used to access the logger.
Definition: logging.h:41
fawkes::BlackBoard::close
virtual void close(Interface *interface)=0
Close interface.
JacoBimanualActThread::loop
virtual void loop()
Main loop.
Definition: bimanual_act_thread.cpp:79
fawkes::jaco_dual_arm_struct
Jaco struct containing all components required for a dual-arm setup.
Definition: types.h:113
JacoBimanualActThread::~JacoBimanualActThread
virtual ~JacoBimanualActThread()
Destructor.
Definition: bimanual_act_thread.cpp:49
fawkes
Fawkes library namespace.
fawkes::Logger::log_warn
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
JacoBimanualActThread::JacoBimanualActThread
JacoBimanualActThread(fawkes::jaco_dual_arm_t *arms)
Constructor.
Definition: bimanual_act_thread.cpp:41
fawkes::jaco_dual_arm_struct::openrave_thread
JacoBimanualOpenraveThread * openrave_thread
OpenraveThread for coordinated manipulation.
Definition: types.h:118
fawkes::JacoBimanualInterface::CartesianGotoMessage::l_e3
float l_e3() const
Get l_e3 value.
Definition: JacoBimanualInterface.cpp:498
fawkes::JacoBimanualInterface::MoveGripperMessage::r_finger1
float r_finger1() const
Get r_finger1 value.
Definition: JacoBimanualInterface.cpp:878
fawkes::Exception::what_no_backtrace
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
Definition: exception.cpp:663
fawkes::JacoBimanualInterface::CartesianGotoMessage::l_z
float l_z() const
Get l_z value.
Definition: JacoBimanualInterface.cpp:408
fawkes::Thread
Thread class encapsulation of pthreads.
Definition: thread.h:46
fawkes::BlackBoardAspect::blackboard
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Definition: blackboard.h:44
JacoOpenraveBaseThread::set_plannerparams
virtual void set_plannerparams(const std::string &params)
Set planner parameters.
Definition: openrave_base_thread.cpp:130
fawkes::Message::id
unsigned int id() const
Get message ID.
Definition: message.cpp:180
JacoBimanualActThread::finalize
virtual void finalize()
Finalize.
Definition: bimanual_act_thread.cpp:63
fawkes::jaco_dual_arm_struct::goto_thread
JacoBimanualGotoThread * goto_thread
GotoThread for coordinated manipulation.
Definition: types.h:117
fawkes::Interface::msgq_first
Message * msgq_first()
Get the first message from the message queue.
Definition: interface.cpp:1167
fawkes::JacoBimanualInterface::MoveGripperMessage::l_finger3
float l_finger3() const
Get l_finger3 value.
Definition: JacoBimanualInterface.cpp:848
fawkes::Logger::log_debug
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
fawkes::Interface::write
void write()
Write from local copy into BlackBoard memory.
Definition: interface.cpp:494
fawkes::jaco_dual_arm_struct::iface
JacoBimanualInterface * iface
interface used for coordinated manipulation.
Definition: types.h:116
fawkes::BlackBoard::open_for_writing
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
fawkes::JacoBimanualInterface::CartesianGotoMessage::l_y
float l_y() const
Get l_y value.
Definition: JacoBimanualInterface.cpp:378
fawkes::JacoBimanualInterface::CartesianGotoMessage
CartesianGotoMessage Fawkes BlackBoard Interface Message.
Definition: JacoBimanualInterface.h:66
fawkes::Exception
Base class for exceptions in Fawkes.
Definition: exception.h:36