24 #include "goto_openrave_thread.h" 25 #include "conversion.h" 26 #include "controller.h" 27 #include "exception.h" 29 #include <interfaces/KatanaInterface.h> 33 #include <utils/time/time.h> 36 #include <plugins/openrave/aspect/openrave_connector.h> 38 #include <plugins/openrave/robot.h> 39 #include <plugins/openrave/environment.h> 40 #include <plugins/openrave/manipulators/katana6M180.h> 41 #include <plugins/openrave/manipulators/neuronics_katana.h> 58 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS =
59 "minimumgoalpaths 16 postprocessingparameters <_nmaxiterations>100</_nmaxiterations>" 60 "<_postprocessing planner=\"parabolicsmoother\"><_nmaxiterations>200</_nmaxiterations>" 61 "</_postprocessing>\n";
62 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS_STRAIGHT =
63 "maxdeviationangle 0.05";
81 unsigned int poll_interval_ms,
82 std::string robot_file,
83 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" ),
99 _openrave( openrave ),
114 KatanaGotoOpenRaveThread::set_target(
float x,
float y,
float z,
115 float phi,
float theta,
float psi)
124 __has_target_quaternion =
false;
125 __is_target_object =
false;
126 __move_straight =
false;
127 __is_arm_extension =
false;
140 KatanaGotoOpenRaveThread::set_target(
float x,
float y,
float z,
141 float quat_x,
float quat_y,
float quat_z,
float quat_w)
151 __has_target_quaternion =
true;
152 __is_target_object =
false;
153 __move_straight =
false;
154 __is_arm_extension =
false;
161 KatanaGotoOpenRaveThread::set_target(
const std::string& object_name,
float rot_x)
163 __target_object = object_name;
165 __is_target_object =
true;
172 KatanaGotoOpenRaveThread::set_theta_error(
float error)
174 __theta_error = error;
183 KatanaGotoOpenRaveThread::set_move_straight(
bool move_straight)
185 __move_straight = move_straight;
194 KatanaGotoOpenRaveThread::set_arm_extension(
bool arm_extension)
196 __is_arm_extension = arm_extension;
204 KatanaGotoOpenRaveThread::set_plannerparams(std::string& params,
bool straight)
207 __plannerparams_straight = params;
209 __plannerparams = params;
218 KatanaGotoOpenRaveThread::set_plannerparams(
const char* params,
bool straight)
221 __plannerparams_straight = params;
223 __plannerparams = params;
231 __OR_robot = _openrave->add_robot(__cfg_robot_file,
false);
233 throw fawkes::Exception(
"Could not add robot '%s' to openrave environment", __cfg_robot_file.c_str());
239 if( __cfg_arm_model ==
"5dof" ) {
241 __OR_manip->add_motor(0,0);
242 __OR_manip->add_motor(1,1);
243 __OR_manip->add_motor(2,2);
244 __OR_manip->add_motor(3,3);
245 __OR_manip->add_motor(4,4);
250 _openrave->set_manipulator(__OR_robot, __OR_manip, 0.f, 0.f, 0.f);
251 __OR_robot->get_robot_ptr()->SetActiveManipulator(
"arm_kni");
253 if( __cfg_autoload_IK ) {
254 _openrave->get_environment()->load_IK_solver(__OR_robot, OpenRAVE::IKP_TranslationDirection5D);
256 }
else if ( __cfg_arm_model ==
"6dof_dummy" ) {
258 __OR_manip->add_motor(0,0);
259 __OR_manip->add_motor(1,1);
260 __OR_manip->add_motor(2,2);
261 __OR_manip->add_motor(4,3);
262 __OR_manip->add_motor(5,4);
267 _openrave->set_manipulator(__OR_robot, __OR_manip, 0.f, 0.f, 0.f);
269 if( __cfg_autoload_IK ) {
270 _openrave->get_environment()->load_IK_solver(__OR_robot, OpenRAVE::IKP_Transform6D);
273 throw fawkes::Exception(
"Unknown entry for 'arm_model':%s", __cfg_arm_model.c_str());
281 if( __cfg_use_viewer)
282 _openrave->start_viewer();
288 _openrave->set_active_robot( NULL );
296 #ifndef EARLY_PLANNING 297 if( !plan_target() ) {
309 __target_traj = __OR_robot->get_trajectory_device();
313 __it = __target_traj->begin();
316 final = move_katana();
317 update_openrave_data();
333 update_openrave_data();
335 final = _katana->final();
337 _logger->log_warn(
"KatanaGotoThread", e.
what());
344 _logger->log_warn(
"KatanaGotoThread",
"Moving along trajectory failed (ignoring): %s", e.
what());
365 KatanaGotoOpenRaveThread::plan_target()
368 if( !update_motor_data() ) {
369 _logger->log_warn(
"KatanaGotoThread",
"Fetching current motor values failed");
375 if( !_katana->joint_angles() ) {
376 encToRad(__motor_encoders, __motor_angles);
378 __OR_manip->set_angles_device(__motor_angles);
381 if( __plannerparams.compare(
"default") == 0 ) {
382 __plannerparams = DEFAULT_PLANNERPARAMS;
384 if( __is_target_object) {
385 _logger->log_debug(name(),
"Check IK for object (%s)", __target_object.c_str());
387 if( !_openrave->set_target_object(__target_object, __OR_robot) ) {
388 _logger->log_warn(
"KatanaGotoThread",
"Initiating goto failed, no IK solution found");
394 bool success =
false;
396 if( __has_target_quaternion ) {
397 _logger->log_debug(name(),
"Check IK(%f,%f,%f | %f,%f,%f,%f)",
398 __x, __y, __z, __quat_x, __quat_y, __quat_z, __quat_w);
399 success = __OR_robot->set_target_quat(__x, __y, __z, __quat_w, __quat_x, __quat_y, __quat_z);
400 }
else if( __move_straight ) {
401 _logger->log_debug(name(),
"Check IK(%f,%f,%f), straight movement",
403 if( __is_arm_extension ) {
404 success = __OR_robot->set_target_rel(__x, __y, __z,
true);
406 success = __OR_robot->set_target_straight(__x, __y, __z);
408 if( __plannerparams_straight.compare(
"default") == 0 ) {
409 __plannerparams_straight = DEFAULT_PLANNERPARAMS_STRAIGHT;
412 float theta_error = 0.0f;
413 while( !success && (theta_error <= __theta_error)) {
414 _logger->log_debug(name(),
"Check IK(%f,%f,%f | %f,%f,%f)",
415 __x, __y, __z, __phi, __theta+theta_error, __psi);
416 success = __OR_robot->set_target_euler(
EULER_ZXZ, __x, __y, __z, __phi, __theta+theta_error, __psi);
418 _logger->log_debug(name(),
"Check IK(%f,%f,%f | %f,%f,%f)",
419 __x, __y, __z, __phi, __theta-theta_error, __psi);
420 success = __OR_robot->set_target_euler(
EULER_ZXZ, __x, __y, __z, __phi, __theta-theta_error, __psi);
426 }
catch(OpenRAVE::openrave_exception &e) {
427 _logger->log_debug(name(),
"OpenRAVE exception:%s", e.what());
431 _logger->log_warn(
"KatanaGotoThread",
"Initiating goto failed, no IK solution found");
436 if( __move_straight ) {
437 __OR_robot->set_target_plannerparams(__plannerparams_straight);
439 __OR_robot->set_target_plannerparams(__plannerparams);
443 float sampling = 0.04f;
445 _openrave->run_planner(__OR_robot, sampling);
447 _logger->log_warn(
"KatanaGotoThread",
"Planner failed (ignoring): %s", e.
what());
458 KatanaGotoOpenRaveThread::update_openrave_data()
461 if( !update_motor_data() ) {
462 _logger->log_warn(
"KatanaGotoThread",
"Fetching current motor values failed");
468 if( !_katana->joint_angles() ) {
469 encToRad(__motor_encoders, __motor_angles);
471 __OR_manip->set_angles_device(__motor_angles);
473 std::vector<OpenRAVE::dReal> angles;
474 __OR_manip->get_angles(angles);
477 OpenRAVE::EnvironmentMutex::scoped_lock lock(__OR_robot->get_robot_ptr()->GetEnv()->GetMutex());
478 __OR_robot->get_robot_ptr()->SetActiveDOFValues(angles);
486 KatanaGotoOpenRaveThread::update_motor_data()
488 short num_errors = 0;
494 _katana->read_sensor_data();
495 _katana->read_motor_data();
497 if (++num_errors <= 10) {
498 _logger->log_warn(
"KatanaGotoThread",
"Receiving motor data failed, retrying");
501 _logger->log_warn(
"KatanaGotoThread",
"Receiving motor data failed too often, aborting");
514 if( _katana->joint_angles()) {
515 _katana->get_angles(__motor_angles,
false);
517 _katana->get_encoders(__motor_encoders,
false);
520 if (++num_errors <= 10) {
521 _logger->log_warn(
"KatanaGotoThread",
"Receiving motor %s failed, retrying", _katana->joint_angles() ?
"angles" :
"encoders");
524 _logger->log_warn(
"KatanaGotoThread",
"Receiving motor %s failed, aborting", _katana->joint_angles() ?
"angles" :
"encoders");
543 KatanaGotoOpenRaveThread::move_katana()
545 if( _katana->joint_angles() ) {
546 _katana->move_to(*__it,
false);
548 std::vector<int> enc;
549 _katana->get_encoders(enc);
550 _katana->move_to(enc,
false);
553 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.
virtual const char * what() const
Get primary string.
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.
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.