gistfile1.txt

diff --git a/demo/kakehashi/spine_3/.gitignore b/demo/kakehashi/spine_3/.gitignore
new file mode 100644
index 0000000..2f836aa
--- /dev/null
+++ b/demo/kakehashi/spine_3/.gitignore
@@ -0,0 +1,2 @@
+*~
+*.pyc
diff --git a/demo/kakehashi/spine_3/CMakeLists.txt b/demo/kakehashi/spine_3/CMakeLists.txt
new file mode 100644
index 0000000..80af96e
--- /dev/null
+++ b/demo/kakehashi/spine_3/CMakeLists.txt
@@ -0,0 +1,187 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(spine_3)
+
+## 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 COMPONENTS
+  dynamixel_controllers
+  pr2eus
+)
+
+## 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()
+
+################################################
+## 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 tag for "message_generation"
+##   * 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 dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * 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
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+# generate_messages(
+#   DEPENDENCIES
+#   std_msgs  # Or other packages containing msgs
+# )
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a run_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## 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 spine_3
+#  CATKIN_DEPENDS dynamixel_controllers pr2eus
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(spine_3
+#   src/${PROJECT_NAME}/spine_3.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(spine_3 ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+# add_executable(spine_3_node src/spine_3_node.cpp)
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(spine_3_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(spine_3_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## 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 spine_3 spine_3_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_spine_3.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)
diff --git a/demo/kakehashi/spine_3/config/dynamixel_joint_controllers.yaml b/demo/kakehashi/spine_3/config/dynamixel_joint_controllers.yaml
new file mode 100644
index 0000000..ff0c16b
--- /dev/null
+++ b/demo/kakehashi/spine_3/config/dynamixel_joint_controllers.yaml
@@ -0,0 +1,56 @@
+j1_controller:
+    controller:
+        package: dynamixel_controllers
+        module: joint_position_controller
+        type: JointPositionController
+    joint_name: joint1
+    joint_speed: 2.0
+    motor:
+        id: 1
+        init: 2048
+        min: 0
+        max: 4096
+
+j2_controller:
+    controller:
+        package: dynamixel_controllers
+        module: joint_position_controller
+        type: JointPositionController
+    joint_name: joint2
+    joint_speed: 2.0
+    motor:
+        id: 2
+        init: 2048
+        min: 0
+        max: 4096
+
+j3_controller:
+    controller:
+        package: dynamixel_controllers
+        module: joint_position_controller
+        type: JointPositionController
+    joint_name: joint3
+    joint_speed: 2.0
+    motor:
+        id: 3
+        init: 2048
+        min: 0
+        max: 4096
+
+joint_state_publisher:
+   controller:
+       package: spine_3
+       module: joint_state_publisher
+       type: JointStatePublisher
+
+fullbody_controller:
+   controller:
+       #package: dynamixel_controllers
+       package: spine_3
+       module: joint_trajectory_action_controller
+       type: JointTrajectoryActionController
+   joint_trajectory_action_node:
+       min_velocity: 0.0001
+       constraints:
+           goal_time: 0.0
+
diff --git a/demo/kakehashi/spine_3/euslisp/spine-3-interface.l b/demo/kakehashi/spine_3/euslisp/spine-3-interface.l
new file mode 100644
index 0000000..273136f
--- /dev/null
+++ b/demo/kakehashi/spine_3/euslisp/spine-3-interface.l
@@ -0,0 +1,56 @@
+;;
+
+(require :robot-interface "package://pr2eus/robot-interface.l")
+
+(defclass spine-3-robot
+  :super robot-model
+  :slots (j1 j2 j3))
+(defmethod spine-3-robot
+  (:init
+   (&rest args)
+   (let (l1 l2 l3 l4)
+     ;; link
+     (setq l1 (instance bodyset-link :init (make-cascoords) :bodies (list (make-cube 10 10 10)) :name :link1)
+	   l2 (instance bodyset-link :init (make-cascoords) :bodies (list (make-cube 10 10 10)) :name :link2)
+	   l3 (instance bodyset-link :init (make-cascoords) :bodies (list (make-cube 10 10 10)) :name :link3)
+	   l4 (instance bodyset-link :init (make-cascoords) :bodies (list (make-cube 10 10 10)) :name :link4))
+     ;; joint
+     (setq j1 (instance rotational-joint :init :parent-link l1 :child-link l2 :name "joint1" :min -300 :max 300)
+	   j2 (instance rotational-joint :init :parent-link l2 :child-link l3 :name "joint2" :min -300 :max 300)
+	   j3 (instance rotational-joint :init :parent-link l3 :child-link l4 :name "joint3" :min -300 :max 300))
+     ;;
+     (setq links (list l1 l2 l3 l4))
+     (setq joint-list (list j1 j2 j3))
+     
+     (send self :init-ending)
+     self))
+  (:joint1 (&rest args) (forward-message-to j1 args))
+  (:joint2 (&rest args) (forward-message-to j2 args))
+  (:joint3 (&rest args) (forward-message-to j3 args))
+  )
+
+(defclass spine-3-interface
+  :super robot-interface
+  :slots ())
+(defmethod spine-3-interface
+  (:init (&rest args)
+	 (send-super :init :robot spine-3-robot))
+  (:default-controller
+   ()
+   (list
+    (list
+     (cons :controller-action "fullbody_controller/follow_joint_trajectory")
+     (cons :controller-state "fullbody_controller/state")
+     (cons :action-type control_msgs::FollowJointTrajectoryAction)
+     (cons :joint-names (mapcar #'(lambda (n) (if (symbolp n) (symbol-name n) n)) (send-all (send robot :joint-list) :name))))))
+  );
+
+;;
+#|
+$ roseus spine-3-interface.l
+(setq *ri* (instance spine-3-interface :init))
+(send *ri* :state :potentio-vector)
+(send *ri* :angle-vector #f(0 10 20) 10000)
+(send *ri* :wait-interpolation)
+|#
+
diff --git a/demo/kakehashi/spine_3/launch/dynamixel_bringup.launch b/demo/kakehashi/spine_3/launch/dynamixel_bringup.launch
new file mode 100644
index 0000000..ae6d137
--- /dev/null
+++ b/demo/kakehashi/spine_3/launch/dynamixel_bringup.launch
@@ -0,0 +1,54 @@
+<!-- -*- mode: XML -*- -->
+
+<launch>
+    <node name="dynamixel_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
+        <rosparam>
+            namespace: dxl_manager
+            serial_ports:
+                port:
+                    port_name: "/dev/ttyUSB0"
+                    baud_rate: 57143
+                    min_motor_id: 1
+                    max_motor_id: 3
+                    update_rate: 20
+        </rosparam>
+    </node>
+
+    <!-- Load controller configuration to parameter server -->
+    <rosparam file="$(find spine_3)/config/dynamixel_joint_controllers.yaml" command="load"/>
+
+    <!-- start specified joint controllers -->
+    <node name="dynamixel_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
+          args="--manager=dxl_manager
+                --port=port
+                --type=simple
+                j1_controller
+                j2_controller
+                j3_controller
+                "
+          output="screen"/>
+
+    <!-- start trajectory controllers for fullbody + join_state_publisher -->
+    <node name="dynamixel_trajectory_controller_spawner_for_fullbody" pkg="dynamixel_controllers" type="controller_spawner.py"
+          args="--manager=dxl_manager
+                --port=port
+                --type=meta
+                fullbody_controller
+                j1_controller
+                j2_controller
+                j3_controller
+                "
+          output="screen"/>
+
+    <node name="dynamixel_trajectory_controller_spawner_for_state_publisher" pkg="dynamixel_controllers" type="controller_spawner.py"
+          args="--manager=dxl_manager
+                --port=port
+                --type=meta
+                joint_state_publisher
+                j1_controller
+                j2_controller
+                j3_controller
+                "
+          output="screen"/>
+
+</launch>
diff --git a/demo/kakehashi/spine_3/package.xml b/demo/kakehashi/spine_3/package.xml
new file mode 100644
index 0000000..d015fe2
--- /dev/null
+++ b/demo/kakehashi/spine_3/package.xml
@@ -0,0 +1,56 @@
+<?xml version="1.0"?>
+<package>
+  <name>spine_3</name>
+  <version>0.0.0</version>
+  <description>The spine_3 package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag --> 
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="nicolo@todo.todo">nicolo</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/spine_3</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>dynamixel_controllers</build_depend>
+  <build_depend>pr2eus</build_depend>
+
+  <run_depend>dynamixel_controllers</run_depend>
+  <run_depend>pr2eus</run_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>
diff --git a/demo/kakehashi/spine_3/sample/example.l b/demo/kakehashi/spine_3/sample/example.l
new file mode 100644
index 0000000..bb60f45
--- /dev/null
+++ b/demo/kakehashi/spine_3/sample/example.l
@@ -0,0 +1,18 @@
+(load "package://spine_3/euslisp/spine-3-interface.l")
+(setq *ri* (instance spine-3-interface :init))
+
+(setq av (send *ri* :state :potentio-vector))
+(print (list 'current-position av))
+(print 'send-angle-vector)
+(send *ri* :angle-vector (v+ av #f(90 0 0)) 10000)
+(print 'wait-for-motion)
+(send *ri* :wait-interpolation)
+
+(print 'send-angle-vector)
+(send *ri* :angle-vector (v+ av #f(0 0 0)) 10000)
+(print 'wait-5-sec)
+(unix:sleep 5)
+(print 'send-angle-vector-again)
+(send *ri* :angle-vector (v+ av #f(90 0 0)) 10000)
+
+
diff --git a/demo/kakehashi/spine_3/scripts/scan_ids.py b/demo/kakehashi/spine_3/scripts/scan_ids.py
new file mode 100755
index 0000000..2d63149
--- /dev/null
+++ b/demo/kakehashi/spine_3/scripts/scan_ids.py
@@ -0,0 +1,42 @@
+#!/usr/bin/env python
+# -*- coding: utf-8 -*-
+
+import sys
+from optparse import OptionParser
+
+import roslib
+roslib.load_manifest('dynamixel_driver')
+
+from dynamixel_driver import dynamixel_io
+
+if __name__ == '__main__':
+    parser = OptionParser(usage='Usage: %prog [options]', description='Changes the unique ID of a Dynamixel servo motor.')
+    parser.add_option('-p', '--port', metavar='PORT', default='/dev/ttyUSB0',
+                      help='motors of specified controllers are connected to PORT [default: %default]')
+    parser.add_option('-b', '--baud', metavar='BAUD', type="int", default=1000000,
+                      help='connection to serial port will be established at BAUD bps [default: %default]')
+    parser.add_option('-f', '--from-id', metavar='FROM_ID', type="int", default=1,
+                      help='from id [default: %default]')
+    parser.add_option('-t', '--to-id', metavar='TO_ID', type="int", default=7,
+                      help='to id [default: %default]')
+                      
+    (options, args) = parser.parse_args(sys.argv)
+    
+    if len(args) < 1:
+        parser.print_help()
+        exit(1)
+        
+    port = options.port
+    baudrate = options.baud
+    
+    try:
+        dxl_io = dynamixel_io.DynamixelIO(port, baudrate)
+    except dynamixel_io.SerialOpenError, soe:
+        print 'ERROR:', soe
+    else:
+        for idx in map(lambda x : x + options.from_id , range(options.to_id - options.from_id + 1)):
+            print 'Scanning %d...' %(idx),
+            if dxl_io.ping(idx):
+                print 'The motor %d respond to a ping' %(idx)
+            else:
+                print 'ERROR: The specified motor did not respond to id %d.' % idx
diff --git a/demo/kakehashi/spine_3/setup.py b/demo/kakehashi/spine_3/setup.py
new file mode 100644
index 0000000..12e01f1
--- /dev/null
+++ b/demo/kakehashi/spine_3/setup.py
@@ -0,0 +1,14 @@
+#!/usr/bin/env python
+
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['spine_3'],
+    package_dir={'': 'src'},
+    )
+
+setup(**d)
+
+
+
diff --git a/demo/kakehashi/spine_3/src/spine_3/__init__.py b/demo/kakehashi/spine_3/src/spine_3/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/demo/kakehashi/spine_3/src/spine_3/joint_state_publisher.py b/demo/kakehashi/spine_3/src/spine_3/joint_state_publisher.py
new file mode 100644
index 0000000..d3aceea
--- /dev/null
+++ b/demo/kakehashi/spine_3/src/spine_3/joint_state_publisher.py
@@ -0,0 +1,117 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2015, Kei Okada.
+# All rights reserved.
+#
+# 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 Kei Okada 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.
+
+### https://github.com/arebgun/dynamixel_motor/pull/27
+from threading import Thread
+
+import rospy
+
+from sensor_msgs.msg import JointState
+
+class JointStatePublisher():
+    def __init__(self, controller_namespace, controllers):
+        self.update_rate = 1000
+        self.state_update_rate = 50
+        self.trajectory = []
+        
+        self.controller_namespace = controller_namespace
+        self.joint_names = [c.joint_name for c in controllers]
+
+        self.joint_to_controller = {}
+        for c in controllers:
+             self.joint_to_controller[c.joint_name] = c
+            
+        self.port_to_joints = {}
+        for c in controllers:
+            if c.port_namespace not in self.port_to_joints: self.port_to_joints[c.port_namespace] = []
+            self.port_to_joints[c.port_namespace].append(c.joint_name)
+            
+        self.port_to_io = {}
+        for c in controllers:
+            if c.port_namespace in self.port_to_io: continue
+            self.port_to_io[c.port_namespace] = c.dxl_io
+
+    def initialize(self):
+        self.msg = JointState()
+        return True
+
+
+    def start(self):
+        self.running = True
+        self.state_pub = rospy.Publisher('joint_states', JointState , queue_size=100)
+
+        Thread(target=self.update_state).start()
+
+
+    def stop(self):
+        self.running = False
+
+    def update_state(self):
+        rate = rospy.Rate(self.state_update_rate)
+        while self.running and not rospy.is_shutdown():
+            self.msg.header.stamp = rospy.Time.now()
+            self.msg.name = []
+            self.msg.position = []
+            self.msg.velocity = []
+            self.msg.effort = []
+            
+            for port, joints in self.port_to_joints.items():
+                vals = []
+                for joint in joints:
+                    j = self.joint_names.index(joint)
+                    
+                    motor_id = self.joint_to_controller[joint].motor_id
+                    co = self.joint_to_controller[joint]
+                    io = self.port_to_io[port]
+
+                    self.msg.name.append(joint)
+                    po = ve = ef = 0
+                    try:
+                        ret = io.get_feedback(motor_id)
+                        po = self.raw_to_rad_pos(ret['position'],co)
+                        ve = self.raw_to_rad_spd(ret['speed'],co)
+                        ef = self.raw_to_rad_spd(ret['load'],co)
+                    except Exception as e:
+                        rospy.logerr(e)
+                    self.msg.position.append(po)
+                    self.msg.velocity.append(ve)
+                    self.msg.effort.append(ef)
+            
+            self.state_pub.publish(self.msg)
+            rate.sleep()
+
+    def raw_to_rad_pos(self, raw, c):
+        return (c.initial_position_raw - raw if c.flipped else raw - c.initial_position_raw) * c.RADIANS_PER_ENCODER_TICK
+    def raw_to_rad_spd(self, raw, c):
+        return (- raw if c.flipped else raw ) * c.RADIANS_PER_ENCODER_TICK
diff --git a/demo/kakehashi/spine_3/src/spine_3/joint_trajectory_action_controller.py b/demo/kakehashi/spine_3/src/spine_3/joint_trajectory_action_controller.py
new file mode 100644
index 0000000..c0c35f3
--- /dev/null
+++ b/demo/kakehashi/spine_3/src/spine_3/joint_trajectory_action_controller.py
@@ -0,0 +1,345 @@
+# -*- coding: utf-8 -*-
+#
+# Software License Agreement (BSD License)
+#
+# Copyright (c) 2010-2011, Antons Rebguns.
+# All rights reserved.
+#
+# 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 University of Arizona 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.
+
+from __future__ import division
+
+
+__author__ = 'Antons Rebguns'
+__copyright__ = 'Copyright (c) 2010-2011 Antons Rebguns'
+
+__license__ = 'BSD'
+__maintainer__ = 'Antons Rebguns'
+__email__ = 'anton@email.arizona.edu'
+
+
+from threading import Thread
+
+import rospy
+import actionlib
+
+from std_msgs.msg import Float64
+from trajectory_msgs.msg import JointTrajectory
+from control_msgs.msg import FollowJointTrajectoryAction
+from control_msgs.msg import FollowJointTrajectoryFeedback
+from control_msgs.msg import FollowJointTrajectoryResult
+
+
+class Segment():
+    def __init__(self, num_joints):
+        self.start_time = 0.0                   # trajectory segment start time
+        self.duration = 0.0                     # trajectory segment duration
+        self.positions = [0.0] * num_joints
+        self.velocities = [0.0] * num_joints
+
+class JointTrajectoryActionController():
+    def __init__(self, controller_namespace, controllers):
+        self.update_rate = 1000
+        self.state_update_rate = 50
+        self.trajectory = []
+        
+        self.controller_namespace = controller_namespace
+        self.joint_names = [c.joint_name for c in controllers]
+        
+        self.joint_to_controller = {}
+        for c in controllers:
+            self.joint_to_controller[c.joint_name] = c
+            
+        self.port_to_joints = {}
+        for c in controllers:
+            if c.port_namespace not in self.port_to_joints: self.port_to_joints[c.port_namespace] = []
+            self.port_to_joints[c.port_namespace].append(c.joint_name)
+            
+        self.port_to_io = {}
+        for c in controllers:
+            if c.port_namespace in self.port_to_io: continue
+            self.port_to_io[c.port_namespace] = c.dxl_io
+            
+        self.joint_states = dict(zip(self.joint_names, [c.joint_state for c in controllers]))
+        self.num_joints = len(self.joint_names)
+        self.joint_to_idx = dict(zip(self.joint_names, range(self.num_joints)))
+
+    def initialize(self):
+        ns = self.controller_namespace + '/joint_trajectory_action_node/constraints'
+        self.goal_time_constraint = rospy.get_param(ns + '/goal_time', 0.0)
+        self.stopped_velocity_tolerance = rospy.get_param(ns + '/stopped_velocity_tolerance', 0.01)
+        self.goal_constraints = []
+        self.trajectory_constraints = []
+        self.min_velocity = rospy.get_param(self.controller_namespace + '/joint_trajectory_action_node/min_velocity', 0.1)
+        
+        for joint in self.joint_names:
+            self.goal_constraints.append(rospy.get_param(ns + '/' + joint + '/goal', -1.0))
+            self.trajectory_constraints.append(rospy.get_param(ns + '/' + joint + '/trajectory', -1.0))
+            
+        # Message containing current state for all controlled joints
+        self.msg = FollowJointTrajectoryFeedback()
+        self.msg.joint_names = self.joint_names
+        self.msg.desired.positions = [0.0] * self.num_joints
+        self.msg.desired.velocities = [0.0] * self.num_joints
+        self.msg.desired.accelerations = [0.0] * self.num_joints
+        self.msg.actual.positions = [0.0] * self.num_joints
+        self.msg.actual.velocities = [0.0] * self.num_joints
+        self.msg.error.positions = [0.0] * self.num_joints
+        self.msg.error.velocities = [0.0] * self.num_joints
+        
+        return True
+
+
+    def start(self):
+        self.running = True
+        
+        self.command_sub = rospy.Subscriber(self.controller_namespace + '/command', JointTrajectory, self.process_command)
+        self.state_pub = rospy.Publisher(self.controller_namespace + '/state', FollowJointTrajectoryFeedback)
+        self.action_server = actionlib.SimpleActionServer(self.controller_namespace + '/follow_joint_trajectory',
+                                                          FollowJointTrajectoryAction,
+                                                          execute_cb=self.process_follow_trajectory,
+                                                          auto_start=False)
+        self.action_server.start()
+        Thread(target=self.update_state).start()
+
+    def stop(self):
+        self.running = False
+
+    def process_command(self, msg):
+        if self.action_server.is_active(): self.action_server.set_preempted()
+        
+        while self.action_server.is_active():
+            sleep(0.01)
+            
+        self.process_trajectory(msg)
+
+    def process_follow_trajectory(self, goal):
+        self.process_trajectory(goal.trajectory)
+
+    def process_trajectory(self, traj):
+        num_points = len(traj.points)
+        
+        # make sure the joints in the goal match the joints of the controller
+        if set(self.joint_names) != set(traj.joint_names):
+            res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.INVALID_JOINTS)
+            msg = 'Incoming trajectory joints do not match the joints of the controller'
+            rospy.logerr(msg)
+            self.action_server.set_aborted(result=res, text=msg)
+            return
+            
+        # make sure trajectory is not empty
+        if num_points == 0:
+            msg = 'Incoming trajectory is empty'
+            rospy.logerr(msg)
+            self.action_server.set_aborted(text=msg)
+            return
+            
+        # correlate the joints we're commanding to the joints in the message
+        # map from an index of joint in the controller to an index in the trajectory
+        lookup = [traj.joint_names.index(joint) for joint in self.joint_names]
+        durations  = [0.0] * num_points
+        
+        # find out the duration of each segment in the trajectory
+        durations[0] = traj.points[0].time_from_start.to_sec()
+        
+        for i in range(1, num_points):
+            durations[i] = (traj.points[i].time_from_start - traj.points[i-1].time_from_start).to_sec()
+            
+        if not traj.points[0].positions:
+            res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.INVALID_GOAL)
+            msg = 'First point of trajectory has no positions'
+            rospy.logerr(msg)
+            self.action_server.set_aborted(result=res, text=msg)
+            return
+            
+        trajectory = []
+        time = rospy.Time.now() + rospy.Duration(0.01)
+        
+        for i in range(num_points):
+            seg = Segment(self.num_joints)
+            
+            if traj.header.stamp == rospy.Time(0.0):
+                seg.start_time = (time + traj.points[i].time_from_start).to_sec() - durations[i]
+            else:
+                seg.start_time = (traj.header.stamp + traj.points[i].time_from_start).to_sec() - durations[i]
+                
+            seg.duration = durations[i]
+            
+            # Checks that the incoming segment has the right number of elements.
+            if traj.points[i].velocities and len(traj.points[i].velocities) != self.num_joints:
+                res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.INVALID_GOAL)
+                msg = 'Command point %d has %d elements for the velocities' % (i, len(traj.points[i].velocities))
+                rospy.logerr(msg)
+                self.action_server.set_aborted(result=res, text=msg)
+                return
+                
+            if len(traj.points[i].positions) != self.num_joints:
+                res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.INVALID_GOAL)
+                msg = 'Command point %d has %d elements for the positions' % (i, len(traj.points[i].positions))
+                rospy.logerr(msg)
+                self.action_server.set_aborted(result=res, text=msg)
+                return
+                
+            for j in range(self.num_joints):
+                if traj.points[i].velocities:
+                    seg.velocities[j] = traj.points[i].velocities[lookup[j]]
+                if traj.points[i].positions:
+                    seg.positions[j] = traj.points[i].positions[lookup[j]]
+                    
+            trajectory.append(seg)
+            
+        rospy.loginfo('Trajectory start requested at %.3lf, waiting...', traj.header.stamp.to_sec())
+        rate = rospy.Rate(self.update_rate)
+        
+        while traj.header.stamp > time:
+            time = rospy.Time.now()
+            rate.sleep()
+            
+        end_time = traj.header.stamp + rospy.Duration(sum(durations))
+        seg_end_times = [rospy.Time.from_sec(trajectory[seg].start_time + durations[seg]) for seg in range(len(trajectory))]
+        
+        rospy.loginfo('Trajectory start time is %.3lf, end time is %.3lf, total duration is %.3lf', time.to_sec(), end_time.to_sec(), sum(durations))
+        
+        self.trajectory = trajectory
+        traj_start_time = rospy.Time.now()
+        
+        for seg in range(len(trajectory)):
+            rospy.logdebug('current segment is %d time left %f cur time %f' % (seg, durations[seg] - (time.to_sec() - trajectory[seg].start_time), time.to_sec()))
+            rospy.logdebug('goal positions are: %s' % str(trajectory[seg].positions))
+            
+            # first point in trajectories calculated by OMPL is current position with duration of 0 seconds, skip it
+            if durations[seg] == 0:
+                rospy.logdebug('skipping segment %d with duration of 0 seconds' % seg)
+                continue
+                
+            multi_packet = {}
+            
+            for port,joints in self.port_to_joints.items():
+                vals = []
+                
+                for joint in joints:
+                    j = self.joint_names.index(joint)
+                    
+                    start_position = self.joint_states[joint].current_pos
+                    if seg != 0: start_position = trajectory[seg-1].positions[j]
+                        
+                    desired_position = trajectory[seg].positions[j]
+                    desired_velocity = max(self.min_velocity, abs(desired_position - start_position) / durations[seg])
+                    
+                    self.msg.desired.positions[j] = desired_position
+                    self.msg.desired.velocities[j] = desired_velocity
+                    
+                    motor_id = self.joint_to_controller[joint].motor_id
+                    pos = self.joint_to_controller[joint].pos_rad_to_raw(desired_position)
+                    spd = self.joint_to_controller[joint].spd_rad_to_raw(desired_velocity)
+                    
+                    vals.append((motor_id,pos,spd))
+                    
+                multi_packet[port] = vals
+                
+            for port,vals in multi_packet.items():
+                self.port_to_io[port].set_multi_position_and_speed(vals)
+                
+            while time < seg_end_times[seg]:
+                # check if new trajectory was received, if so abort current trajectory execution
+                # by setting the goal to the current position
+                if self.action_server.is_preempt_requested():
+                    msg = 'New trajectory received. Aborting old trajectory.'
+                    multi_packet = {}
+                    
+                    for port,joints in self.port_to_joints.items():
+                        vals = []
+                        
+                        for joint in joints:
+                            cur_pos = self.joint_states[joint].current_pos
+                            
+                            motor_id = self.joint_to_controller[joint].motor_id
+                            pos = self.joint_to_controller[joint].pos_rad_to_raw(cur_pos)
+                            
+                            vals.append((motor_id,pos))
+                            
+                        multi_packet[port] = vals
+                        
+                    for port,vals in multi_packet.items():
+                        self.port_to_io[port].set_multi_position(vals)
+                        
+                    self.action_server.set_preempted(text=msg)
+                    rospy.logwarn(msg)
+                    return
+                    
+                rate.sleep()
+                time = rospy.Time.now()
+                
+            # Verifies trajectory constraints
+            for j, joint in enumerate(self.joint_names):
+                if self.trajectory_constraints[j] > 0 and self.msg.error.positions[j] > self.trajectory_constraints[j]:
+                    res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.PATH_TOLERANCE_VIOLATED)
+                    msg = 'Unsatisfied position constraint for %s, trajectory point %d, %f is larger than %f' % \
+                           (joint, seg, self.msg.error.positions[j], self.trajectory_constraints[j])
+                    rospy.logwarn(msg)
+                    self.action_server.set_aborted(result=res, text=msg)
+                    return
+                    
+        # let motors roll for specified amount of time
+        rospy.sleep(self.goal_time_constraint)
+        
+        for i, j in enumerate(self.joint_names):
+            rospy.logdebug('desired pos was %f, actual pos is %f, error is %f' % (trajectory[-1].positions[i], self.joint_states[j].current_pos, self.joint_states[j].current_pos - trajectory[-1].positions[i]))
+            
+        # Checks that we have ended inside the goal constraints
+        for (joint, pos_error, pos_constraint) in zip(self.joint_names, self.msg.error.positions, self.goal_constraints):
+            if pos_constraint > 0 and abs(pos_error) > pos_constraint:
+                res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.GOAL_TOLERANCE_VIOLATED, "")
+                msg = 'Aborting because %s joint wound up outside the goal constraints, %f is larger than %f' % \
+                      (joint, pos_error, pos_constraint)
+                rospy.logwarn(msg)
+                self.action_server.set_aborted(result=res, text=msg)
+                break
+        else:
+            print("SUCC"< FollowJointTrajectoryResult.SUCCESSFUL)
+            res = FollowJointTrajectoryResult(FollowJointTrajectoryResult.SUCCESSFUL, "")
+            msg = 'Trajectory execution successfully completed'
+            rospy.loginfo(msg)
+            self.action_server.set_succeeded(result=res, text=msg)
+
+    def update_state(self):
+        rate = rospy.Rate(self.state_update_rate)
+        while self.running and not rospy.is_shutdown():
+            self.msg.header.stamp = rospy.Time.now()
+            
+            # Publish current joint state
+            for i, joint in enumerate(self.joint_names):
+                state = self.joint_states[joint]
+                self.msg.actual.positions[i] = state.current_pos
+                self.msg.actual.velocities[i] = abs(state.velocity)
+                self.msg.error.positions[i] = self.msg.actual.positions[i] - self.msg.desired.positions[i]
+                self.msg.error.velocities[i] = self.msg.actual.velocities[i] - self.msg.desired.velocities[i]
+                
+            self.state_pub.publish(self.msg)
+            rate.sleep()
+