Project

General

Profile

dynamixel component

This component controls a chain of dynamixel motors.

The component requires dynamixel motors using the protocol 2.0. Currently, MX and XM series have been tested, but any dynamixel motor supporting protocol 2.0 should work. Up to 8 motors can be controlled simulltaneously.

Motors must be connected to a USB port, either with the USB2Dynamixel device, or with the USB2AX custom product. Other devices may work, but have not been tested.

Warning Currently, the effort member of motors (out) and joint_input (in) are in Amperes and are a measure of the the current draw in the motor (for motors (out)) or the desired current target (for joint_input (in)). Conversion from Amperes to Newton meters may be done according to the plots provided in the motors datasheet. This is not currently done since no explicit numerical value is given and it must be derived from the plot bitmap which may not be acurate.

Ports

joint_input (in)

Data structure
  • struct ::or_joint::input joint_input

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • optional< sequence< double, 8 > > position

    • optional< sequence< double, 8 > > velocity

    • optional< sequence< double, 8 > > effort


motors (out)

Data structure
  • struct ::or_joint::state motors

    • struct ::or::time::ts ts

      • long sec

      • long nsec

    • sequence< string<32>, 8 > name

    • sequence< struct ::or_joint::flags, 8 > flag

      • boolean active

      • boolean emerg

    • sequence< double, 8 > position

    • sequence< double, 8 > velocity

    • sequence< double, 8 > effort

Provide a view of joints state.

ts is the last update time. Other elements are a sequence of up to 8 joints, indexed by their id. name is a human-readable description of the joint and flag indicates if the joint is present (active) or in an 'emergency' state. position and velocity are given in meters and meters per second for linear joints and in radians and radians per second for revolute joints. effort is either the measured force in Newtons for linear joints or the torque in Newton meters for revolute joints.


Services

connect (activity)

Inputs
  • string<64> serial (default "/dev/ttyACM0") Serial device

  • unsigned long baud (default "1000000") Baud rate

Throws
  • exception ::dynamixel::e_sys

    • short code

    • string<128> what

  • exception ::dynamixel::e_baddev

    • string<256> dev

Context

Connect to the hardware.

This opens the USB serial device serial at the given baud rate and detect connected motors. Discovered motors are printed on the standard output. After a successful connection, motors are configured in "postition control" mode and servoed to their current position.

Note that motors should all have a different id, starting from 1 and up to 8. They need not to have consecutive id, though.


disconnect (activity)

Throws
  • exception ::dynamixel::e_sys

    • short code

    • string<128> what

Context

Disconnect from the hardware.

All connected motors are first stopped and then released, so that they are not servoed anymore


set_position (attribute)

Inputs
  • sequence< double, 8 > position

Context

Set a desired position target for all motors.

This sends an array of desired position to motors starting from id 1 and up. The position in the array represents the motor id. The positions are in radian, and the origin can be set with set_offset (function).


set_velocity (attribute)

Inputs
  • sequence< double, 8 > velocity

Context

Set a desired velocity target for all motors.

This sends an array of desired velocities to motors starting from id 1 and up. The position in the array represents the motor id and velocities are given in rad/s.


set_effort (attribute)

Inputs
  • sequence< double, 8 > effort

Context

Set a desired torque target for all motors.

This sends an array of desired torques to motors starting from id 1 and up. The position in the array represents the motor id and torques are given in Amperes.


set_position_effort (attribute)

Inputs
  • sequence< double, 8 > position

  • sequence< double, 8 > effort

Context

Set a desired position target with a torque limit for all motors.

This sends an array of desired position and torque limits to motors starting from id 1 and up. The position in the array represents the motor id. Positions are in radians and torques in Amperes.


stop (function)

Context

Set a zero velocity target on all motors.


servo (activity)

Throws
  • exception ::dynamixel::e_sys

    • short code

    • string<128> what

  • exception ::dynamixel::e_input

Context

Control the joints via the input port joint_input (in)

The desired target present in the port is set in the joint controller, according to which member of position, velocity or effort is present. If several members are present, the position takes precedence over the velocity: if both are present, only the position is considered. If position and effort are both present, the position target is set with the indicated torque limit (in Amperes). If there is no position, the velocity takes precedence over effort.


