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: