diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1e6bb535f58970ecc144663df0615254c97f4455..b6a1c2f0cbccd873f24ea67b8a1864311fa1dd66 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -11,15 +11,17 @@ FIND_PACKAGE(comm REQUIRED)
FIND_PACKAGE(dynamixel REQUIRED)
+FIND_PACKAGE(trajectory REQUIRED)
+
# edit the following line to add the necessary include directories
-INCLUDE_DIRECTORIES(. ${iriutils_INCLUDE_DIR} ${comm_INCLUDE_DIR} ${dynamixel_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(. ${iriutils_INCLUDE_DIR} ${comm_INCLUDE_DIR} ${dynamixel_INCLUDE_DIR} ${trajectory_INCLUDE_DIR})
SET_SOURCE_FILES_PROPERTIES(${XSD_SOURCES} PROPERTIES GENERATED 1)
ADD_LIBRARY(dynamixel_motor_cont SHARED ${sources} ${XSD_SOURCES})
#edit the following line to add the necessary system libraries (if any)
-TARGET_LINK_LIBRARIES(dynamixel_motor_cont ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} ${XSD_LIBRARY})
+TARGET_LINK_LIBRARIES(dynamixel_motor_cont ${iriutils_LIBRARY} ${comm_LIBRARY} ${dynamixel_LIBRARY} ${XSD_LIBRARY} ${trajectory_LIBRARY})
ADD_DEPENDENCIES(dynamixel_motor_cont xsd_files_gen)
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index f612e3d22e871b11f81a1d3194a3d37591220429..9fd22e29473dc7ec1733a8cf7c4e9683922ea40f 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -31,12 +31,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id)
this->dyn_server=CDynamixelServer::instance();
this->clear();
this->mode=angle_ctrl;
- /* initialize motion sequence attributes */
- this->sequence_state=mtn_empty;
- this->sequence_current_step=-1;
- this->sequence_error_msg="Motion sequence ended successfully";
- this->init_events();
- this->init_threads();
}
}
@@ -55,10 +49,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
this->dyn_server=CDynamixelServer::instance();
this->clear();
this->mode=torque_ctrl;
- /* initialize motion sequence attributes */
- this->sequence_state=mtn_empty;
- this->sequence_current_step=-1;
- this->sequence_error_msg="Motion sequence ended successfully";
if(ids.size()==0)
{
/* handle exceptions */
@@ -69,8 +59,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
for(i=0;i<ids.size();i++)
this->init_motor(ids[i],version);
this->set_control_mode(angle_ctrl);
- this->init_events();
- this->init_threads();
}catch(CException &e){
/* handle exceptions */
this->clear();
@@ -104,8 +92,6 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu
for(i=0;i<ids.size();i++)
this->init_motor(ids[i],version);
this->set_control_mode(angle_ctrl);
- this->init_events();
- this->init_threads();
}catch(CException &e){
/* handle exceptions */
this->clear();
@@ -662,92 +648,6 @@ void CDynamixelMotorGroup::get_pid_control(unsigned int index,TDynamixel_pid &co
}
}
-void *CDynamixelMotorGroup::sequence_thread(void *param)
-{
- CDynamixelMotorGroup *mtn_seq=(CDynamixelMotorGroup *)param;
- std::list<std::string> events;
- int event_id=0,to=-1;
- bool end=false;
-
- // initialize the motion events
- events.push_back(mtn_seq->finish_thread_event_id);
- events.push_back(mtn_seq->start_sequence_event_id);
- events.push_back(mtn_seq->stop_sequence_event_id);
- events.push_back(mtn_seq->resume_sequence_event_id);
-
- while(!end)
- {
- try{
- event_id=mtn_seq->event_server->wait_first(events,to);
- mtn_seq->motion_access.enter();
- switch(event_id)
- {
- case -1: mtn_seq->sequence_current_step++;
- to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
- break;
- case 0: /* finish the thread */
- end=true;
- break;
- case 1: /* start sequence */
- to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
- break;
- case 2: /* stop sequence */
- mtn_seq->stop();
- mtn_seq->sequence_error_msg="Motion sequence stopped by user";
- mtn_seq->sequence_current_step=0;
- to=-1;
- if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id))
- mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id);
- break;
- case 3: /* resume sequence */
- mtn_seq->sequence_current_step++;
- to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
- break;
- }
- mtn_seq->motion_access.exit();
- }catch(CEventTimeoutException &e){
- if(!mtn_seq->event_server->event_is_set(mtn_seq->pause_sequence_event_id))
- {
- /* load and execute the next step */
- mtn_seq->motion_access.enter();
- mtn_seq->sequence_current_step++;
- try{
- to=mtn_seq->load_motion(mtn_seq->sequence_current_step);
- }catch(CException &e){
- to=-1;
- mtn_seq->stop();
- mtn_seq->sequence_state=mtn_loaded;
- mtn_seq->sequence_error_msg=e.what();
- // signal the user
- if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_error_event_id))
- mtn_seq->event_server->set_event(mtn_seq->sequence_error_event_id);
- if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id))
- mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id);
- }
- mtn_seq->motion_access.exit();
- }
- else
- {
- mtn_seq->event_server->reset_event(mtn_seq->pause_sequence_event_id);
- to=-1;
- }
- }catch(CException &e){
- to=-1;
- mtn_seq->motion_access.exit();
- mtn_seq->stop();
- mtn_seq->sequence_state=mtn_loaded;
- mtn_seq->sequence_error_msg=e.what();
- // signal the user
- if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_error_event_id))
- mtn_seq->event_server->set_event(mtn_seq->sequence_error_event_id);
- if(!mtn_seq->event_server->event_is_set(mtn_seq->sequence_complete_event_id))
- mtn_seq->event_server->set_event(mtn_seq->sequence_complete_event_id);
- }
- }
-
- pthread_exit(NULL);
-}
-
void CDynamixelMotorGroup::init_motor(unsigned int id,dyn_version_t version)
{
TDynamixel_info info;
@@ -777,35 +677,6 @@ void CDynamixelMotorGroup::init_motor(unsigned int id,dyn_version_t version)
this->servo_id.push_back(id);
}
-void CDynamixelMotorGroup::init_threads(void)
-{
- this->thread_server=CThreadServer::instance();
- this->sequence_thread_id=this->group_id + "sequence_thread";
- this->thread_server->create_thread(this->sequence_thread_id);
- this->thread_server->attach_thread(this->sequence_thread_id,this->sequence_thread,this);
- this->thread_server->start_thread(this->sequence_thread_id);
-}
-
-void CDynamixelMotorGroup::init_events(void)
-{
- this->event_server=CEventServer::instance();
- this->finish_thread_event_id=this->group_id + "_finish_thread_event";
- this->event_server->create_event(this->finish_thread_event_id);
- this->sequence_complete_event_id=this->group_id + "_sequence_complete_event";
- this->event_server->create_event(this->sequence_complete_event_id);
- this->event_server->set_event(this->sequence_complete_event_id);
- this->sequence_error_event_id=this->group_id + "_sequence_error_event";
- this->event_server->create_event(this->sequence_error_event_id);
- this->start_sequence_event_id=this->group_id + "_start_sequence_event";
- this->event_server->create_event(this->start_sequence_event_id);
- this->stop_sequence_event_id=this->group_id + "_stop_sequence_event";
- this->event_server->create_event(this->stop_sequence_event_id);
- this->pause_sequence_event_id=this->group_id + "_pause_sequence_event";
- this->event_server->create_event(this->pause_sequence_event_id);
- this->resume_sequence_event_id=this->group_id + "_resume_sequence_event";
- this->event_server->create_event(this->resume_sequence_event_id);
-}
-
void CDynamixelMotorGroup::clear(void)
{
unsigned int i=0;
@@ -827,11 +698,6 @@ void CDynamixelMotorGroup::clear(void)
this->info.clear();
this->alarms.clear();
this->servo_id.clear();
- /* clear any motion attribute */
- this->sequence_state=mtn_empty;
- this->sequence_current_step=-1;
- this->sequence_error_msg="Motion sequence ended successfully";
- this->seq.clear();
}
void CDynamixelMotorGroup::set_control_mode(control_mode mode)
@@ -872,51 +738,6 @@ void CDynamixelMotorGroup::set_control_mode(control_mode mode)
}
}
-int CDynamixelMotorGroup::load_motion(unsigned int step_id)
-{
- static std::vector<int> time(this->get_num_motors());
- std::vector<double> current_position;
- unsigned int i=0;
- double distance;
- int max=0;
-
- if(step_id<0)
- {
- /* handle errors */
- throw CDynamixelMotionSequenceException(_HERE_,"Invalid step index");
- }
- else if(step_id>=this->seq.size())
- {
- this->sequence_state=mtn_loaded;
- this->sequence_error_msg="Motion sequence completed successfully";
- /* signal the completion of the sequence */
- if(!this->event_server->event_is_set(this->sequence_complete_event_id))
- this->event_server->set_event(this->sequence_complete_event_id);
- return -1;
- }
- else
- {
- this->get_current_angle(current_position);
- for(i=0;i<this->get_num_motors();i++)
- {
- if(this->seq[step_id].absolute_motion)// absolute motion
- distance=fabs(current_position[i]-this->seq[step_id].position[i]);
- else// relative motion
- distance=fabs(this->seq[step_id].position[i]);
- time[i]=distance/this->seq[step_id].velocity[i]*1000;// time in ms
- }
- if(this->seq[step_id].absolute_motion)
- this->move_absolute_angle(this->seq[step_id].position,this->seq[step_id].velocity);
- else
- this->move_relative_angle(this->seq[step_id].position,this->seq[step_id].velocity);
- max=time[0];
- for(i=1;i<this->get_num_motors();i++)
- if(time[i]>max)
- max=time[i];
- return max+this->seq[step_id].delay;
- }
-}
-
void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
{
std::vector<TDynamixel_compliance> compliance;
@@ -1105,105 +926,6 @@ void CDynamixelMotorGroup::save_config(std::string &filename)
}
-void CDynamixelMotorGroup::load_sequence(std::string &filename)
-{
- dyn_sequence_t::step_iterator iterator;
- TDynamixelMotionStep step;
- struct stat buffer;
-
- if(stat(filename.c_str(),&buffer)==0)
- {
- // 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();
- 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(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());
- }
- }
- else
- CDynamixelMotionSequenceException(_HERE_,"The motion sequence file does not exist");
-}
-
-void CDynamixelMotorGroup::save_sequence(std::string &filename)
-{
- xml_schema::namespace_infomap map;
- unsigned int i=0,j=0;
- std::string mode;
-
- try{
- // save the base class parameters
- std::ofstream output_file(filename.c_str(),std::ios_base::out);
- dyn_sequence_t sequence_file(this->get_num_motors());
- dyn_sequence_t::step_sequence& motion_sequence(sequence_file.step());
-
- for(i=0;i<this->seq.size();i++)
- {
- if(this->seq[i].absolute_motion)
- mode="absolute";
- else
- mode="relative";
- step_t step(mode,this->seq[i].delay);
- step_t::position_sequence& position(step.position());
- step_t::velocity_sequence& velocity(step.velocity());
- for(j=0;j<this->get_num_motors();j++)
- {
- position.push_back(this->seq[i].position[j]);
- velocity.push_back(this->seq[i].velocity[j]);
- }
- motion_sequence.push_back(step);
- }
- map[""].name="";
- map[""].schema="motion_sequence.xsd";
- dyn_motion_seq(output_file,sequence_file,map);
- }catch (const xml_schema::exception& e){
- std::ostringstream os;
- os << e;
- /* handle exceptions */
- throw CDynamixelMotorException(_HERE_,os.str());
- }
-}
#endif
void CDynamixelMotorGroup::set_compliance_control(std::vector<TDynamixel_compliance> &config)
@@ -1687,296 +1409,15 @@ void CDynamixelMotorGroup::set_punch(std::vector<unsigned short int> &punches)
}
}
-TDynamixelMotionStates CDynamixelMotorGroup::get_sequence_current_state(void)
-{
- return this->sequence_state;
-}
-
-control_mode CDynamixelMotorGroup::get_control_mode(void)
-{
- return this->mode;
-}
-
-void CDynamixelMotorGroup::add_sequence_step(std::vector<double> &pos,std::vector<double> &vel, double delay, bool abs_motion)
-{
- TDynamixelMotionStep step;
-
- if(pos.size()!=this->get_num_motors())
- {
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"The position vector has a wrong length");
- }
- if(vel.size()!=this->get_num_motors())
- {
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"The velocity vector has a wrong length");
- }
- if(delay<0.0)
- {
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"Negative delay time");
- }
- this->motion_access.enter();
- step.position=pos;
- step.velocity=vel;
- step.delay=delay;
- step.absolute_motion=abs_motion;
- this->seq.push_back(step);
- if(this->sequence_state!=mtn_loaded)
- {
- this->sequence_state=mtn_loaded;
- this->sequence_current_step=0;
- }
- this->motion_access.exit();
-}
-
-void CDynamixelMotorGroup::insert_sequence_step(unsigned int index,std::vector<double> &pos,std::vector<double> &vel, double delay, bool abs_motion)
-{
- std::vector<TDynamixelMotionStep> sequence;
- TDynamixelMotionStep step;
- unsigned int i;
-
- if(index < 0 || index > this->seq.size())
- {
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"The index is out of range");
- }
- if(pos.size()!=this->get_num_motors())
- {
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"The position vector has a wrong length");
- }
- if(vel.size()!=this->get_num_motors())
- {
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"The velocity vector has a wrong length");
- }
- if(delay<0.0)
- {
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"Negative delay time");
- }
- this->motion_access.enter();
- step.position=pos;
- step.velocity=vel;
- step.delay=delay;
- step.absolute_motion=abs_motion;
- sequence=this->seq;
- this->seq.clear();
- for(i=0;i<sequence.size();i++)
- {
- if(i==index)
- this->seq.push_back(step);
- this->seq.push_back(sequence[i]);
- }
- if(this->sequence_state!=mtn_loaded)
- {
- this->sequence_state=mtn_loaded;
- this->sequence_current_step=0;
- }
- this->motion_access.exit();
-}
-
-void CDynamixelMotorGroup::delete_sequence_step(unsigned int index)
-{
- std::vector<TDynamixelMotionStep> sequence;
- unsigned int i;
-
- if(index < 0 || index > this->seq.size())
- {
- /* handle exceptions */
- throw CDynamixelMotionSequenceException(_HERE_,"The index is out of range");
- }
- this->motion_access.enter();
- sequence=this->seq;
- this->seq.clear();
- for(i=0;i<sequence.size();i++)
- {
- if(i!=index)
- this->seq.push_back(sequence[i]);
- }
- if(this->seq.size()==0)
- this->sequence_state=mtn_empty;
- this->motion_access.exit();
-}
-
-void CDynamixelMotorGroup::clear_sequence(void)
-{
- this->stop_sequence();
- this->motion_access.enter();
- this->seq.clear();
- this->sequence_state=mtn_empty;
- this->motion_access.exit();
-}
-
-void CDynamixelMotorGroup::start_sequence(void)
-{
- this->motion_access.enter();
- switch(this->sequence_state)
- {
- case mtn_empty: /* handle exceptions */
- //this->motion_access.exit();
- //throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
- break;
- case mtn_loaded: // enable the motors
- this->enable();
- this->sequence_current_step=0;
- if(this->event_server->event_is_set(this->sequence_complete_event_id))
- this->event_server->reset_event(this->sequence_complete_event_id);
- if(this->event_server->event_is_set(this->sequence_error_event_id))
- this->event_server->reset_event(this->sequence_error_event_id);
- if(!this->event_server->event_is_set(this->start_sequence_event_id))
- this->event_server->set_event(this->start_sequence_event_id);
- this->sequence_state=mtn_started;
- break;
- case mtn_started: /* do nothing */
- break;
- case mtn_paused: /* handle exceptions */
- //this->motion_access.exit();
- //throw CMotionSequenceException(_HERE_,"A sequence is currently pause, stop it before starting a new one.");
- break;
- }
- this->motion_access.exit();
-}
-
-void CDynamixelMotorGroup::pause_sequence(void)
-{
- this->motion_access.enter();
- switch(this->sequence_state)
- {
- case mtn_empty: /* handle exceptions */
- //this->motion_access.exit();
- //throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
- break;
- case mtn_loaded: /* handle exceptions */
- //this->motion_access.exit();
- //throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
- break;
- case mtn_started: if(!this->event_server->event_is_set(this->pause_sequence_event_id))
- this->event_server->set_event(this->pause_sequence_event_id);
- this->sequence_state=mtn_paused;
- break;
- case mtn_paused: /* do nothing */
- break;
- }
- this->motion_access.exit();
-}
-
-void CDynamixelMotorGroup::resume_sequence(void)
-{
- this->motion_access.enter();
- switch(this->sequence_state)
- {
- case mtn_empty: /* handle exceptions */
- //this->motion_access.exit();
- //throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
- break;
- case mtn_loaded: /* handle exceptions */
- //this->motion_access.exit();
- //throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
- break;
- case mtn_started: /* do nothing */
- break;
- case mtn_paused: if(!this->event_server->event_is_set(this->resume_sequence_event_id))
- this->event_server->set_event(this->resume_sequence_event_id);
- this->sequence_state=mtn_started;
- break;
- }
- this->motion_access.exit();
-}
-
-void CDynamixelMotorGroup::stop_sequence(void)
-{
- this->motion_access.enter();
- switch(this->sequence_state)
- {
- case mtn_empty: /* handle exceptions */
- //this->motion_access.exit();
- //throw CMotionSequenceException(_HERE_,"There is no motion sequence loaded");
- break;
- case mtn_loaded: /* handle exceptions */
- //this->motion_access.exit();
- //throw CMotionSequenceException(_HERE_,"There is no motion sequence currently being executed");
- break;
- case mtn_started: if(!this->event_server->event_is_set(this->stop_sequence_event_id))
- this->event_server->set_event(this->stop_sequence_event_id);
- this->sequence_state=mtn_loaded;
- break;
- case mtn_paused: if(!this->event_server->event_is_set(this->stop_sequence_event_id))
- this->event_server->set_event(this->stop_sequence_event_id);
- this->sequence_state=mtn_loaded;
- break;
- }
- this->motion_access.exit();
-}
-
-std::string CDynamixelMotorGroup::get_sequence_complete_event_id(void)
-{
- return this->sequence_complete_event_id;
-}
-
-std::string CDynamixelMotorGroup::get_sequence_error_event_id(void)
-{
- return this->sequence_error_event_id;
-}
-
-std::string CDynamixelMotorGroup::get_sequence_error_message(void)
-{
- return this->sequence_error_msg;
-}
-
-int CDynamixelMotorGroup::get_sequence_current_step(void)
-{
- return this->sequence_current_step;
-}
-
-double CDynamixelMotorGroup::get_sequence_completed_percentage(void)
-{
- double ratio;
-
- if(this->sequence_state!=mtn_empty)
- ratio=((double)this->sequence_current_step*100.0)/(this->seq.size());
- else
- {
- /* handle errors */
- throw CDynamixelMotionSequenceException(_HERE_,"There is no motion sequence loaded");
- }
-
- return ratio;
-}
-
CDynamixelMotorGroup::~CDynamixelMotorGroup()
{
/* stop the motor */
try{
this->stop();
/* stop any active sequence */
- if(!this->event_server->event_is_set(this->sequence_complete_event_id))
- this->stop_sequence();
this->disable();
}catch(CException &e){
/* do nothing */
}
- /* stop the internal trhead */
- this->event_server->set_event(this->finish_thread_event_id);
- this->thread_server->end_thread(this->sequence_thread_id);
- this->thread_server->detach_thread(this->sequence_thread_id);
- this->thread_server->delete_thread(this->sequence_thread_id);
- this->sequence_thread_id="";
- this->event_server->set_event(this->finish_thread_event_id);
- this->event_server->delete_event(this->finish_thread_event_id);
- this->finish_thread_event_id="";
- this->event_server->delete_event(this->start_sequence_event_id);
- this->start_sequence_event_id="";
- this->event_server->delete_event(this->stop_sequence_event_id);
- this->stop_sequence_event_id="";
- this->event_server->delete_event(this->pause_sequence_event_id);
- this->pause_sequence_event_id="";
- this->event_server->delete_event(this->resume_sequence_event_id);
- this->resume_sequence_event_id="";
- this->event_server->delete_event(this->sequence_complete_event_id);
- this->sequence_complete_event_id="";
- this->event_server->delete_event(this->sequence_error_event_id);
- this->sequence_error_event_id="";
this->clear();
}
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index 7c127013e80f5a6f5357d0aac40830a0ba162d97..d7e35b3de09bd3959ade7f1080c2af2550c860eb 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -5,8 +5,6 @@
#include "mutex.h"
#include <vector>
-typedef enum {mtn_empty,mtn_loaded,mtn_started,mtn_paused} TDynamixelMotionStates;
-
typedef struct
{
std::string bus_id;
@@ -18,14 +16,6 @@ typedef struct
std::vector<TDynamixel_compliance> compliance_control;
}TDynamixelGroup_config;
-typedef struct
-{
- std::vector<double> position;
- std::vector<double> velocity;
- bool absolute_motion;
- double delay;
-}TDynamixelMotionStep;
-
class CDynamixelMotorGroup
{
private:
@@ -69,58 +59,6 @@ class CDynamixelMotorGroup
*
*/
control_mode mode;
- /* thread attributes */
- /**
- * \brief
- *
- */
- CThreadServer *thread_server;
- /**
- * \brief
- *
- */
- std::string sequence_thread_id;
- /* event attributes */
- /**
- * \brief
- *
- */
- CEventServer *event_server;
- /**
- * \brief
- *
- */
- std::string finish_thread_event_id;
- /**
- * \brief
- *
- */
- std::string sequence_complete_event_id;
- /**
- * \brief
- *
- */
- std::string sequence_error_event_id;
- /**
- * \brief
- *
- */
- std::string start_sequence_event_id;
- /**
- * \brief
- *
- */
- std::string stop_sequence_event_id;
- /**
- * \brief
- *
- */
- std::string pause_sequence_event_id;
- /**
- * \brief
- *
- */
- std::string resume_sequence_event_id;
/**
* \brief
*
@@ -137,32 +75,6 @@ class CDynamixelMotorGroup
*
*/
CDynamixelServer *dyn_server;
- /* Motion sequence attributes */
- /**
- * \brief
- *
- */
- TDynamixelMotionStates sequence_state;
- /**
- * \brief
- *
- */
- std::vector<TDynamixelMotionStep> seq;
- /**
- * \brief
- *
- */
- int sequence_current_step;
- /**
- * \brief
- *
- */
- std::string sequence_error_msg;
- /**
- * \brief
- *
- */
- CMutex motion_access;
/**
* \brief
*
@@ -258,16 +170,6 @@ class CDynamixelMotorGroup
*
*/
void init_motor(unsigned int id,dyn_version_t version=dyn_version1);
- /**
- * \brief
- *
- */
- void init_threads(void);
- /**
- * \brief
- *
- */
- void init_events(void);
/**
* \brief
*
@@ -278,17 +180,6 @@ class CDynamixelMotorGroup
*
*/
void set_control_mode(control_mode mode);
- /* motion sequence protected functions */
- /**
- * \brief
- *
- */
- static void *sequence_thread(void *param);
- /**
- * \brief
- *
- */
- int load_motion(unsigned int step_id);
public:
/**
* \brief
@@ -413,89 +304,6 @@ class CDynamixelMotorGroup
*
*/
control_mode get_control_mode(void);
- /* motion sequence public functions */
- /**
- * \brief
- *
- */
- TDynamixelMotionStates get_sequence_current_state(void);
-#ifdef _HAVE_XSD
- /**
- * \brief
- *
- */
- void load_sequence(std::string &filename);
- /**
- * \brief
- *
- */
- void save_sequence(std::string &filename);
-#endif
- /**
- * \brief
- *
- */
- void add_sequence_step(std::vector<double> &pos,std::vector<double> &vel,double delay=0.0,bool abs_motion=true);
- /**
- * \brief
- *
- */
- void insert_sequence_step(unsigned int index,std::vector<double> &pos,std::vector<double> &vel,double delay=0.0,bool abs_motion=true);
- /**
- * \brief
- *
- */
- void delete_sequence_step(unsigned int index);
- /**
- * \brief
- *
- */
- void clear_sequence(void);
- /**
- * \brief
- *
- */
- void start_sequence(void);
- /**
- * \brief
- *
- */
- void pause_sequence(void);
- /**
- * \brief
- *
- */
- void resume_sequence(void);
- /**
- * \brief
- *
- */
- void stop_sequence(void);
- /**
- * \brief
- *
- */
- std::string get_sequence_complete_event_id(void);
- /**
- * \brief
- *
- */
- std::string get_sequence_error_event_id(void);
- /**
- * \brief
- *
- */
- std::string get_sequence_error_message(void);
- /**
- * \brief
- *
- */
- int get_sequence_current_step(void);
- /**
- * \brief
- *
- */
- double get_sequence_completed_percentage(void);
/**
* \brief
*
diff --git a/src/examples/test_dynamixel_motion_seq.cpp b/src/examples/test_dynamixel_motion_seq.cpp
index 08154aa0cc0619cc7be58ea22f8e313e31781836..1a0d4d48efe9b2c0a1a46181ccc41b818a518ded 100755
--- a/src/examples/test_dynamixel_motion_seq.cpp
+++ b/src/examples/test_dynamixel_motion_seq.cpp
@@ -4,6 +4,7 @@
#include "exceptions.h"
#include "dynamixel_motor_group.h"
#include "dynamixel_motor.h"
+#include "trajectory.h"
#include <iostream>
#include <math.h>
@@ -19,17 +20,20 @@ int main(int argc, char *argv[])
CEventServer *event_server=CEventServer::instance();
std::vector<double> angles,speeds;
CDynamixelMotorGroup group(group_name);
+ CTrajectory<CDynamixelMotorGroup> dyn_traj("dyn_traj",&group,&CDynamixelMotorGroup::stop,&CDynamixelMotorGroup::get_current_angle,
+ &CDynamixelMotorGroup::move_absolute_angle,&CDynamixelMotorGroup::move_relative_angle,
+ &CDynamixelMotorGroup::get_num_motors);
try{
if(dyn_server->get_num_buses()>0)
{
#ifdef _HAVE_XSD
group.load_config(config_file);
- group.load_sequence(sequence_filename);
- group.start_sequence();
- while(!event_server->event_is_set(group.get_sequence_complete_event_id()))
+ dyn_traj.load_trajectory(sequence_filename);
+ dyn_traj.start_trajectory();
+ while(!event_server->event_is_set(dyn_traj.get_trajectory_complete_event_id()))
{
- std::cout << group.get_sequence_completed_percentage() << "% of sequence completed" << std::endl;
+ std::cout << dyn_traj.get_trajectory_completed_percentage() << "% of trajectory completed" << std::endl;
usleep(200000);
}
#endif
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index 4396ee5b92805c536f991808171dc7f5747c35fc..dd79d3dc9d183bff51bb236fe2696ff92c2e3da2 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -70,21 +70,21 @@ int main(int argc, char *argv[])
sleep(1);
cont->turn_led_off();
return 1;
- // cont->move_absolute_angle(-90,300);
+ // cont->move_absolute_angle(-90,300,0);
// sleep(2);
- // cont->move_absolute_angle(90,100);
+ // cont->move_absolute_angle(90,100,0);
// sleep(2);
- // cont->move_relative_angle(-5,100);
+ // cont->move_relative_angle(-5,100,0);
// sleep(1);
- // cont->move_relative_angle(-5,100);
+ // cont->move_relative_angle(-5,100,0);
// sleep(1);
- // cont->move_relative_angle(-5,100);
+ // cont->move_relative_angle(-5,100,0);
// sleep(1);
- // cont->move_relative_angle(-5,100);
+ // cont->move_relative_angle(-5,100,0);
// sleep(1);
- // cont->move_relative_angle(-5,100);
+ // cont->move_relative_angle(-5,100,0);
// sleep(1);
- // cont->move_relative_angle(-5,100);
+ // cont->move_relative_angle(-5,100,0);
// sleep(1);
// cont->move_torque(50);
// sleep(5);
diff --git a/src/xml/CMakeLists.txt b/src/xml/CMakeLists.txt
index ebe9cfcd7a4d72ce059df78fdc291d2d4d133271..b8a8cebeb1d7c7fa05bbff0f56c65d6c8d4c08ef 100755
--- a/src/xml/CMakeLists.txt
+++ b/src/xml/CMakeLists.txt
@@ -13,7 +13,7 @@ ENDIF(EXISTS "/usr/include/xsd/cxx")
IF(XSD_FOUND)
SET(XSD_LIBRARY ${XSD_LIBRARY} PARENT_SCOPE)
- SET(XSD_FILES dynamixel_motor_cfg_file.xsd dynamixel_motor_group_cfg_file.xsd dyn_motion_seq_file.xsd)
+ SET(XSD_FILES dynamixel_motor_cfg_file.xsd dynamixel_motor_group_cfg_file.xsd)
IF(XSD_FILES)
SET(XSD_PATH ${CMAKE_CURRENT_SOURCE_DIR})
diff --git a/src/xml/dyn_group_config.xml b/src/xml/dyn_group_config.xml
index ac4a86718957c649812f87a7469e4793cf90e9d7..85c2803ab3d42e3bef563688018004e02b245066 100755
--- a/src/xml/dyn_group_config.xml
+++ b/src/xml/dyn_group_config.xml
@@ -3,14 +3,14 @@
<dyn_motor_group_config xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
xsi:noNamespaceSchemaLocation="dyn_motor_group_cfg_file.xsd">
- <bus_id>A400gaIt</bus_id>
+ <bus_id>A400gavq</bus_id>
<baudrate>1000000</baudrate>
<dyn_motor_config>
- <id>1</id>
+ <id>2</id>
<config_file>dyn_config.xml</config_file>
</dyn_motor_config>
<dyn_motor_config>
- <id>2</id>
+ <id>3</id>
<config_file>dyn_config.xml</config_file>
</dyn_motor_config>
</dyn_motor_group_config>
diff --git a/src/xml/sequence1.xml b/src/xml/sequence1.xml
index 01f634145cd92e8e987c2aed40c1bb5b62986898..9afa1d48517e0879fe931768c8c2a10bb11999b9 100755
--- a/src/xml/sequence1.xml
+++ b/src/xml/sequence1.xml
@@ -1,63 +1,79 @@
<?xml version="1.0"?>
-<dyn_motion_seq xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
- xsi:noNamespaceSchemaLocation="dyn_motion_seq_file.xsd">
+<trajectory xmlns:xsi="http://www.w3.org/10.01/XMLSchema-instance"
+ xsi:noNamespaceSchemaLocation="motion_traj_file.xsd">
<num_motors>2</num_motors>
<step>
- <position>0.0</position>
- <position>0.0</position>
- <velocity>100.0</velocity>
- <velocity>100.0</velocity>
+ <position>10.0</position>
+ <position>30.0</position>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
+ <position>-10.0</position>
+ <position>-30.0</position>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
+ <mode>absolute</mode>
+ <delay>0</delay>
+ </step>
+ <step>
+ <position>10.0</position>
<position>30.0</position>
- <position>30.0</position>
- <velocity>100.0</velocity>
- <velocity>100.0</velocity>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
+ <position>-10.0</position>
<position>-30.0</position>
- <position>-30.0</position>
- <velocity>100.0</velocity>
- <velocity>100.0</velocity>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
<step>
<position>20.0</position>
- <position>20.0</position>
- <velocity>20.0</velocity>
- <velocity>20.0</velocity>
+ <position>60.0</position>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
<mode>relative</mode>
<delay>0</delay>
</step>
<step>
- <position>20.0</position>
- <position>20.0</position>
- <velocity>20.0</velocity>
- <velocity>20.0</velocity>
+ <position>-20.0</position>
+ <position>-60.0</position>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
<mode>relative</mode>
<delay>0</delay>
</step>
<step>
- <position>20.0</position>
- <position>20.0</position>
- <velocity>20.0</velocity>
- <velocity>20.0</velocity>
- <mode>relative</mode>
+ <position>10.0</position>
+ <position>30.0</position>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
+ <mode>absolute</mode>
<delay>0</delay>
</step>
<step>
+ <position>-10.0</position>
<position>-30.0</position>
- <position>-30.0</position>
- <velocity>100.0</velocity>
- <velocity>100.0</velocity>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
+ <mode>absolute</mode>
+ <delay>0</delay>
+ </step>
+ <step>
+ <position>0.0</position>
+ <position>0.0</position>
+ <velocity>10.0</velocity>
+ <velocity>10.0</velocity>
<mode>absolute</mode>
<delay>0</delay>
</step>
-</dyn_motion_seq>
+</trajectory>