franka_cartesian_command_interface.h

Go to the documentation of this file.

// Copyright (c) 2017 Franka Emika GmbH
// Use of this source code is governed by the Apache-2.0 license, see LICENSE
#pragma once

#include <array>

#include <franka_hw/franka_state_interface.h>
#include <hardware_interface/internal/hardware_resource_manager.h>

namespace franka_hw {

class FrankaCartesianPoseHandle : public FrankaStateHandle {
 public:
  FrankaCartesianPoseHandle() = delete;

  FrankaCartesianPoseHandle(const FrankaStateHandle& franka_state_handle,
                            std::array<double, 16>& command,
                            std::array<double, 2>& elbow)
      : FrankaStateHandle(franka_state_handle), command_(&command), elbow_(&elbow) {}

  void setCommand(const std::array<double, 16>& command) noexcept {
    *command_ = command;
    *elbow_ = {};
  }

  void setCommand(const std::array<double, 16>& command,
                  const std::array<double, 2>& elbow) noexcept {
    *command_ = command;
    *elbow_ = elbow;
  }

  const std::array<double, 16>& getCommand() const noexcept { return *command_; }

 private:
  std::array<double, 16>* command_;
  std::array<double, 2>* elbow_;
};

class FrankaPoseCartesianInterface
    : public hardware_interface::HardwareResourceManager<FrankaCartesianPoseHandle,
                                                         hardware_interface::ClaimResources> {};

class FrankaCartesianVelocityHandle : public FrankaStateHandle {
 public:
  FrankaCartesianVelocityHandle() = delete;

  FrankaCartesianVelocityHandle(const FrankaStateHandle& franka_state_handle,
                                std::array<double, 6>& command,
                                std::array<double, 2>& elbow)
      : FrankaStateHandle(franka_state_handle), command_(&command), elbow_(&elbow) {}

  void setCommand(std::array<double, 6>& command) noexcept {
    *command_ = command;
    *elbow_ = {};
  }

  void setCommand(const std::array<double, 6>& command,
                  const std::array<double, 2>& elbow) noexcept {
    *command_ = command;
    *elbow_ = elbow;
  }

  const std::array<double, 6>& getCommand() const noexcept { return *command_; }

 private:
  std::array<double, 6>* command_;
  std::array<double, 2>* elbow_;
};

class FrankaVelocityCartesianInterface
    : public hardware_interface::HardwareResourceManager<FrankaCartesianVelocityHandle,
                                                         hardware_interface::ClaimResources> {};

}  // namespace franka_hw