24 #include "goto_openrave_thread.h"
25 #include "conversion.h"
26 #include "controller.h"
27 #include "exception.h"
31 #include <utils/time/time.h>
35 #include <plugins/openrave/robot.h>
36 #include <plugins/openrave/environment.h>
37 #include <plugins/openrave/manipulators/katana6M180.h>
38 #include <plugins/openrave/manipulators/neuronics_katana.h>
39 #include <plugins/openrave/types.h>
56 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS =
57 "minimumgoalpaths 16 postprocessingparameters <_nmaxiterations>100</_nmaxiterations>"
58 "<_postprocessing planner=\"parabolicsmoother\"><_nmaxiterations>200</_nmaxiterations>"
59 "</_postprocessing>\n";
60 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS_STRAIGHT =
61 "maxdeviationangle 0.05";
79 unsigned int poll_interval_ms,
80 std::string robot_file,
81 std::string arm_model,
87 __target_object(
"" ),
89 __cfg_robot_file( robot_file ),
90 __cfg_arm_model( arm_model ),
91 __cfg_autoload_IK( autoload_IK ),
92 __cfg_use_viewer( use_viewer ),
93 __is_target_object( 0 ),
94 __has_target_quaternion( 0 ),
96 __is_arm_extension( 0 ),
97 __plannerparams(
"default" ),
98 __plannerparams_straight(
"default" ),
113 KatanaGotoOpenRaveThread::set_target(
float x,
float y,
float z,
114 float phi,
float theta,
float psi)
123 __has_target_quaternion =
false;
124 __is_target_object =
false;
125 __move_straight =
false;
126 __is_arm_extension =
false;
139 KatanaGotoOpenRaveThread::set_target(
float x,
float y,
float z,
140 float quat_x,
float quat_y,
float quat_z,
float quat_w)
150 __has_target_quaternion =
true;
151 __is_target_object =
false;
152 __move_straight =
false;
153 __is_arm_extension =
false;
160 KatanaGotoOpenRaveThread::set_target(
const std::string& object_name,
float rot_x)
162 __target_object = object_name;
164 __is_target_object =
true;
171 KatanaGotoOpenRaveThread::set_theta_error(
float error)
173 __theta_error = error;
182 KatanaGotoOpenRaveThread::set_move_straight(
bool move_straight)
184 __move_straight = move_straight;
193 KatanaGotoOpenRaveThread::set_arm_extension(
bool arm_extension)
195 __is_arm_extension = arm_extension;
203 KatanaGotoOpenRaveThread::set_plannerparams(std::string& params,
bool straight)
206 __plannerparams_straight = params;
208 __plannerparams = params;
217 KatanaGotoOpenRaveThread::set_plannerparams(
const char* params,
bool straight)
220 __plannerparams_straight = params;
222 __plannerparams = params;
230 __OR_robot = _openrave->add_robot(__cfg_robot_file,
false);
234 if( __cfg_arm_model ==
"5dof" ) {
236 __OR_manip->add_motor(0,0);
237 __OR_manip->add_motor(1,1);
238 __OR_manip->add_motor(2,2);
239 __OR_manip->add_motor(3,3);
240 __OR_manip->add_motor(4,4);
245 _openrave->set_manipulator(__OR_robot, __OR_manip, 0.f, 0.f, 0.f);
246 __OR_robot->get_robot_ptr()->SetActiveManipulator(
"arm_kni");
248 if( __cfg_autoload_IK ) {
249 _openrave->get_environment()->load_IK_solver(__OR_robot, OpenRAVE::IKP_TranslationDirection5D);
251 }
else if ( __cfg_arm_model ==
"6dof_dummy" ) {
253 __OR_manip->add_motor(0,0);
254 __OR_manip->add_motor(1,1);
255 __OR_manip->add_motor(2,2);
256 __OR_manip->add_motor(4,3);
257 __OR_manip->add_motor(5,4);
262 _openrave->set_manipulator(__OR_robot, __OR_manip, 0.f, 0.f, 0.f);
264 if( __cfg_autoload_IK ) {
265 _openrave->get_environment()->load_IK_solver(__OR_robot, OpenRAVE::IKP_Transform6D);
268 throw fawkes::Exception(
"Unknown entry for 'arm_model':%s", __cfg_arm_model.c_str());
276 if( __cfg_use_viewer)
277 _openrave->start_viewer();
293 #ifndef EARLY_PLANNING
294 if( !plan_target() ) {
306 __target_traj = __OR_robot->get_trajectory_device();
310 __it = __target_traj->begin();
313 final = move_katana();
314 update_openrave_data();
330 update_openrave_data();
341 _logger->
log_warn(
"KatanaGotoThread",
"Moving along trajectory failed (ignoring): %s", e.
what());
362 KatanaGotoOpenRaveThread::plan_target()
365 if( !update_motor_data() ) {
366 _logger->
log_warn(
"KatanaGotoThread",
"Fetching current motor values failed");
373 encToRad(__motor_encoders, __motor_angles);
375 __OR_manip->set_angles_device(__motor_angles);
378 if( __plannerparams.compare(
"default") == 0 ) {
379 __plannerparams = DEFAULT_PLANNERPARAMS;
381 if( __is_target_object) {
384 if( !_openrave->set_target_object(__target_object, __OR_robot) ) {
385 _logger->
log_warn(
"KatanaGotoThread",
"Initiating goto failed, no IK solution found");
391 bool success =
false;
393 if( __has_target_quaternion ) {
395 __x, __y, __z, __quat_x, __quat_y, __quat_z, __quat_w);
396 success = __OR_robot->set_target_quat(__x, __y, __z, __quat_w, __quat_x, __quat_y, __quat_z);
397 }
else if( __move_straight ) {
400 if( __is_arm_extension ) {
401 success = __OR_robot->set_target_rel(__x, __y, __z,
true);
403 success = __OR_robot->set_target_straight(__x, __y, __z);
405 if( __plannerparams_straight.compare(
"default") == 0 ) {
406 __plannerparams_straight = DEFAULT_PLANNERPARAMS_STRAIGHT;
409 float theta_error = 0.0f;
410 while( !success && (theta_error <= __theta_error)) {
412 __x, __y, __z, __phi, __theta+theta_error, __psi);
413 success = __OR_robot->set_target_euler(EULER_ZXZ, __x, __y, __z, __phi, __theta+theta_error, __psi);
416 __x, __y, __z, __phi, __theta-theta_error, __psi);
417 success = __OR_robot->set_target_euler(EULER_ZXZ, __x, __y, __z, __phi, __theta-theta_error, __psi);
423 }
catch(OpenRAVE::openrave_exception &e) {
428 _logger->
log_warn(
"KatanaGotoThread",
"Initiating goto failed, no IK solution found");
433 if( __move_straight ) {
434 __OR_robot->set_target_plannerparams(__plannerparams_straight);
436 __OR_robot->set_target_plannerparams(__plannerparams);
440 float sampling = 0.04f;
442 _openrave->run_planner(__OR_robot, sampling);
455 KatanaGotoOpenRaveThread::update_openrave_data()
458 if( !update_motor_data() ) {
459 _logger->
log_warn(
"KatanaGotoThread",
"Fetching current motor values failed");
466 encToRad(__motor_encoders, __motor_angles);
468 __OR_manip->set_angles_device(__motor_angles);
470 std::vector<OpenRAVE::dReal> angles;
471 __OR_manip->get_angles(angles);
474 OpenRAVE::EnvironmentMutex::scoped_lock lock(__OR_robot->get_robot_ptr()->GetEnv()->GetMutex());
475 __OR_robot->get_robot_ptr()->SetActiveDOFValues(angles);
483 KatanaGotoOpenRaveThread::update_motor_data()
485 short num_errors = 0;
494 if (++num_errors <= 10) {
495 _logger->
log_warn(
"KatanaGotoThread",
"Receiving motor data failed, retrying");
498 _logger->
log_warn(
"KatanaGotoThread",
"Receiving motor data failed too often, aborting");
517 if (++num_errors <= 10) {
540 KatanaGotoOpenRaveThread::move_katana()
545 std::vector<int> enc;
550 return (++__it == __target_traj->end());
static const uint32_t ERROR_NO_SOLUTION
ERROR_NO_SOLUTION constant.
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void once()
Execute an action exactly once.
Class containing information about all katana6M180 motors.
Time & stamp_systime()
Set this time to the current system time.
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
Fawkes library namespace.
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
fawkes::Logger * _logger
Logger.
A class for handling time.
Katana motion thread base class.
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
void encToRad(std::vector< int > &enc, std::vector< float > &rad)
Convert encoder vaulues of katana arm to radian angles.
Class containing information about all neuronics-katana motors.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
virtual const char * what() const
Get primary string.
Base class for exceptions in Fawkes.
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void finalize()
Finalize the thread.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
virtual void move_to(float x, float y, float z, float phi, float theta, float psi, bool blocking=false)=0
Move endeffctor to given coordinates.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
const char * name() const
Get name of thread.
Interface for a OpenRave connection creator.
At least one motor crashed.
bool _finished
Set to true when motion is finished, to false on reset.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
unsigned int _error_code
Set to the desired error code on error.
virtual void init()
Initialize the thread.
virtual bool final()=0
Check if movement is final.
virtual void stop()=0
Stop movement immediately.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.