joint_wall.h

Go to the documentation of this file.

// Copyright (c) 2022 Franka Emika GmbH
#pragma once

#include <array>
#include <memory>

// clang-format off
// clang-format on

namespace franka_example_controllers {

class JointWall {
 public:
  JointWall() = delete;

  JointWall(double soft_upper_joint_position_limit,
            double soft_lower_joint_position_limit,
            double PD_zone_width,
            double D_zone_width,
            double PD_zone_stiffness,
            double PD_zone_damping,
            double D_zone_damping);

  double computeTorque(double q, double dq);

  void reset();

 private:
  enum class MotionInWall {
    EnteringNormal,
    PenetratingLowerLimit,
    LeavingLowerLimit,
    PenetratingUpperLimit,
    LeavingUpperLimit
  };

  // Joint wall parameters
  double soft_upper_joint_position_limit_{0};
  double soft_lower_joint_position_limit_{0};
  double PD_zone_width_{0};
  double D_zone_width_{0};
  double PD_zone_stiffness_{0};
  double PD_zone_damping_{0};
  double D_zone_damping_{0};

  bool initialized_{false};  // Internal states q, dq will be reset to the current states in
                             // computeTorque if initialized_ = false;

  // Indicates whether the joint wall is still moving, which occurs after initializing the state
  // inside joint wall.
  bool moving_wall_{false};

  double zone_width_scale_{1};

  static bool inRange(double low, double high, double x);

  static double positiveCheck(double value);

  void init(double q, double dq);

  void adjustMovingWall(double q, double dq);

  MotionInWall getMotionInWall(double q, double dq) const;
};

template <size_t num_dof>
class JointWallContainer {
 public:
  JointWallContainer(const std::array<double, num_dof>& soft_upper_joint_position_limits,
                     const std::array<double, num_dof>& soft_lower_joint_position_limits,
                     const std::array<double, num_dof>& PD_zone_widths,
                     const std::array<double, num_dof>& D_zone_widths,
                     const std::array<double, num_dof>& PD_zone_stiffnesses,
                     const std::array<double, num_dof>& PD_zone_dampings,
                     const std::array<double, num_dof>& D_zone_dampings) {
    for (size_t i = 0; i < num_dof; i++) {
      joint_walls_.at(i) = std::make_unique<JointWall>(
          soft_upper_joint_position_limits[i], soft_lower_joint_position_limits[i],
          PD_zone_widths[i], D_zone_widths[i], PD_zone_stiffnesses[i], PD_zone_dampings[i],
          D_zone_dampings[i]);
    }
  }

  JointWallContainer() = delete;

  std::array<double, num_dof> computeTorque(const std::array<double, num_dof>& q,
                                            const std::array<double, num_dof>& dq) {
    std::array<double, num_dof> torque;
    for (size_t i = 0; i < num_dof; i++) {
      torque[i] = joint_walls_[i]->computeTorque(q[i], dq[i]);
    };
    return torque;
  }

  void reset() {
    for (auto& jw : joint_walls_) {
      jw->reset();
    }
  }

 private:
  std::array<std::unique_ptr<JointWall>, num_dof> joint_walls_;
};

}  // namespace franka_example_controllers