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();
332 final = _katana->final();
334 _logger->log_warn(
"KatanaMotorControlTrhead", e.
what());
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");
372 if( !_katana->joint_angles() ) {
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) {
382 _logger->log_debug(name(),
"Check IK for object (%s)", __target_object.c_str());
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 ) {
394 _logger->log_debug(name(),
"Check IK(%f,%f,%f | %f,%f,%f,%f)",
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 ) {
398 _logger->log_debug(name(),
"Check IK(%f,%f,%f), straight movement",
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)) {
411 _logger->log_debug(name(),
"Check IK(%f,%f,%f | %f,%f,%f)",
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);
415 _logger->log_debug(name(),
"Check IK(%f,%f,%f | %f,%f,%f)",
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) {
424 _logger->log_debug(name(),
"OpenRAVE exception:%s", e.what());
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);
444 _logger->log_warn(
"KatanaGotoThread",
"Planner failed (ignoring): %s", e.
what());
455 KatanaGotoOpenRaveThread::update_openrave_data()
458 if( !update_motor_data() ) {
459 _logger->log_warn(
"KatanaGotoThread",
"Fetching current motor values failed");
465 if( !_katana->joint_angles() ) {
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;
491 _katana->read_sensor_data();
492 _katana->read_motor_data();
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");
511 if( _katana->joint_angles()) {
512 _katana->get_angles(__motor_angles,
false);
514 _katana->get_encoders(__motor_encoders,
false);
517 if (++num_errors <= 10) {
518 _logger->log_warn(
"KatanaGotoThread",
"Receiving motor %s failed, retrying", _katana->joint_angles() ?
"angles" :
"encoders");
521 _logger->log_warn(
"KatanaGotoThread",
"Receiving motor %s failed, aborting", _katana->joint_angles() ?
"angles" :
"encoders");
540 KatanaGotoOpenRaveThread::move_katana()
542 if( _katana->joint_angles() ) {
543 _katana->move_to(*__it,
false);
545 std::vector<int> enc;
546 _katana->get_encoders(enc);
547 _katana->move_to(enc,
false);
550 return (++__it == __target_traj->end());
static const uint32_t ERROR_NO_SOLUTION
ERROR_NO_SOLUTION constant.
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.
Fawkes library namespace.
A class for handling time.
Katana motion thread base class.
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 void finalize()
Finalize the thread.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
Interface for a OpenRave connection creator.
At least one motor crashed.
virtual void init()
Initialize the thread.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.