Error reading position value after write position

Hello,

I am trying to control Dynamixel using Python code with the SDK.
The system consists of a Dynamixel, OpenCR, and Raspberry Pi.

Issue:

After writing the goal position of the Dynamixel:

packetHandler.write4ByteTxRx(portHandler, DXL_ID, ADDR_GOAL_POSITION, goal_position)

An error occurs when I try to read the present position:

packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION)

There are no errors when I only write or only read, but an error occurs when I read after writing.

To solve this, I have tried the following:

  • Verified DXL_ID, baud rate, and protocol version
    
  • Performed factory reset and firmware update using DynamixelWizard 2.0
    
  • Experimented with time.sleep(0.1) and different sleep times
    
  • Checked the torque enable status and re-enabled it if disabled
    
  • Cleared the port with portHandler.clearPort()
    
  • Used a while loop to wait until the goal position is reached before reading, without using time.sleep
    

However, an error still occurs when I try to read.

I suspect this might be due to sending the instruction packet twice in a row.

I believe I need to add a delay time because of consecutive transmissions, but I wonder if there is another delay mechanism besides time.sleep.

The error I get with time.sleep is:

DXL_MAKEWORD(data[2], data[3]) if result == COMM_SUCCESS
IndexError: list index out of range

The error I get without time.sleep is:

Incorrect status packet TxRxResult Communication error

Thank you for your help.


DYNAMIXEL Servo:

  • XL540-W270
  • XM430-W210

DYNAMIXEL Controller:

OpenCR, Raspberry Pi


Software Interface:

Python, Ubuntu 20.04, ROS Noetic

In my personal experiences in using DXL SDK on Python and C/C++ on RPi, PC or Arduino boards, I also could never successfully send a Read Command RIGHT AFTER a Write command. I had always needed a delay time or do something else in my code before sending a Read command (On Arduino board such as the OpenRB-150, it was about 125 ms delay).

Have you tried to reduce the USB Latency Timer from the default 16 ms to 1 ms? See this link

Also have you tried to reduce or tinker with the “timeout” and “write_timeout” parameters of your Serial Port as per the PySerial API?

https://pyserial.readthedocs.io/en/latest/pyserial_api.html

Also you could use a more adaptive delay with a while loop checking on Address 122 not equal to 1.

Thank you for your response.

I solved the issue.

I don’t know the exact reason of this solution but I want to provide it.

the solution is sync read & write or bulk read & write.

Even though I only use 1 dynamixel, the sync read & write and bulk read & write does not make error for this problem.

The problem is error of read command right after write command.

What I want to do is control the position of dynamixel using previous position and present position difference.

Previous code snippet is like below.

def get_present_position():
    position, dxl_comm_result, dxl_error = packetHandler.read4ByteTxRx(portHandler, DXL_ID, addr_present_position)
    
    if dxl_comm_result != COMM_SUCCESS:
        rospy.logerr("%s", packetHandler.getTxRxResult(dxl_comm_result))
    elif dxl_error != 0:
        rospy.logerr("%s", packetHandler.getRxPacketError(dxl_error))
    else:
        return position

def send_goal_position(goal_position):
    packetHandler.write4ByteTxOnly(portHandler, DXL_ID, addr_goal_position, goal_position)


step_length = 20

previous_position = get_present_position() # read

previous_position_ = previous_position + step_length

send_goal_position(previous_position_) # write

present_position = get_present_position() # read


position_difference = abs(prev_position - present_position)

the error occurs at the write → read moment.

I changed the read and write function with sync.

def read_sync():
    groupSyncRead = GroupSyncRead(portHandler, packetHandler, addr_present_position, 4)
    
    add_param_result = groupSyncRead.addParam(DXL_ID)
    if not add_param_result:
        print(f"Failed to add parameters for ID {DXL_ID}")
        
    sync_read_result = groupSyncRead.txRxPacket()
    if sync_read_result != COMM_SUCCESS:
        print(f"Sync Read error: {packetHandler.getTxRxResult(sync_read_result)}")
    
    present_position = groupSyncRead.getData(DXL_ID, addr_present_position, 4)

    groupSyncRead.clearParam()
    return present_position

def write_sync(goal_position):
    groupSyncWrite = GroupSyncWrite(portHandler, packetHandler,addr_goal_position, 4)
    
    param_goal_position = [
    DXL_LOBYTE(DXL_LOWORD(goal_position)),
    DXL_HIBYTE(DXL_LOWORD(goal_position)),
    DXL_LOBYTE(DXL_HIWORD(goal_position)),
    DXL_HIBYTE(DXL_HIWORD(goal_position))
]
    add_param_result = groupSyncWrite.addParam(DXL_ID, param_goal_position)
    if not add_param_result:
        print(f"Failed to add parameters for ID {DXL_ID}")
    
    sync_write_result = groupSyncWrite.txPacket()
    if sync_write_result != COMM_SUCCESS:
        print(f"Sync Write error: {packetHandler.getTxRxResult(sync_write_result)}")

    groupSyncWrite.clearParam()

step_length = 20

previous_position = get_present_position() # read

previous_position_ = previous_position + step_length

write_sync(previous_position_) # write

present_position = read_sync() # read

position_difference = abs(prev_position - present_position)

If you want to use current value, you can use bulk read.