Added an improved version of the darwin controller.

darwin_controller/CMakeLists.txt

+cmake_minimum_required(VERSION 2.8.3)
+project(darwin_controller)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED cmake_modules roscpp urdf controller_interface hardware_interface controller_manager xacro actionlib_msgs message_generation iri_action_server)
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+FIND_PACKAGE(robotis_mtn REQUIRED)
+FIND_PACKAGE(robotis_mtn_parser REQUIRED)
+FIND_PACKAGE(robotis_bin_parser REQUIRED)
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependencies might have been
+##     pulled in transitively but can be declared for certainty nonetheless:
+##     * add a build_depend tag for "message_generation"
+##     * add a run_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+add_action_files(
+  FILES
+  darwin_motion.action
+#   Action1.action
+#   Action2.action
+)
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+  DEPENDENCIES
+  std_msgs actionlib_msgs # Or other packages containing msgs
+)
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+ INCLUDE_DIRS include 
+ LIBRARIES ${PROJECT_NAME} 
+ CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager xacro message_runtime actionlib_msgs iri_action_server
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(include ${Boost_INCLUDE_DIR} ${catkin_INCLUDE_DIRS})
+INCLUDE_DIRECTORIES(${robotis_mtn_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${robotis_mtn_parser_INCLUDE_DIR})
+INCLUDE_DIRECTORIES(${robotis_bin_parser_INCLUDE_DIR})
+
+## Declare a cpp library
+# add_library(darwin_controller
+#   src/${PROJECT_NAME}/darwin_controller.cpp
+# )
+add_library(${PROJECT_NAME} src/darwin_controller.cpp
+                            src/action_process.cpp
+                            include/darwin_controller.h
+                            include/darwin_controller_impl.h)
+
+
+## Declare a cpp executable
+# add_executable(darwin_controller_node src/darwin_controller_node.cpp)
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(darwin_controller darwin_controller_generate_messages_cpp)
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(darwin_controller_node
+#   ${catkin_LIBRARIES}
+# )
+target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${robotis_mtn_LIBRARY})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${robotis_mtn_parser_LIBRARY})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${robotis_bin_parser_LIBRARY})
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+# install(PROGRAMS
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables and/or libraries for installation
+# install(TARGETS darwin_controller darwin_controller_node
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_darwin_controller.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

darwin_controller/action/darwin_motion.action

+#goal definition
+int32   motion_id
+---
+#result definition
+bool    successful
+---
+#feedback
+int32   current_page
+int32   current_step

darwin_controller/include/action_process.h

+#ifndef _ACTION_PROCESS_H
+#define _ACTION_PROCESS_H
+
+unsigned char action_load_page(unsigned char page_id);
+void action_start(void);
+void action_stop(void);
+unsigned char action_is_running(void);
+unsigned char action_process(void);
+void action_init(void);
+
+#endif

darwin_controller/include/darwin_controller.h

+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+//   * Redistributions of source code must retain the above copyright notice,
+//     this list of conditions and the following disclaimer.
+//   * Redistributions in binary form must reproduce the above copyright
+//     notice, this list of conditions and the following disclaimer in the
+//     documentation and/or other materials provided with the distribution.
+//   * Neither the name of PAL Robotics S.L. nor the names of its
+//     contributors may be used to endorse or promote products derived from
+//     this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+/// \author Adolfo Rodriguez Tsouroukdissian, Stuart Glaser
+
+#ifndef BIOLOID_CONTROLLER_H
+#define BIOLOID_CONTROLLER_H
+
+// C++ standard
+#include <cassert>
+#include <iterator>
+#include <stdexcept>
+#include <string>
+
+// Boost
+#include <boost/shared_ptr.hpp>
+#include <boost/scoped_ptr.hpp>
+
+// ROS
+#include <ros/node_handle.h>
+#include "std_msgs/Int32.h"
+#include "sensor_msgs/Imu.h"
+
+// URDF
+#include <urdf/model.h>
+
+// ros_controls
+#include <controller_interface/controller.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <control_toolbox/pid.h>
+
+#include <iri_action_server/iri_action_server.h>
+#include <darwin_controller/darwin_motionAction.h>
+
+// darwin motion files parsers
+#include "mtn_file_parser.hpp"
+#include "robotis_bin_parser.h"
+#include "mtn_exceptions.h"
+
+namespace darwin_controller
+{
+  /**
+   * \brief 
+   *
+   */
+  template <class HardwareInterface>
+  class DarwinController : public controller_interface::Controller<HardwareInterface>
+  {
+    public:
+
+      DarwinController();
+      ~DarwinController();
+
+      bool init(HardwareInterface* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);
+      void starting(const ros::Time& time);
+      void stopping(const ros::Time& time);
+      void update(const ros::Time& time, const ros::Duration& period);
+
+      // action subscriber
+      IriActionServer<darwin_controller::darwin_motionAction> *motion_action_aserver_;
+      void motion_action_startCallback(const darwin_controller::darwin_motionGoalConstPtr& goal);
+      void motion_action_stopCallback(void);
+      bool motion_action_isFinishedCallback(void);
+      bool motion_action_hasSucceedCallback(void);
+      void motion_action_getResultCallback(darwin_controller::darwin_motionResultPtr& result);
+      void motion_action_getFeedbackCallback(darwin_controller::darwin_motionFeedbackPtr& feedback);
+
+      ros::Subscriber imu_sub;
+      void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
+
+    private:
+      typedef typename HardwareInterface::ResourceHandleType JointHandle;
+      typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
+
+      std::string name_;               ///< Controller name.
+      std::vector<JointHandle> joints_;             ///< Handles to controlled joints.
+      std::vector<std::string> joint_names_;        ///< Controlled joint names.
+      std::vector<PidPtr> pids_;
+
+      double balance_pelvis_roll_left;
+      double balance_pelvis_roll_right;
+      double balance_ankle_roll_left;
+      double balance_ankle_roll_right;
+
+      double balance_ankle_pitch_left;
+      double balance_ankle_pitch_right;
+      double balance_knee_left;
+      double balance_knee_right;
+
+      // ROS API
+      ros::NodeHandle controller_nh_;
+  };
+
+} // namespace
+
+#include <darwin_controller_impl.h>
+
+#endif // header guard

