This ROS package contains hardware interface to Kondo Kagaku's ICS motor system.
ICS (Interactive Communication System) is a kind of serial bus to control RC servo motors. Basically it can control measure its position. It has additional features such as setting gain, getting current.
kondo_motor is a ROS meta-package, contains the following two packages:
- kondo_driver: driver node for ICS servo motors
- kondo_msgs: message files for kondo_motor
The other KRS series servo motors are not checked with this package.
Two USB-ICS adapters are available on the Kondo's on-line store. Both adapters use the FTD232RL chip to interact with servo motors.
You need to install ftdi_sio. Currently the KONDO's usb ICS adapters are not registered in the driver source code. You can setup the device using a script file as follows. You need to enter sudo password.
$ rosrun kondo_driver setup_device.sh
This script installs ftdt_sio module and set the target device IDs into /sys/bus/usb-serial/drivers/ftdi_sio/new_id. After this procedure, you should see /dev/ttyUSB* device file when the device is connected.
Connect each servo and the adapter one to one. To set the servo ID, you can use ics_set_id.
$ rosrun kondo_driver set_ics_id <-d /dev/ttyUSB0> <-i id>
If you want to set the ID as 2,
$ rosrun kondo_driver set_ics_id -i 2
Current servo ID: 0
Set servo ID correctly: 2
After setting the id of each servo motor, you can connect your motors on the same bus.
The driver setting is described in a yaml file as ROS parameters. 'config/driver_sample.yaml' shows the example in which two motors (ID0, ID1) connected.
The servo_0 and servo_1 fields are the servo settings. Each field means:
- id: Servo's ICS id. The ID on the same bus must not be identical.
- joint_name: Name of the joint attached to the servo.
The following parameters are the interanal characteristics of each servo motor.
- min_anlge: Minimum angle of servo [deg]
- max_anlge: Maximum angle of servo [deg]
- stretch: stretch parameter
- speed: speed paramter
- current_limit: current limit parameter
- temperature_limit: temperature limit parameter
You can see more about each parameter from the ICS command reference.
The kondo_driver is fit for using ros_controllers. Check config/controller_sample.yaml for ros_control configuration. This example uses three type of controllers.
-
joint_state_controller/JointStateController This controller publish the joint angle and velocity to the /joint_states.
-
position_controllers/JointPositionController This controller receive the position command for the servo motor.
-
velocity_controllers/JointVelocityController This controller receive the velocity command for the servo motor.
See the ros_control for more details.
The following sample launch file brings up the controllers.
$ roslaunch kondo_driver kondo_driver.launch
Initially all servo motors are free (not powered at all). You can use the service call to to power on the motor.
$ rosservce call /joint_0_position_controller/power_on "request: true"
Check if you can command the sevo position. Publish /joint_0_position_controller/command (std_msgs/Float64) to control joint angle. Unit is in radian.
$ rostopic pub -1 /joint_0_position_controller/command std_msgs/Float64 "data: 0.0"
- to command to set EEPROM parameters
- to swith the controller from/to position/vecocity controller