As I'm continuing to build my su_chef robotic food prep project, I've realized it will be more efficient to design and prototype using robotics simulations. So last weekend, I took a deep dive into integrating motion planning via ROS MoveIt with the Gazebo physics simulator using a virtual model of my Arduino Braccio arm. Using these tools, I'm able to simulate my robot interacting with objects to quickly test how it will work without needing to have my physical setup running.

In this demo, you can see the arm grabbing and moving the red block and the blue bowl.

The red block is a stand-in for the vegetables that I will eventually want the su_chef to pick for chopping. Rolling down a ramp into a bowl is a major simplification of the actual process, but I thought I would first make a simplified package so that others could reuse it and have an easier time getting started simulating with the Braccio arm and Gazebo.

I wasn't able to find anyone else's working simulator for the Braccio so I combined bits from several other examples I found. I built a very bare-bones command line program that controls the robotic simulator to make it easier to use right off the bat.

If you'd like to jump into using it, the code and instructions are available here. Below I've describe how arm is able to determine where to pick up, as well as some testing I did to see how often the arm succeeds at its task.

The Inverse Kinematics solver

Since the Braccio arm is fairly small and simple, it has a limited domain where it's able to pick up items. To overcome this, I needed to program the ability to pick up from above and from the side, as well as subroutines that reposition the block if it's outside of the graspable region.

The problem is that the arm itself is controlled by its joint angles so there is no direct way to tell it to go to a certain point in space. Instead, I had to compute what the arm's angles should be given a target location and orientation. Working out the direction to turn the arm's base is fairly easy, but it is more difficult to figure out how to adjust the shoulder, elbow, and wrist angles to get to a specific distance from the base, labeled r in this image.

In my previous version I just hard coded the equation to determine the angles needed for a given position. However, this isn't ideal because (a) it's a lot of messy math to debug, and (2) this limits me to exactly 1 solution when really there are many acceptable solutions within a small window around the exact solution. To do this more generally, I needed an inverse kinematics (IK) solver. There are existing robotic arm inverse kinematics solvers, but most of them work on 6 degree-of-freedom (DOF) arms, while the Braccio only has 5 DOF. This doesn't sound like a huge difference, but it means that the Braccio is actually highly constrained in the range of poses that it can take, which breaks most standard IK solvers.

So for this attempt, I switched to using a simple 2D IK solver based on this post from Travis DeWolf. This solver aims to get the arm as close to the target point as posibble while maintaining allowed angle restrictions and certain height restrictions to keep it the right distance from the ground. If you'd like to know more about how it works, I suggest Travis's blog post or check out the working solver code in my simulation package.

Performance

It took a few days of fine-tuning and retesting to get both the physics and the robot measurements worked out to the point where it was working consistently. Once I had it working somewhat consistently, I collected data on many trials sequentially to see how it performs. I reset the block to random positions, and then attempt to pick it it up.

For the "top" picker the success rate was about 50%, while for the "side" picker it was only 33%.

At first, I assumed that there were probably consistent regions where it was failing, but after plotting the start positions of each, I couldn't see any real pattern.

As you can see from this longer video there are a lot of ways to mess up.

After a little more digging, it became clear that the main problem was the sliding phase. Since the range of motion is so constrained there were only two narrow bands where the pick up was successful. So if the block starts outside of that area, it needs to be slid back into that area first. For the successful attempts, they all get into the pickup range correctly. In the figure on the right, you can see the two areas where the arm can reach to pick up.

For a little more analysis check out this notebook.

Next steps

I could definitely optimize this further, but I figured it would be more valuable to wait until I have a more specific environment for my su_chef design. But I figured I'd release this playground package for Arduino and ROS hackers like me to work with.

If you are interest in making it work better, always feel free to contact me or submit issues and pull requestson Github. And if you want to hear more about su_chef and my dream to start a worker-owned automated foodtruck follow me on Mastodon or check out my newsletter below for updates.