darwin_controller/include/darwin_controller_impl.h

+#ifndef DARWIN_CONTROLLER_IMP_H
+#define DARWIN_CONTROLLER_IMP_H
+
+#include "action_process.h"
+
+extern CRobotisMtn motions;
+extern long long int current_angles[31];
+const double initial_angles[31]={-81.1523,80.8594,-68.2517,67.9688,-14.6484,14.3555,-45.1172,45.1172,-1.4648,1.1719,-50.0977,49.8047,-79.6875,79.3945,39.5508,-39.8438,-1.4648,1.1719,};
+
+namespace darwin_controller
+{
+  namespace internal
+  {
+    std::vector<std::string> getStrings(const ros::NodeHandle& nh, const std::string& param_name)
+    {
+      using namespace XmlRpc;
+      XmlRpcValue xml_array;
+      if (!nh.getParam(param_name, xml_array))
+      {
+	ROS_ERROR_STREAM("Could not find '" << param_name << "' parameter (namespace: " << nh.getNamespace() << ").");
+	return std::vector<std::string>();
+      }
+      if (xml_array.getType() != XmlRpcValue::TypeArray)
+      {
+	ROS_ERROR_STREAM("The '" << param_name << "' parameter is not an array (namespace: " <<
+	    nh.getNamespace() << ").");
+	return std::vector<std::string>();
+      }
+
+      std::vector<std::string> out;
+      for (int i = 0; i < xml_array.size(); ++i)
+      {
+	if (xml_array[i].getType() != XmlRpcValue::TypeString)
+	{
+	  ROS_ERROR_STREAM("The '" << param_name << "' parameter contains a non-string element (namespace: " <<
+	      nh.getNamespace() << ").");
+	  return std::vector<std::string>();
+	}
+	out.push_back(static_cast<std::string>(xml_array[i]));
+      }
+      return out;
+    }
+
+    boost::shared_ptr<urdf::Model> getUrdf(const ros::NodeHandle& nh, const std::string& param_name)
+    {
+      boost::shared_ptr<urdf::Model> urdf(new urdf::Model);
+
+      std::string urdf_str;
+      // Check for robot_description in proper namespace
+      if (nh.getParam(param_name, urdf_str))
+      {
+	if (!urdf->initString(urdf_str))
+	{
+	  ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter (namespace: " <<
+	      nh.getNamespace() << ").");
+	  return boost::shared_ptr<urdf::Model>();
+	}
+      }
+      // Check for robot_description in root
+      else if (!urdf->initParam("robot_description"))
+      {
+	ROS_ERROR_STREAM("Failed to parse URDF contained in '" << param_name << "' parameter");
+	return boost::shared_ptr<urdf::Model>();
+      }
+      return urdf;
+    }
+
+    typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
+    std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
+    {
+      std::vector<UrdfJointConstPtr> out;
+      for (unsigned int i = 0; i < joint_names.size(); ++i)
+      {
+	UrdfJointConstPtr urdf_joint = urdf.getJoint(joint_names[i]);
+	if (urdf_joint)
+	{
+	  out.push_back(urdf_joint);
+	}
+	else
+	{
+	  ROS_ERROR_STREAM("Could not find joint '" << joint_names[i] << "' in URDF model.");
+	  return std::vector<UrdfJointConstPtr>();
+	}
+      }
+      return out;
+    }
+
+    std::string getLeafNamespace(const ros::NodeHandle& nh)
+    {
+      const std::string complete_ns = nh.getNamespace();
+      std::size_t id   = complete_ns.find_last_of("/");
+      return complete_ns.substr(id + 1);
+    }
+
+  } // namespace
+
+  template <class HardwareInterface>
+    inline void DarwinController<HardwareInterface>::starting(const ros::Time& time)
+    {
+      for(unsigned int i = 0; i <this->joints_.size(); ++i)
+      {
+        current_angles[i] = (joints_[i].getPosition()*180.0/3.14159)*65536.0;
+        pids_[i]->reset();
+        this->joints_[i].setCommand(0.0);
+      }
+    }
+
+  template <class HardwareInterface>
+    inline void DarwinController<HardwareInterface>::stopping(const ros::Time& time)
+    {
+    }
+
+  template <class HardwareInterface>
+    DarwinController<HardwareInterface>::DarwinController()
+    {
+      this->motion_action_aserver_=NULL;
+    }
+
+  template <class HardwareInterface>
+    bool DarwinController<HardwareInterface>::init(HardwareInterface* hw,ros::NodeHandle&   root_nh,ros::NodeHandle&   controller_nh)
+    {
+      using namespace internal;
+
+      // Cache controller node handle
+      controller_nh_ = controller_nh;
+
+      this->motion_action_aserver_=new IriActionServer<darwin_controller::darwin_motionAction>(root_nh,"motion_action");
+      this->motion_action_aserver_->registerStartCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_startCallback, this, _1));
+      this->motion_action_aserver_->registerStopCallback(boost::bind(& DarwinController<HardwareInterface>::motion_action_stopCallback, this));
+      this->motion_action_aserver_->registerIsFinishedCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_isFinishedCallback, this));
+      this->motion_action_aserver_->registerHasSucceedCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_hasSucceedCallback, this));
+      this->motion_action_aserver_->registerGetResultCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_getResultCallback, this, _1));
+      this->motion_action_aserver_->registerGetFeedbackCallback(boost::bind(&DarwinController<HardwareInterface>::motion_action_getFeedbackCallback, this, _1));
+      this->motion_action_aserver_->start();
+
+      // Controller name
+      name_ = getLeafNamespace(controller_nh_);
+
+      // Action status checking update rate
+      double action_monitor_rate = 20.0;
+      controller_nh_.getParam("action_monitor_rate", action_monitor_rate);
+
+      // List of controlled joints
+      joint_names_ = getStrings(controller_nh_, "joints");
+      if (joint_names_.empty()) {return false;}
+      const unsigned int n_joints = joint_names_.size();
+ 
+      // URDF joints
+      boost::shared_ptr<urdf::Model> urdf = getUrdf(root_nh, "robot_description");
+      if (!urdf) {return false;}
+
+      std::vector<UrdfJointConstPtr> urdf_joints = getUrdfJoints(*urdf, joint_names_);
+      if (urdf_joints.empty()) {return false;}
+      assert(n_joints == urdf_joints.size());
+
+      // Initialize members
+      this->joints_.resize(n_joints);
+      this->pids_.resize(n_joints);
+      for (unsigned int i = 0; i < n_joints; ++i)
+      {
+	// Joint handle
+	try {joints_[i] = hw->getHandle(joint_names_[i]);}
+	catch (...)
+	{
+	  ROS_ERROR_STREAM_NAMED(name_, "Could not find joint '" << joint_names_[i] << "' in '" <<
+	      this->getHardwareInterfaceType() << "'.");
+	  return false;
+	}
+        // Node handle to PID gains
+        ros::NodeHandle joint_nh(this->controller_nh_, std::string("gains/") + joint_names_[i]);
+
+        // Init PID gains from ROS parameter server
+        this->pids_[i].reset(new control_toolbox::Pid());
+        if (!this->pids_[i]->init(joint_nh))
+        {
+          ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
+          return false;
+        }
+
+	// Whether a joint is continuous (ie. has angle wraparound)
+	const std::string not_if = urdf_joints[i]->type == urdf::Joint::CONTINUOUS ? "" : "non-";
+
+	ROS_DEBUG_STREAM_NAMED(name_, "Found " << not_if << "continuous joint '" << joint_names_[i] << "' in '" << this->getHardwareInterfaceType() << "'.");
+      }
+
+      ROS_DEBUG_STREAM_NAMED(name_, "Initialized controller '" << name_ << "' with:" <<
+	  "\n- Number of joints: " << joints_.size() <<
+	  "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");
+
+      // load the motion file
+      std::string motion_file = "";
+      ros::param::get("motion_file", motion_file);
+
+      try{
+        std::string servo_type="AX-12A";
+        CBinFileParser bin_parser;
+        MTN::CMtnFileParser mtn_parser;
+        if(motion_file.find(".mtn")!=std::string::npos)
+        {
+          mtn_parser.parse(motion_file.c_str());
+          mtn_parser.get_motions(motions);
+        }
+        else if(motion_file.find(".bin")!=std::string::npos)
+        {
+          bin_parser.parse(motion_file.c_str());
+          bin_parser.get_motions(motions);
+        }
+        else
+        {
+          ROS_ERROR_STREAM_NAMED(name_,"Invalid file format. Supported formats are .mtn and .bin.");
+          return false;
+        }
+        std::cout << "Number of servos: " << motions.get_num_servos() << std::endl;
+        for(unsigned int i=0;i<motions.get_num_servos();i++)
+          motions.set_motor_type(i,servo_type);
+        // initialize action module
+        action_init();
+      }catch(CMtnException &e){
+        std::cout << e.what() << std::endl;
+      }catch(...){
+        std::cout << "unknown exception" << std::endl;
+      }
+
+      // ros communications
+      this->imu_sub = controller_nh.subscribe("raw_imu", 1, &DarwinController<HardwareInterface>::imu_callback,this);
+
+      return true;
+    }
+
+  template <class HardwareInterface>
+    void DarwinController<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
+    {
+      std::vector<double> angles(joints_.size());
+      std::vector<double> target_angles(joints_.size());
+
+      action_process();
+      for (unsigned int i = 0; i < this->joints_.size(); ++i)
+      {
+        angles[i]=joints_[i].getPosition();
+        target_angles[i] = (((current_angles[i+1])/65536.0)*3.14159)/180.0;
+      }
+/*      target_angles[8]+=balance_pelvis_roll_right;
+      target_angles[9]+=balance_pelvis_roll_left;
+      target_angles[16]+=balance_ankle_roll_right;
+      target_angles[17]+=balance_ankle_roll_left;
+
+      target_angles[12]+=balance_knee_right;
+      target_angles[13]+=balance_knee_left;
+      target_angles[14]+=balance_ankle_pitch_right;
+      target_angles[15]+=balance_ankle_pitch_left;*/
+
+      for (unsigned int i = 0; i < this->joints_.size(); ++i)
+      {
+        const double command = pids_[i]->computeCommand(target_angles[i]-angles[i],period);
+        this->joints_[i].setCommand(command);
+      }
+    }
+
+  template <class HardwareInterface>
+    void DarwinController<HardwareInterface>::motion_action_startCallback(const darwin_controller::darwin_motionGoalConstPtr& goal)
+    {
+      std::cout << "Start action" << std::endl;
+      try{
+        /* load a page and start execution */
+        if(!action_load_page(goal->motion_id))
+          std::cout << "Impossible to load the desired page" << std::endl;
+        else
+        {
+          std::cout << "Page loaded successfully" << std::endl;
+          action_start();
+        }
+      }catch(CMtnException &e){
+        std::cout << e.what() << std::endl;
+      }
+    }
+     
+  template <class HardwareInterface>
+    void DarwinController<HardwareInterface>::motion_action_stopCallback(void)
+    {
+      std::cout << "Stop action" << std::endl;
+      action_stop();
+    }
+     
+  template <class HardwareInterface>
+    bool DarwinController<HardwareInterface>::motion_action_isFinishedCallback(void)
+    {
+      if(action_is_running())
+        return false;
+      else
+        return true;
+    }
+
+  template <class HardwareInterface>
+    bool DarwinController<HardwareInterface>::motion_action_hasSucceedCallback(void)
+    {
+      return true;
+    }
+     
+  template <class HardwareInterface>
+    void DarwinController<HardwareInterface>::motion_action_getResultCallback(darwin_controller::darwin_motionResultPtr& result)
+    {
+    }
+  
+  template <class HardwareInterface>
+    void DarwinController<HardwareInterface>::motion_action_getFeedbackCallback(darwin_controller::darwin_motionFeedbackPtr& feedback)
+    {
+    }
+
+  template <class HardwareInterface>
+    void DarwinController<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
+    {
+      double x_error=-msg->angular_velocity.x;
+      double y_error=-msg->angular_velocity.z;
+      double gain=3.0;
+
+      balance_pelvis_roll_left=gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/40.0)*300)/1024)*3.14159)/180.0;
+      balance_pelvis_roll_right=gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/40.0)*300)/1024)*3.14159)/180.0;
+      balance_ankle_roll_left=-gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/20.0)*300)/1024)*3.14159)/180.0;
+      balance_ankle_roll_right=-gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/20.0)*300)/1024)*3.14159)/180.0;
+
+      balance_ankle_pitch_left=-gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/18.0)*300)/1024)*3.14159)/180.0;
+      balance_ankle_pitch_right=gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/18.0)*300)/1024)*3.14159)/180.0;
+      balance_knee_left=-gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/54.0)*300)/1024)*3.14159)/180.0;
+      balance_knee_right=gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/54.0)*300)/1024)*3.14159)/180.0;
+
+//      std::cout << balance_pelvis_roll_left << "," << balance_pelvis_roll_right << "," << balance_ankle_roll_left << "," << balance_ankle_roll_right << std::endl;
+//      std::cout << balance_ankle_pitch_left << "," << balance_ankle_pitch_right << "," << balance_knee_left << "," << balance_knee_right << std::endl;
+
+    }
+
+  template <class HardwareInterface>
+    DarwinController<HardwareInterface>::~DarwinController()
+    {
+      if(this->motion_action_aserver_!=NULL)
+      {
+        delete this->motion_action_aserver_;
+        this->motion_action_aserver_=NULL;
+      }
+    }
+
+} // namespace
+
+#endif // header guard

