Unable to control Dynamixel XM430 actuator

I am very new to robotics and i am trying to play with Dynamixel motor to understand it better.
Dynamixel Xm430-W350-R motor work fine as i have tested it with Dynamixel Wizard 2 and all settings are set to default.
While debug mode is set to true, with any speed between 1.0 to 6.0 motor works fine.

I have created an app where i send angular speed to motor and from motor i receive the state to debug.

class XM430_MG(Codelet):

def start(self):
    self.tx = self.isaac_proto_tx("StateProto", "state_tx")
    self.tick_periodically(0.2)

def tick(self):
    # print('TICK TEST')
    speed = 3.01
    # position = 300
    tx_message = self.tx.init()
    ''' Adding to Proto'''
    tx_message.proto.pack.elementType = 3
    # print('Set Size ')
    tx_message.proto.pack.sizes = [1,1,1]
    tx_message.proto.pack.scanlineStride = 0
    tx_message.proto.pack.dataBufferIndex = 0
    tx_message.proto.schema = "StateProto"

    ''' Adding to Buffer'''
    buffer = np.empty((1,1,1), dtype=np.dtype('float64'))
    buffer[0] = speed
    # buffer[0] = position
    tx_message.buffers = [buffer]
    self.tx.publish()

I have created above MX43_MG message generator codelet to isaac.dynamixel.DynamixelDriver component. My json file is this

 {
"name": "xm430",
"modules": [
"dynamixel",
"sight"
],
"config": {
"dynamixel": {
  "driver": {
    "tick_period": "10000Hz",
    "port": "/dev/ttyUSB0",
    "servo_ids": [1], 
    "servo_model": "XM430",
    "max_speed":6.0,
    "command_timeout":1,
    "baudrate": "k57600",
    "control_mode":"position"
  },
  "isaac.alice.Failsafe": {
    "name": "robot_failsafe"
  },
  "Dynamixel Motors": {
    "type": "plot",
    "channels": [ 
       { "name": "dynamixel/driver/motor_1.command" },
      { "name": "dynamixel/driver/motor_1.state" }
    ]
  }
}
},
 "graph": {
"nodes": [
  {
    "name": "dynamixel",
    "components": [
      {
        "name": "message_ledger",
        "type": "isaac::alice::MessageLedger"
      },
      {
        "name": "driver",
        "type": "isaac::dynamixel::DynamixelDriver"
      },
      {
        "name": "Dynamixel Motors",
        "type": "isaac::sight::SightWidget"
      },
      {
        "name": "isaac.alice.Failsafe",
        "type": "isaac::alice::Failsafe"
      }
    ]
  },
  {
    "name":"xm430",
    "components": [
      {
        "name": "message_ledger",
        "type": "isaac::alice::MessageLedger"
      },
      {
        "name": "XM430",
        "type": "isaac::alice::PyCodelet"
      }
    ]
  },
  {
    "name":"xm430_mg",
    "components": [
      {
        "name": "message_ledger",
        "type": "isaac::alice::MessageLedger"
      },
      {
        "name": "XM430_MG",
        "type": "isaac::alice::PyCodelet"
      }
    ]
  }
],
"edges": [
  {
    "source": "xm430_mg/XM430_MG/state_tx",
    "target": "dynamixel/driver/command"
  },
  {
    "source": "dynamixel/driver/state",
    "target": "xm430/XM430/command"
  }
]
}
}

My approach was to generate the same proto and tensor values as it was, when debug mode was set to true.
I was successful is doing so, but for some reason i am unable to control the motor.

Please help me approach this problem or point out where i am going wrong.

The approach in general seems fine (your component sends out StateProto to DynamixelDriver command and receives back StateProto for state though does nothing with it).

Have you confirmed with Isaac Sight perhaps that the state_tx protos are getting received by the DynamixelDriver? The issue could be message transport (edges not configured correctly), message is malformed (worth trying speed of 1.0 and recreate the debug conditions), or the DynamixelDriver is not connecting to the right servo (only fork in the code around get_debug_mode()).

Hi @hemals,

Yes my component sends out StateProto to DynamixelDriver command and receives back StateProto for state though does nothing with it.

Just for convenience i had changes the edge’s hook/tag from state_tx to state( as per ComponentAPI) and ran the application again and this is what my isaac sight displays.

"edges": [
  {
    "source": "xm430_mg/XM430_MG/state",
    "target": "dynamixel/driver/command"
  }
  1. Debug Mode in DynamixelDriver - To confirm that my servo is indeed connected i had run same application in debug_mode in debug_speed and the servo indeed runs fine. Output from servo below.

  2. Edges not configured properly - For debugging purposes i initially had received a state from DynamixelDriver in debug mode where it output constant speed state of dynamixel motor.
    OUTPUT FROM SERVO

    State Proto  ( elementType = float64,
     sizes = [1, 1, 1],
     scanlineStride = 0,
     dataBufferIndex = 0 )
     Schema <schema for state.capnp:StateProto>
     Data [  ]
     State_Message Tenso Type :  <class 'numpy.ndarray'>
     State_Message Tenso Value :  [[[0.95923296]]]
    

I then created a similar message command and sent my message as command or tx proto to DynamixelDriver.

My message:

State Proto Pack  ( elementType = float64,
    sizes = [1, 1, 1],
    scanlineStride = 0,
    dataBufferIndex = 0 )
    Schema <schema for state.capnp:StateProto>
    Tensor Type  <class 'numpy.ndarray'>
    Tensor  [[[2.   ]]]

When debug_mode is true and i output the state i get a speed around 1 and motor works.
But when i create my message and send as command to dynamixel, i get the state of dynamixel motor state as [[[0. ]]] or 0.

Here is a screenshot of my Isaac Sight Output for application.

The only issue i could think of is that maybe my message creation is wrong.

def tick(self):
# print('TICK TEST')
speed = 3.01
# position = 300
tx_message = self.tx.init()
''' Adding to Proto'''
tx_message.proto.pack.elementType = 3
# print('Set Size ')
tx_message.proto.pack.sizes = [1,1,1]
tx_message.proto.pack.scanlineStride = 0
tx_message.proto.pack.dataBufferIndex = 0
tx_message.proto.schema = "StateProto"

''' Adding to Buffer'''
buffer = np.empty((1,1,1), dtype=np.dtype('float64'))
buffer[0] = speed
# buffer[0] = position
tx_message.buffers = [buffer]
self.tx.publish()

This was my approach for creating a similar message. If format of allocation or buffer or assigning of speed is wrong somewhere please point out.

I had added failsafe to dynamixel motor which consistently stopped motor since it never found failsafe. Found this while debugging DynamixelDriver file.

Removed that from json and motor worked fine.

1 Like