Fawkes API
Fawkes Development Version
|
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",
JoystickPushMessage Fawkes BlackBoard Interface Message.
AngularGotoMessage Fawkes BlackBoard Interface Message.
JacoGotoThread * goto_thread
the GotoThread of this arm.
bool msgq_first_is()
Check if first message has desired type.
virtual bool final()
Check if arm is final.
void msgq_pop()
Erase first message from queue.
bool msgq_empty()
Check if queue is empty.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
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 push_joystick(unsigned int button)=0
Simulate a push of a button on the joystick of the Kinova Jaco arm.
StopMessage Fawkes BlackBoard Interface Message.
Mutex mutual exclusion lock.
virtual void stop()
Stops the current movement.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
RefPtr<> is a reference-counting shared smartpointer.
virtual bool initialized()=0
Check if arm is initialized.
virtual void release_joystick()=0
Simulate releasing the joystick of the Kinova Jaco arm.
float e1() const
Get e1 value.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
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.
float j4() const
Get j4 value.
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.
std::string get_name() const
Get the name of the arm.
@ CONFIG_SINGLE
we only have one arm.
virtual void init()
Initialize.
virtual void initialize()=0
Initialize the arm.
RetractMessage Fawkes BlackBoard Interface Message.
Thread aspect to use blocked timing.
float z() const
Get z value.
float j1() const
Get j1 value.
const char * name() const
Get name of thread.
float j5() const
Get j5 value.
virtual void pos_ready()
Moves the arm to the "READY" position.
const char * id() const
Get identifier of interface.
float trajec_color[4]
the color used for plotting the trajectory.
@ CONFIG_RIGHT
this arm is the right one out of two.
Class for simulating a dummy Kinova Jaco Arm.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
float j3() const
Get j3 value.
virtual void loop()
Main loop.
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
float finger1() const
Get finger1 value.
Logger * logger
This is the Logger member used to access the logger.
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 close(Interface *interface)=0
Close interface.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
MoveGripperMessage Fawkes BlackBoard Interface Message.
@ CONFIG_LEFT
this arm is the left one out of two.
virtual void finalize()
Finalize.
float e2() const
Get e2 value.
Fawkes library namespace.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float y() const
Get y value.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
virtual ~JacoActThread()
Destructor.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
uint32_t button() const
Get button value.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
float finger3() const
Get finger3 value.
Configuration * config
This is the Configuration member used to access the configuration.
float j6() const
Get j6 value.
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
JacoActThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
Class for commanding a Kinova Jaco Arm, using libkindrv.
virtual const char * what_no_backtrace() const
Get primary string (does not implicitly print the back trace).
bool started() const
Check if thread has been started.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
Thread class encapsulation of pthreads.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
Jaco struct containing all components required for one arm.
virtual void set_plannerparams(const std::string ¶ms)
Set planner parameters.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
unsigned int id() const
Get message ID.
char * params() const
Get params value.
float finger2() const
Get finger2 value.
float j2() const
Get j2 value.
Message * msgq_first()
Get the first message from the message queue.
jaco_arm_config_t config
configuration for this arm
float e3() const
Get e3 value.
CalibrateMessage Fawkes BlackBoard Interface Message.
void set_initialized(const bool new_initialized)
Set initialized value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
void write()
Write from local copy into BlackBoard memory.
JacoInterface Fawkes BlackBoard Interface.
std::list< RefPtr< jaco_target_t > > jaco_target_queue_t
FIFO target queue, holding RefPtr to targets.
void set_final(const bool new_final)
Set final value.
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
JoystickReleaseMessage Fawkes BlackBoard Interface Message.
float x() const
Get x value.
Base class for exceptions in Fawkes.