Comment on page
Controlling PhantomX Pincher Robot Arm
In this tutorial, we will show you how to configure and remotely control PhantomX Pincher Robot connected to the Rover.

source: trossenrobotics.com
Before you begin, make sure you have internet connection on your Rover.
Also, you need to have ROS installed both on a Rover and your computer
And, of course, assembled Pincher Arm together with ArbotiX-M Robocontroller connected to your Rover.
If everything works, open and upload the following sketch
File -> Sketchbook -> ArbotiX Sketches -> ros
You can install arbotix ROS packages from repository (
ros-kinetic-arbotix
package), but for the pincher gripper to work, you need to build an unrealeased 0.11.0 version which adds support for prismatic joints.To build it, you can follow these steps:
source /opt/ros/kinetic/setup.bash
mkdir -p ~/ros_ws/src && cd ~/ros_ws/src
git clone https://github.com/corb555/arbotix_ros
cd ~/ros_ws
rosdep update
rosdep install --from-paths src -iry
catkin build
Source your catkin workspace (either
/opt/ros/kinetic
or ~/ros_ws/devel
depending on how you installed Arbotix ROS drivers) and run:arbotix_terminal
arbotix_terminal
by default, assumes the board is connected on /dev/ttyUSB0
port. If it is attached to another device, you can specify it like this: arbotix_terminal [PORT]
To check which port the device connects to, you can, for instance, run
dmesg -w
(Ctrl+C to exit), connect the device and check kernel logs. If you are using Ubuntu on
Windows Subsytem for Linux
, you need to open Device Manager and look under Ports for COM port number of the device. COM[N]
corresponds to /dev/ttyS[N]
path. (e.g. COM4 -> /dev/ttyS4). You might need to run: sudo chmod 666 /dev/ttyS[N]
A terminal prompt should appear. Type
help
for list of commands.
Connect the Dynamixel you want to set id to. Then type
ls
to see id of connected servo and mv [source] [target]
to change it. For example, when the servo has id 1 and we want to set it to 2, just type mv 1 2
. On PhantomX pincher arm, servo ids should look like this:

source: trossenrobotics.com
When done, type Ctrl+C to exit terminal
We will create a package that tests arbotix driver. Let's start by creating an empty package
source /opt/ros/kinetic/setup.bash
mkdir -p ~/ros_ws/src && cd ~/ros_ws/src
catkin create pkg arbotix_test --catkin-deps arbotix_python
Inside your package, create
config/test.yaml
file with following content (change port if needed):port: /dev/ttyUSB0
rate: 15
joints: {
servo1: {id: 1},
servo2: {id: 2},
servo3: {id: 3},
servo4: {id: 4},
servo5: {id: 5}
}
You can set more parameters for each joint, like maximum speed, minimum and maximum angle etc. A brief documentation (Unfortunately, a little outdated) can be found here
And
launch/test.launch
with following:<launch>
<node name="arbotix" pkg="arbotix_python" type="arbotix_driver" output="screen">
<rosparam file="$(find arbotix_test)/config/test.yaml" command="load" />
</node>
</launch>
Now, build your workspace
cd ~/ros_ws
catkin build
From now on, you need to run
source ~/ros_ws/devel/setup.bash
on every terminal session you open (or add it to ~/.bashrc
)Run Master node on one terminal
roscore
roslaunch arbotix_test test.launch
Now, check available topics and services
rostopic list
rosservice list
You should see
command
topics that let you set position (in radians) for each servo and joint_states
topic that informs about position and velocity of each joint. There should also be services that allow enabling, relaxing or setting speed (in radians/sec) for each servo.Let's try to move
servo1
joint. First, set speed to a safe value (0.2 r/s in this case):
rosservice call /servo1/set_speed 0.2
Move servo to a default neutral value:
rostopic pub /servo1/command std_mgs/Float64 -- 0.0
The maximum angle range for a dynamixel servo is [-150, 150] degrees which is equal to approximately [-2.62, 2.62] in radians
Relax joint:
rosservice call /servo1/relax
You can also use
arbotix_gui
to control each joint. Just type:arbotix_gui
A graphical application should appear:

