#include <iostream>
#include <cstdlib>
#include <memory>
#include "drake/common/eigen_types.h"
#include "drake/systems/plants/RigidBodyTree.h"