Dual Dynamixel controller with moveit [closed]

edit

Hello,

I am working on PhantomX Reactor Robot Arm which is 5 dof and using dynamixel motors. I am trying to control the arm using moveit. However I have a problem when I try to run joint with dual motor I get this error appeare in the shell with controller_manager

    [ERROR] [WallTime: 1469691515.740486] Exception in your execute callback: JointPositionControllerDual instance has no attribute 'motor_id'
Traceback (most recent call last):
  File "/opt/ros/indigo/lib/python2.7/dist-packages/actionlib/simple_action_server.py", line 291, in executeLoop
    self.execute_callback(goal)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/dynamixel_controllers/joint_trajectory_action_controller.py", line 142, in process_follow_trajectory
    self.process_trajectory(goal.trajectory)
  File "/opt/ros/indigo/lib/python2.7/dist-packages/dynamixel_controllers/joint_trajectory_action_controller.py", line 257, in process_trajectory
    motor_id = self.joint_to_controller[joint].motor_id
AttributeError: JointPositionControllerDual instance has no attribute 'motor_id'

I try to run the arm with a single motor and it working fine just I got this problem when I run dual motors joints.

this is the controller manager

<launch>
  <node name="fda_controller_manager" pkg="dynamixel_controllers" type="controller_manager.py" required="true" output="screen">
    <rosparam>
        namespace: fda_manager
        serial_ports:
            port_0:
                port_name: "/dev/ttyUSB0"
                baud_rate: 1000000
                min_motor_id: 1
                max_motor_id: 25
                update_rate: 20
    </rosparam>
  </node>
</launch>

and this is my start_controller

<launch>
  <rosparam file="$(find fda_controllers)/config/fda_controller.yaml" command="load"/>
  <node name="fda_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
        args="--manager=fda_manager
              --port port_0
              --type=simple
        joint1_position_controller
                joint2_position_controller      
        joint3_position_controller
        joint4_position_controller
        joint5_position_controller"
        output="screen"/>


  <node name="fda_action_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
        args="--manager=fda_manager
              --type=meta
                fda_joint_trajectory_action_controller
        joint1_position_controller
                joint2_position_controller      
        joint3_position_controller
        joint4_position_controller
        joint5_position_controller"
        output="screen"/>
</launch>

this is may script

#!/usr/bin/env python
import roslib
roslib.load_manifest('fda_controllers')

import rospy
import actionlib
from std_msgs.msg import Float64
import trajectory_msgs.msg 
import control_msgs.msg  
from trajectory_msgs.msg import JointTrajectoryPoint
from control_msgs.msg import JointTrajectoryAction, JointTrajectoryGoal, FollowJointTrajectoryAction, FollowJointTrajectoryGoal



class Joint:
        def __init__(self, motor_name):
            #arm_name should be b_arm or f_arm
            self.name = motor_name           
            self.jta = actionlib.SimpleActionClient('fda_joint_trajectory_action_controller', FollowJointTrajectoryAction)
            rospy.loginfo('Waiting for joint trajectory action')
            self.jta.wait_for_server()
            rospy.loginfo('Found joint trajectory action!')


        def move_joint(self, angles):
            goal = FollowJointTrajectoryGoal()                  
            char = self.name[0] #either 'f' or 'b'
            goal.trajectory.joint_names = [char+'_joint',char+'_joint',char+'_joint',char+'_joint',char+'_joint']
            point = JointTrajectoryPoint()
            point.positions = angles
            point.time_from_start = rospy.Duration(6)                   
            goal.trajectory.points.append(point)
            self.jta.send_goal_and_wait(goal)


def main():
            arm = Joint('f_arm')
            arm.move_joint([0.5,1.5,1.0])
            arm.move_joint([6.28,3.14,6.28])


if __name__ == '__main__':
      rospy.init_node('joint_position_tester')
      main()

Thank you