Jacobian.hpp

#pragma once
 
#include "state_representation/State.hpp"
#include "state_representation/exceptions/IncompatibleSizeException.hpp"
#include "state_representation/space/joint/JointTorques.hpp"
#include "state_representation/space/joint/JointVelocities.hpp"
#include "state_representation/space/cartesian/CartesianTwist.hpp"
#include "state_representation/space/cartesian/CartesianWrench.hpp"
 
namespace state_representation {
class CartesianTwist;
class CartesianWrench;
class JointVelocities;
class JointTorques;
 
class Jacobian : public State {
private:
  std::vector<std::string> joint_names_;
  std::string frame_;                   
  std::string reference_frame_;         
  unsigned int rows_;                   
  unsigned int cols_;                   
  Eigen::MatrixXd data_;                
 
public:
  Jacobian();
 
  Jacobian(const std::string& robot_name,
           unsigned int nb_joints,
           const std::string& frame,
           const std::string& reference_frame = "world");
 
  Jacobian(const std::string& robot_name,
           const std::vector<std::string>& joint_names,
           const std::string& frame,
           const std::string& reference_frame = "world");
 
  Jacobian(const std::string& robot_name,
           const std::string& frame,
           const Eigen::MatrixXd& data,
           const std::string& reference_frame = "world");
 
  Jacobian(const std::string& robot_name,
           const std::vector<std::string>& joint_names,
           const std::string& frame,
           const Eigen::MatrixXd& data,
           const std::string& reference_frame = "world");
 
  Jacobian(const Jacobian& jacobian);
 
  static Jacobian Random(const std::string& robot_name,
                         unsigned int nb_joints,
                         const std::string& frame,
                         const std::string& reference_frame = "world");
 
  static Jacobian Random(const std::string& robot_name,
                         const std::vector<std::string>& joint_names,
                         const std::string& frame,
                         const std::string& reference_frame = "world");
 
  friend void swap(Jacobian& jacobian1, Jacobian& jacobian2);
 
  Jacobian& operator=(const Jacobian& jacobian);
 
  unsigned int rows() const;
 
  Eigen::VectorXd row(unsigned int index) const;
 
  unsigned int cols() const;
 
  Eigen::VectorXd col(unsigned int index) const;
 
  const std::vector<std::string>& get_joint_names() const;
 
  void set_joint_names(unsigned int nb_joints);
 
  void set_joint_names(const std::vector<std::string>& joint_names);
 
  const std::string& get_frame() const;
 
  const std::string& get_reference_frame() const;
 
  void set_reference_frame(const CartesianPose& reference_frame);
 
  const Eigen::MatrixXd& data() const;
 
  void set_data(const Eigen::MatrixXd& data) override;
 
  bool is_compatible(const State& state) const override;
 
  void initialize() override;
 
  Jacobian transpose() const;
 
  Jacobian inverse() const;
 
  Jacobian pseudoinverse() const;
 
  Eigen::MatrixXd operator*(const Eigen::MatrixXd& matrix) const;
 
  Eigen::MatrixXd operator*(const Jacobian& jacobian) const;
 
  CartesianTwist operator*(const JointVelocities& dq) const;
 
  JointVelocities operator*(const CartesianTwist& twist) const;
 
  JointTorques operator*(const CartesianWrench& wrench) const;
 
  Eigen::MatrixXd solve(const Eigen::MatrixXd& matrix) const;
 
  JointVelocities solve(const CartesianTwist& twist) const;
 
  double& operator()(unsigned int row, unsigned int col);
 
  const double& operator()(unsigned int row, unsigned int col) const;
 
  Jacobian copy() const;
 
  friend std::ostream& operator<<(std::ostream& os, const Jacobian& jacobian);
 
  friend Jacobian operator*(const CartesianPose& pose, const Jacobian& jacobian);
 