darwin_controller/package.xml

+<?xml version="1.0"?>
+<package>
+  <name>darwin_controller</name>
+  <version>0.0.0</version>
+  <description>The darwin_controller package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="sergi@todo.todo">sergi</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but mutiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/bioloid_controller</url> -->
+
+
+  <!-- Author tags are optional, mutiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintianers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">Jane Doe</author> -->
+
+
+  <!-- The *_depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use run_depend for packages you need at runtime: -->
+  <!--   <run_depend>message_runtime</run_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>cmake_modules</build_depend>
+  <build_depend>roscpp</build_depend>
+  <build_depend>urdf</build_depend>
+  <build_depend>iri_action_server</build_depend>
+  <build_depend>message_generation</build_depend>
+  <build_depend>actionlib_msgs</build_depend>
+
+  <build_depend>controller_interface</build_depend>
+  <build_depend>hardware_interface</build_depend>
+
+  <build_depend>controller_manager</build_depend> <!--Tests-only!-->
+  <build_depend>xacro</build_depend>              <!--Tests-only!-->
+
+  <run_depend>roscpp</run_depend>
+  <run_depend>urdf</run_depend>
+  <run_depend>iri_action_server</run_depend>
+  <run_depend>message_runtime</run_depend>
+  <run_depend>actionlib_msgs</run_depend>
+
+  <run_depend>controller_interface</run_depend>
+  <run_depend>hardware_interface</run_depend>
+
+  <run_depend>controller_manager</run_depend> <!--Tests-only!-->
+  <run_depend>xacro</run_depend>              <!--Tests-only!-->
+
+  <export>
+    <controller_interface plugin="${prefix}/ros_control_plugins.xml"/>
+  </export>
+</package>

