PlanarRangeVis2(Pose plane_pose, LegController<n_joints>* controller, double samples_per_unit, double max_range) : controller(controller), point(point), normal(normal) {
   double d_r = 1.0/samples_per_unit;
   Eigen::Vector3d center = (point.dot(normal) / normal.squaredNorm()) * normal;
   /*
   double yaw = atan2(normal[1], normal[0]);
   double pitch = atan2(normal[2], sqrt(normal[0]*normal[0] + normal[1]*normal[1]));
   center_pose = Pose(center[0], center[1], center[2], yaw, pitch, 0);
   */
   center_pose = plane_pose;
   center_pose.x = center[0];
   center_pose.y = center[1];
   center_pose.z = center[2];
   double angles[n_joints];
   for (double x = -max_range; x < max_range; x += d_r) {
     for (double y = -max_range; y < max_range; y += d_r) {
       Eigen::Vector3d goal = Vector4dTo3d(center_pose.FromFrame(Eigen::Vector4d(0.0, x, y, 1.0)));
       plane_query_scores.push_back(controller->GetJointCommands(goal, angles));
       plane_query.push_back(goal); 
     }
   }
 }
 double operator() (const column_vector& arg) const {
   double angles[n_joints];
   Eigen::Vector3d goal = Vector4dTo3d(center_pose.FromFrame(Eigen::Vector4d(0.0, arg(0), arg(1), 1.0)));
   double score = controller->GetJointCommands(goal, angles);
   return score;
 }