configure (activity)

Inputs
  • string<64> serial (default "/dev/ttyACM0") Serial device

  • unsigned short id (default "1") Motor id

  • unsigned long baud (default "1000000") Baud rate

Throws
  • exception ::dynamixel::e_sys

    • short code

    • string<128> what

  • exception ::dynamixel::e_badrate

Context
  • In task comm (frequency 500.0 Hz)

Configure a single motor to the given id and baud rate.

This is a configuration service that requires that only one motor is connected to the bus. The service will discover the current motor settings by testing different baud rates. If a motor is successfully detected, it will be reprogrammed to match the given settings.


set_limits (function)

Inputs
  • unsigned short id

  • struct ::dynamixel::limits_s limits

    • double temperature_max

    • double voltage_min

    • double voltage_max

    • double pwm_max

    • double current_max

    • double acceleration_max

    • double velocity_max

Throws
  • exception ::dynamixel::e_range

Set various software limits for motor #id.


get_limits (attribute)

Outputs
  • sequence< struct ::dynamixel::limits_s, 8 > limits

    • double temperature_max

    • double voltage_min

    • double voltage_max

    • double pwm_max

    • double current_max

    • double acceleration_max

    • double velocity_max

Retrieve various software limits for all motors.


set_position_pidff (function)

Inputs
  • unsigned short id

  • double kP

  • double kI

  • double kD

  • double kFF1

  • double kFF2

Throws
  • exception ::dynamixel::e_range

Set the position controller parameters for motor #id.


get_postion_pidff (attribute)

Outputs
  • sequence< struct ::dynamixel::pidff_s, 8 > position_pidff

    • double kP

    • double kI

    • double kD

    • double kFF1

    • double kFF2

Get the position controller parameters for all motors.


set_velocity_pi (function)

Inputs
  • unsigned short id

  • double kP

  • double kI

Throws
  • exception ::dynamixel::e_range

Set the velocity controller parameters for motor #id.


get_velocity_pi (attribute)

Outputs
  • sequence< struct ::dynamixel::pi_s, 8 > velocity_pi

    • double kP

    • double kI

Get the velocity controller parameters for all motors.


set_offset (function)

Inputs
  • unsigned short id

  • double position

Throws
  • exception ::dynamixel::e_range

Set the zero offset for motor #id.

This service sets the zero offset of the position of motor id in radian. The set_position (attribute) parameters are given relative to this offset.

Note that this offset is not cumulative, i.e. passing 0.0 to this service will actually reset the motor offset to the absolute hardware value.


get_offset (attribute)

Outputs
  • sequence< double, 8 > offset

Get the zero offset for all motors.


set_profile (function)

Inputs
  • unsigned short id

  • struct ::dynamixel::profile_s desired

    • double velocity

    • double acceleration

Throws
  • exception ::dynamixel::e_range

Set motion profile for motor #id.

The motion profile is an acceleration/deceleration control method to reduce vibration, noise and load of the motor by controlling dramatically changing velocity and acceleration in position control mode.

A zero velocity profile means acceleration and velocities will only be limited by the hardware limits and no smoothing is done. A non zero velocity profile with a zero acceleration profile will provide a rectangular velocity shape for reaching a distant position. Non zero for both velocity and acceleration will provide a trapezoïdal shape for the velocity. Refer to the dynamixel documentation for details.


get_profile (attribute)

Outputs
  • sequence< struct ::dynamixel::profile_s, 8 > profile

    • double velocity

    • double acceleration

Get motion profile for all motors.


log (function)

Inputs
  • string<64> path (default "/tmp/dynamixel.log") Log file name

  • unsigned long decimation (default "1") Reduced logging frequency

Throws
  • exception ::dynamixel::e_sys

    • short code

    • string<128> what


log_stop (function)


log_info (function)

Outputs
  • unsigned long miss Missed log entries

  • unsigned long total Total log entries


Tasks

comm

Context
Throws
  • exception ::dynamixel::e_sys

    • short code

    • string<128> what