Note: This tutorial assumes that you have completed the previous tutorials: Connecting to Dynamixel Bus.
(!) Please ask about problems and questions regarding this tutorial on answers.ros.org. Don't forget to include in your question the link to this page, the versions of your OS & ROS, and also add appropriate tags.

Controlling the PhantomX Pincher Robot Arm

Description: This tutorial shows how to control the PhantomX Pincher Robot Arm with ROS via FTDI.

Keywords: Pincher, PhantomX, Dynamixel Motor

Tutorial Level: ADVANCED

PhantomX Pincher AX-12 Manipulator

It has come the time to move a precious didactic robot, the PhantomX Pincher AX-12 Robot Arm. This manipulator has 4 degrees of freedom, and it can be controlled with ROS via a FTDI interface.

(Image on the right taken from 1).

Setting the Robot Up

Be aware the robot is well mounted in the base where you are going to use it.

With certain speed the robot may fall down or damage parts close to it.

Connect the robot via FTDI following the next diagram:

Connection Diagram

You should result with something like:

Connection Example

note that the Power Hub is being used here. Also note the computer being used here is a Raspberry Pi 3 Model B, however any computer with ROS in it should do the work.

Specifying the Controller Parameters

It is time to specify the parameters that will receive the dynamixel-controllers package in order to connect to the motors. It is assumed that you already know the motor ids, obtained in the tutorial Connecting to Dynamixel Bus.

The Parameters

Create a YAML file named tilt.yaml and paste the following in it.

waist_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: waist_joint
    joint_speed: 0.17
    motor:
        id: 1
        init: 0
        min: 0
        max: 1023
        
shoulder_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: shoulder_joint
    joint_speed: 0.17
    motor:
        id: 2
        init: 0
        min: 0
        max: 1023

elbow_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: elbow_joint
    joint_speed: 0.17
    motor:
        id: 3
        init: 0
        min: 0
        max: 1023

wrist_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: wrist_joint
    joint_speed: 0.17
    motor:
        id: 4
        init: 0
        min: 0
        max: 1023

hand_controller:
    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController
    joint_name: hand_joint
    joint_speed: 0.17
    motor:
        id: 5
        init: 0
        min: 0
        max: 1023

The YAML file explained

A quick look at the objects created before let us know the parameters that are being sent to the dynamixel_controllers package

waist_controller:

sets an object called waist_controller (this can be changed at your will, it will also change the topic name for the commands you send to it), with 4 attributes: controller, joint_name, joint_speed and motor. From these for parameters, the only arbitrary is the joint_name.

    controller:
        package: dynamixel_controllers
        module: joint_position_controller
        type: JointPositionController

Makes the controller package to be dynamixel_controllers and lets it know the motor is being set will be working in joint mode (not wheel mode).

    joint_speed: 0.17

Sets the motor speed to 0.17 radians per second, this is very important for the set up because when running the controller it will go to the initial position, if the speed is too high damage can be made to the robot. This can be changed in your node after you set things up ;).

    motor:
        id: 1
        init: 0
        min: 0
        max: 1023

The motor attribute also has sub-attributes, between them, the motor id, it is important that you remember which motor you are setting with each object you are creating here and that you can recall the name of the object for each motor.

init sets the initial position for the motor, you may refer to Creating a Joint Controller for more information about the motor's actual angle limits.

You don't want to exceed the 1023 value in either of the joints if you don't want to hurt your robot.

min and max are the minimum and maximum angle values, respectively, and the previous note also applies for these values.

The rest of the file represents the objects created for the other motors.

Linking the Controller

The controller manager created in Connecting to Dynamixel Bus shall be linked to by means of using another launch file.

<launch>
    <!-- Start tilt joint controller -->
    <rosparam file="$(find my_dynamixel_tutorial)/tilt.yaml" command="load"/>
    <node name="tilt_controller_spawner" pkg="dynamixel_controllers" type="controller_spawner.py"
          args="--manager=dxl_manager
                --port pan_tilt_port
                waist_controller
                shoulder_controller
                elbow_controller
                wrist_controller
                hand_controller"
          output="screen"/>
</launch>

Starting it up

In order to use the controller run

roslaunch my_dynamixel_tutorial controller_manager.launch

To make the controller manager available and to prove accessibility to the robot motors, then run

roslaunch my_dynamixel_tutorial start_tilt_controller.launch

Move your Robot

A quick look at rostopic list let us see the topics available, among which we will find waist_controller/command, shoulder_controller/command, elbow_controller/command, wrist_controller/command and hand_controller/command, there are the topics you have to publish to in order to move the robot.

You may want to try the following in a terminal.

rostopic pub -1 /waist_controller/command std_msgs/Float64 -- 2.61
rostopic pub -1 /shoulder_controller/command std_msgs/Float64 -- 2.61
rostopic pub -1 /elbow_controller/command std_msgs/Float64 -- 2.61
rostopic pub -1 /wrist_controller/command std_msgs/Float64 -- 2.61
rostopic pub -1 /hand_controller/command std_msgs/Float64 -- 2.61

Acknowledgment Notice

This tutorial was created by Hans Toquica, feel free to contact me at toquica@engineer.com in case you have any questions in regards of this way of connecting to the robot.

References

  1. PhantomX Pincher Robot Arm Kit Mark II - Turtlebot Arm. N.a. Trossen Robotics. http://www.trossenrobotics.com/p/PhantomX-Pincher-Robot-Arm.aspx (Retrieved Nov-7-2017).

Wiki: dynamixel_controllers/Tutorials/Controlling the PhantomX Pincher Robot Arm (last edited 2018-01-02 16:17:29 by HansToquica)