Program Listing for File robot.hpp

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

// Copyright (c) 2023 Franka Robotics GmbH
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#pragma once

#include <array>
#include <atomic>
#include <iostream>
#include <memory>
#include <mutex>
#include <string>
#include <thread>

#include <franka/active_control.h>
#include <franka/active_control_base.h>
#include <franka/active_motion_generator.h>
#include <franka/active_torque_control.h>

#include <franka/model.h>
#include <franka/robot.h>
#include <franka_hardware/model.hpp>

#include <franka_msgs/srv/set_cartesian_stiffness.hpp>
#include <franka_msgs/srv/set_force_torque_collision_behavior.hpp>
#include <franka_msgs/srv/set_full_collision_behavior.hpp>
#include <franka_msgs/srv/set_joint_stiffness.hpp>
#include <franka_msgs/srv/set_load.hpp>
#include <franka_msgs/srv/set_stiffness_frame.hpp>
#include <franka_msgs/srv/set_tcp_frame.hpp>

#include <rclcpp/logger.hpp>

namespace franka_hardware {

class Robot {
 public:
  explicit Robot(std::unique_ptr<franka::Robot> robot, std::unique_ptr<Model> model);
  explicit Robot(const std::string& robot_ip, const rclcpp::Logger& logger);
  Robot(const Robot&) = delete;
  Robot& operator=(const Robot& other) = delete;
  Robot& operator=(Robot&& other) = delete;
  Robot(Robot&& other) = delete;

  virtual ~Robot();

  virtual void initializeTorqueInterface();

  virtual void initializeJointVelocityInterface();

  virtual void initializeJointPositionInterface();

  virtual void initializeCartesianVelocityInterface();

  virtual void initializeCartesianPoseInterface();

  virtual void stopRobot();

  virtual franka::RobotState readOnce();

  virtual franka_hardware::Model* getModel();

  virtual void writeOnce(const std::array<double, 7>& joint_hardware_command);

  virtual void writeOnce(const std::array<double, 6>& cartesian_velocity_command);

  virtual void writeOnce(const std::array<double, 6>& cartesian_velocity_command,
                         const std::array<double, 2>& elbow_command);

  virtual void writeOnce(const std::array<double, 16>& cartesian_pose_command);

  virtual void writeOnce(const std::array<double, 16>& cartesian_pose_command,
                         const std::array<double, 2>& elbow_command);

  virtual void setJointStiffness(
      const franka_msgs::srv::SetJointStiffness::Request::SharedPtr& req);

  virtual void setCartesianStiffness(
      const franka_msgs::srv::SetCartesianStiffness::Request::SharedPtr& req);

  virtual void setLoad(const franka_msgs::srv::SetLoad::Request::SharedPtr& req);

  virtual void setTCPFrame(const franka_msgs::srv::SetTCPFrame::Request::SharedPtr& req);

  virtual void setStiffnessFrame(
      const franka_msgs::srv::SetStiffnessFrame::Request::SharedPtr& req);

  virtual void setForceTorqueCollisionBehavior(
      const franka_msgs::srv::SetForceTorqueCollisionBehavior::Request::SharedPtr& req);

  virtual void setFullCollisionBehavior(
      const franka_msgs::srv::SetFullCollisionBehavior::Request::SharedPtr& req);

  virtual void automaticErrorRecovery();

 protected:
  Robot() = default;

 private:
  virtual franka::RobotState readOnceActiveControl();

  virtual void writeOnceEfforts(const std::array<double, 7>& efforts);

  virtual void writeOnceJointVelocities(const std::array<double, 7>& joint_velocities);

  virtual void writeOnceJointPositions(const std::array<double, 7>& positions);

  franka::CartesianVelocities preProcessCartesianVelocities(
      const franka::CartesianVelocities& cartesian_velocities);

  franka::CartesianPose preProcessCartesianPose(const franka::CartesianPose& cartesian_pose);

  std::mutex write_mutex_;
  std::mutex control_mutex_;

  std::unique_ptr<franka::Robot> robot_;
  std::unique_ptr<franka::ActiveControlBase> active_control_ = nullptr;
  std::unique_ptr<franka::Model> model_;
  std::unique_ptr<Model> franka_hardware_model_;

  bool effort_interface_active_{false};
  bool joint_velocity_interface_active_{false};
  bool joint_position_interface_active_{false};
  bool cartesian_velocity_interface_active_{false};
  bool cartesian_pose_interface_active_{false};

  bool torque_command_rate_limiter_active_{true};
  bool velocity_command_rate_limit_active_{false};

  bool cartesian_velocity_command_rate_limit_active_{false};
  bool cartesian_velocity_low_pass_filter_active_{false};

  bool cartesian_pose_low_pass_filter_active_{false};
  bool cartesian_pose_command_rate_limit_active_{false};

  bool joint_position_command_rate_limit_active_{false};
  bool joint_position_command_low_pass_filter_active_{false};

  double low_pass_filter_cut_off_freq{100.0};

  franka::RobotState current_state_;
};
}  // namespace franka_hardware