joint_position_controller.h

Go to the documentation of this file.

// Refered to https://github.com/frankaemika/franka_ros/tree/develop/franka_example_controllers
 
#pragma once
 
#include <array>
#include <string>
#include <vector>
 
#include <controller_interface/multi_interface_controller.h>
#include <hardware_interface/joint_command_interface.h>
#include <hardware_interface/robot_hw.h>
#include <ros/node_handle.h>
#include <ros/time.h>
 
namespace serl_franka_controllers {
 
class JointPositionController : public controller_interface::MultiInterfaceController<
                                           hardware_interface::PositionJointInterface> {
 public:
  bool init(hardware_interface::RobotHW* robot_hardware, ros::NodeHandle& node_handle) override;
  void starting(const ros::Time&) override;
  void update(const ros::Time&, const ros::Duration& period) override;
 
 private:
  double cubicInterpolation(double p0, double p1, double t);
  hardware_interface::PositionJointInterface* position_joint_interface_;
  std::vector<hardware_interface::JointHandle> position_joint_handles_;
  ros::Duration elapsed_time_;
  std::array<double, 7> initial_pose_{};
  std::array<double, 7> reset_pose_{};
};
 
}  // namespace serl_franka_controllers