Enable and relax servos by clicking on checkboxes and set position by moving sliders.
Here's a recording of a terminal session in which we do things mentioned in the tutorial up to this point:
Let's build it
source ~/ros_ws/devel/setup.bash
cd ~/ros_ws/src
git clone https://github.com/turtlebot/turtlebot_arm.git -b kinetic-devel
cd ~/ros_ws
rosdep install --from-paths src -iry
catkin build
To use this packages with PhantomX Pincher, you need to have
TURTLEBOT_ARM1
environment variable set to pincher
. To set it automatically on every terminal session, add export
command to .bashrc
file:echo "export TURTLEBOT_ARM1=pincher" >> ~/.bashrc
source ~/.bashrc
Make sure to have
roscore
running, then type:roslaunch turtlebot_arm_bringup arm.launch
To view robot arm model with actual position:
- Open
rviz
- For
Fixed Frame
selectarm_base_link
- Click
Add
in Displays panel - Select
RobotModel
and clickOk

To test Motion Planning with MoveIt! :
- Run
turtlebot_arm_moveit
launch file:roslaunch turtlebot_arm_moveit_config turtlebot_arm_moveit.launch sim:=false - Click on
Planning
tab in Motion Planning display - Move interactive marker to intended position
- Click on
Plan
to see Motion visualization and thenExecute
or just click onPlan and Execute
- Run pick and place demo (in another terminal session)rosrun turtlebot_arm_moveit_demos pick_and_place.py

Let's prepare a catkin workspace for pincher arm. Start by sourcing the workspace you want to extend. If you don't have an existing development workspace, just do:
source /opt/ros/kinetic/setup.bash
Download necessary packages
mkdir -p ~/ros_ws/src && cd ~/ros_ws/src
git clone https://github.com/corb555/arbotix_ros.git
git clone https://github.com/turtlebot/turtlebot_arm.git
We only need
turtlebot_arm_bringup
and turtlebot_arm_description
packages, so we can remove the restcd ~/ros_ws/src/turtlebot_arm
mv turtlebot_arm_bringup turtlebot_arm_description ~/ros_ws/src
cd ~/ros_ws/src
rm -rf turtlebot_arm
Use
rosdep
to check for missing dependenciescd ~/ros_ws
rosdep update
rosdep install --from-paths src -i
You should see an error message similar to this:
ERROR: the following packages/stacks could not have their rosdep keys resolved
to system dependencies:
arbotix_python: No definition of [actionlib] for OS [debian]
turtlebot_arm_description: No definition of [xacro] for OS [debian]
arbotix_controllers: No definition of [tf] for OS [debian]
turtlebot_arm_bringup: No definition of [robot_state_publisher] for OS [debian]
Download missing packages and their recursive dependencies
rosinstall_generator actionlib xacro tf robot_state_publisher control_msgs --deps --wet-only --exclude RPP > src/.rosinstall
wstool update -t src
Run rosdep again to install any new system dependencies
rosdep install --from-paths src -iry
Now, build the workspace
catkin build -j 2
Source your workspace
source ~/ros_ws/devel/setup.bash
export TURTLEBOT_ARM1=pincher
export ROS_IP=10.0.0.1
export ROS_MASTER_URI=http://10.0.0.1:11311
Add these lines to
~/.bashrc
to set them automaticallyNow, run Master node on one terminal
roscore
And on another, type:
roslaunch turtlebot_arm_bringup arm.launch
Source your workspace containing turtlebot_arm packages
source ~/ros_ws/devel/setup.bash
Set required variables
export TURTLEBOT_ARM1=pincher
export ROS_IP=X.X.X.X
export ROS_MASTER_URI=http://10.0.0.1:11311
Replace X.X.X.X with your local IP address. To check your address, you can use
ip address
commandYou can now use the examples we described earlier, e.g.:
arbotix_gui
or MoveIt! demo:
roslaunch turtlebot_arm_moveit_config turtlebot_arm_moveit.launch sim:=false