23 #include "act_thread.h"
25 #include "arm_dummy.h"
26 #include "arm_kindrv.h"
27 #include "goto_thread.h"
28 #include "openrave_thread.h"
31 #include <core/utils/refptr.h>
32 #include <interfaces/JacoInterface.h>
71 cfg_auto_init_ =
config->
get_bool(
"/hardware/jaco/auto_initialize");
72 cfg_auto_calib_ =
config->
get_bool(
"/hardware/jaco/auto_calibrate");
76 if (cfg_arm.compare(
"libkindrv") && cfg_arm.compare(
"dummy"))
77 throw fawkes::Exception(
"Bad config entry /hardware/jaco/arm '%s'", cfg_arm.c_str());
79 std::string arm_name, arm_iface;
101 if (!cfg_arm.compare(
"dummy")) {
116 "Could not open interface %s for writing. Exception follows.",
152 "Could not close JacoInterface interface. Er:%s",
175 if (_is_initializing())
203 JacoActThread::_initialize()
222 JacoActThread::_is_initializing()
237 JacoActThread::_process_msgs()
271 "%s: SetPlannerParamsMessage rcvd. params:%s",
282 "%s: CartesianGotoMessage rcvd. x:%f y:%f z:%f e1:%f e2:%f e3:%f",
292 "%s: CartesianGotoMessage is being passed to openrave",
296 msg->
x(), msg->
y(), msg->
z(), msg->
e1(), msg->
e2(), msg->
e3());
300 "Failed executing CartesianGotoMessage, arm %s and/or thread %s could not "
313 "%s: AngularGotoMessage rcvd. x:%f y:%f z:%f e1:%f e2:%f e3:%f",
323 "%s: AngularGotoMessage is being passed to openrave",
327 msg->
j1(), msg->
j2(), msg->
j3(), msg->
j4(), msg->
j5(), msg->
j6());
331 "Failed executing AngularGotoMessage, given target joints for arm %s are "
332 "invalid or in self-collision",
337 msg->
j1(), msg->
j2(), msg->
j3(), msg->
j4(), msg->
j5(), msg->
j6());
343 "%s: MoveGripperMessage rcvd. f1:%f f2:%f f3:%f",
virtual void init()
Initialize.
virtual void loop()
Main loop.
JacoActThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
virtual ~JacoActThread()
Destructor.
virtual void finalize()
Finalize.
virtual bool final()
Check if arm is final.
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
virtual void set_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, float f1=0.f, float f2=0.f, float f3=0.f)
Set new target, given joint positions This target is added to the queue, skipping trajectory planning...
virtual void set_target(float x, float y, float z, float e1, float e2, float e3, float f1=0.f, float f2=0.f, float f3=0.f)
Set new target, given cartesian coordinates.
virtual void stop()
Stops the current movement.
virtual void pos_ready()
Moves the arm to the "READY" position.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
virtual void set_plannerparams(const std::string ¶ms)
Set planner parameters.
virtual bool add_target(float x, float y, float z, float e1, float e2, float e3, bool plan=true)
Solve IK and add target to the queue.
virtual bool add_target_ang(float j1, float j2, float j3, float j4, float j5, float j6, bool plan=true)
Add target joint values to the queue.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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.
Configuration * config
This is the Configuration member used to access the configuration.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
Base class for exceptions in Fawkes.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
bool msgq_first_is()
Check if first message has desired type.
void msgq_pop()
Erase first message from queue.
Message * msgq_first()
Get the first message from the message queue.
void write()
Write from local copy into BlackBoard memory.
const char * id() const
Get identifier of interface.
bool msgq_empty()
Check if queue is empty.
Class for simulating a dummy Kinova Jaco Arm.
Class for commanding a Kinova Jaco Arm, using libkindrv.
virtual void release_joystick()=0
Simulate releasing the joystick of the Kinova Jaco arm.
virtual bool initialized()=0
Check if arm is initialized.
std::string get_name() const
Get the name of the arm.
virtual void initialize()=0
Initialize the arm.
virtual void push_joystick(unsigned int button)=0
Simulate a push of a button on the joystick of the Kinova Jaco arm.
AngularGotoMessage Fawkes BlackBoard Interface Message.
float j1() const
Get j1 value.
float j4() const
Get j4 value.
float j5() const
Get j5 value.
float j3() const
Get j3 value.
float j6() const
Get j6 value.
float j2() const
Get j2 value.
CalibrateMessage Fawkes BlackBoard Interface Message.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float e1() const
Get e1 value.
float x() const
Get x value.
float y() const
Get y value.
float z() const
Get z value.
float e2() const
Get e2 value.
float e3() const
Get e3 value.
JoystickPushMessage Fawkes BlackBoard Interface Message.
uint32_t button() const
Get button value.
JoystickReleaseMessage Fawkes BlackBoard Interface Message.
MoveGripperMessage Fawkes BlackBoard Interface Message.
float finger1() const
Get finger1 value.
float finger3() const
Get finger3 value.
float finger2() const
Get finger2 value.
RetractMessage Fawkes BlackBoard Interface Message.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
char * params() const
Get params value.
StopMessage Fawkes BlackBoard Interface Message.
JacoInterface Fawkes BlackBoard Interface.
void set_initialized(const bool new_initialized)
Set initialized value.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
void set_final(const bool new_final)
Set final value.
void set_msgid(const uint32_t new_msgid)
Set msgid 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.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
Logger * logger
This is the Logger member used to access the logger.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
Mutex mutual exclusion lock.
RefPtr<> is a reference-counting shared smartpointer.
Thread class encapsulation of pthreads.
const char * name() const
Get name of thread.
bool started() const
Check if thread has been started.
Fawkes library namespace.
@ CONFIG_SINGLE
we only have one arm.
@ CONFIG_LEFT
this arm is the left one out of two.
@ CONFIG_RIGHT
this arm is the right one out of two.
std::list< RefPtr< jaco_target_t > > jaco_target_queue_t
FIFO target queue, holding RefPtr to targets.
Jaco struct containing all components required for one arm.
jaco_arm_config_t config
configuration for this arm
float trajec_color[4]
the color used for plotting the trajectory.
JacoGotoThread * goto_thread
the GotoThread of this arm.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm