Program Listing for File ur10_inverse_dynamics_solver.hpp

↰ Return to documentation for file (include/ur10_inverse_dynamics_solver/ur10_inverse_dynamics_solver.hpp)

/* -------------------------------------------------------------------
 *
 * This module has been developed by the Automatic Control Group
 * of the University of Salerno, Italy.
 *
 * Title:   ur10_inverse_dynamics_solver.hpp
 * Author:  Antonio Annunziata, Vincenzo Petrone
 * Org.:    UNISA
 * Date:    Jun 23, 2023
 *
 * This is an implementation of the InverseDynamicsSolver interface
 * from inverse_dynamics_solver, using regressive form of the UR10
 * dynamic model estimated on MATLAB.
 *
 * -------------------------------------------------------------------
 */

#pragma once

#include <inverse_dynamics_solver/inverse_dynamics_solver.hpp>

namespace ur10_inverse_dynamics_solver
{

// Motor gains
static const double K1 = 13.5841;
static const double K2 = 14.2959;
static const double K3 = 11.3716;
static const double K4 = 11.2408;
static const double K5 = 11.7681;
static const double K6 = 11.7682;

static const unsigned short int NUMBER_OF_JOINTS = 6;
typedef Eigen::Matrix<double, NUMBER_OF_JOINTS, 1> Vector6d;
typedef Eigen::Matrix<double, NUMBER_OF_JOINTS, NUMBER_OF_JOINTS> Matrix6d;

class InverseDynamicsSolverUR10 : public inverse_dynamics_solver::InverseDynamicsSolver
{
public:
  InverseDynamicsSolverUR10() {}

  void initialize(rclcpp::node_interfaces::NodeParametersInterface::ConstSharedPtr = nullptr, const std::string& = "",
                  const std::string& = "") override;

  Eigen::MatrixXd getInertiaMatrix(const Eigen::VectorXd& joint_positions) const override;

  Eigen::VectorXd getCoriolisVector(const Eigen::VectorXd& joint_positions, const Eigen::VectorXd& joint_velocities) const override;

  Eigen::VectorXd getGravityVector(const Eigen::VectorXd& joint_positions) const override;

  Eigen::VectorXd getFrictionVector(const Eigen::VectorXd& joint_velocities) const override;

  Eigen::VectorXd getTorques(const Eigen::VectorXd& joint_positions, const Eigen::VectorXd& joint_velocities,
                             const Eigen::VectorXd& joint_accelerations) const override;

private:
  Eigen::VectorXd getFrictionCurrents_(const Eigen::VectorXd& joint_velocities) const;

  Eigen::MatrixXd getDriveGainsMatrix_() const;
};

}  // namespace ur10_inverse_dynamics_solver