ForceTorqueCalibration.hpp

Go to the documentation of this file.

#include <eigen3/Eigen/Core>
#include <eigen3/Eigen/Dense>
#include <ros/console.h>

namespace rokubimini
{
namespace calibration
{
class ForceTorqueCalibration
{
public:
  ForceTorqueCalibration();

  ~ForceTorqueCalibration();

  void addMeasurement(const Eigen::Vector3d& acc, const Eigen::Vector3d& gravity, Eigen::Vector3d angVel,
                      Eigen::Vector3d angAcc, const Eigen::VectorXd& ftRaw);

  Eigen::VectorXd getCalibParams();

  void resetCalibration();

protected:
  Eigen::MatrixXd createMeasurementMat(const Eigen::Vector3d& acc, const Eigen::Vector3d& gravity,
                                       Eigen::Vector3d angVel, Eigen::Vector3d angAcc);

  Eigen::Matrix3d skewMatrix(Eigen::Vector3d inVec);

  Eigen::VectorXd ftReadings_;
  Eigen::MatrixXd measurementMat_;
  int numMeasurements_;
};
}  // namespace calibration
}  // namespace rokubimini