#include <iostream>
#include <Eigen/Dense>
#include "drake/common/eigen_types.h"
#include "drake/systems/plants/RigidBodyTree.h"