#include <iostream>
#include <gtest/gtest.h>
#include "drake/systems/plants/parser_urdf.h"
#include "drake/systems/plants/RigidBodyTree.h"