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 {
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;
 
  const std::string& get_frame() const;
 
  const std::string& get_reference_frame() const;
 
  const Eigen::MatrixXd& data() const;
 
  void set_joint_names(unsigned int nb_joints);
 
  void set_joint_names(const std::vector<std::string>& joint_names);
 
  void set_frame(const std::string& frame);
 
  void set_reference_frame(const std::string& reference_frame);
 
  void set_data(const Eigen::MatrixXd& data) override;
 
  void set_zero();
 
  Jacobian copy() const;
 
  void reset() override;
 
  Eigen::MatrixXd inverse() const;
 
  Eigen::MatrixXd inverse(const Eigen::MatrixXd& matrix) const;
 
  JointVelocities inverse(const CartesianTwist& twist) const;
 
  bool is_incompatible(const State& state) const override;
 
  Eigen::MatrixXd pseudoinverse() const;
 
  Eigen::MatrixXd pseudoinverse(const Eigen::MatrixXd& matrix) const;
 
  JointVelocities pseudoinverse(const CartesianTwist& twist) const;
 
  Eigen::MatrixXd transpose() const;
 
  JointTorques transpose(const CartesianWrench& wrench) const;
 
  Eigen::MatrixXd operator*(const Eigen::MatrixXd& matrix) const;
 
  friend Jacobian operator*(const Eigen::Matrix<double, 6, 6>& matrix, const Jacobian& jacobian);
 
  CartesianTwist operator*(const JointVelocities& dq) const;
 
  friend Jacobian operator*(const CartesianPose& pose, const Jacobian& jacobian);
 
  double& operator()(unsigned int row, unsigned int col);
 
  const double& operator()(unsigned int row, unsigned int col) const;
 
  friend std::ostream& operator<<(std::ostream& os, const Jacobian& jacobian);
 
private:
  std::vector<std::string> joint_names_;
  std::string frame_;                   
  std::string reference_frame_;         
  Eigen::MatrixXd data_;                
};
 
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.data_, jacobian2.data_);
}
}// namespace state_representation