math_utils.h

#ifndef QUAD_MATH_UTILS_H
#define QUAD_MATH_UTILS_H
 
// Just include ros to access a bunch of other functions, fuck good code
#include <nav_msgs/Odometry.h>
#include <quad_msgs/MultiFootPlanContinuous.h>
#include <quad_msgs/MultiFootState.h>
#include <quad_msgs/RobotPlan.h>
#include <quad_msgs/RobotState.h>
#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
 
#include <cmath>
#include <eigen3/Eigen/Eigen>
 
#include "quad_utils/function_timer.h"
#include "quad_utils/quad_kd.h"
 
namespace math_utils {
 
inline double lerp(double a, double b, double t) { return (a + t * (b - a)); }
 
inline double wrapTo2Pi(double val) {
  return fmod(2 * M_PI + fmod(val, 2 * M_PI), 2 * M_PI);
}
 
inline double wrapToPi(double val) {
  return -M_PI + wrapTo2Pi(val + M_PI);
  // double new_val = fmod(val + M_PI, 2*M_PI);
  // while (new_val < 0) {
  //   new_val += 2*M_PI;
  // }
  // return new_val-M_PI;
}
 
inline std::vector<double> wrapToPi(std::vector<double> data) {
  std::vector<double> data_wrapped = data;
  for (int i = 0; i < data.size(); i++) {
    data_wrapped[i] = wrapToPi(data[i]);
  }
  return data_wrapped;
}
 
std::vector<double> interpMat(const std::vector<double> input_vec,
                              const std::vector<std::vector<double>> output_mat,
                              const double query_point);
 
Eigen::Vector3d interpVector3d(const std::vector<double> input_vec,
                               const std::vector<Eigen::Vector3d> output_mat,
                               const double query_point);
 
std::vector<Eigen::Vector3d> interpMatVector3d(
    const std::vector<double> input_vec,
    const std::vector<std::vector<Eigen::Vector3d>> output_mat,
    const double query_point);
 
int interpInt(const std::vector<double> input_vec, std::vector<int> output_vec,
              const double query_point);
 
std::vector<double> movingAverageFilter(std::vector<double> data,
                                        int window_size);
 
std::vector<double> centralDiff(std::vector<double> data, double dt);
 
std::vector<double> unwrap(std::vector<double> data);
 
Eigen::MatrixXd sdlsInv(const Eigen::MatrixXd &jacobian);
}  // namespace math_utils
 
#endif  // QUAD_MATH_UTILS_H