control_types.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 <cmath>
#include <initializer_list>

namespace franka {

enum class ControllerMode { kJointImpedance, kCartesianImpedance };

enum class RealtimeConfig { kEnforce, kIgnore };

struct Finishable {
  bool motion_finished = false;
};

class Torques : public Finishable {
 public:
  Torques(const std::array<double, 7>& torques) noexcept;

  Torques(std::initializer_list<double> torques);

  std::array<double, 7> tau_J{};  // NOLINT(readability-identifier-naming)
};

class JointPositions : public Finishable {
 public:
  JointPositions(const std::array<double, 7>& joint_positions) noexcept;

  JointPositions(std::initializer_list<double> joint_positions);

  std::array<double, 7> q{};
};

class JointVelocities : public Finishable {
 public:
  JointVelocities(const std::array<double, 7>& joint_velocities) noexcept;

  JointVelocities(std::initializer_list<double> joint_velocities);

  std::array<double, 7> dq{};
};

class CartesianPose : public Finishable {
 public:
  CartesianPose(const std::array<double, 16>& cartesian_pose) noexcept;

  CartesianPose(const std::array<double, 16>& cartesian_pose,
                const std::array<double, 2>& elbow) noexcept;

  CartesianPose(std::initializer_list<double> cartesian_pose);

  CartesianPose(std::initializer_list<double> cartesian_pose, std::initializer_list<double> elbow);

  std::array<double, 16> O_T_EE{};  // NOLINT(readability-identifier-naming)

  std::array<double, 2> elbow{};

  bool hasElbow() const noexcept;
};

class CartesianVelocities : public Finishable {
 public:
  CartesianVelocities(const std::array<double, 6>& cartesian_velocities) noexcept;

  CartesianVelocities(const std::array<double, 6>& cartesian_velocities,
                      const std::array<double, 2>& elbow) noexcept;

  CartesianVelocities(std::initializer_list<double> cartesian_velocities);

  CartesianVelocities(std::initializer_list<double> cartesian_velocities,
                      std::initializer_list<double> elbow);

  std::array<double, 6> O_dP_EE{};  // NOLINT(readability-identifier-naming)

  std::array<double, 2> elbow{};

  bool hasElbow() const noexcept;
};

inline Torques MotionFinished(Torques command) noexcept {  // NOLINT(readability-identifier-naming)
  command.motion_finished = true;
  return command;
}

inline JointPositions MotionFinished(  // NOLINT(readability-identifier-naming)
    JointPositions command) noexcept {
  command.motion_finished = true;
  return command;
}

inline JointVelocities MotionFinished(  // NOLINT(readability-identifier-naming)
    JointVelocities command) noexcept {
  command.motion_finished = true;
  return command;
}

inline CartesianPose MotionFinished(  // NOLINT(readability-identifier-naming)
    CartesianPose command) noexcept {
  command.motion_finished = true;
  return command;
}

inline CartesianVelocities MotionFinished(  // NOLINT(readability-identifier-naming)
    CartesianVelocities command) noexcept {
  command.motion_finished = true;
  return command;
}

}  // namespace franka