  friend Eigen::MatrixXd operator*(const Eigen::MatrixXd& matrix, const Jacobian& jacobian);
};
 
inline void swap(Jacobian& jacobian1, Jacobian& jacobian2) {
  swap(static_cast<State&>(jacobian1), static_cast<State&>(jacobian2));
  std::swap(jacobian1.joint_names_, jacobian2.joint_names_);
  std::swap(jacobian1.frame_, jacobian2.frame_);
  std::swap(jacobian1.reference_frame_, jacobian2.reference_frame_);
  std::swap(jacobian1.cols_, jacobian2.cols_);
  std::swap(jacobian1.rows_, jacobian2.rows_);
  std::swap(jacobian1.data_, jacobian2.data_);
}
 
inline Jacobian& Jacobian::operator=(const Jacobian& jacobian) {
  Jacobian tmp(jacobian);
  swap(*this, tmp);
  return *this;
}
 
inline unsigned int Jacobian::rows() const {
  return this->rows_;
}
 
inline unsigned int Jacobian::cols() const {
  return this->cols_;
}
 
inline Eigen::VectorXd Jacobian::row(unsigned int index) const {
  return this->data_.row(index);
}
 
inline Eigen::VectorXd Jacobian::col(unsigned int index) const {
  return this->data_.col(index);
}
 
inline const std::vector<std::string>& Jacobian::get_joint_names() const {
  return this->joint_names_;
}
 
inline void Jacobian::set_joint_names(unsigned int nb_joints) {
  if (this->joint_names_.size() != nb_joints) {
    throw exceptions::IncompatibleSizeException("Input number of joints is of incorrect size, expected "
                                                    + std::to_string(this->joint_names_.size())
                                                    + " got " + std::to_string(nb_joints));
  }
  for (unsigned int i = 0; i < nb_joints; ++i) {
    this->joint_names_[i] = "joint" + std::to_string(i);
  }
}
 
inline void Jacobian::set_joint_names(const std::vector<std::string>& joint_names) {
  if (this->joint_names_.size() != joint_names.size()) {
    throw exceptions::IncompatibleSizeException("Input vector of joint names is of incorrect size, expected "
                                                    + std::to_string(this->joint_names_.size())
                                                    + " got " + std::to_string(joint_names.size()));
  }
  this->joint_names_ = joint_names;
}
 
inline const std::string& Jacobian::get_frame() const {
  return this->frame_;
}
 
inline const std::string& Jacobian::get_reference_frame() const {
  return this->reference_frame_;
}
 
inline const Eigen::MatrixXd& Jacobian::data() const {
  return this->data_;
}
 
inline void Jacobian::set_data(const Eigen::MatrixXd& data) {
  if (this->rows() != data.rows() || this->cols() != data.cols()) {
    throw exceptions::IncompatibleSizeException("Input matrix is of incorrect size, expected "
                                                    + std::to_string(this->rows_) + "x" + std::to_string(this->cols_)
                                                    + " got " + std::to_string(data.rows()) + "x"
                                                    + std::to_string(data.cols()));
  }
  this->set_filled();
  this->data_ = data;
}
 
inline double& Jacobian::operator()(unsigned int row, unsigned int col) {
  if (row > this->rows_) {
    throw std::out_of_range("Given row is out of range: number of rows is " + std::to_string(this->rows_));
  }
  if (col > this->cols_) {
    throw std::out_of_range("Given column is out of range: number of columns is " + std::to_string(this->cols_));
  }
  return this->data_(row, col);
}
 
inline const double& Jacobian::operator()(unsigned int row, unsigned int col) const {
  if (row > this->rows_) {
    throw std::out_of_range("Given row is out of range: number of rows is " + std::to_string(this->rows_));
  }
  if (col > this->cols_) {
    throw std::out_of_range("Given column is out of range: number of columns is " + std::to_string(this->cols_));
  }
  return this->data_(row, col);
}
}// namespace state_representation