darwin_controller/ros_control_plugins.xml

+<library path="lib/libdarwin_controller">
+
+  <class name="effort_controllers/DarwinController"
+         type="effort_controllers::DarwinController"
+         base_class_type="controller_interface::ControllerBase">
+    <description>
+      this is a description
+    </description>
+  </class>
+
+</library>
+

darwin_controller/src/action_process.cpp

+#include "mtn_file_parser.hpp"
+#include "robotis_bin_parser.h"
+#include "mtn_exceptions.h"
+#include "string.h"
+#include "math.h"
+
+typedef enum {ACTION_PRE,ACTION_MAIN,ACTION_POST,ACTION_PAUSE} action_states;
+
+// global variables
+CRobotisMtn motions;
+CRobotisPage next_page;
+CRobotisPage current_page;
+unsigned char current_step_index;
+// angle variables
+long long int moving_angles[32];// fixed point 48|16 format
+long long int current_angles[32];// fixed point 48|16 format
+long long int start_angles[32];// fixed point 48|16 format
+// speed variables
+long long int start_speed[32];// fixed point 48|16 format
+long long int main_speed[32];// fixed point 48|16 format
+// control variables
+bool end_action,stop_action;
+bool zero_speed_finish[32];
+unsigned char next_index;
+bool action_running;
+// time variables (in time units (7.8 ms each time unit))
+long long int total_time;// fixed point 48|16 format
+long long int pre_time;// fixed point 48|16 format
+long long int main_time;// fixed point 48|16 format
+long long int step_time_num;// fixed point 48|16 format
+long long int pause_time_num;// fixed point 48|16 format
+long long int current_time_num;// fixed point 48|16 format
+long long int section_time_num;// fixed point 48|16 format
+long long int STEP_TIME=0x000001FF;
+
+// private functions
+void action_load_next_step(void)
+{
+  // page and step information
+  static unsigned char current_index=0;
+  static CRobotisStep current_step,next_step;
+  static char num_repetitions=0;
+  // angle variables
+  short int next_angle;// information from the flash memory (fixed point 9|7 format)
+  long long int max_angle;// fixed point format 48|16 format
+  long long int tmp_angle;// fixed point format 48|16 format
+  long long int angle;// fixed point format 48|16 format
+  long long int target_angles;// fixed point 48|16 format
+  long long int next_angles;// fixed point 48|16 format
+  long long int pre_time_initial_speed_angle;// fixed point 48|16 format
+  long long int moving_angle;// fixed point 48|16 format
+  // time variables
+  long long int accel_time_num;// fixed point 48|16 format
+  long long int zero_speed_divider;// fixed point 48|16 format
+  long long int non_zero_speed_divider;// fixed point 48|16 format
+  // control variables
+  bool dir_change;
+  int schedule=0x0A;
+  // control information
+  unsigned int i=0;
+  TServoInfo info;
+
+  current_step_index++;
+  if(current_step_index>=current_page.get_num_steps())// the last step has been executed
+  {
+    if(current_page.get_num_steps()==1)
+    {
+      if(stop_action)
+      {
+        if(current_page.get_exit_page()==0)
+          next_index=current_page.get_exit_page();
+        else
+          next_index=current_page.get_exit_page()-1;
+      }
+      else
+      {
+        num_repetitions--;
+        if(num_repetitions>0)
+          next_index=current_index;
+        else
+        {
+          if(current_page.get_next_page()==0)
+            next_index=current_page.get_next_page();
+          else
+            next_index=current_page.get_next_page()-1;
+        }
+      }
+      if(next_index==0)
+        end_action=true;
+      else
+      {
+        if(next_index!=current_index)
+          next_page=motions.get_page(next_index);
+        if(next_page.get_repetitions()==0 || next_page.get_num_steps()==0)
+          end_action=true;
+      }
+    }
+    current_page=next_page;// make the next page active
+    if(current_index!=next_index)
+      num_repetitions=current_page.get_repetitions();
+    current_step_index=0;
+    current_index=next_index;
+  }
+  else if(current_step_index==current_page.get_num_steps()-1)// it is the last step
+  {
+    if(stop_action)
+    {
+      if(current_page.get_exit_page()==0)
+	next_index=current_page.get_exit_page();
+      else
+	next_index=current_page.get_exit_page()-1;
+    }
+    else
+    {
+      num_repetitions--;
+      if(num_repetitions>0)
+	next_index=current_index;
+      else
+      {
+	if(current_page.get_next_page()==0)
+	  next_index=current_page.get_next_page();
+	else
+	  next_index=current_page.get_next_page()-1;
+      }
+    }
+    if(next_index==0)
+      end_action=true;
+    else
+    {
+      if(next_index!=current_index)
+	next_page=motions.get_page(next_index);
+      if(next_page.get_repetitions()==0 || next_page.get_num_steps()==0)
+	end_action=true;
+    }
+  }
+  // compute trajectory
+  current_step=current_page.get_step(current_step_index);
+  pause_time_num=((((unsigned short int)(current_step.get_pause_time()/0.0078))<<21)/((unsigned char)(current_page.get_speed_rate()*32)<<7));
+  step_time_num=((((unsigned short int)(current_step.get_step_time()/0.0078))<<21)/((unsigned char)(current_page.get_speed_rate()*32)<<7));
+  if(step_time_num<STEP_TIME)
+    step_time_num=STEP_TIME;// 0.078 in 16|16 format
+  if(current_step_index==current_page.get_num_steps()-1)
+    next_step=next_page.get_step(0);
+  else
+    next_step=current_page.get_step(current_step_index+1);
+  max_angle=0;
+  for(i=0;i<motions.get_num_servos();i++)
+  {
+    angle=motions.get_angle(i,current_index,current_step_index)*128;
+    if(angle==0xFFFF)
+      target_angles=current_angles[i];
+    else
+      target_angles=angle<<9;
+    moving_angles[i]=target_angles-current_angles[i];
+    if(end_action)
+      next_angles=target_angles;
+    else
+    {
+      if(current_step_index==current_page.get_num_steps()-1)
+	next_angle=motions.get_angle(i,next_index,0)*128;
+      else
+	next_angle=motions.get_angle(i,current_index,current_step_index+1)*128;
+      if(next_angle==0xFFFF)
+	next_angles=target_angles;
+      else
+	next_angles=next_angle<<9;
+    }
+    // check changes in direction
+    if(((current_angles[i] < target_angles) && (target_angles < next_angles)) ||
+	((current_angles[i] > target_angles) && (target_angles > next_angles)))
+      dir_change=false;
+    else
+      dir_change=true;
+    // check the type of ending
+    if(dir_change || pause_time_num>0 || end_action)
+      zero_speed_finish[i]=true;
+    else
+      zero_speed_finish[i]=false;
+    // find the maximum angle of motion in speed base schedule
+    if(schedule==0x00)// speed base schedule
+    {
+      // fer el valor absolut
+      if(moving_angles[i]<0)
+	tmp_angle=-moving_angles[i];
+      else
+	tmp_angle=moving_angles[i];
+      if(tmp_angle>max_angle)
+	max_angle=tmp_angle;
+    }
+  }
+  if(schedule==0x00)
+    step_time_num=(max_angle*256)/(step_time_num*720);
+  accel_time_num=current_page.get_inertial();
+  accel_time_num=accel_time_num<<9;
+  if(step_time_num<=(accel_time_num<<1))
+  {
+    if(step_time_num==0)
+      accel_time_num=0;
+    else
+    {
+      accel_time_num=(step_time_num-STEP_TIME)>>1;
+      if(accel_time_num==0)
+	step_time_num=0;
+    }
+  }
+  total_time=step_time_num;
+  pre_time=accel_time_num;
+  main_time=total_time-pre_time;
+  non_zero_speed_divider=(pre_time>>1)+main_time;
+  if(non_zero_speed_divider<STEP_TIME)
+    non_zero_speed_divider=STEP_TIME;
+  zero_speed_divider=main_time;
+  if(zero_speed_divider<STEP_TIME)
+    zero_speed_divider=STEP_TIME;
+  for(i=0;i<motions.get_num_servos();i++)
+  {
+    info=motions.get_motor_info(i);
+    pre_time_initial_speed_angle=(start_speed[i]*pre_time)>>1;
+    moving_angle=moving_angles[i]<<16;
+    if(zero_speed_finish[i])
+      main_speed[i]=(moving_angle-pre_time_initial_speed_angle)/zero_speed_divider;
+    else
+      main_speed[i]=(moving_angle-pre_time_initial_speed_angle)/non_zero_speed_divider;
+//    if((main_speed[i]>>16)>info.max_speed)
+//      main_speed[i]=(info.max_speed*65536);
+//    else if((main_speed[i]>>16)<-info.max_speed)
+//      main_speed[i]=-(info.max_speed*65536);
+  }
+}
+
+// public functions
+unsigned char action_load_page(unsigned char page_id)
+{
+  next_index=page_id; 
+  if(next_index<0 || next_index>255)
+  {
+    std::cout << "Invalid page identifier" << std::endl;
+    return 0x00;
+  }
+  current_page=motions.get_page(next_index);
+  if(current_page.get_next_page()==0)
+    next_page=motions.get_page(current_page.get_next_page());
+  else
+    next_page=motions.get_page(current_page.get_next_page()-1);
+  
+  return 0x01;
+}
+
+void action_start(void)
+{
+  unsigned char i;
+
+  for(i=0;i<32;i++)
+    start_angles[i]=current_angles[i];
+
+  action_running=true;
+  stop_action=false;
+  current_time_num=0;
+  section_time_num=0;
+  current_step_index=-1;
+}
+
+void action_stop(void)
+{
+  stop_action=true;
+}
+
+unsigned char action_is_running(void)
+{
+  return action_running;
+}
+
+unsigned char action_process(void)
+{
+  unsigned int i;
+  // angle variables 
+  static long long int accel_angles[32];// fixed point 48|16 format
+  static long long int main_angles[32];// fixed point 48|16 format
+  // speed variables (in controller units)
+  static long long int current_speed[32]={0};// fixed point 48|16 format
+  long long int delta_speed;
+  // control variables 
+  static action_states state=ACTION_PAUSE;
+
+  if(action_running)
+  {
+    switch(state)
+    {
+      case ACTION_PRE: current_time_num+=STEP_TIME;
+		       if(current_time_num>section_time_num)
+		       {
+			 for(i=0;i<motions.get_num_servos();i++)
+			 {
+			   /* the last segment of the pre section */
+			   delta_speed=(main_speed[i]-start_speed[i]);
+			   current_speed[i]=start_speed[i]+delta_speed;
+			   accel_angles[i]=(start_speed[i]*section_time_num+((delta_speed*section_time_num)>>1))>>16;
+			   current_angles[i]=start_angles[i]+accel_angles[i];
+			   /* update of the state */
+			   if(!zero_speed_finish[i])
+			   {
+			     if((step_time_num-pre_time)==0)
+			       main_angles[i]=0;
+			     else
+			       main_angles[i]=((moving_angles[i]-accel_angles[i])*(step_time_num-(pre_time<<1)))/(step_time_num-pre_time);
+			     start_angles[i]=current_angles[i];
+			   }
+			   else
+			   {
+			     main_angles[i]=moving_angles[i]-accel_angles[i]-(((main_speed[i]*pre_time)>>1)>>16);
+			     start_angles[i]=current_angles[i];
+			   }
+			   /* the first step of the main section */
+			   current_angles[i]=start_angles[i]+(main_angles[i]*(current_time_num-section_time_num))/(step_time_num-(pre_time<<1));
+			   current_speed[i]=main_speed[i];
+			 }
+			 current_time_num=(current_time_num-section_time_num);
+			 section_time_num=step_time_num-(pre_time<<1);
+			 state=ACTION_MAIN;
+		       }
+		       else
+		       {
+			 for(i=0;i<motions.get_num_servos();i++)
+			 {
+			   delta_speed=((main_speed[i]-start_speed[i])*current_time_num)/section_time_num;
+			   current_speed[i]=start_speed[i]+delta_speed;
+			   accel_angles[i]=((start_speed[i]*current_time_num)+((delta_speed*current_time_num)>>1))>>16;
+			   current_angles[i]=start_angles[i]+accel_angles[i];
+			 }
+			 state=ACTION_PRE;
+		       }
+		       break;
+      case ACTION_MAIN: current_time_num+=STEP_TIME;
+			if(current_time_num>section_time_num)
+			{
+			  for(i=0;i<motions.get_num_servos();i++)
+			  {
+			    /* last segment of the main section */
+			    current_angles[i]=start_angles[i]+main_angles[i];
+			    current_speed[i]=main_speed[i];
+			    /* update state */
+			    start_angles[i]=current_angles[i];
+			    main_angles[i]=moving_angles[i]-main_angles[i]-accel_angles[i];
+			    /* first segment of the post section */
+			    if(zero_speed_finish[i])
+			    {
+			      delta_speed=((0.0-main_speed[i])*(current_time_num-section_time_num))/pre_time;
+			      current_speed[i]=main_speed[i]+delta_speed;
+			      current_angles[i]=start_angles[i]+(((main_speed[i]*(current_time_num-section_time_num))+((delta_speed*(current_time_num-section_time_num))>>1))>>16);
+			    }
+			    else
+			    {
+			      current_angles[i]=start_angles[i]+((main_angles[i]*(current_time_num-section_time_num))/pre_time);
+			      current_speed[i]=main_speed[i];
+			    }
+			  }
+			  current_time_num=current_time_num-section_time_num;
+			  section_time_num=pre_time;
+			  state=ACTION_POST;
+			}
+			else
+			{
+			  for(i=0;i<motions.get_num_servos();i++)
+			  {
+			    current_angles[i]=start_angles[i]+(main_angles[i]*current_time_num)/section_time_num;
+			    current_speed[i]=main_speed[i];
+			  }
+			  state=ACTION_MAIN;
+			}
+			break;
+      case ACTION_POST: current_time_num+=STEP_TIME;
+			if(current_time_num>section_time_num)
+			{
+			  for(i=0;i<motions.get_num_servos();i++)
+			  {
+			    /* last segment of the post section */
+			    if(zero_speed_finish[i])
+			    {
+			      delta_speed=-main_speed[i];
+			      current_speed[i]=main_speed[i]+delta_speed;
+			      current_angles[i]=start_angles[i]+(((main_speed[i]*section_time_num)+((delta_speed*section_time_num)>>1))>>16);
+			    }
+			    else
+			    {
+			      current_angles[i]=start_angles[i]+main_angles[i];
+			      current_speed[i]=main_speed[i];
+			    }
+			    /* update state */
+			    start_angles[i]=current_angles[i];
+			    start_speed[i]=current_speed[i];
+			  }
+			  /* load the next step */
+			  if(pause_time_num==0)
+			  {
+			    if(end_action)
+                            {
+                              state=ACTION_PAUSE;
+			      action_running=false;
+                              end_action=false;
+                            }
+			    else
+			    {
+			      action_load_next_step();
+			      /* first step of the next section */
+			      for(i=0;i<motions.get_num_servos();i++)
+			      {
+				delta_speed=((main_speed[i]-start_speed[i])*(current_time_num-section_time_num))/pre_time;
+				current_speed[i]=start_speed[i]+delta_speed;
+				accel_angles[i]=(((start_speed[i]*(current_time_num-section_time_num))+((delta_speed*(current_time_num-section_time_num))>>1))>>16);
+				current_angles[i]=start_angles[i]+accel_angles[i];
+			      }
+			      current_time_num=current_time_num-section_time_num;
+			      section_time_num=pre_time;
+			      state=ACTION_PRE;
+			    }
+			  }
+			  else
+			  {
+			    current_time_num=current_time_num-section_time_num;
+			    section_time_num=pause_time_num;
+			    state=ACTION_PAUSE;
+			  }
+			}
+			else
+			{
+			  for(i=0;i<motions.get_num_servos();i++)
+			  {
+			    if(zero_speed_finish[i])
+			    {
+			      delta_speed=((0.0-main_speed[i])*current_time_num)/section_time_num;
+			      current_speed[i]=main_speed[i]+delta_speed;
+			      current_angles[i]=start_angles[i]+(((main_speed[i]*current_time_num)+((delta_speed*current_time_num)>>1))>>16);
+			    }
+			    else
+			    {
+			      current_angles[i]=start_angles[i]+(main_angles[i]*current_time_num)/section_time_num;
+			      current_speed[i]=main_speed[i];
+			    }
+			  }
+			  state=ACTION_POST;
+			}
+			break;
+      case ACTION_PAUSE: current_time_num+=STEP_TIME;
+			 if(current_time_num>section_time_num)
+			 {
+			   if(end_action)
+                           {
+			     action_running=false;
+                             state=ACTION_PAUSE; 
+                             end_action=false;
+                           }
+			   else
+			   {
+			     // find next page to execute
+			     action_load_next_step();
+			     /* first step of the next section */
+			     for(i=0;i<motions.get_num_servos();i++)
+			     {
+			       delta_speed=((main_speed[i]-start_speed[i])*(current_time_num-section_time_num))/pre_time;
+			       current_speed[i]=start_speed[i]+delta_speed;
+			       accel_angles[i]=(((start_speed[i]*(current_time_num-section_time_num))+((delta_speed*(current_time_num-section_time_num))>>1))>>16);
+			       current_angles[i]=start_angles[i]+accel_angles[i];
+			     }
+			     current_time_num=current_time_num-section_time_num;
+			     section_time_num=pre_time;
+			     state=ACTION_PRE;
+			   }
+			 }
+			 else// pause section still active
+			   state=ACTION_PAUSE;
+			 break;
+      default: break;
+    }
+  }
+
+  return action_running;
+}
+
+void action_init(void)
+{
+  unsigned char i;  
+  TServoInfo info;
+
+  // init current_angles with the current position of the servos
+  for(i=0;i<32;i++)
+  { 
+    info=motions.get_motor_info(i);
+    // angle variables
+    moving_angles[i]=0;// fixed point 48|16 format
+    current_angles[i]=0;// middle of range
+    start_angles[i]=current_angles[i];
+    // speed variables
+    start_speed[i]=0;// fixed point 48|16 format
+    main_speed[i]=0;// fixed point 48|16 format
+    // control variables
+    zero_speed_finish[i]=false;
+  }
+  action_running=0;
+  // clear the contents of the next page
+  next_page.clear();
+  // control variables
+  end_action=false;
+  stop_action=false;
+  action_running=false;
+  // time variables (in time units (7.8 ms each time unit))
+  total_time=0;// fixed point 48|16 format
+  pre_time=0;// fixed point 48|16 format
+  main_time=0;// fixed point 48|16 format
+  step_time_num=0;// fixed point 48|16 format
+  pause_time_num=0;// fixed point 48|16 format
+  current_time_num=0;// fixed point 48|16 format
+  section_time_num=0;// fixed point 48|16 format
+}
+
+int main(int argc,char *argv[])
+{
+  MTN::CMtnFileParser mtn_parser;
+  CBinFileParser bin_parser;
+  TServoInfo info;
+  bool action_on=false;
+  unsigned int i,j,num;
+  double time=0;
+  std::string servo_type="AX-12A";
+
+  // start process
+  if(argc!=3)
+  {
+    std::cout << "A motion file (.mtn) and the motron to be executed must be provided" << std::endl;
+    return (EXIT_FAILURE);
+  }
+  else
+  {
+    try{
+      if(strstr(argv[1],".mtn")!=NULL)
+      {
+	mtn_parser.parse(argv[1]);
+	mtn_parser.get_motions(motions);
+      }
+      else if(strstr(argv[1],".bin")!=NULL)
+      {
+        bin_parser.parse(argv[1]);
+        bin_parser.get_motions(motions);
+      }
+      else
+      {
+	std::cout << "Invalid file format. Supported formats are .mtn and .bin." << std::endl;
+	return (EXIT_FAILURE);
+      }
+      for(i=0;i<motions.get_num_servos();i++)
+        motions.set_motor_type(i,servo_type);
+      for(i=0;i<2;i++)
+      {
+	// initialize variables
+        if(!action_load_page(atoi(argv[2])))
+	  return (EXIT_FAILURE);        
+	// state machine
+        action_start();
+        num=0;
+	do{
+	  action_on=action_process();
+	  time+=0.0078;
+	  for(j=0;j<motions.get_num_servos();j++)
+	  {
+	    info=motions.get_motor_info(j);
+	    std::cout << (unsigned short)((((((double)current_angles[j])/65536.0)+info.center_angle)*info.encoder_resolution)/info.max_angle) << ",";
+	  }
+	  std::cout << time << std::endl;
+          num++;
+          if(num==5000)
+            action_stop();
+	}while(action_on);
+      }
+    }catch(CMtnException &e){
+      std::cout << e.what() << std::endl;
+    }
+  }
+}

