23 #include "act_thread.h" 24 #include "goto_thread.h" 25 #include "openrave_thread.h" 28 #include "arm_kindrv.h" 29 #include "arm_dummy.h" 31 #include <interfaces/JacoInterface.h> 32 #include <core/utils/refptr.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")
77 && cfg_arm.compare(
"dummy") )
78 throw fawkes::Exception(
"Bad config entry /hardware/jaco/arm '%s'", cfg_arm.c_str());
80 std::string arm_name, arm_iface;
104 if( !cfg_arm.compare(
"dummy") ) {
118 logger->
log_error(
name(),
"Could not open interface %s for writing. Exception follows.", arm_iface.c_str());
174 if( _is_initializing() )
204 JacoActThread::_initialize()
223 JacoActThread::_is_initializing()
238 JacoActThread::_process_msgs()
280 msg->
x(), msg->
y(), msg->
z(), msg->
e1(), msg->
e2(), msg->
e3());
287 logger->
log_warn(
name(),
"Failed executing CartesianGotoMessage, arm %s and/or thread %s could not find IK solution",
298 msg->
j1(), msg->
j2(), msg->
j3(), msg->
j4(), msg->
j5(), msg->
j6());
303 if( !joints_valid ) {
305 logger->
log_warn(
name(),
"Failed executing AngularGotoMessage, given target joints for arm %s are invalid or in self-collision",
JacoGotoThread * goto_thread
the GotoThread of this arm.
float j6() const
Get j6 value.
Jaco struct containing all components required for one arm.
Base class for all messages passed through interfaces in Fawkes BlackBoard.
unsigned int id() const
Get message ID.
virtual void finalize()
Finalize.
float z() const
Get z value.
bool msgq_empty()
Check if queue is empty.
float finger2() const
Get finger2 value.
JacoActThread(const char *name, fawkes::jaco_arm_t *arm)
Constructor.
virtual void update_openrave()
Update the openrave environment to represent the current situation.
Fawkes library namespace.
virtual bool get_bool(const char *path)=0
Get value from configuration which is of type bool.
SetPlannerParamsMessage Fawkes BlackBoard Interface Message.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
const char * id() const
Get identifier of interface.
float trajec_color[4]
the color used for plotting the trajectory.
float j2() const
Get j2 value.
virtual void pos_retract()
Moves the arm to the "RETRACT" position.
virtual void loop()
Main loop.
Class for simulating a dummy Kinova Jaco Arm.
Thread class encapsulation of pthreads.
MoveGripperMessage Fawkes BlackBoard Interface Message.
std::list< RefPtr< jaco_target_t > > jaco_target_queue_t
FIFO target queue, holding RefPtr to targets.
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.
RefPtr< Mutex > trajec_mutex
mutex, used for modifying trajectory of a target.
void write()
Write from local copy into BlackBoard memory.
Logger * logger
This is the Logger member used to access the logger.
float y() const
Get y value.
float finger3() const
Get finger3 value.
this arm is the right one out of two.
JacoInterface * iface
pointer to JacoInterface, assigned to this arm
JacoOpenraveThread * openrave_thread
the OpenraveThread of this arm.
Thread aspect to use blocked timing.
virtual void pos_ready()
Moves the arm to the "READY" position.
void msgq_pop()
Erase first message from queue.
virtual void initialize()=0
Initialize the arm.
virtual void release_joystick()=0
Simulate releasing the joystick of the Kinova Jaco arm.
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 stop()
Stops the current movement.
Base class for exceptions in Fawkes.
Message * msgq_first()
Get the first message from the message queue.
RefPtr< Mutex > target_mutex
mutex, used for accessing the target_queue
bool started() const
Check if thread has been started.
virtual bool final()
Check if arm is final.
CalibrateMessage Fawkes BlackBoard Interface Message.
StopMessage Fawkes BlackBoard Interface Message.
const char * name() const
Get name of thread.
void set_error_code(const uint32_t new_error_code)
Set error_code value.
virtual void move_gripper(float f1, float f2, float f3)
Moves only the gripper.
jaco_arm_config_t config
configuration for this arm
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 msgq_first_is()
Check if first message has desired type.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
void set_initialized(const bool new_initialized)
Set initialized value.
virtual void log_error(const char *component, const char *format,...)=0
Log error message.
float e3() const
Get e3 value.
float j4() const
Get j4 value.
RefPtr< jaco_target_queue_t > target_queue
queue of targets, which is processed FIFO.
float e2() const
Get e2 value.
uint32_t button() const
Get button value.
this arm is the left one out of two.
RefPtr<> is a reference-counting shared smartpointer.
void set_final(const bool new_final)
Set final value.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
char * params() const
Get params value.
JoystickPushMessage Fawkes BlackBoard Interface Message.
float j1() const
Get j1 value.
void set_msgid(const uint32_t new_msgid)
Set msgid value.
JacoInterface Fawkes BlackBoard Interface.
virtual void set_plannerparams(const std::string ¶ms)
Set planner parameters.
AngularGotoMessage Fawkes BlackBoard Interface Message.
virtual ~JacoActThread()
Destructor.
float x() const
Get x value.
virtual void push_joystick(unsigned int button)=0
Simulate a push of a button on the joystick of the Kinova Jaco arm.
virtual void init()
Initialize.
JoystickReleaseMessage Fawkes BlackBoard Interface Message.
float j3() const
Get j3 value.
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.
Mutex mutual exclusion lock.
CartesianGotoMessage Fawkes BlackBoard Interface Message.
float j5() const
Get j5 value.
Configuration * config
This is the Configuration member used to access the configuration.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
Open interface for writing.
RetractMessage Fawkes BlackBoard Interface Message.
virtual bool initialized()=0
Check if arm is initialized.
virtual std::string get_string(const char *path)=0
Get value from configuration which is of type string.
float finger1() const
Get finger1 value.
BlackBoard * blackboard
This is the BlackBoard instance you can use to interact with the BlackBoard.
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...
std::string get_name() const
Get the name of the arm.
float e1() const
Get e1 value.
virtual void close(Interface *interface)=0
Close interface.