fts_grav_comp_plugin.hpp

 
#ifndef MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_
#define MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_
 
#include <geometry_msgs/msg/wrench.hpp>
#include <rclcpp/rclcpp.hpp>
#include "mujoco_ros2_control_plugins/mujoco_ros2_control_plugins_base.hpp"
 
namespace mujoco_ros2_control_plugins
{
 
struct FtsData
{
  // sensor name of FTS. Mujoco should have sensors <sensor_name>_force and <sensor_name>_torque
  std::string sensor_name;
  // mujoco sensor address for the force and torque sensors
  int sensor_adr_force{ -1 };
  int sensor_adr_torque{ -1 };
  // site id of the site used for center of gravity reference
  int cog_site_id{ -1 };
  // array of doubles of the center of gravity w.r.t. cog_site_it in meters for [x, y, z]
  mjtNum cog_pos[3];
  // mass that is being compensated in kg
  mjtNum cog_mass;
  // site id of the site used for the mujoco force and torque sensors
  int fts_site_id{ -1 };
};
 
class FtsGravCompPlugin : public MuJoCoROS2ControlPluginBase
{
public:
  FtsGravCompPlugin() = default;
  ~FtsGravCompPlugin() override = default;
 
  std::vector<std::string> get_sensor_names_from_parameters();
 
  bool register_fts(const mjModel* model);
 
  bool init(rclcpp::Node::SharedPtr node, const mjModel* model, mjData* data) override;
 
  void update_read(const mjModel* model, mjData* data) override;
 
  void cleanup() override;
 
  std::vector<FtsData> get_fts_data() const
  {
    return fts_;
  }
 
private:
  rclcpp::Node::SharedPtr node_;
  rclcpp::Logger logger_ = rclcpp::get_logger("FtsGravCompPlugin");
 
  std::vector<FtsData> fts_;
};
 
}  // namespace mujoco_ros2_control_plugins
 
#endif  // MUJOCO_ROS2_CONTROL_PLUGINS__FTS_GRAV_COMP_PLUGIN_HPP_