Position을 write 후 read 할 때 에러 발생

안녕하세요
다이나믹셀의 sdk를 사용해 python 코드로 제어를 하고자 합니다.
시스템은 dynamixel, OpenCR, Raspberry Pi로 구성되어 있습니다.

문제:
다이나믹셀의 goal position을 write한 후

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

present position을 read 할 때,

packetHandler.read4ByteTxRx(portHandler, DXL_ID, ADDR_PRESENT_POSITION)

에러가 발생합니다.

write만 하거나 read만 하는 것은 에러가 발생하지 않으나

write 이후에 read를 할 때 에러가 발생합니다.

이를 해결하기 위해 시도한 것은

  • DXL_ID, baudrate, protocol version 확인
  • DynamixelWizard 2.0으로 공장초기화, 펌웨어 업데이트 실행
  • time.sleep(0.1) 및 시간 바꿔가며 실험
  • torque enable의 여부 판단 후, disable이면 다시 켜주기
  • port 비워주기 ( portHandler.clearPort() )
  • time.sleep을 쓰지 않고 while 문으로 goal position에 도달할 때까지 기다린 후 read하기

입니다.

하지만 read 시에 여전히 에러가 발생합니다.

instruction packet을 두 번 연속으로 쓰는 것 때문이라고 추측하고 있습니다.

송신을 연속으로 하기에 delay time을 넣어주어야 한다고 생각하는데 time.sleep과 다른 delay time이 혹시 있는지도 궁금합니다.

time.sleep을 했을 때의 에러는

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

입니다.

time.sleep을 주지 않은 경우의 에러는

  • Incorrect status packet TxRxResult Communication error

입니다.

도움주셔서 감사드립니다.


DYNAMIXEL 서보:

XL540-W270
XM430-W210


DYNAMIXEL 컨트롤러:

OpenCR, RPi


소프트웨어 인터페이스:

python, ubuntu 20.04, ROS noetic