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