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",
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.
virtual void stop()
Stops the current movement.
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
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.
float z() const
Get z value.
float j1() const
Get j1 value.
const char * name() const
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.
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.
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
virtual void log_error(const char *component, const char *format,...)=0
@ CONFIG_LEFT
this arm is the left one out of two.
virtual void finalize()
Finalize.
float e2() const
Get e2 value.
fawkes::JacoArm * arm
pointer to actual JacoArm instance, controlling this arm
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
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.
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.
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.
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
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.
void set_initialized(const bool new_initialized)
Set initialized value.
virtual void log_debug(const char *component, const char *format,...)=0
void write()
Write from local copy into BlackBoard memory.
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.
virtual Interface * open_for_writing(const char *interface_type, const char *identifier, const char *owner=NULL)=0
void set_msgid(const uint32_t new_msgid)
Set msgid value.
float x() const
Get x value.