darwin_controller/src/darwin_controller.cpp

+///////////////////////////////////////////////////////////////////////////////
+// Copyright (C) 2013, PAL Robotics S.L.
+// Copyright (c) 2008, Willow Garage, Inc.
+//
+// Redistribution and use in source and binary forms, with or without
+// modification, are permitted provided that the following conditions are met:
+// * Redistributions of source code must retain the above copyright notice,
+// this list of conditions and the following disclaimer.
+// * Redistributions in binary form must reproduce the above copyright
+// notice, this list of conditions and the following disclaimer in the
+// documentation and/or other materials provided with the distribution.
+// * Neither the name of PAL Robotics S.L. nor the names of its
+// contributors may be used to endorse or promote products derived from
+// this software without specific prior written permission.
+//
+// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+// POSSIBILITY OF SUCH DAMAGE.
+//////////////////////////////////////////////////////////////////////////////
+
+// Pluginlib
+#include <pluginlib/class_list_macros.h>
+
+// Project
+#include <darwin_controller.h>
+
+namespace effort_controllers
+{
+  /**
+   * \brief Joint trajectory controller that represents trajectory segments as <b>quintic splines</b> and sends
+   * commands to a \b position interface.
+   */
+  typedef darwin_controller::DarwinController<hardware_interface::EffortJointInterface> DarwinController;
+}
+
+PLUGINLIB_EXPORT_CLASS(effort_controllers::DarwinController, controller_interface::ControllerBase)