ROS MoveIt Robotic Arm Part 2 : Robot Controller

https://github.com/AIWintermuteAI/ros-moveit-arm.git

In the previous part of the article we have created URDF and XACRO files for our robotic arm and launched RVIZ to control our robotic arm in simulated environment.This time we'll do it with the real robotic arm! We will add the gripper, write a robot controller and (optional) generate IKfast inverse kinematics solver.

Geronimo!

Step 1: Adding the Gripper

Adding gripper was a bit confusing at first, so I skipped this part in the previous article. It turned out to be not so difficult after all.You'll need to modify your URDF file to add gripper links and joints. The modified URDF file for my robot is attached to this step. Basically it follows the same logic as the arm part, I just added three new links (claw_base, claw_r and claw_l) and three new joints(joint5 is fixed, and joint6, joint7 are revolute joints). After you modified your URDF file you'll also need to update MoveIt generated package and xacro file by using MoveIt setup assistant.Launch setup assistant with the following command

roslaunch moveit_setup_assistant setup_assistant.launch

Click on Edit Existing MoveIt Configuration and choose the folder with your MoveIt package. Add new planning group gripper (with links and joints for the gripper) and also an end effector. My settings are in the screenshots below. Notice that you don't choose kinematics solver for the gripper, it is not necessary. Generate the package and overwrite the files.Run

catkin make

command in your catkin workspace. Alright, now we have an arm with a gripper!

Step 2: Building the Arm

As I mentioned before the arm 3D model is made by Juergenlessner, thank you for an amazing work. The detailed assembly instructions can be found if you follow the link.

I had to modify the control system though. I use Arduino Uno with Sensor shield for controlling servos. Sensor shield helps a lot with simplifying the wiring and also makes it easy to provide the external power to servos. I use 12V 6A power adapter wired through step-down module(6V) to Sensor Shield. A note on servos. I use MG 996 HR servos bought from Taobao, but the quality is really bad. It is definitely a cheap Chinese knock-off. The one for the elbow joint didn't provide enough torque and even started fuming once under heavy load. I had to replace elbow joint servo with MG 946 HR from a better quality manufacturer.

Long story short - buy quality servos. If magic smoke comes out of your servos, use better servos. 6V is a very safe voltage, don't increase it. It won't increase torque, but can damage the servos.

Wiring for servos as follows:

base 2

shoulder2 4
shoulder1 3

elbow 6

gripper 8

wrist 11

Feel free to change it as long as you also remember to change the Arduino sketch. After you finished with hardware, let's have a look at the bigger picture!

Step 3: MoveIt RobotCommander Interface

So, now what? Why do you need MoveIt and ROS anyway? Can't you just control the arm through Arduino code directly?

Yes, you can.

Okay, now how about using GUI or Python/C++ code to provide robot pose to go to? Can Arduino do that?

Sort of. For that you will need to write an inverse kinematics solver which will take a robot pose(translation and rotation coordinates in 3D space) and convert it to joint angle messages for servos. Despite you can do it yourself, it's a hell of a lot of work to do. So, MoveIt and ROS provide a nice interface for IK(inverse kinematics) solver to do all the heavy trigonometrical lifting for you.

Short answer: Yes, you can do a simple robotic arm that will execute a hard-coded Arduino sketch to go from one pose to another. But if you want to make your robot more intelligent and add computer vision capabilities, MoveIt and ROS is the way to go.

I made a very simplified diagram explaining how MoveIt framework works. In our case it is going to be even simpler, since we don't have feedback from our servos and going to use /joint_states topic to provide robot controller with the angles for servos. We only lack one component which is the robot controller.

What are we waiting for? Let's write some robot controllers, so our robot would be ... you know, more controllable.

Step 4: Arduino Code for Robot Controller

In our case Arduino Uno running a ROS node with rosserial will be the robot controller. The Arduino sketch code is attached to this step and also available on GitHub.

The ROS node running on Arduino Uno basically subscribes to /JointState topic published on the computer running MoveIt and then converts the joint angles from the array from radians to degrees and passes them to servos using standard Servo.h library.

This solution is a bit hacky and not how it is done with industrial robots. Ideally you are supposed to publish the movement trajectory on /FollowJointState topic and then receive the feedback on /JointState topic. But in our arm the hobby servos can't provide the feedback, so we'll just directly subscribe to /JointState topic, published by FakeRobotController node. Basically we'll be assuming that whatever angles we passed to servos are executed ideally.

For more information on how the rosserial works you can consult the following tutorials

http://wiki.ros.org/rosserial_arduino/Tutorials

After you upload the sketch to Arduino Uno, you'll need to connect it with the serial cable to the computer running your ROS installation. To bring up the whole system execute the following commands

roslaunch my_arm_xacro demo.launch rviz_tutorial:=true

sudo chmod -R 777 /dev/ttyUSB0

rosrun rosserial_python serial_node.py _port:=/dev/ttyUSB0 _baud:=115200

Now you can use interactive markers in RVIZ to move the robot arm to a pose and then press Plan and Execute for it to actually move into the position.

Magic!

Now we are ready to write Python code for our ramp test. Well, almost...

Step 5: (Optional)Generating IKfast Plug-in

By default MoveIt suggests using KDL kinematics solver, which doesn't really work with less than 6 DOF arms. If you following this tutorial closely then you will notice that the arm model in RVIZ can't go to some poses which should supported by the arm configuration.

The recommended solution is to create custom kinematics solver using OpenRave. It is not that difficult, but you'll have to build it and it's dependencies from source or use the docker container, whichever you prefer. The procedure is very well documented in this tutorial. It is confirmed to work on VM running Ubuntu 16.04 and ROS Kinetic.I used the following command to generate the solver

openrave.py --database inversekinematics --robot=arm.xml --iktype=translation3d --iktests=1000

and then ran

rosrun moveit_kinematics create_ikfast_moveit_plugin.py test_robot arm my_arm_xacro ikfast0x1000004a.Translation3D.0_1_2_f3.cpp

to generate MoveIt IKfast plug-in.The whole procedure is a bit time consuming, but not very difficult if you follow the tutorial closely.

If you have questions about this part, please contact me in the comments or PM.

Step 6: The Ramp Test!

Now we are ready to try the ramp test, which we will execute using ROS MoveIt Python API.The Python code is attached to this step and also available on github repository. If you don't have a ramp or want to try another test you will need to change the robot poses in the code. For that first execute

rostopic echo /rviz_moveit_motion_planning_display/robot_interaction_interactive_marker_topic/feedback

in the terminal when already running RVIZ and MoveIt. Then move robot with interactive markers to the desired position. The position and orientation values will be displayed in the terminal. Just copy them to the Python code.To execute the ramp test run

rosrun my_arm_xacro pick/pick_2.py

with RVIZ and rosserial node already running.

Stay tuned for the third part of the article, where I will use stereo camera for object detection and execute pick and place pipeline for simple objects!