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