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();
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");
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) {
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 ) {
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 ) {
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)) {
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);
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) {
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);
458 KatanaGotoOpenRaveThread::update_openrave_data()
461 if( !update_motor_data() ) {
462 _logger->
log_warn(
"KatanaGotoThread",
"Fetching current motor values failed");
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;
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");
520 if (++num_errors <= 10) {
543 KatanaGotoOpenRaveThread::move_katana()
548 std::vector<int> enc;
553 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 const char * what() const
Get primary string.
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.
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.
const char * name() const
Get name of thread.
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.
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.