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; }