Hacking Robotic Arm With Leap Motion

Hacking Robotic Arm With Leap Motion

Here is a look of how the system runs together.

Video
We had so much fun playing with the robot that the kids forced me to do another video. Yes they did.  This time we decided to come up with a system that would use a lot of the technologies that we use day to day at Translucent Computing.  We had a quick brain storming session and we decided to use a web application and a mobile application with a leap motion device (https://www.leapmotion.com).  Here’s a block diagram that shows how the system got put together.

The IOIO board connection to the Android phone via Bluetooth and connection to the robotic arm was explained in previous blog (http://blog.translucentcomputing.com/2014/01/hacking-robotic-arm-with-android.html).
Lets start with leap motion.
Leap motion tracks hands, fingers, and tools in wide open space around the device. The field of view of the device is large enough to track both hands and all fingers at the same time.It has API for all the popular programming languages, C++, Java, Objective-C, JavaScript, and more.  We chose to go with the JavaScript API and integrate leap into a web application server.  We started with the https://github.com/leapmotion/leapjs/blob/master/examples/roll-pitch-yaw.html example from https://github.com/leapmotion/leapjs . We added jQuery JavaScript library to it to make ajax calls, http://jquery.com.  And here is the “Hello World” leap example with some modifications.

 
<script type="text/javascript" src="<c:url value="/resources/js/leap.js"/>"></script>
<script type="text/javascript" src="<c:url value="/resources/js/jquery-2.0.3.min.js"/>"></script>
<script>
 var paused = false;
 window.onkeypress = function(e) {
  if (e.charCode == 32) {
   if (paused == false) {
    paused = true;
   } else {
    paused = false;
   }
  }
 };
 var controller = new Leap.Controller({
  enableGestures : false
 });

 controller.loop(function(frame) {
    latestFrame = frame;
    if (paused) {
     document.getElementById('pause').innerHTML = "<strong>PAUSED</strong>";
     return;
    } else {
     document.getElementById('pause').innerHTML = "";
    }

    var str = "";
    for ( var i in frame.handsMap) {
     var hand = frame.handsMap[i];

     var direction = -1;
     document.getElementById("direction").innerHTML = "STOP";

     if (hand.roll() < -0.5) {
      document.getElementById("direction").innerHTML = "RIGHT";
      direction = 1;
     } else if (hand.roll() > 0.5) {
      document.getElementById("direction").innerHTML = "LEFT";
      direction = 2;
     } else if (hand.pitch() > 0.5) {
      document.getElementById("direction").innerHTML = "UP";
      direction = 3;
     } else if (hand.pitch() < -0.5) {
      document.getElementById("direction").innerHTML = "DOWN";
      direction = 4;
     }
     
     $.getJSON(
       "${pageContext.request.contextPath}/saveDirection?direction="
         + direction, function(result) {
       });

     str += "<p>" + "<strong>Roll:</strong> " + hand.roll()
       + "<br/><strong>Pitch:</strong> " + hand.pitch()
       + "<br/><strong>Yaw:</strong> " + hand.yaw()
       + "</p>";
    }
    console.log(str);
    document.getElementById('out').innerHTML = str;
   });
</script>

<body>
 <div id="pause"></div>
 <div id="out"></div>

 <h3>Direction</h3>
 <div id="direction"></div>
</body>

It starts with some basic wiring for the pause key.  All the leap code runs inside the controller loop.  For each frame, basic data segment returned by leap, we check the hand object.  We wired the roll to left and right arm movements and pitch to up and down arm movements.  We send the pitch and roll data to the server using ajax and jQuery.  The leap runs at a high frame rate; for each frame it calls the server. (This is not optimal and in a real app we would limit the chatter between leap and the server.)

The laptop is running a server that runs the leap web page and saves the leap data.  We really like Spring framework (http://projects.spring.io/spring-framework/)  and for most of our Java web apps we use Spring and Spring MVC.  If you are working with Spring and setting up a new project the best way to do it is to use Spring Tool Suite (STS).  STS is an Eclipse based IDE that has been designed for Spring development.  We created a blank Spring MVC project and set up the tc server developer edition to use with the app.  The server is integrated with STS so that makes for easy setup.  We also added flexjson dependency to the pom.xml Maven file. The flexjson library is used for serializing object to JSON and it is the one I prefer to use.  A setup detail that is often missed with a Maven project structure is that any resources like JavaScript and CSS should be placed in resources directory under webapp directory. You should also have the resources mapping the servlet-contect.xml. 
<!– Handles HTTP GET requests for /resources/** by efficiently serving up static resources in the ${webappRoot}/resources directory –><resources mapping=”/resources/**” location=”/resources/” />
We placed both the jQuery and leap Javascript in the resources directory.  Since this is a simple app we used a single controller for all of the requests. We mapped the main request and a few other requests for testing purposes in the HomeController.java. Here is the controller:

 
@Controller
public class HomeController {

 @Autowired
 private ServletContext servletContext;

 private static final Logger logger = LoggerFactory.getLogger(HomeController.class);

 @RequestMapping(value = "/", method = RequestMethod.GET)
 public String home() {
  logger.info("Welcome home!");
  return "leapdata";
 }

 @SuppressWarnings("unchecked")
 @RequestMapping(value = "/listDirections", method = RequestMethod.GET)
 public String listOfDirections(Model model) {
  List queue = (List) servletContext
    .getAttribute("queue");
  model.addAttribute("queue", queue);
  return "listDirections";
 }

 @RequestMapping(value = "/saveDirection", method = RequestMethod.GET, headers = "Accept=application/json")
 @ResponseBody
 public void saveDirection(@RequestParam Integer direction) {
  logger.info("Getting direction :" + direction);
  if (direction != null) {   
   saveToQueue(direction);
  }
 }

 @RequestMapping(value = "/eatQueueItem", method = RequestMethod.GET, headers = "Accept=application/json")
 @ResponseBody
 public ResponseEntity eatQueueItem() {
  logger.info("Eating");
  QueueItem lastItem = getLastItem();

  HttpHeaders headers = new HttpHeaders();
  headers.add("Content-Type", "application/json; charset=utf-8");
  
  if (lastItem == null) {
   return new ResponseEntity(headers, HttpStatus.NOT_FOUND);
  }

  return new ResponseEntity(lastItem.toJSON(), headers,HttpStatus.OK);
 }

 @SuppressWarnings("unchecked")
 private QueueItem getLastItem() {
  List queue = (List) servletContext.getAttribute("queue");
  if (queue != null && !queue.isEmpty())
   return ((LinkedList) queue).removeLast();
  else
   return null;
 }

 @SuppressWarnings("unchecked")
 private void saveToQueue(Integer direction) {
  List queue = (List) servletContext.getAttribute("queue");
  if (queue == null) {
   queue = new LinkedList();
   servletContext.setAttribute("queue", queue);
  }

  if (!queue.isEmpty()) {
   QueueItem queueItem = ((LinkedList) queue).getLast();
   if (!queueItem.getDirection().equals(direction)) {
    queue.add(new QueueItem(direction));
   }
  } else {
   queue.add(new QueueItem(direction));
  }
 }

 static public class QueueItem {
  private Integer direction = -1;
  private String strDirection = "STOP";

  public QueueItem(Integer direction) {
   this.direction = direction;
   switch (direction) {
   case -1:
    logger.info("STOP");    
    strDirection = "STOP";
    break;
   case 1:
    logger.info("RIGHT");
    strDirection = "RIGHT";
    break;
   case 2:
    logger.info("LEFT");
    strDirection = "LEFT";
    break;
   case 3:
    logger.info("UP");
    strDirection = "UP";
    break;
   case 4:
    logger.info("DOWN");
    strDirection = "DOWN";
    break;
   default:
    logger.info("STOP DEFAULT");
    strDirection = "STOP";
    break;
   }
  }

  public Integer getDirection() {
   return direction;
  }

  public void setDirection(Integer direction) {
   this.direction = direction;
  }

  public String getStrDirection() {
   return strDirection;
  }

  public void setStrDirection(String strDirection) {
   this.strDirection = strDirection;
  }
  
  public String toJSON(){
   return new JSONSerializer().exclude("*.class").serialize(this);
  }
 }
}

The first thing that happens in the controller is the wiring of the servlet context.  Since we did not want to set up Hibernate for this simple app, we decided to save the leap data in the servlet context.  The home method displays the leap web page.  The saveDirection method is called using ajax in the leap web page to save the leap data.  The eatQueueItem method is the method that is used to retrieved the leap data.  Other methods are used for testing.

Once the server is started and the leap motion is used by moving your hand over it, the data is queued up on the server.  We decided to use an Android phone to pull the data from the web server.  We added a REST web service code to the IOIO control code.  The web service makes a request to eatQueueItem method and gets back a JSON string that we then use to decide which I/O pins to turn on in the IOIO. Here is the web service code used in the Android app :

 
 class Looper extends BaseIOIOLooper {
        private DigitalOutput pinM1up;
        private DigitalOutput pinM1down;
        private DigitalOutput pinM5left;
        private DigitalOutput pinM5right;

        private HttpClient httpClient;
        private HttpGet httpGet;

        private boolean processingWebservice = false;

        @Override
        protected void setup() throws ConnectionLostException {
            this.httpClient = new DefaultHttpClient();
            this.httpGet = new HttpGet("http://192.168.1.9:8080/diy/eatQueueItem");

            pinM1up = ioio_.openDigitalOutput(19, DigitalOutput.Spec.Mode.OPEN_DRAIN, true);
            pinM1down = ioio_.openDigitalOutput(18, DigitalOutput.Spec.Mode.OPEN_DRAIN, true);

            pinM5left = ioio_.openDigitalOutput(26, DigitalOutput.Spec.Mode.OPEN_DRAIN, true);
            pinM5right = ioio_.openDigitalOutput(14, DigitalOutput.Spec.Mode.OPEN_DRAIN, true);
        }

        /**
         * Called repetitively while the IOIO is connected.
         */
        @Override
        public void loop() throws ConnectionLostException {
            if (!processingWebservice)
                pullNextDirection();

            pinM1up.write(!m1up);
            pinM1down.write(!m1down);

            pinM5left.write(!m5left);
            pinM5right.write(!m5right);

            try {
                Thread.sleep(100);
            } catch (InterruptedException ignore) {
            }
        }

        public void pullNextDirection() {
            processingWebservice = true;
            try {
                HttpResponse response = httpClient.execute(httpGet);

                HttpEntity entity = response.getEntity();
                if (entity != null) {
                    InputStream is = entity.getContent();
                    BufferedReader reader = new BufferedReader(new InputStreamReader(is));

                    String line = null;
                    try {
                        line = reader.readLine();
                    } catch (IOException e) {
                        e.printStackTrace();
                    } finally {
                        try {
                            is.close();
                        } catch (IOException e) {
                            e.printStackTrace();
                        }
                    }

                    if (line != null) {
                        Log.d("Http Response:", line);
                        try {
                            JSONObject jsonObject = new JSONObject(line);
                            if (jsonObject.has("direction")) {
                                processDirection(jsonObject.getInt("direction"), jsonObject.getString("strDirection"));
                            }
                            else {
                                processDirection(-1, "STOP");
                            }
                        } catch (JSONException e) {
                            e.printStackTrace();
                        }
                    }
                    else {
                        processDirection(-1, "STOP");
                    }
                }
            } catch (ClientProtocolException e) {
                e.printStackTrace();
            } catch (IOException e) {
                e.printStackTrace();
            }

            processingWebservice = false;
        }

        private void processDirection(int direction, final String strDirection) {
            switch (direction) {
                case -1:
                    m5right = false;
                    m5left = false;
                    m1up = false;
                    m1down = false;
                    break;
                case 1:
                    m5right = true;
                    break;
                case 2:
                    m5left = true;
                    break;
                case 3:
                    m1up = true;
                    break;
                case 4:
                    m1down = true;
                    break;
                default:
                    m5right = false;
                    m5left = false;
                    m1up = false;
                    m1down = false;
                    break;
            }

            WebserviceActivity.this.runOnUiThread(new Runnable() {
                @Override
                public void run() {
                    ((TextView) findViewById(R.id.currentDirection)).setText(strDirection);
                }
            });
        }
    }

The http client is set up at the same time as the I/O pins.  The web service call is made every time the loop method is called. Since that happens at a high rate, we add a very simple “semaphore” to stop the web service being called while another web service call is running.  The JSON string from the response is converted into a JSON object and processed.  If the last piece of code looks wierd, that’s because UI updates can only be done in a UI thread.  Since the IOIO runs in its own thread we need to tell the app to run the text update in a UI thread.

Some other things that you can do with this system is to control the arm over the internet.  May be control a Mars rover arm to collect samples. Record the hand motions and replay them later. Use the record feature to pre program a robot arm in an assembly plant. Or may be create a perfect basketball thrower.  Endless possibilities.

By on January 14th, 2014 in Technology
Tags: , , , , , , , ,


Go back to the Blog