24 #include "goto_openrave_thread.h"
26 #include "controller.h"
27 #include "conversion.h"
28 #include "exception.h"
30 #include <interfaces/KatanaInterface.h>
31 #include <utils/time/time.h>
36 # include <plugins/openrave/aspect/openrave_connector.h>
37 # include <plugins/openrave/environment.h>
38 # include <plugins/openrave/manipulators/katana6M180.h>
39 # include <plugins/openrave/manipulators/neuronics_katana.h>
40 # include <plugins/openrave/robot.h>
57 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS =
58 "minimumgoalpaths 16 postprocessingparameters <_nmaxiterations>100</_nmaxiterations>"
59 "<_postprocessing planner=\"parabolicsmoother\"><_nmaxiterations>200</_nmaxiterations>"
60 "</_postprocessing>\n";
61 const std::string KatanaGotoOpenRaveThread::DEFAULT_PLANNERPARAMS_STRAIGHT =
62 "maxdeviationangle 0.05";
79 unsigned int poll_interval_ms,
80 const std::string & robot_file,
81 const std::string & arm_model,
87 cfg_robot_file_(robot_file),
88 cfg_arm_model_(arm_model),
89 cfg_autoload_IK_(autoload_IK),
90 cfg_use_viewer_(use_viewer),
92 has_target_quaternion_(0),
95 plannerparams_(
"default"),
96 plannerparams_straight_(
"default"),
111 KatanaGotoOpenRaveThread::set_target(
float x,
float y,
float z,
float phi,
float theta,
float psi)
120 has_target_quaternion_ =
false;
121 is_target_object_ =
false;
122 move_straight_ =
false;
123 is_arm_extension_ =
false;
136 KatanaGotoOpenRaveThread::set_target(
float x,
152 has_target_quaternion_ =
true;
153 is_target_object_ =
false;
154 move_straight_ =
false;
155 is_arm_extension_ =
false;
162 KatanaGotoOpenRaveThread::set_target(
const std::string &object_name,
float rot_x)
164 target_object_ = object_name;
166 is_target_object_ =
true;
173 KatanaGotoOpenRaveThread::set_theta_error(
float error)
175 theta_error_ = error;
184 KatanaGotoOpenRaveThread::set_move_straight(
bool move_straight)
186 move_straight_ = move_straight;
195 KatanaGotoOpenRaveThread::set_arm_extension(
bool arm_extension)
197 is_arm_extension_ = arm_extension;
205 KatanaGotoOpenRaveThread::set_plannerparams(std::string ¶ms,
bool straight)
208 plannerparams_straight_ = params;
210 plannerparams_ = params;
219 KatanaGotoOpenRaveThread::set_plannerparams(
const char *params,
bool straight)
222 plannerparams_straight_ = params;
224 plannerparams_ = params;
232 OR_robot_ = _openrave->add_robot(cfg_robot_file_,
false);
235 cfg_robot_file_.c_str());
241 if (cfg_arm_model_ ==
"5dof") {
243 OR_manip_->add_motor(0, 0);
244 OR_manip_->add_motor(1, 1);
245 OR_manip_->add_motor(2, 2);
246 OR_manip_->add_motor(3, 3);
247 OR_manip_->add_motor(4, 4);
252 _openrave->set_manipulator(OR_robot_, OR_manip_, 0.f, 0.f, 0.f);
253 OR_robot_->get_robot_ptr()->SetActiveManipulator(
"arm_kni");
255 if (cfg_autoload_IK_) {
256 _openrave->get_environment()->load_IK_solver(OR_robot_,
257 OpenRAVE::IKP_TranslationDirection5D);
259 }
else if (cfg_arm_model_ ==
"6dof_dummy") {
261 OR_manip_->add_motor(0, 0);
262 OR_manip_->add_motor(1, 1);
263 OR_manip_->add_motor(2, 2);
264 OR_manip_->add_motor(4, 3);
265 OR_manip_->add_motor(5, 4);
270 _openrave->set_manipulator(OR_robot_, OR_manip_, 0.f, 0.f, 0.f);
272 if (cfg_autoload_IK_) {
273 _openrave->get_environment()->load_IK_solver(OR_robot_, OpenRAVE::IKP_Transform6D);
276 throw fawkes::Exception(
"Unknown entry for 'arm_model':%s", cfg_arm_model_.c_str());
285 _openrave->start_viewer();
291 _openrave->set_active_robot(NULL);
299 # ifndef EARLY_PLANNING
300 if (!plan_target()) {
312 target_traj_ = OR_robot_->get_trajectory_device();
316 it_ = target_traj_->begin();
319 final = move_katana();
320 update_openrave_data();
336 update_openrave_data();
348 "Moving along trajectory failed (ignoring): %s",
368 KatanaGotoOpenRaveThread::plan_target()
371 if (!update_motor_data()) {
372 _logger->
log_warn(
"KatanaGotoThread",
"Fetching current motor values failed");
379 encToRad(motor_encoders_, motor_angles_);
381 OR_manip_->set_angles_device(motor_angles_);
384 if (plannerparams_.compare(
"default") == 0) {
385 plannerparams_ = DEFAULT_PLANNERPARAMS;
387 if (is_target_object_) {
390 if (!_openrave->set_target_object(target_object_, OR_robot_)) {
391 _logger->
log_warn(
"KatanaGotoThread",
"Initiating goto failed, no IK solution found");
396 bool success =
false;
398 if (has_target_quaternion_) {
400 "Check IK(%f,%f,%f | %f,%f,%f,%f)",
408 success = OR_robot_->set_target_quat(x_, y_, z_, quat_w_, quat_x_, quat_y_, quat_z_);
409 }
else if (move_straight_) {
411 if (is_arm_extension_) {
412 success = OR_robot_->set_target_rel(x_, y_, z_,
true);
414 success = OR_robot_->set_target_straight(x_, y_, z_);
416 if (plannerparams_straight_.compare(
"default") == 0) {
417 plannerparams_straight_ = DEFAULT_PLANNERPARAMS_STRAIGHT;
420 float theta_error = 0.0f;
421 while (!success && (theta_error <= theta_error_)) {
423 "Check IK(%f,%f,%f | %f,%f,%f)",
428 theta_ + theta_error,
431 OR_robot_->set_target_euler(EULER_ZXZ, x_, y_, z_, phi_, theta_ + theta_error, psi_);
434 "Check IK(%f,%f,%f | %f,%f,%f)",
439 theta_ - theta_error,
442 OR_robot_->set_target_euler(EULER_ZXZ, x_, y_, z_, phi_, theta_ - theta_error, psi_);
448 }
catch (OpenRAVE::openrave_exception &e) {
453 _logger->
log_warn(
"KatanaGotoThread",
"Initiating goto failed, no IK solution found");
458 if (move_straight_) {
459 OR_robot_->set_target_plannerparams(plannerparams_straight_);
461 OR_robot_->set_target_plannerparams(plannerparams_);
466 float sampling = 0.04f;
467 _openrave->run_planner(OR_robot_, sampling);
480 KatanaGotoOpenRaveThread::update_openrave_data()
483 if (!update_motor_data()) {
484 _logger->
log_warn(
"KatanaGotoThread",
"Fetching current motor values failed");
491 encToRad(motor_encoders_, motor_angles_);
493 OR_manip_->set_angles_device(motor_angles_);
495 std::vector<OpenRAVE::dReal> angles;
496 OR_manip_->get_angles(angles);
499 OpenRAVE::EnvironmentMutex::scoped_lock lock(OR_robot_->get_robot_ptr()->GetEnv()->GetMutex());
500 OR_robot_->get_robot_ptr()->SetActiveDOFValues(angles);
508 KatanaGotoOpenRaveThread::update_motor_data()
510 short num_errors = 0;
519 if (++num_errors <= 10) {
520 _logger->
log_warn(
"KatanaGotoThread",
"Receiving motor data failed, retrying");
523 _logger->
log_warn(
"KatanaGotoThread",
"Receiving motor data failed too often, aborting");
544 if (++num_errors <= 10) {
546 "Receiving motor %s failed, retrying",
551 "Receiving motor %s failed, aborting",
570 KatanaGotoOpenRaveThread::move_katana()
575 std::vector<int> enc;
580 return (++it_ == target_traj_->end());
Katana motion thread base class.
fawkes::Logger * _logger
Logger.
fawkes::RefPtr< fawkes::KatanaController > _katana
Katana object for interaction with the arm.
bool _finished
Set to true when motion is finished, to false on reset.
unsigned int _error_code
Set to the desired error code on error.
Base class for exceptions in Fawkes.
virtual const char * what() const
Get primary string.
virtual bool joint_angles()=0
Check if controller provides joint angle values.
virtual void read_motor_data()=0
Read motor data of currently active joints from device into controller libray.
virtual void read_sensor_data()=0
Read all sensor data from device into controller libray.
virtual bool final()=0
Check if movement is final.
virtual void get_encoders(std::vector< int > &to, bool refresh=false)=0
Get encoder values of joints/motors.
virtual void stop()=0
Stop movement immediately.
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.
virtual void get_angles(std::vector< float > &to, bool refresh=false)=0
Get angle values of joints/motors.
static const uint32_t ERROR_NO_SOLUTION
ERROR_NO_SOLUTION constant.
static const uint32_t ERROR_COMMUNICATION
ERROR_COMMUNICATION constant.
static const uint32_t ERROR_NONE
ERROR_NONE constant.
static const uint32_t ERROR_MOTOR_CRASHED
ERROR_MOTOR_CRASHED constant.
static const uint32_t ERROR_CMD_START_FAILED
ERROR_CMD_START_FAILED constant.
static const uint32_t ERROR_UNSPECIFIC
ERROR_UNSPECIFIC constant.
At least one motor crashed.
virtual void log_debug(const char *component, const char *format,...)=0
Log debug message.
virtual void log_warn(const char *component, const char *format,...)=0
Log warning message.
Interface for a OpenRave connection creator.
Class containing information about all katana6M180 motors.
Class containing information about all neuronics-katana motors.
const char * name() const
Get name of thread.
virtual void once()
Execute an action exactly once.
virtual void init()
Initialize the thread.
virtual void finalize()
Finalize the thread.
A class for handling time.
Time & stamp_systime()
Set this time to the current system time.
Fawkes library namespace.
void encToRad(std::vector< int > &enc, std::vector< float > &rad)
Convert encoder vaulues of katana arm to radian angles.