diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 3648b1d35f56624bda3711511d69579707574b62..591e7ecdc158005012d1f74b5d5f388180f4ee21 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -8,6 +8,9 @@
#include <iostream>
#include <sstream>
#include <fstream>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
#ifdef _HAVE_XSD
#include "xml/dynamixel_motor_cfg_file.hxx"
#endif
@@ -314,39 +317,45 @@ void CDynamixelMotor::load_config(std::string &filename)
{
TDynamixel_compliance compliance;
TDynamixel_pid pid;
+ struct stat buffer;
- // try to open the specified file
- try{
- std::auto_ptr<dynamixel_motor_config_t> cfg(dynamixel_motor_config(filename.c_str(), xml_schema::flags::dont_validate));
- // configure the parameters of the controller
- this->set_turn_off_alarms(cfg->alarm_shtdwn());
- this->set_position_range(cfg->min_angle(), cfg->max_angle());
- this->set_temperature_limit(cfg->temp_limit());
- this->set_voltage_limits(cfg->min_voltage(),cfg->max_voltage());
- if(this->info.pid_control)
- {
- pid.p=cfg->kp();
- pid.i=cfg->ki();
- pid.d=cfg->kd();
- this->set_pid_control(pid);
- }
- else
- {
- compliance.cw_compliance_margin=cfg->cw_comp_margin();
- compliance.ccw_compliance_margin=cfg->ccw_comp_margin();
- compliance.cw_compliance_slope=cfg->cw_comp_slope();
- compliance.ccw_compliance_slope=cfg->ccw_comp_slope();
- compliance.punch=cfg->punch();
- this->set_compliance_control(compliance);
- }
- this->set_max_torque(cfg->max_torque());
- this->set_limit_torque(cfg->max_torque());
- }catch (const xml_schema::exception& e){
- std::ostringstream os;
- os << e;
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,os.str());
+ if(stat(filename.c_str(),&buffer)==0)
+ {
+ // try to open the specified file
+ try{
+ std::auto_ptr<dynamixel_motor_config_t> cfg(dynamixel_motor_config(filename.c_str(), xml_schema::flags::dont_validate));
+ // configure the parameters of the controller
+ this->set_turn_off_alarms(cfg->alarm_shtdwn());
+ this->set_position_range(cfg->min_angle(), cfg->max_angle());
+ this->set_temperature_limit(cfg->temp_limit());
+ this->set_voltage_limits(cfg->min_voltage(),cfg->max_voltage());
+ if(this->info.pid_control)
+ {
+ pid.p=cfg->kp();
+ pid.i=cfg->ki();
+ pid.d=cfg->kd();
+ this->set_pid_control(pid);
+ }
+ else
+ {
+ compliance.cw_compliance_margin=cfg->cw_comp_margin();
+ compliance.ccw_compliance_margin=cfg->ccw_comp_margin();
+ compliance.cw_compliance_slope=cfg->cw_comp_slope();
+ compliance.ccw_compliance_slope=cfg->ccw_comp_slope();
+ compliance.punch=cfg->punch();
+ this->set_compliance_control(compliance);
+ }
+ this->set_max_torque(cfg->max_torque());
+ this->set_limit_torque(cfg->max_torque());
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,os.str());
+ }
}
+ else
+ throw CDynamixelMotorException(_HERE_,"The configuration file does not exist");
}
void CDynamixelMotor::save_config(std::string &filename)
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index d383bd2cd826ca2679476bf1ae019feb442d36b8..7f27a17d78ac56f0736d88a17fe02c50d1af195d 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -8,6 +8,9 @@
#include <math.h>
#include <iostream>
#include <fstream>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <unistd.h>
#ifdef _HAVE_XSD
#include "xml/dynamixel_motor_cfg_file.hxx"
#include "xml/dynamixel_motor_group_cfg_file.hxx"
@@ -924,65 +927,71 @@ void CDynamixelMotorGroup::load_config(std::string &filename)
dyn_motor_group_config_t::dyn_motor_config_iterator iterator;
std::vector<TDynamixel_compliance> compliance;
std::vector<TDynamixel_pid> pid;
+ struct stat buffer;
unsigned int i=0;
this->clear();
- // try to open the specified file
- try{
- std::auto_ptr<dyn_motor_group_config_t> cfg(dyn_motor_group_config(filename.c_str(), xml_schema::flags::dont_validate));
- this->dyn_server->config_bus(cfg->bus_id(),cfg->baudrate());
- pid.resize(cfg->dyn_motor_config().size());
- compliance.resize(cfg->dyn_motor_config().size());
- for(iterator=cfg->dyn_motor_config().begin(),i=0;iterator!=cfg->dyn_motor_config().end();iterator++,i++)
- {
- this->init_motor(iterator->id());
- // load the configuration file
- std::auto_ptr<dynamixel_motor_config_t> motor(dynamixel_motor_config(iterator->config_file().c_str(),xml_schema::flags::dont_validate));
- this->set_position_range(i,motor->min_angle(),motor->max_angle());
- this->set_temperature_limit(i,motor->temp_limit());
- this->set_voltage_limits(i,motor->min_voltage(),motor->max_voltage());
- if(this->info[i].pid_control)
+ if(stat(filename.c_str(),&buffer)==0)
+ {
+ // try to open the specified file
+ try{
+ std::auto_ptr<dyn_motor_group_config_t> cfg(dyn_motor_group_config(filename.c_str(), xml_schema::flags::dont_validate));
+ this->dyn_server->config_bus(cfg->bus_id(),cfg->baudrate());
+ pid.resize(cfg->dyn_motor_config().size());
+ compliance.resize(cfg->dyn_motor_config().size());
+ for(iterator=cfg->dyn_motor_config().begin(),i=0;iterator!=cfg->dyn_motor_config().end();iterator++,i++)
{
- pid[i].p=motor->kp();
- pid[i].i=motor->ki();
- pid[i].d=motor->kd();
+ this->init_motor(iterator->id());
+ // load the configuration file
+ std::auto_ptr<dynamixel_motor_config_t> motor(dynamixel_motor_config(iterator->config_file().c_str(),xml_schema::flags::dont_validate));
+ this->set_position_range(i,motor->min_angle(),motor->max_angle());
+ this->set_temperature_limit(i,motor->temp_limit());
+ this->set_voltage_limits(i,motor->min_voltage(),motor->max_voltage());
+ if(this->info[i].pid_control)
+ {
+ pid[i].p=motor->kp();
+ pid[i].i=motor->ki();
+ pid[i].d=motor->kd();
+ }
+ else
+ {
+ compliance[i].cw_compliance_margin=motor->cw_comp_margin();
+ compliance[i].ccw_compliance_margin=motor->ccw_comp_margin();
+ compliance[i].cw_compliance_slope=motor->cw_comp_slope();
+ compliance[i].ccw_compliance_slope=motor->ccw_comp_slope();
+ compliance[i].punch=motor->punch();
+ }
+ this->set_max_torque(i,motor->max_torque());
+ this->set_limit_torque(i,motor->max_torque());
+ }
+ if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
+ {
+ this->mode=torque_ctrl;
+ for(i=1;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
}
else
{
- compliance[i].cw_compliance_margin=motor->cw_comp_margin();
- compliance[i].ccw_compliance_margin=motor->ccw_comp_margin();
- compliance[i].cw_compliance_slope=motor->cw_comp_slope();
- compliance[i].ccw_compliance_slope=motor->ccw_comp_slope();
- compliance[i].punch=motor->punch();
+ this->mode=angle_ctrl;
+ for(i=1;i<this->servo_id.size();i++)
+ this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
}
- this->set_max_torque(i,motor->max_torque());
- this->set_limit_torque(i,motor->max_torque());
- }
- if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
- {
- this->mode=torque_ctrl;
- for(i=1;i<this->servo_id.size();i++)
- this->set_position_range(i,-this->info[i].center_angle,-this->info[i].center_angle);
- }
- else
- {
- this->mode=angle_ctrl;
- for(i=1;i<this->servo_id.size();i++)
- this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
+ this->set_pid_control(pid);
+ this->set_compliance_control(compliance);
+ }catch (const xml_schema::exception& e){
+ std::ostringstream os;
+ os << e;
+ this->clear();
+ /* handle exceptions */
+ throw CDynamixelMotorException(_HERE_,os.str());
+ }catch(CException &e){
+ this->clear();
+ throw e;
}
- this->set_pid_control(pid);
- this->set_compliance_control(compliance);
- }catch (const xml_schema::exception& e){
- std::ostringstream os;
- os << e;
- this->clear();
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,os.str());
- }catch(CException &e){
- this->clear();
- throw e;
}
+ else
+ throw CDynamixelMotorException(_HERE_,"The configuration file does not exist");
}
void CDynamixelMotorGroup::save_config(std::string &filename)
@@ -994,55 +1003,61 @@ void CDynamixelMotorGroup::load_sequence(std::string &filename)
{
dyn_sequence_t::step_iterator iterator;
TDynamixelMotionStep step;
+ struct stat buffer;
- // try to open the specified file
- try{
- // stop any current motion
- this->motion_access.enter();
- if(this->sequence_state==mtn_started || this->sequence_state==mtn_paused)
- {
- this->motion_access.exit();
- this->stop_sequence();
+ if(stat(filename.c_str(),&buffer)==0)
+ {
+ // try to open the specified file
+ try{
+ // stop any current motion
this->motion_access.enter();
- this->sequence_state=mtn_loaded;
- }
- // load the new sequence
- std::auto_ptr<dyn_sequence_t> seq(dyn_motion_seq(filename.c_str(),xml_schema::flags::dont_validate));
- if(seq->num_motors()!=this->get_num_motors())
- {
- this->motion_access.exit();
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"The number of motors in the loaded sequence does not coincide with the number of motors in the associated motor group");
- }
- else
- {
- // erase the previous sequence
- this->seq.clear();
- for(iterator=seq->step().begin();iterator!=seq->step().end();iterator++)
+ if(this->sequence_state==mtn_started || this->sequence_state==mtn_paused)
{
- if(iterator->mode()=="absolute")
- step.absolute_motion=true;
- else
- step.absolute_motion=false;
- step.delay=iterator->delay();
- step.position.clear();
- step.position=iterator->position();
- step.velocity.clear();
- step.velocity=iterator->velocity();
- this->seq.push_back(step);
+ this->motion_access.exit();
+ this->stop_sequence();
+ this->motion_access.enter();
+ this->sequence_state=mtn_loaded;
}
- this->sequence_state=mtn_loaded;
- this->sequence_current_step=0;
+ // load the new sequence
+ std::auto_ptr<dyn_sequence_t> seq(dyn_motion_seq(filename.c_str(),xml_schema::flags::dont_validate));
+ if(seq->num_motors()!=this->get_num_motors())
+ {
+ this->motion_access.exit();
+ /* handle exceptions */
+ throw CDynamixelMotionSequenceException(_HERE_,"The number of motors in the loaded sequence does not coincide with the number of motors in the associated motor group");
+ }
+ else
+ {
+ // erase the previous sequence
+ this->seq.clear();
+ for(iterator=seq->step().begin();iterator!=seq->step().end();iterator++)
+ {
+ if(iterator->mode()=="absolute")
+ step.absolute_motion=true;
+ else
+ step.absolute_motion=false;
+ step.delay=iterator->delay();
+ step.position.clear();
+ step.position=iterator->position();
+ step.velocity.clear();
+ step.velocity=iterator->velocity();
+ this->seq.push_back(step);
+ }
+ this->sequence_state=mtn_loaded;
+ this->sequence_current_step=0;
+ }
+ this->motion_access.exit();
+ }catch (const xml_schema::exception& e){
+ this->sequence_state=mtn_empty;
+ this->motion_access.exit();
+ /* handle exceptions */
+ std::ostringstream os;
+ os << e;
+ throw CDynamixelMotionSequenceException(_HERE_,os.str());
}
- this->motion_access.exit();
- }catch (const xml_schema::exception& e){
- this->sequence_state=mtn_empty;
- this->motion_access.exit();
- /* handle exceptions */
- std::ostringstream os;
- os << e;
- throw CDynamixelMotionSequenceException(_HERE_,os.str());
}
+ else
+ CDynamixelMotionSequenceException(_HERE_,"The motion sequence file does not exist");
}
void CDynamixelMotorGroup::save_sequence(std::string &filename)