diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 969571b2a1ea8a07471304992d780861da89a94c..1c99f00d7dcc5423fb969bdb96da0d03db38be9e 100755
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,20 +1,7 @@
-# locate the necessary dependencies
-FIND_PACKAGE(iriutils REQUIRED)
-FIND_PACKAGE(comm REQUIRED)
+ADD_SUBDIRECTORY(fw_downloader)
-# add the necessary include directories
-INCLUDE_DIRECTORIES(.)
-INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
-INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
+ADD_SUBDIRECTORY(robotis_mtn)
-# application source files
-SET(sources fw_downloader.cpp hex2bin.cpp)
-# application header files
-SET(headers fw_downloader.h hex2bin.h)
+ADD_SUBDIRECTORY(robotis_mtn_file)
-# create the executable file
-ADD_EXECUTABLE(fw_downloader ${sources})
-
-# link necessary libraries
-TARGET_LINK_LIBRARIES(fw_downloader ${iriutils_LIBRARY})
-TARGET_LINK_LIBRARIES(fw_downloader ${comm_LIBRARY})
+ADD_SUBDIRECTORY(robotis_bin_file)
diff --git a/src/fw_downloader/CMakeLists.txt b/src/fw_downloader/CMakeLists.txt
new file mode 100755
index 0000000000000000000000000000000000000000..969571b2a1ea8a07471304992d780861da89a94c
--- /dev/null
+++ b/src/fw_downloader/CMakeLists.txt
@@ -0,0 +1,20 @@
+# locate the necessary dependencies
+FIND_PACKAGE(iriutils REQUIRED)
+FIND_PACKAGE(comm REQUIRED)
+
+# add the necessary include directories
+INCLUDE_DIRECTORIES(.)
+INCLUDE_DIRECTORIES(${iriutils_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${comm_INCLUDE_DIR})
+
+# application source files
+SET(sources fw_downloader.cpp hex2bin.cpp)
+# application header files
+SET(headers fw_downloader.h hex2bin.h)
+
+# create the executable file
+ADD_EXECUTABLE(fw_downloader ${sources})
+
+# link necessary libraries
+TARGET_LINK_LIBRARIES(fw_downloader ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(fw_downloader ${comm_LIBRARY})
diff --git a/src/fw_downloader.cpp b/src/fw_downloader/fw_downloader.cpp
similarity index 100%
rename from src/fw_downloader.cpp
rename to src/fw_downloader/fw_downloader.cpp
diff --git a/src/hex2bin.cpp b/src/fw_downloader/hex2bin.cpp
similarity index 100%
rename from src/hex2bin.cpp
rename to src/fw_downloader/hex2bin.cpp
diff --git a/src/hex2bin.h b/src/fw_downloader/hex2bin.h
similarity index 100%
rename from src/hex2bin.h
rename to src/fw_downloader/hex2bin.h
diff --git a/src/robotis_bin_file/CMakeLists.txt b/src/robotis_bin_file/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..772e4c9d97c4a6b1480859d30ed7ec768b329789
--- /dev/null
+++ b/src/robotis_bin_file/CMakeLists.txt
@@ -0,0 +1,21 @@
+# driver source files
+SET(sources robotis_bin_parser.cpp)
+# application header files
+SET(headers robotis_bin_parser.h)
+# locate the necessary dependencies
+FIND_PACKAGE(robotis_mtn REQUIRED)
+
+# add the necessary include directories
+INCLUDE_DIRECTORIES(.)
+INCLUDE_DIRECTORIES(${robotis_mtn_INCLUDE_DIR})
+# create the shared library
+ADD_LIBRARY(robotis_bin_parser SHARED ${sources})
+
+TARGET_LINK_LIBRARIES(robotis_bin_parser ${robotis_mtn_LIBRARY})
+# link necessary libraries
+INSTALL(TARGETS robotis_bin_parser
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib)
+INSTALL(FILES ${headers} DESTINATION include)
+ADD_SUBDIRECTORY(examples)
diff --git a/src/robotis_bin_file/examples/CMakeLists.txt b/src/robotis_bin_file/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..592f2554e4d9e281199bdcadff81b733ae017e48
--- /dev/null
+++ b/src/robotis_bin_file/examples/CMakeLists.txt
@@ -0,0 +1,4 @@
+# create an example application
+ADD_EXECUTABLE(robotis_bin_file_test robotis_bin_file_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(robotis_bin_file_test robotis_bin_parser)
diff --git a/src/robotis_bin_file/examples/robotis_bin_file_test.cpp b/src/robotis_bin_file/examples/robotis_bin_file_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e4e2eeab37d04bf7e7572e63698f07d8f670be6e
--- /dev/null
+++ b/src/robotis_bin_file/examples/robotis_bin_file_test.cpp
@@ -0,0 +1,63 @@
+#include <iostream>
+#include <cstdlib>
+#include <string.h>
+
+#include "robotis_bin_parser.h"
+#include "mtn_exceptions.h"
+
+int main(int argc, char *argv[])
+{
+ std::vector<unsigned short int> angles;
+ CBinFileParser mtn_parser;
+ unsigned int i=0,j=0,k=0;
+ CRobotisMtn motions;
+ CRobotisPage page;
+ CRobotisStep step;
+
+ if(argc!=2)
+ {
+ std::cout << "A motion file must be provided (.bin)" << std::endl;
+ return (EXIT_FAILURE);
+ }
+ else
+ {
+ if(strstr(argv[1],".bin")!=NULL)
+ {
+ try{
+ mtn_parser.parse(argv[1]);
+ mtn_parser.get_motions(motions);
+ }catch(CMtnException &e){
+ std::cout << e.what() << std::endl;
+ }
+ }
+ else
+ {
+ std::cout << "Invalid file format. Supported format is .bin" << std::endl;
+ return (EXIT_FAILURE);
+ }
+ }
+ /* show motion pages */
+ std::cout << "Num. pages: " << motions.get_num_pages() << std::endl;
+ for(i=0;i<motions.get_num_pages();i++)
+ {
+ page=motions.get_page(i);
+ std::cout << "name: " << page.get_name() << std::endl;
+ std::cout << "next page: " << page.get_next_page() << std::endl;
+ std::cout << "exit page: " << page.get_exit_page() << std::endl;
+ std::cout << "repetitions: " << page.get_repetitions() << std::endl;
+ std::cout << "speed rate: " << page.get_speed_rate() << std::endl;
+ std::cout << "inertial force: " << page.get_inertial() << std::endl;
+ std::cout << "Num. steps: " << page.get_num_steps() << std::endl;
+ for(j=0;j<page.get_num_steps();j++)
+ {
+ step=page.get_step(j);
+ std::cout << "angles: ";
+ angles=step.get_angles();
+ for(k=0;k<angles.size();k++)
+ std::cout << angles[k] << ",";
+ std::cout << std::endl;
+ std::cout << "pausetime: " << step.get_pause_time() << std::endl;
+ std::cout << "step time: " << step.get_step_time() << std::endl;
+ }
+ }
+}
diff --git a/src/robotis_bin_file/robotis_bin_parser.cpp b/src/robotis_bin_file/robotis_bin_parser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..e86a0a836616d1dc8c836fe9146fe1c2ea7b7667
--- /dev/null
+++ b/src/robotis_bin_file/robotis_bin_parser.cpp
@@ -0,0 +1,124 @@
+#include "robotis_bin_parser.h"
+#include "mtn_exceptions.h"
+#include <fstream>
+#include <string.h>
+#include <iostream>
+#include <stdlib.h>
+
+CBinFileParser::CBinFileParser()
+{
+ memset(this->pages,0x00,sizeof(this->pages));
+}
+
+unsigned char CBinFileParser::generate_checksum(TPage *page)
+{
+ unsigned char checksum=0x00;
+ unsigned int i=0;
+
+ page->header.checksum=0x00;
+ for(i=0;i<sizeof(TPage);i++)
+ checksum+=((char *)page)[i];
+
+ return checksum;
+}
+
+bool CBinFileParser::check_checksum(TPage *page)
+{
+ unsigned char checksum=0x00;
+ unsigned int i=0;
+
+ for(i=0;i<sizeof(TPage);i++)
+ checksum+=((char *)page)[i];
+ if(checksum==0xFF)
+ return true;
+ else
+ return false;
+}
+
+void CBinFileParser::parse(const char *filename)
+{
+ std::vector<unsigned short int> angles;
+ std::vector<unsigned char> cw_compliance;
+ std::vector<unsigned char> ccw_compliance;
+ std::ifstream in_file(filename);
+ int length=0,i=0,j=0,k=0;
+ CRobotisPage page;
+ CRobotisStep step;
+
+ if(!in_file.good())
+ {
+ std::cout << " File << " << filename << " not found" << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ in_file.seekg (0, in_file.end);
+ length = in_file.tellg();
+ in_file.seekg (0, in_file.beg);
+ if(length!=sizeof(this->pages))
+ {
+ std::cout << "Invalid binary motion file" << std::endl;
+ exit(EXIT_FAILURE);
+ }
+ in_file.read((char *)this->pages,length);
+ in_file.close();
+ // initialize the CMtnFile class
+ this->motions.set_type(std::string("motion"));
+ this->motions.set_version(1.0);
+ this->motions.set_num_servos(PAGE_MAX_NUM_SERVOS);
+ for(i=0;i<MAX_NUM_PAGES;i++)
+ {
+ page.clear();
+ page.set_num_servos(PAGE_MAX_NUM_SERVOS);
+ page.set_name(std::string(this->pages[i].header.name));
+ cw_compliance.resize(PAGE_MAX_NUM_SERVOS);
+ ccw_compliance.resize(PAGE_MAX_NUM_SERVOS);
+ for(j=0;j<PAGE_MAX_NUM_SERVOS;j++)
+ {
+ cw_compliance[j]=(this->pages[i].header.slope[j]>>4);
+ ccw_compliance[j]=(this->pages[i].header.slope[j]&0x0F);
+ }
+ page.set_cw_compliances(cw_compliance);
+ page.set_ccw_compliances(ccw_compliance);
+ page.set_next_page(this->pages[i].header.next);
+ page.set_exit_page(this->pages[i].header.exit);
+ page.set_repetitions(this->pages[i].header.repeat);
+ page.set_speed_rate(this->pages[i].header.speed/32.0);
+ page.set_inertial(this->pages[i].header.accel);
+ for(j=0;j<this->pages[i].header.stepnum;j++)
+ {
+ step.clear();
+ step.set_num_servos(PAGE_MAX_NUM_SERVOS);
+ step.set_pause_time(this->pages[i].steps[j].pause*0.0078);
+ step.set_step_time(this->pages[i].steps[j].time*0.0078);
+ angles.resize(PAGE_MAX_NUM_SERVOS);
+ for(k=0;k<PAGE_MAX_NUM_SERVOS;k++)
+ angles[k]=this->pages[i].steps[j].position[k];
+ step.set_angles(angles);
+ page.add_step(step);
+ }
+ this->motions.add_page(page);
+ if(!this->check_checksum(&this->pages[i]))
+ std::cout << "Page with invalid checksum" << std::endl;
+ }
+}
+
+void CBinFileParser::serialize(const char *filename)
+{
+
+}
+
+void CBinFileParser::get_motions(CRobotisMtn &motions)
+{
+ motions.clear();
+ motions=this->motions;
+}
+
+void CBinFileParser::set_motions(CRobotisMtn &motions)
+{
+ this->motions.clear();
+ this->motions=motions;
+}
+
+CBinFileParser::~CBinFileParser()
+{
+ memset(this->pages,0x00,sizeof(this->pages));
+}
diff --git a/src/robotis_bin_file/robotis_bin_parser.h b/src/robotis_bin_file/robotis_bin_parser.h
new file mode 100644
index 0000000000000000000000000000000000000000..2e8c86f1b7138c80ee36a69849b29998b09d4888
--- /dev/null
+++ b/src/robotis_bin_file/robotis_bin_parser.h
@@ -0,0 +1,59 @@
+#ifndef _BIN_FILE_PARSER_H
+#define _BIN_FILE_PARSER_H
+
+#include "robotis_mtn.h"
+
+#define MAX_NUM_PAGES 256
+#define PAGE_MAX_NUM_SERVOS 31
+#define POSE_NUMBER_OF_POSES_PER_PAGE 7
+
+typedef struct // Header Structure (total 64unsigned char)
+{
+ char name[14]; // Name 0~13
+ unsigned char reserved1; // Reserved1 14
+ unsigned char repeat; // Repeat count 15
+ unsigned char schedule; // schedule 16
+ unsigned char reserved2[3]; // reserved2 17~19
+ unsigned char stepnum; // Number of step 20
+ unsigned char reserved3; // reserved3 21
+ unsigned char speed; // Speed 22
+ unsigned char reserved4; // reserved4 23
+ unsigned char accel; // Acceleration time 24
+ unsigned char next; // Link to next 25
+ unsigned char exit; // Link to exit 26
+ unsigned char reserved5[4]; // reserved5 27~30
+ unsigned char checksum; // checksum 31
+ unsigned char slope[PAGE_MAX_NUM_SERVOS]; // CW/CCW compliance slope 32~62
+ unsigned char reserved6; // reserved6 63
+} TPageHeader;
+
+typedef struct // Step Structure (total 64unsigned char)
+{
+ unsigned short int position[PAGE_MAX_NUM_SERVOS]; // Joint position 0~61
+ unsigned char pause; // Pause time 62
+ unsigned char time; // Time 63
+} TStep;
+
+typedef struct // Page Structure (total 512unsigned char)
+{
+ TPageHeader header; // Page header 0~64
+ TStep steps[POSE_NUMBER_OF_POSES_PER_PAGE]; // Page step 65~511
+} TPage;
+
+class CBinFileParser
+{
+ private:
+ CRobotisMtn motions;
+ TPage pages[MAX_NUM_PAGES];
+ unsigned char generate_checksum(TPage *page);
+ bool check_checksum(TPage *page);
+ public:
+ CBinFileParser();
+ void parse(const char *filename);
+ void serialize(const char *filename);
+ void get_motions(CRobotisMtn &motions);
+ void set_motions(CRobotisMtn &motions);
+ ~CBinFileParser();
+};
+
+#endif
diff --git a/src/robotis_mtn/CMakeLists.txt b/src/robotis_mtn/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..7e5a6aae822a80d63f9472bcc66ada7ab83b356a
--- /dev/null
+++ b/src/robotis_mtn/CMakeLists.txt
@@ -0,0 +1,16 @@
+# driver source files
+SET(sources robotis_mtn.cpp robotis_page.cpp robotis_step.cpp mtn_exceptions.cpp)
+# application header files
+SET(headers robotis_mtn.h robotis_page.h robotis_step.h mtn_exceptions.h)
+# locate the necessary dependencies
+# add the necessary include directories
+INCLUDE_DIRECTORIES(.)
+# create the shared library
+ADD_LIBRARY(robotis_mtn SHARED ${sources})
+# link necessary libraries
+INSTALL(TARGETS robotis_mtn
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib)
+INSTALL(FILES ${headers} DESTINATION include)
+ADD_SUBDIRECTORY(examples)
diff --git a/src/robotis_mtn/examples/CMakeLists.txt b/src/robotis_mtn/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..e0c2cfec0f403f880b1f2b99d08ca0a575741056
--- /dev/null
+++ b/src/robotis_mtn/examples/CMakeLists.txt
@@ -0,0 +1,4 @@
+# create an example application
+ADD_EXECUTABLE(robotis_mtn_test robotis_mtn_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(robotis_mtn_test robotis_mtn)
diff --git a/src/robotis_mtn/examples/robotis_mtn_test.cpp b/src/robotis_mtn/examples/robotis_mtn_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f5e42eb34a3ab45292d2de25c61ce3857202f6c7
--- /dev/null
+++ b/src/robotis_mtn/examples/robotis_mtn_test.cpp
@@ -0,0 +1,5 @@
+#include "robotis_mtn.h"
+
+int main(int argc, char *argv[])
+{
+}
diff --git a/src/robotis_mtn/mtn_exceptions.cpp b/src/robotis_mtn/mtn_exceptions.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..34720cd019d5ed036a7f07407896b03fa706f41d
--- /dev/null
+++ b/src/robotis_mtn/mtn_exceptions.cpp
@@ -0,0 +1,23 @@
+#include "mtn_exceptions.h"
+#include <string.h>
+#include <stdio.h>
+
+const std::string mtn_exception_msg="[Motion Exception caught] - ";
+
+CMtnException::CMtnException(const std::string& where, const std::string& error_msg)
+{
+ this->error_msg=mtn_exception_msg;
+ this->error_msg+=where;
+ this->error_msg+="\nError: ";
+ this->error_msg+=error_msg;
+}
+
+const std::string& CMtnException::what(void)
+{
+ return this->error_msg;
+}
+
+CMtnException::~CMtnException() throw()
+{
+ /* the error message is destroyed by its own destructor */
+}
diff --git a/src/robotis_mtn/mtn_exceptions.h b/src/robotis_mtn/mtn_exceptions.h
new file mode 100644
index 0000000000000000000000000000000000000000..3ecc0a3788b562a71d5535a2cf451e48d1758b85
--- /dev/null
+++ b/src/robotis_mtn/mtn_exceptions.h
@@ -0,0 +1,30 @@
+#ifndef _MTN_EXCEPTIONS_H
+#define _MTN_EXCEPTIONS_H
+
+#include <exception>
+#include <sstream>
+#include <string>
+
+template <typename T_>
+inline std::string tostring (const T_ & src)
+{
+ std::ostringstream out;
+
+ out << src;
+ return out.str();
+}
+
+#define _HERE_ (std::string(__PRETTY_FUNCTION__) + " at " + \
+ std::string(__FILE__) + ":" +tostring(__LINE__))
+
+class CMtnException : public std::exception
+{
+ protected:
+ std::string error_msg;
+ public:
+ CMtnException(const std::string& where, const std::string& error_msg);
+ virtual const std::string& what(void);
+ virtual ~CMtnException() throw();
+};
+
+#endif
diff --git a/src/robotis_mtn/robotis_mtn.cpp b/src/robotis_mtn/robotis_mtn.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a4e5271ba313c6b925f24e5148149cdb82526fae
--- /dev/null
+++ b/src/robotis_mtn/robotis_mtn.cpp
@@ -0,0 +1,388 @@
+#include "robotis_mtn.h"
+#include "mtn_exceptions.h"
+
+CRobotisMtn::CRobotisMtn()
+{
+ this->type="";
+ this->version=0.0;
+ this->enable.clear();
+ this->pages.clear();
+ this->info.clear();
+ this->num_servos=0;
+}
+
+CRobotisMtn::CRobotisMtn(CRobotisMtn &mtn)
+{
+ unsigned int i=0;
+
+ this->type=mtn.type;
+ this->version=mtn.version;
+ this->enable.clear();
+ for(i=0;i<mtn.get_num_servos();i++)
+ this->enable.push_back(mtn.enable.at(i));
+ this->info.clear();
+ for(i=0;i<mtn.get_num_servos();i++)
+ this->info.push_back(mtn.info[i]);
+ this->pages.clear();
+ for(i=0;i<mtn.pages.size();i++)
+ this->pages.push_back(mtn.pages[i]);
+ this->num_servos=mtn.get_num_servos();
+}
+
+void CRobotisMtn::set_type(const std::string &type)
+{
+ this->type=type;
+}
+
+void CRobotisMtn::set_version(double version)
+{
+ this->version=version;
+}
+
+void CRobotisMtn::set_num_servos(unsigned int num_servos)
+{
+ this->enable.resize(num_servos);
+ this->info.resize(num_servos);
+ this->num_servos=num_servos;
+}
+
+void CRobotisMtn::set_enable(unsigned int servo,bool enable)
+{
+ if(servo>this->get_num_servos())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ this->enable[servo]=enable;
+}
+
+void CRobotisMtn::set_enables(std::vector<bool> &enables)
+{
+ unsigned int i=0;
+
+ if(this->get_num_servos()!=enables.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid enable vector size");
+ }
+ else
+ for(i=0;i<this->get_num_servos();i++)
+ this->enable[i]=enables[i];
+}
+
+void CRobotisMtn::set_motor_type(unsigned int servo, int motor_type)
+{
+ if(servo>this->get_num_servos())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ {
+ switch(motor_type)
+ {
+ case SERVO_AX12A: this->info[servo].encoder_resolution=1024;
+ this->info[servo].max_angle=300;
+ this->info[servo].center_angle=150;
+ this->info[servo].max_speed=354;
+ this->info[servo].model=SERVO_AX12A;
+ this->info[servo].model_string="AX-12A";
+ break;
+ case SERVO_AX12W: this->info[servo].encoder_resolution=1023;
+ this->info[servo].max_angle=3000;
+ this->info[servo].center_angle=1500;
+ this->info[servo].max_speed=2830;
+ this->info[servo].model=SERVO_AX12W;
+ this->info[servo].model_string="AX-12W";
+ break;
+ case SERVO_AX18A: this->info[servo].encoder_resolution=1023;
+ this->info[servo].max_angle=3000;
+ this->info[servo].center_angle=1500;
+ this->info[servo].max_speed=582;
+ this->info[servo].model=SERVO_AX18A;
+ this->info[servo].model_string="AX-18A";
+ break;
+ case SERVO_MX28: this->info[servo].encoder_resolution=4095;
+ this->info[servo].max_angle=3600;
+ this->info[servo].center_angle=1800;
+ this->info[servo].max_speed=330;
+ this->info[servo].model=SERVO_MX28;
+ this->info[servo].model_string="MX-28";
+ break;
+ case SERVO_RX24F: this->info[servo].encoder_resolution=1023;
+ this->info[servo].max_angle=3000;
+ this->info[servo].center_angle=1500;
+ this->info[servo].max_speed=756;
+ this->info[servo].model=SERVO_RX24F;
+ this->info[servo].model_string="RX-24F";
+ break;
+ case SERVO_RX28: this->info[servo].encoder_resolution=1023;
+ this->info[servo].max_angle=3000;
+ this->info[servo].center_angle=1500;
+ this->info[servo].max_speed=402;
+ this->info[servo].model=SERVO_RX28;
+ this->info[servo].model_string="RX-28";
+ break;
+ case SERVO_RX64: this->info[servo].encoder_resolution=1023;
+ this->info[servo].max_angle=3000;
+ this->info[servo].center_angle=1500;
+ this->info[servo].max_speed=294;
+ this->info[servo].model=SERVO_RX64;
+ this->info[servo].model_string="RX-64";
+ break;
+ case SERVO_MX64: this->info[servo].encoder_resolution=4095;
+ this->info[servo].max_angle=3600;
+ this->info[servo].center_angle=1800;
+ this->info[servo].max_speed=378;
+ this->info[servo].model=SERVO_MX64;
+ this->info[servo].model_string="MX-64";
+ break;
+ case SERVO_EX106: this->info[servo].encoder_resolution=1023;
+ this->info[servo].max_angle=2500;
+ this->info[servo].center_angle=1250;
+ this->info[servo].max_speed=414;
+ this->info[servo].model=SERVO_EX106;
+ this->info[servo].model_string="EX-106";
+ break;
+ case SERVO_MX106: this->info[servo].encoder_resolution=4095;
+ this->info[servo].max_angle=3600;
+ this->info[servo].center_angle=1800;
+ this->info[servo].max_speed=270;
+ this->info[servo].model=SERVO_MX106;
+ this->info[servo].model_string="MX-106";
+ break;
+ default: throw CMtnException(_HERE_,"Invalid servo type");
+ break;
+ }
+ }
+}
+
+void CRobotisMtn::set_motor_type(unsigned int servo, std::string model)
+{
+ if(servo>this->get_num_servos())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ {
+ if(model=="AX-12A")
+ this->set_motor_type(servo,SERVO_AX12A);
+ else if(model=="AX-12W")
+ this->set_motor_type(servo,SERVO_AX12W);
+ else if(model=="AX-18A")
+ this->set_motor_type(servo,SERVO_AX18A);
+ else if(model=="MX-28")
+ this->set_motor_type(servo,SERVO_MX28);
+ else if(model=="RX24F")
+ this->set_motor_type(servo,SERVO_RX24F);
+ else if(model=="RX-28")
+ this->set_motor_type(servo,SERVO_RX28);
+ else if(model=="RX-64")
+ this->set_motor_type(servo,SERVO_RX64);
+ else if(model=="MX64")
+ this->set_motor_type(servo,SERVO_MX64);
+ else if(model=="EX-106")
+ this->set_motor_type(servo,SERVO_EX106);
+ else if(model=="MX-106")
+ this->set_motor_type(servo,SERVO_MX106);
+ else
+ throw CMtnException(_HERE_,"Invalid servo type");
+ }
+}
+
+void CRobotisMtn::set_motor_types(std::vector<std::string> &models)
+{
+ unsigned int i=0;
+
+ if(this->get_num_servos()!=models.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid motor type vector size");
+ }
+ else
+ for(i=0;i<this->get_num_servos();i++)
+ this->set_motor_type(i,models[i]);
+}
+
+void CRobotisMtn::set_motor_types(std::vector<int> &motor_types)
+{
+ unsigned int i=0;
+
+ if(this->get_num_servos()!=motor_types.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid motor type vector size");
+ }
+ else
+ for(i=0;i<this->get_num_servos();i++)
+ this->set_motor_type(i,motor_types[i]);
+}
+
+void CRobotisMtn::add_page(CRobotisPage &page)
+{
+ this->pages.push_back(page);
+}
+
+std::string CRobotisMtn::get_type(void) const
+{
+ return this->type;
+}
+
+double CRobotisMtn::get_version(void) const
+{
+ return this->version;
+}
+
+unsigned int CRobotisMtn::get_num_servos(void) const
+{
+ return this->num_servos;
+}
+
+bool CRobotisMtn::get_enable(unsigned int servo) const
+{
+ if(servo>this->get_num_servos())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ return this->enable[servo];
+}
+
+std::vector<bool> CRobotisMtn::get_enables(void) const
+{
+ return this->enable;
+}
+
+int CRobotisMtn::get_motor_type(unsigned int servo) const
+{
+ if(servo>this->get_num_servos())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ return this->info[servo].model;
+}
+
+std::string CRobotisMtn::get_motor_model(unsigned int servo) const
+{
+ if(servo>this->get_num_servos())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ return this->info[servo].model_string;
+}
+
+std::vector<int> CRobotisMtn::get_motor_types(void) const
+{
+ std::vector<int> types;
+ unsigned int i=0;
+
+ for(i=0;i<this->get_num_servos();i++)
+ types.push_back(this->info[i].model);
+
+ return types;
+}
+
+std::vector<std::string> CRobotisMtn::get_motor_models(void) const
+{
+ std::vector<std::string> models;
+ unsigned int i=0;
+
+ for(i=0;i<this->get_num_servos();i++)
+ models.push_back(this->info[i].model_string);
+
+ return models;
+}
+
+TServoInfo CRobotisMtn::get_motor_info(unsigned int servo) const
+{
+ if(servo>this->get_num_servos())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ return this->info[servo];
+}
+
+std::vector<TServoInfo> CRobotisMtn::get_motor_infos(void) const
+{
+ return this->info;
+}
+
+unsigned int CRobotisMtn::get_num_pages(void) const
+{
+ return this->pages.size();
+}
+
+CRobotisPage CRobotisMtn::get_page(unsigned int page) const
+{
+ if(page>this->pages.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid page index");
+ }
+ else
+ return this->pages[page];
+}
+
+void CRobotisMtn::clear(void)
+{
+ this->type="";
+ this->version=0.0;
+ this->enable.clear();
+ this->info.clear();
+ this->pages.clear();
+}
+
+CRobotisMtn CRobotisMtn::operator =(CRobotisMtn mtn)
+{
+ unsigned int i=0;
+
+ this->type=mtn.type;
+ this->version=mtn.version;
+ this->enable.clear();
+ for(i=0;i<mtn.get_num_servos();i++)
+ this->enable.push_back(mtn.enable[i]);
+ this->info.clear();
+ for(i=0;i<mtn.get_num_servos();i++)
+ this->info.push_back(mtn.info[i]);
+ pages.clear();
+ for(i=0;i<mtn.pages.size();i++)
+ this->pages.push_back(mtn.pages[i]);
+ this->num_servos=mtn.get_num_servos();
+
+ return *this;
+}
+
+double CRobotisMtn::get_angle(unsigned int servo_id, unsigned char page_id, unsigned char step_id)
+{
+ CRobotisPage page;
+ CRobotisStep step;
+ unsigned short int value;
+
+ page=this->get_page(page_id);
+ step=page.get_step(step_id);
+ if(servo_id>this->get_num_servos())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ {
+ value=step.get_angle(servo_id);
+ return (value*this->info[servo_id].max_angle)/this->info[servo_id].encoder_resolution-this->info[servo_id].center_angle;
+ }
+}
+
+CRobotisMtn::~CRobotisMtn()
+{
+ this->clear();
+}
+
diff --git a/src/robotis_mtn/robotis_mtn.h b/src/robotis_mtn/robotis_mtn.h
new file mode 100644
index 0000000000000000000000000000000000000000..e50f377c4d86bc323ac577cd117715487f07f6c2
--- /dev/null
+++ b/src/robotis_mtn/robotis_mtn.h
@@ -0,0 +1,69 @@
+#ifndef _ROBOTIS_MTN_H
+#define _ROBOTIS_MTN_H
+
+#include "robotis_page.h"
+
+#define SERVO_AX12A 0x000C
+#define SERVO_AX12W 0x012C
+#define SERVO_AX18A 0x0012
+#define SERVO_MX28 0x001D
+#define SERVO_RX24F 0x0018
+#define SERVO_RX28 0x001C
+#define SERVO_RX64 0x0040
+#define SERVO_MX64 0x0136
+#define SERVO_EX106 0x006B
+#define SERVO_MX106 0x0140
+
+typedef struct
+{
+ unsigned short int model;
+ std::string model_string;
+ unsigned short int encoder_resolution;
+ double max_angle;
+ double center_angle;
+ double max_speed;
+}TServoInfo;
+
+class CRobotisMtn
+{
+ private:
+ std::string type;
+ double version;
+ int num_servos;
+ std::vector<bool> enable;
+ std::vector<CRobotisPage> pages;
+ std::vector<TServoInfo> info;
+ public:
+ CRobotisMtn();
+ CRobotisMtn(CRobotisMtn &mtn);
+ void set_type(const std::string &type);
+ void set_version(double version);
+ void set_num_servos(unsigned int num_servos);
+ void set_enable(unsigned int servo,bool enable);
+ void set_enables(std::vector<bool> &enables);
+ void set_motor_type(unsigned int servo, int motor_type);
+ void set_motor_type(unsigned int servo, std::string model);
+ void set_motor_types(std::vector<int> &motor_types);
+ void set_motor_types(std::vector<std::string> &models);
+ void add_page(CRobotisPage &page);
+ std::string get_type(void) const;
+ double get_version(void) const;
+ unsigned int get_num_servos(void) const;
+ bool get_enable(unsigned int servo) const;
+ std::vector<bool> get_enables(void) const;
+ int get_motor_type(unsigned int servo) const;
+ std::string get_motor_model(unsigned int servo) const;
+ std::vector<int> get_motor_types(void) const;
+ std::vector<std::string> get_motor_models(void) const;
+ TServoInfo get_motor_info(unsigned int servo) const;
+ std::vector<TServoInfo> get_motor_infos(void) const;
+ unsigned int get_num_pages(void) const;
+ CRobotisPage get_page(unsigned int page) const;
+ void clear(void);
+ CRobotisMtn operator=(CRobotisMtn mtn);
+ // angle conversion functions
+ double get_angle(unsigned int servo_id, unsigned char page_id, unsigned char step_id);
+ ~CRobotisMtn();
+};
+
+#endif
diff --git a/src/robotis_mtn/robotis_page.cpp b/src/robotis_mtn/robotis_page.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f35cd31ca434e44ef3ba4da9c819ba552fbf2ce9
--- /dev/null
+++ b/src/robotis_mtn/robotis_page.cpp
@@ -0,0 +1,283 @@
+#include "robotis_page.h"
+#include "mtn_exceptions.h"
+
+CRobotisPage::CRobotisPage()
+{
+ this->name="";
+ this->cw_compliance.clear();
+ this->ccw_compliance.clear();
+ this->next_page=0;
+ this->exit_page=0;
+ this->repetitions=0;
+ this->speed_rate=0;
+ this->inertial=0;
+ this->steps.clear();
+}
+
+CRobotisPage::CRobotisPage(const CRobotisPage &page)
+{
+ unsigned int i=0;
+
+ this->name=page.name;
+ this->cw_compliance.resize(page.cw_compliance.size());
+ this->ccw_compliance.resize(page.ccw_compliance.size());
+ for(i=0;i<page.cw_compliance.size();i++)
+ {
+ this->cw_compliance[i]=page.cw_compliance[i];
+ this->ccw_compliance[i]=page.ccw_compliance[i];
+ }
+ this->next_page=page.next_page;
+ this->exit_page=page.exit_page;
+ this->repetitions=page.repetitions;
+ this->speed_rate=page.speed_rate;
+ this->inertial=page.inertial;
+ this->steps.clear();
+ for(i=0;i<page.steps.size();i++)
+ this->steps.push_back(page.get_step(i));
+}
+
+void CRobotisPage::set_num_servos(unsigned int num_servos)
+{
+ this->cw_compliance.resize(num_servos);
+ this->ccw_compliance.resize(num_servos);
+}
+
+void CRobotisPage::set_name(const std::string &name)
+{
+ this->name=name;
+}
+
+void CRobotisPage::set_cw_compliance(unsigned int servo,unsigned char cw_compliance)
+{
+ if(servo>this->cw_compliance.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ this->cw_compliance[servo]=cw_compliance;
+}
+
+void CRobotisPage::set_cw_compliances(std::vector<unsigned char> cw_compliances)
+{
+ unsigned int i=0;
+
+ if(this->cw_compliance.size()!=cw_compliances.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid compliance vector size");
+ }
+ else
+ {
+ for(i=0;i<this->cw_compliance.size();i++)
+ this->cw_compliance[i]=cw_compliances[i];
+ }
+}
+
+void CRobotisPage::set_ccw_compliance(unsigned int servo,unsigned char ccw_compliance)
+{
+ if(servo>this->ccw_compliance.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ this->ccw_compliance[servo]=ccw_compliance;
+}
+
+void CRobotisPage::set_ccw_compliances(std::vector<unsigned char> ccw_compliances)
+{
+ unsigned int i=0;
+
+ if(this->ccw_compliance.size()!=ccw_compliances.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid compliance vector size");
+ }
+ else
+ {
+ for(i=0;i<this->ccw_compliance.size();i++)
+ this->ccw_compliance[i]=ccw_compliances[i];
+ }
+}
+
+void CRobotisPage::set_next_page(unsigned int page)
+{
+ if(page>=256)
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid next page index");
+ }
+ else
+ this->next_page=page;
+}
+
+void CRobotisPage::set_exit_page(unsigned int page)
+{
+ if(page>=256)
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid exit page index");
+ }
+ else
+ this->exit_page=page;
+}
+
+void CRobotisPage::set_repetitions(unsigned int num)
+{
+ if(num>=256)
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid number of repetitions");
+ }
+ else
+ this->repetitions=num;
+}
+
+void CRobotisPage::set_speed_rate(double rate)
+{
+ this->speed_rate=rate;
+}
+
+void CRobotisPage::set_inertial(unsigned int inertia)
+{
+ if(inertia>=256)
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid inertial force value");
+ }
+ else
+ this->inertial=inertia;
+}
+
+void CRobotisPage::add_step(CRobotisStep &step)
+{
+ if(this->steps.size()>=7)
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Impossible to add more steps");
+ }
+ else
+ this->steps.push_back(step);
+}
+
+std::string CRobotisPage::get_name(void) const
+{
+ return this->name;
+}
+
+unsigned char CRobotisPage::get_cw_compliance(unsigned int servo) const
+{
+ if(servo>this->cw_compliance.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ return this->cw_compliance[servo];
+}
+
+std::vector<unsigned char> CRobotisPage::get_cw_compliances(void) const
+{
+ return this->cw_compliance;
+}
+
+unsigned char CRobotisPage::get_ccw_compliance(unsigned int servo) const
+{
+ if(servo>this->ccw_compliance.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ return this->ccw_compliance[servo];
+}
+
+std::vector<unsigned char> CRobotisPage::get_ccw_compliances(void) const
+{
+ return this->ccw_compliance;
+}
+
+unsigned int CRobotisPage::get_next_page(void) const
+{
+ return this->next_page;
+}
+
+unsigned int CRobotisPage::get_exit_page(void) const
+{
+ return this->exit_page;
+}
+
+unsigned int CRobotisPage::get_repetitions(void) const
+{
+ return this->repetitions;
+}
+
+double CRobotisPage::get_speed_rate(void) const
+{
+ return this->speed_rate;
+}
+
+unsigned int CRobotisPage::get_inertial(void) const
+{
+ return this->inertial;
+}
+
+unsigned int CRobotisPage::get_num_steps(void) const
+{
+ return this->steps.size();
+}
+
+CRobotisStep CRobotisPage::get_step(unsigned int step) const
+{
+ if(step>this->steps.size())
+ {
+ /* handle error */
+ throw CMtnException(_HERE_,"Invalid step index");
+ }
+ else
+ return this->steps[step];
+}
+
+void CRobotisPage::clear(void)
+{
+ this->name.clear();
+ this->cw_compliance.clear();
+ this->ccw_compliance.clear();
+ this->next_page=0;
+ this->exit_page=0;
+ this->repetitions=0;
+ this->speed_rate=0;
+ this->inertial=0;
+ this->steps.clear();
+}
+
+CRobotisPage CRobotisPage::operator =(CRobotisPage page)
+{
+ unsigned int i=0;
+
+ this->name=page.name;
+ this->cw_compliance.resize(page.cw_compliance.size());
+ this->ccw_compliance.resize(page.ccw_compliance.size());
+ for(i=0;i<page.cw_compliance.size();i++)
+ {
+ this->cw_compliance[i]=page.cw_compliance[i];
+ this->ccw_compliance[i]=page.ccw_compliance[i];
+ }
+ this->next_page=page.next_page;
+ this->exit_page=page.exit_page;
+ this->repetitions=page.repetitions;
+ this->speed_rate=page.speed_rate;
+ this->inertial=page.inertial;
+ this->steps.clear();
+ for(i=0;i<page.steps.size();i++)
+ this->steps.push_back(page.get_step(i));
+
+ return *this;
+}
+
+CRobotisPage::~CRobotisPage()
+{
+ this->clear();
+}
+
diff --git a/src/robotis_mtn/robotis_page.h b/src/robotis_mtn/robotis_page.h
new file mode 100644
index 0000000000000000000000000000000000000000..890b3f35a871fa9d05bc6ea066e45a7b1957a6ad
--- /dev/null
+++ b/src/robotis_mtn/robotis_page.h
@@ -0,0 +1,51 @@
+#ifndef _ROBOTIS_PAGE_H
+#define _ROBOTIS_PAGE_H
+
+#include "robotis_step.h"
+#include <string>
+
+class CRobotisPage
+{
+ private:
+ std::string name;
+ std::vector<unsigned char> cw_compliance;
+ std::vector<unsigned char> ccw_compliance;
+ unsigned int next_page;
+ unsigned int exit_page;
+ unsigned int repetitions;
+ double speed_rate;
+ unsigned int inertial;
+ std::vector<CRobotisStep> steps;
+ public:
+ CRobotisPage();
+ CRobotisPage(const CRobotisPage &page);
+ void set_num_servos(unsigned int num_servos);
+ void set_name(const std::string &name);
+ void set_cw_compliance(unsigned int servo,unsigned char cw_compliance);
+ void set_cw_compliances(std::vector<unsigned char> cw_compliances);
+ void set_ccw_compliance(unsigned int servo,unsigned char ccw_compliance);
+ void set_ccw_compliances(std::vector<unsigned char> ccw_compliances);
+ void set_next_page(unsigned int page);
+ void set_exit_page(unsigned int page);
+ void set_repetitions(unsigned int num);
+ void set_speed_rate(double rate);
+ void set_inertial(unsigned int inertia);
+ void add_step(CRobotisStep &step);
+ std::string get_name(void) const;
+ unsigned char get_cw_compliance(unsigned int servo) const;
+ std::vector<unsigned char> get_cw_compliances(void) const;
+ unsigned char get_ccw_compliance(unsigned int servo) const;
+ std::vector<unsigned char> get_ccw_compliances(void) const;
+ unsigned int get_next_page(void) const;
+ unsigned int get_exit_page(void) const;
+ unsigned int get_repetitions(void) const;
+ double get_speed_rate(void) const;
+ unsigned int get_inertial(void) const;
+ unsigned int get_num_steps(void) const;
+ CRobotisStep get_step(unsigned int step) const;
+ void clear(void);
+ CRobotisPage operator =(CRobotisPage page);
+ ~CRobotisPage();
+};
+
+#endif
diff --git a/src/robotis_mtn/robotis_step.cpp b/src/robotis_mtn/robotis_step.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..8519811dc711c03f44284133d730d2c335068317
--- /dev/null
+++ b/src/robotis_mtn/robotis_step.cpp
@@ -0,0 +1,126 @@
+#include "robotis_step.h"
+#include "mtn_exceptions.h"
+
+CRobotisStep::CRobotisStep()
+{
+ this->angles.clear();
+ this->pause_time=0.0;
+ this->step_time=0.0;
+}
+
+CRobotisStep::CRobotisStep(const CRobotisStep &step)
+{
+ unsigned int i=0;
+
+ this->pause_time=step.pause_time;
+ this->step_time=step.step_time;
+ this->angles.resize(step.angles.size());
+ for(i=0;i<step.angles.size();i++)
+ this->angles[i]=step.angles[i];
+}
+
+void CRobotisStep::set_num_servos(unsigned int num_servos)
+{
+ this->angles.resize(num_servos);
+}
+
+void CRobotisStep::set_angle(unsigned int servo,unsigned short int angle)
+{
+ if(servo>this->angles.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ this->angles[servo]=angle;
+}
+
+void CRobotisStep::set_angles(std::vector<unsigned short int> &angles)
+{
+ unsigned int i=0;
+
+ if(angles.size()!=this->angles.size())
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid vector size");
+ }
+ else
+ {
+ for(i=0;i<angles.size();i++)
+ this->angles[i]=angles[i];
+ }
+}
+
+void CRobotisStep::set_pause_time(double time)
+{
+ if(time<0.0)
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid pause time");
+ }
+ else
+ this->pause_time=time;
+}
+
+void CRobotisStep::set_step_time(double time)
+{
+ if(time<0.0)
+ {
+ /* handle errors */
+ throw CMtnException(_HERE_,"Invalid step time");
+ }
+ else
+ this->step_time=time;
+}
+
+unsigned short int CRobotisStep::get_angle(unsigned int servo) const
+{
+ if(servo>this->angles.size())
+ {
+ /* handler errors */
+ throw CMtnException(_HERE_,"Invalid servo index");
+ }
+ else
+ return this->angles[servo];
+}
+
+std::vector<unsigned short int> CRobotisStep::get_angles(void) const
+{
+ return this->angles;
+}
+
+double CRobotisStep::get_pause_time(void) const
+{
+ return this->pause_time;
+}
+
+double CRobotisStep::get_step_time(void) const
+{
+ return this->step_time;
+}
+
+void CRobotisStep::clear(void)
+{
+ this->angles.clear();
+ this->pause_time=0.0;
+ this->step_time=0.0;
+}
+
+CRobotisStep CRobotisStep::operator =(CRobotisStep step)
+{
+ unsigned int i=0;
+
+ this->pause_time=step.pause_time;
+ this->step_time=step.step_time;
+ this->angles.resize(step.angles.size());
+ for(i=0;i<step.angles.size();i++)
+ this->angles[i]=step.angles[i];
+
+ return *this;
+}
+
+CRobotisStep::~CRobotisStep()
+{
+ this->clear();
+}
+
diff --git a/src/robotis_mtn/robotis_step.h b/src/robotis_mtn/robotis_step.h
new file mode 100644
index 0000000000000000000000000000000000000000..4d5dd573831918629b62e9f0aabd33ad2298d593
--- /dev/null
+++ b/src/robotis_mtn/robotis_step.h
@@ -0,0 +1,29 @@
+#ifndef _ROBOTIS__STEP_H
+#define _ROBOTIS_STEP_H
+
+#include <vector>
+
+class CRobotisStep
+{
+ private:
+ std::vector<unsigned short int> angles;
+ double pause_time;
+ double step_time;
+ public:
+ CRobotisStep();
+ CRobotisStep(const CRobotisStep &object);
+ void set_num_servos(unsigned int num_servos);
+ void set_angle(unsigned int servo,unsigned short int angle);
+ void set_angles(std::vector<unsigned short int> &angles);
+ void set_pause_time(double time);
+ void set_step_time(double time);
+ unsigned short int get_angle(unsigned int servo) const;
+ std::vector<unsigned short int> get_angles(void) const;
+ double get_pause_time(void) const;
+ double get_step_time(void) const;
+ void clear();
+ CRobotisStep operator =(CRobotisStep step);
+ ~CRobotisStep();
+};
+
+#endif
diff --git a/src/robotis_mtn_file/CMakeLists.txt b/src/robotis_mtn_file/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..2be43f12d2f251fd780ec02326289ae5cf8fef73
--- /dev/null
+++ b/src/robotis_mtn_file/CMakeLists.txt
@@ -0,0 +1,50 @@
+# Create target for the parser
+FIND_PACKAGE(BISON REQUIRED)
+FIND_PACKAGE(FLEX REQUIRED)
+FIND_PACKAGE(robotis_mtn REQUIRED)
+
+SET(BisonOutput mtn_parser.cpp)
+IF(BISON_FOUND)
+ ADD_CUSTOM_COMMAND(
+ OUTPUT ${BisonOutput}
+ COMMAND ${BISON_EXECUTABLE}
+ --output=${BisonOutput}
+ ${CMAKE_CURRENT_SOURCE_DIR}/mtn_file.y
+ COMMENT "Generating mtn_parser.cpp"
+ )
+ENDIF(BISON_FOUND)
+
+FIND_PACKAGE(FLEX REQUIRED)
+SET(FlexOutput mtn_scanner.cpp)
+IF(FLEX_FOUND)
+ ADD_CUSTOM_COMMAND(
+ OUTPUT ${FlexOutput}
+ COMMAND ${FLEX_EXECUTABLE}
+ --outfile=${FlexOutput}
+ ${CMAKE_CURRENT_SOURCE_DIR}/mtn_file.l
+ COMMENT "Generating mtn_scanner.cpp"
+ )
+ENDIF(FLEX_FOUND)
+
+SET(sources mtn_file_parser.cpp)
+SET(headers mtn_file_parser.hpp mtn_file_scanner.hpp)
+
+include_directories(.)
+include_directories(${robotis_mtn_INCLUDE_DIR})
+include_directories(${CMAKE_CURRENT_BINARY_DIR})
+
+add_library(robotis_mtn_parser SHARED ${BisonOutput} ${FlexOutput} ${sources})
+
+target_link_libraries(robotis_mtn_parser ${robotis_mtn_LIBRARY})
+
+INSTALL(TARGETS robotis_mtn_parser
+ RUNTIME DESTINATION bin
+ LIBRARY DESTINATION lib
+ ARCHIVE DESTINATION lib)
+INSTALL(FILES ${headers} DESTINATION include)
+INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/mtn_parser.hpp DESTINATION include)
+INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/location.hh DESTINATION include)
+INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/position.hh DESTINATION include)
+INSTALL(FILES ${CMAKE_CURRENT_BINARY_DIR}/stack.hh DESTINATION include)
+ADD_SUBDIRECTORY(examples)
+
diff --git a/src/robotis_mtn_file/examples/CMakeLists.txt b/src/robotis_mtn_file/examples/CMakeLists.txt
new file mode 100644
index 0000000000000000000000000000000000000000..595cbf73befdc8646c74eea0756e1dfde8506ec6
--- /dev/null
+++ b/src/robotis_mtn_file/examples/CMakeLists.txt
@@ -0,0 +1,4 @@
+# create an example application
+ADD_EXECUTABLE(robotis_mtn_file_test robotis_mtn_file_test.cpp)
+# link necessary libraries
+TARGET_LINK_LIBRARIES(robotis_mtn_file_test robotis_mtn_parser)
diff --git a/src/robotis_mtn_file/examples/robotis_mtn_file_test.cpp b/src/robotis_mtn_file/examples/robotis_mtn_file_test.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..56b985366711c2fe2d1ce33539dd30f1208b613c
--- /dev/null
+++ b/src/robotis_mtn_file/examples/robotis_mtn_file_test.cpp
@@ -0,0 +1,69 @@
+#include <iostream>
+#include <cstdlib>
+#include <string.h>
+
+#include "mtn_file_parser.hpp"
+#include "mtn_exceptions.h"
+
+int main(int argc, char *argv[])
+{
+ std::vector<unsigned short int> angles;
+ MTN::CMtnFileParser mtn_parser;
+ unsigned int i=0,j=0,k=0;
+ CRobotisMtn motions;
+ CRobotisPage page;
+ CRobotisStep step;
+
+ if(argc!=2)
+ {
+ std::cout << "A motion file must be provided (.mtn)" << std::endl;
+ return (EXIT_FAILURE);
+ }
+ else
+ {
+ if(strstr(argv[1],".mtn")!=NULL)
+ {
+ try{
+ mtn_parser.parse(argv[1]);
+ mtn_parser.get_motions(motions);
+ }catch(CMtnException &e){
+ std::cout << e.what() << std::endl;
+ }
+ }
+ else
+ {
+ std::cout << "Invalid file format. Supported format is .mtn" << std::endl;
+ return (EXIT_FAILURE);
+ }
+ }
+ /* show motion pages */
+ std::cout << "Num. pages: " << motions.get_num_pages() << std::endl;
+ for(i=0;i<motions.get_num_pages();i++)
+ {
+ page.clear();
+ page.set_num_servos(motions.get_num_servos());
+ page=motions.get_page(i);
+ std::cout << "name: " << page.get_name() << std::endl;
+ std::cout << "next page: " << page.get_next_page() << std::endl;
+ std::cout << "exit page: " << page.get_exit_page() << std::endl;
+ std::cout << "repetitions: " << page.get_repetitions() << std::endl;
+ std::cout << "speed rate: " << page.get_speed_rate() << std::endl;
+ std::cout << "inertial force: " << page.get_inertial() << std::endl;
+ std::cout << "Num. steps: " << page.get_num_steps() << std::endl;
+ for(j=0;j<page.get_num_steps();j++)
+ {
+ step.clear();
+ step.set_num_servos(motions.get_num_servos());
+ step=page.get_step(j);
+ std::cout << "angles: ";
+ angles=step.get_angles();
+ for(k=0;k<angles.size();k++)
+ std::cout << angles[k] << ",";
+ std::cout << std::endl;
+ std::cout << "pausetime: " << step.get_pause_time() << std::endl;
+ std::cout << "step time: " << step.get_step_time() << std::endl;
+ }
+ }
+
+ return(EXIT_SUCCESS);
+}
diff --git a/src/robotis_mtn_file/mtn_file.l b/src/robotis_mtn_file/mtn_file.l
new file mode 100644
index 0000000000000000000000000000000000000000..04d5f5860a6219cac95925f4a35b656a8e8d8f9c
--- /dev/null
+++ b/src/robotis_mtn_file/mtn_file.l
@@ -0,0 +1,37 @@
+%{
+/* Implementation of yyFlexScanner */
+#include "mtn_file_scanner.hpp"
+#include "mtn_parser.hpp"
+
+/* typedef to make the returns for the tokens shorter */
+typedef MTN::MTN_Parser::token token;
+
+%}
+
+%option debug
+%option nodefault
+%option yyclass="MTN_File_Scanner"
+%option noyywrap
+%option c++
+%option yylineno
+
+%%
+[ \t\n] ;
+"type" { return token::TYPE; }
+"version" { return token::VERSION; }
+"enable" { return token::ENABLE; }
+"motor_type" { return token::MOTOR_TYPE; }
+"page_begin" { return token::BEGIN_PAGE; }
+"name" { return token::NAME; }
+"compliance" { return token::COMPLIANCE; }
+"play_param" { return token::PLAY_PARAMS; }
+"step" { return token::STEP; }
+"page_end" { return token::END_PAGE; }
+"=" { return token::EQUAL; }
+[0-9]+"."[0-9]+ { yylval->fval=atof(yytext); return token::FLOAT; }
+[0-9]+ { yylval->ival=atoi(yytext); return token::INT; }
+[a-zA-Z0-9_]+ { yylval->sval=new std::string(yytext);
+ return token::STRING; }
+. { std::cout << "bad input character " << yytext << " at line " << yylineno << std::endl; }
+%%
+
diff --git a/src/robotis_mtn_file/mtn_file.y b/src/robotis_mtn_file/mtn_file.y
new file mode 100644
index 0000000000000000000000000000000000000000..b94beb8843730da723c33050c0bdcd782a596f6c
--- /dev/null
+++ b/src/robotis_mtn_file/mtn_file.y
@@ -0,0 +1,163 @@
+%skeleton "lalr1.cc"
+%require "2.5"
+%defines
+%define namespace "MTN"
+%define parser_class_name "MTN_Parser"
+%name-prefix "mtn_"
+
+%code requires
+{
+ namespace MTN
+ {
+ class CMtnFileParser;
+ class CMtnFileScanner;
+ }
+}
+
+%lex-param { CMtnFileScanner &scanner }
+%parse-param { CMtnFileScanner &scanner }
+
+%lex-param { CMtnFileParser &parser }
+%parse-param { CMtnFileParser &parser }
+
+%code
+{
+ #include <iostream>
+ #include <cstdlib>
+ #include <fstream>
+ /* include for all driver functions */
+ #include "mtn_file_parser.hpp"
+
+ namespace MTN
+ {
+ /* motion files variables */
+ int num_servos;
+ std::vector<bool> enables;
+ std::vector<int> motor_types;
+ CRobotisPage page;
+ std::string motion_name;
+ std::vector<unsigned char> compliances;
+ CRobotisStep step;
+ std::vector<unsigned short int> angles;
+
+ /* this is silly, but I can't figure out a way around */
+ static int yylex(MTN::MTN_Parser::semantic_type *yylval,MTN::CMtnFileScanner &scanner,MTN::CMtnFileParser &parser);
+ }
+}
+
+/* token types */
+%union {
+ int ival;
+ float fval;
+ std::string *sval;
+}
+
+%token TYPE VERSION ENABLE MOTOR_TYPE BEGIN_PAGE END_PAGE
+%token NAME COMPLIANCE PLAY_PARAMS STEP
+%token EQUAL
+%token <ival> INT
+%token <fval> FLOAT
+%token <sval> STRING
+
+/* destructor rule for <sval> objects */
+%destructor { if ($$) { delete ($$); ($$) = NULL; } } <sval>
+
+%%
+motion_file: type version enables motor_types pages
+ ;
+
+type: TYPE EQUAL STRING { parser.set_type(*($3));
+ num_servos=0; }
+ ;
+
+version: VERSION EQUAL FLOAT { parser.set_version($3); }
+ ;
+
+enables: ENABLE EQUAL enable { parser.set_num_servos(num_servos);
+ parser.set_enables(enables); }
+ ;
+
+enable: enable INT { num_servos++;
+ enables.push_back($2); }
+ | INT { num_servos++;
+ enables.push_back($1); }
+ ;
+
+motor_types:
+ | MOTOR_TYPE EQUAL motor_type { parser.set_motor_types(motor_types); }
+ ;
+
+motor_type: motor_type INT { motor_types.push_back($2); }
+ | INT { motor_types.push_back($1); }
+ ;
+
+pages: pages page
+ | page
+ ;
+
+page: BEGIN_PAGE name compliances params steps END_PAGE { parser.add_page(page);
+ motion_name="";
+ compliances.clear(); }
+ | BEGIN_PAGE name compliances params END_PAGE { parser.add_page(page);
+ motion_name="";
+ compliances.clear(); }
+ ;
+
+name: NAME EQUAL { page.clear();
+ page.set_num_servos(num_servos);
+ page.set_name(std::string("")); }
+ | NAME EQUAL strings { page.clear();
+ page.set_num_servos(num_servos);
+ page.set_name(motion_name); }
+ ;
+
+strings: strings STRING { motion_name+=" "+*($2); }
+ | STRING { motion_name+=*($1); }
+ ;
+
+compliances: COMPLIANCE EQUAL compliance { page.set_cw_compliances(compliances);
+ page.set_ccw_compliances(compliances); }
+ ;
+
+compliance: compliance INT { compliances.push_back($2); }
+ | INT { compliances.push_back($1); }
+ ;
+
+params: PLAY_PARAMS EQUAL INT INT INT FLOAT INT { page.set_next_page($3);
+ page.set_exit_page($4);
+ page.set_repetitions($5);
+ page.set_speed_rate($6);
+ page.set_inertial($7); }
+
+
+steps: steps step
+ | step
+ ;
+
+
+step: STEP EQUAL angles FLOAT FLOAT { step.clear();
+ step.set_num_servos(num_servos);
+ step.set_angles(angles);
+ angles.clear();
+ step.set_pause_time($4);
+ step.set_step_time($5);
+ page.add_step(step); }
+ ;
+
+angles: angles INT { angles.push_back($2); }
+ | INT { angles.push_back($1); }
+ ;
+%%
+void
+MTN::MTN_Parser::error(const MTN::MTN_Parser::location_type &l,const std::string &err_message )
+{
+ std::cerr << "Error: " << err_message << "\n";
+}
+
+/* include for access to scanner.yylex */
+#include "mtn_file_scanner.hpp"
+
+static int MTN::yylex(MTN::MTN_Parser::semantic_type *yylval,MTN::CMtnFileScanner &scanner,MTN::CMtnFileParser &parser)
+{
+ return(scanner.yylex(yylval));
+}
diff --git a/src/robotis_mtn_file/mtn_file_parser.cpp b/src/robotis_mtn_file/mtn_file_parser.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..1dd6a80fa0f5b6efb7e20412be8f20ac2efb4ce9
--- /dev/null
+++ b/src/robotis_mtn_file/mtn_file_parser.cpp
@@ -0,0 +1,93 @@
+#include <cctype>
+#include <fstream>
+#include <cassert>
+
+#include "mtn_file_parser.hpp"
+#include "mtn_parser.hpp"
+
+MTN::CMtnFileParser::CMtnFileParser()
+{
+ this->scanner=NULL;
+ this->parser=NULL;
+}
+
+MTN::CMtnFileParser::~CMtnFileParser()
+{
+ if(this->scanner!=NULL)
+ {
+ delete this->scanner;
+ this->scanner=NULL;
+ }
+ if(this->parser!=NULL)
+ {
+ delete this->parser;
+ this->parser=NULL;
+ }
+}
+
+void MTN::CMtnFileParser::parse(const char *filename)
+{
+ std::ifstream in_file(filename);
+ if(!in_file.good())
+ exit( EXIT_FAILURE );
+ if(this->scanner!=NULL)
+ delete this->scanner;
+ try
+ {
+ scanner=new MTN::CMtnFileScanner(&in_file);
+ }catch(std::bad_alloc &ba){
+ std::cerr << "Failed to allocate scanner: (" << ba.what() << "), exiting!!\n";
+ exit( EXIT_FAILURE );
+ }
+ if(this->parser!=NULL)
+ delete this->parser;
+ try
+ {
+ parser = new MTN::MTN_Parser((*scanner),(*this));
+ }catch(std::bad_alloc &ba){
+ std::cerr << "Failed to allocate parser: (" << ba.what() << "), exiting!!\n";
+ exit( EXIT_FAILURE );
+ }
+
+ const int accept(0);
+ if(parser->parse()!=accept)
+ {
+ std::cerr << "Parse failed!!\n";
+ }
+}
+
+void MTN::CMtnFileParser::set_type(std::string &type)
+{
+ this->motions.set_type(type);
+}
+
+void MTN::CMtnFileParser::set_version(double version)
+{
+ this->motions.set_version(version);
+}
+
+void MTN::CMtnFileParser::set_num_servos(unsigned int num_servos)
+{
+ this->motions.set_num_servos(num_servos);
+}
+
+void MTN::CMtnFileParser::set_enables(std::vector<bool> &enables)
+{
+ this->motions.set_enables(enables);
+}
+
+void MTN::CMtnFileParser::set_motor_types(std::vector<int> &motor_types)
+{
+ this->motions.set_motor_types(motor_types);
+}
+
+void MTN::CMtnFileParser::add_page(CRobotisPage &page)
+{
+ this->motions.add_page(page);
+}
+
+void MTN::CMtnFileParser::get_motions(CRobotisMtn &motions)
+{
+ motions.clear();
+ motions=this->motions;
+}
diff --git a/src/robotis_mtn_file/mtn_file_parser.hpp b/src/robotis_mtn_file/mtn_file_parser.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..e9ca45cf84f6d5e5cc9e22b632b1b4db5af8d093
--- /dev/null
+++ b/src/robotis_mtn_file/mtn_file_parser.hpp
@@ -0,0 +1,33 @@
+#ifndef _MTN_FILE_PARSER_HPP
+#define _MTN_FILE_PARSER_HPP
+
+#include <string>
+#include "mtn_file_scanner.hpp"
+#include "robotis_mtn.h"
+
+namespace MTN
+{
+ class CMtnFileParser
+ {
+ friend class MTN_Parser;
+ private:
+ MTN::MTN_Parser *parser;
+ MTN::CMtnFileScanner *scanner;
+ CRobotisMtn motions;
+ protected:
+ // motion files functions
+ void set_type(std::string &type);
+ void set_version(double version);
+ void set_num_servos(unsigned int num_servos);
+ void set_enables(std::vector<bool> &enables);
+ void set_motor_types(std::vector<int> &motor_types);
+ void add_page(CRobotisPage &page);
+ public:
+ CMtnFileParser();
+ void parse(const char *filename);
+ virtual ~CMtnFileParser();
+ void get_motions(CRobotisMtn &motions);
+ };
+}
+
+#endif
diff --git a/src/robotis_mtn_file/mtn_file_scanner.hpp b/src/robotis_mtn_file/mtn_file_scanner.hpp
new file mode 100644
index 0000000000000000000000000000000000000000..02f31a54d243c345966f245b86c4029d6559cf72
--- /dev/null
+++ b/src/robotis_mtn_file/mtn_file_scanner.hpp
@@ -0,0 +1,35 @@
+#ifndef _MTN_FILE_SCANNER_HPP
+#define _MTN_FILE_SCANNER_HPP
+
+#if !defined(yyFlexLexerOnce)
+#include <FlexLexer.h>
+#endif
+
+#undef YY_DECL
+#define YY_DECL int MTN::CMtnFileScanner::yylex()
+
+#include "mtn_parser.hpp"
+
+namespace MTN
+{
+ class CMtnFileScanner : public yyFlexLexer
+ {
+ public:
+ CMtnFileScanner(std::istream *in) : yyFlexLexer(in), yylval( NULL )
+ {
+ };
+ int yylex(MTN::MTN_Parser::semantic_type *lval)
+ {
+ yylval = lval;
+ return( yylex() );
+ }
+
+ private:
+ /* hide this one from public view */
+ int yylex();
+ /* yyval ptr */
+ MTN::MTN_Parser::semantic_type *yylval;
+ };
+} /* end namespace MC */
+
+#endif