HOW TO program in python the Dynamixel motor for XM430-W350-T multiturn task to several goal positions, and control the speed and acceleration in ROS

Hello everyone.
I hope You are doing well these days.

I’m student and beginner in the work of the Dynamixel motor through ROS and ROS itself.
I am using the U2D2 converter.Ubuntu 18.04. Python 2.7.
Earlier I have made nodes of server and client and controlled the positon of goal position by applying one number. I would like now into one node apply to two goal postions in one time running nodes. For example, before my node rotated from 0 degree to 30 (or any other I applied to) , and now i want to make node of multi turn rotation like from 0 to 30 to 90 degree through the ROS communication between client and server. Here is question: How to do this?
Below here are nodes I have made:
The main node:

import os
import rospy
from std_msgs.msg import String
from motor_control.msg import Motor
from motor_control.srv import Motor2


if os.name == 'nt':
    import msvcrt
    def getch():
        return msvcrt.getch().decode()
else:
    import sys, tty, termios
    fd = sys.stdin.fileno()
    old_settings = termios.tcgetattr(fd)
    def getch():
        try:
            tty.setraw(sys.stdin.fileno())
            ch = sys.stdin.read(1)
        finally:
            termios.tcsetattr(fd, termios.TCSADRAIN, old_settings)
        return ch

from dynamixel_sdk import *                    # Uses Dynamixel SDK library


class DynamixelController(object):
    def __init__(self):
        print("1")

        self.motor_sub = rospy.Subscriber("/motor_topic", Motor, self.motor_callback)
        self.test_motor_client(1)                #here we can put request order as 1 for the using id=14 and position= 1500; or as 2 is id same and position is 2500. The cases could be found in the server.py node
    
    def motor_callback(self, data):
        _id = data.id
        _position = data.position

        print("_id : {0}, _position : {1}".format(_id, _position))
        self.read_write(_id, _position)

    def test_motor_client(self, request):
        print("3")
        rospy.wait_for_service('test_motor_service')
        try:
            proxy = rospy.ServiceProxy('test_motor_service', Motor2)
            response = proxy(request)
            print("response is id : {} position : {}".format(response.id, response.position))

            self.read_write(response.id, response.position)

        except rospy.ServiceException as e:
            print("Service call failed: %s"%e)
    
    def read_write(self, _id, _position):
        print("Start the node")
        print(_position)

        _degreeposition = (0.088 * (_position))  #Degree per pulse multiple _position
        _degreeposition = int(_degreeposition)
		#print(_degreeposition)
        print(type(_degreeposition))
       
        # Control table address
        ADDR_PRO_TORQUE_ENABLE      = 64               # Control table address is different in Dynamixel model
        ADDR_PRO_GOAL_POSITION      = 116
        ADDR_PRO_PRESENT_POSITION   = 132
        #print(type(ADDR_PRO_GOAL_POSITION))
        # Protocol version
        PROTOCOL_VERSION            = 2.0               # See which protocol version is used in the Dynamixel

        # Default setting
        DXL_ID                      = _id                 # Dynamixel ID : 14
        BAUDRATE                    = 1000000             # Dynamixel default baudrate : 57600
        DEVICENAME                  = '/dev/ttyUSB0'    # Check which port is being used on your controller
                                                        # ex) Windows: "COM1"   Linux: "/dev/ttyUSB0" Mac: "/dev/tty.usbserial-*"

        TORQUE_ENABLE               = 1                 # Value for enabling the torque
        TORQUE_DISABLE              = 0                 # Value for disabling the torque
        DXL_MINIMUM_POSITION_VALUE  = 10           # Dynamixel will rotate between this value
        DXL_MAXIMUM_POSITION_VALUE  = _position            # and this value (note that the Dynamixel would not move when the position value is out of movable range. Check e-manual about the range of the Dynamixel you use.)
        DXL_MOVING_STATUS_THRESHOLD = 20                # Dynamixel moving status threshold

        index = 0
        dxl_goal_position = [DXL_MINIMUM_POSITION_VALUE, DXL_MAXIMUM_POSITION_VALUE]         # Goal position


        # Initialize PortHandler instance
        # Set the port path
        # Get methods and members of PortHandlerLinux or PortHandlerWindows
        portHandler = PortHandler(DEVICENAME)

        # Initialize PacketHandler instance
        # Set the protocol version
        # Get methods and members of Protocol1PacketHandler or Protocol2PacketHandler
        packetHandler = PacketHandler(PROTOCOL_VERSION)

        # Open port
        if portHandler.openPort():
            print("Succeeded to open the port")
        else:
            print("Failed to open the port")
            print("Press any key to terminate...")
            getch()
            quit()


        # Set port baudrate
        if portHandler.setBaudRate(BAUDRATE):
            print("Succeeded to change the baudrate")
        else:
            print("Failed to change the baudrate")
            print("Press any key to terminate...")
            getch()
            quit()

        # Enable Dynamixel Torque
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_ENABLE)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))
        else:
            print("Dynamixel has been successfully connected")

        while 1:
            print("Press any key to continue! (or press ESC to quit!)")
            if getch() == chr(0x1b):
                break

            # Write goal position
            dxl_comm_result, dxl_error = packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_GOAL_POSITION, dxl_goal_position[index])
            if dxl_comm_result != COMM_SUCCESS:
                print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
            elif dxl_error != 0:
                print("%s" % packetHandler.getRxPacketError(dxl_error))

            while 1:
                # Read present position
                dxl_present_position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRO_PRESENT_POSITION)
                if dxl_comm_result != COMM_SUCCESS:
                    print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
                elif dxl_error != 0:
                    print("%s" % packetHandler.getRxPacketError(dxl_error))

                print("[ID:%03d] GoalPos:%03d  PresPos:%03d" % (DXL_ID, dxl_goal_position[index], dxl_present_position))

                if not abs(dxl_goal_position[index] - dxl_present_position) > DXL_MOVING_STATUS_THRESHOLD:
                    break

            # Change goal position
            if index == 0:
                index = 1
            else:
                index = 0


        # Disable Dynamixel Torque
        dxl_comm_result, dxl_error = packetHandler.write1ByteTxRx(portHandler, DXL_ID, ADDR_PRO_TORQUE_ENABLE, TORQUE_DISABLE)
        if dxl_comm_result != COMM_SUCCESS:
            print("%s" % packetHandler.getTxRxResult(dxl_comm_result))
        elif dxl_error != 0:
            print("%s" % packetHandler.getRxPacketError(dxl_error))

        # Close port
        portHandler.closePort()
if __name__ == '__main__':
    rospy.init_node('read_write', anonymous=True)
    controller = DynamixelController()
    #controller.read_write(14, 1500)

The server node:

#!/usr/bin/env python
import rospy
from motor_control.srv import Motor2, Motor2Response


def motor_info(req):
    print("receive the request : {}".format(req.order))
    if req.order == 1:
        _id = 14
        #_position = 1000
        _position = 30                  # change it degree value
        _position = (11.4 * (_position))
		#_position = int(_position)   # here is using there 11.4 the number which helps rotate the motor in the correct degree; We get the number encoder number/degree we want to rotate the motor.    
    elif req.order == 2:
        _id = 14
        #_position = 2047.5
        _positiondegree = 90
        _position = (11.4 * (_positiondegree))
       

    print("Response to this request id : {} and postiion : {}".format(_id, _position))
    return Motor2Response(_id, _position)
   

    
def motor_info_server():

    rospy.init_node('test_motor_service')
    s = rospy.Service('test_motor_service', Motor2, motor_info)
    print("Ready to response some id and position")

    # spin() simply keeps python from exiting until this node is stopped
    rospy.spin()

if __name__ == '__main__':
    motor_info_server()

And, another question is how to apply the velocity and acceleration control parameters into these nodes?

I will appreciate any help you could give me.
Thank you for your time and help.
I wish you have a good day :slight_smile:

Hi @AnastasiyaRybakova,

If you are trying to send different Goal Positions, I’d create a msg or srv that can deliver the desired Goal Position along with a message either using Topic or Service.
Here’s an instruction video that describes how to create a topic and service that sends and receives data using ROS1 + DYNAMIXEL SDK.

When your DYNAMIXEL is configured as Position Control Mode, the Profile Acceleration and Profile Velocity controls the acceleration/deceleration and the maximum velocity respectively.

You can simply write values to these acceleration and velocity before sending a Goal Position.

For example, setting Profile Acceleration = 10 and Profile Velocity = 50 will generate a trapezoidal trajectory that accelerates and decelerates smoothly.

1 Like

thank you very much dear for the video and information. I am a bit confused how through python code we could control the velocity and acceleration?
And I am trying to follow you video but set_position is not rotating the motor ( I am trying to make simple controller by python to be able control joints by position, velocity and acceleration.

I have found only one way is through the wizard program. But how I could do it from my python node?

Thank you so much for the information and give me the answer. I truly appreciate it :slight_smile:

Hi @AnastasiyaRybakova

Hopefully I wasn’t too late to get back to you.
I just updated DYNAMIXEL SDK develop branch with working python example on ROS1.
Please check the source code and see if this helps you to understand how to use DYNAMIXEL SDK in ROS.

Thank you.