joint_impedance_base.hpp

Go to the documentation of this file.

#pragma once
 
#include <memory>
#include <optional>
 
#include "franky/motion/cartesian_impedance_base.hpp"
#include "franky/motion/motion.hpp"
#include "franky/motion/torque_control_utils.hpp"
#include "franky/types.hpp"
#include "franky/wait_free_triple_buffer.hpp"
 
namespace franky {
 
inline Vector7d defaultJointImpedanceStiffness() { return Vector7d::Constant(50.0); }
 
inline Vector7d defaultJointImpedanceDamping(const Vector7d &stiffness) { return 2.0 * stiffness.cwiseSqrt(); }
 
inline Vector7d defaultJointImpedanceDamping() {
  return defaultJointImpedanceDamping(defaultJointImpedanceStiffness());
}
 
struct JointReference {
  Vector7d q{Vector7d::Zero()};
  Vector7d dq{Vector7d::Zero()};
  Vector7d tau_ff{Vector7d::Zero()};
 
  void validate() const {
    validateFinite(q, "q");
    validateFinite(dq, "dq");
    validateFinite(tau_ff, "tau_ff");
  }
};
 
struct JointImpedanceGains {
  JointImpedanceGains() = default;
 
  explicit JointImpedanceGains(
      const std::optional<Vector7d> &stiffness, const std::optional<Vector7d> &damping = std::nullopt)
      : stiffness(stiffness.value_or(defaultJointImpedanceStiffness())),
        damping(damping.has_value() ? *damping : defaultJointImpedanceDamping(this->stiffness)) {
    validate();
  }
 
  Vector7d stiffness{defaultJointImpedanceStiffness()};
  Vector7d damping{defaultJointImpedanceDamping()};
 
  void validate() const {
    validateNonNegativeFinite(stiffness, "stiffness");
    validateNonNegativeFinite(damping, "damping");
  }
};
 
struct JointImpedanceParams {
  Vector7d stiffness{defaultJointImpedanceStiffness()};
 
  Vector7d damping{defaultJointImpedanceDamping()};
 
  Vector7d error_clip{Vector7d::Constant(0.5)};
 
  Vector7d constant_torque_offset{Vector7d::Zero()};
 
  bool compensate_coriolis{true};
 
  TorqueSafetyParams safety{};
 
  FrictionCompensationParams friction{};
 
  std::optional<CartesianImpedanceGains> cartesian_gains{std::nullopt};
 
  void validate() const {
    validateNonNegativeFinite(stiffness, "stiffness");
    validateNonNegativeFinite(damping, "damping");
    validateNonNegativeFinite(error_clip, "error_clip");
    if (cartesian_gains.has_value()) {
      cartesian_gains->validate();
    }
    friction.validate();
  }
};
 
class JointImpedanceBase : public Motion<franka::Torques> {
 public:
  [[nodiscard]] const Vector7d &target() const { return target_; }
  [[nodiscard]] const Vector7d &target_velocity() const { return target_velocity_; }
  [[nodiscard]] const JointImpedanceParams &params() const { return params_; }
 
  void setGains(const JointImpedanceGains &gains) {
    gains.validate();
    gains_handle_.set(gains);
  }
  [[nodiscard]] JointImpedanceGains getGains() const { return gains_handle_.getLastWritten(); }
 
  void setCartesianGains(const CartesianImpedanceGains &gains) {
    gains.validate();
    cartesian_gains_handle_.set(gains);
  }
  [[nodiscard]] CartesianImpedanceGains getCartesianGains() const { return cartesian_gains_handle_.getLastWritten(); }
 
 protected:
  explicit JointImpedanceBase(
      const Vector7d &target, const Vector7d &target_velocity, const JointImpedanceParams &params,
      double gains_time_constant = 0.1);
 
  [[nodiscard]] franka::Torques computeCommand(
      const RobotState &robot_state, const JointReference &reference, double dt);
 
  JointImpedanceParams params_;
  Vector7d target_;
  Vector7d target_velocity_;
 
 private:
  struct CartesianShapingState {
    Matrix6d stiffness;
    Matrix6d damping;                                    // always concrete, interpolated
    Matrix6d critical_damping;                           // cached critical(stiffness)
    std::optional<Matrix6d> critical_damping_stiffness;  // stiffness the cache was computed for
  };
 
  // Critical damping for the shaping stiffness; recomputes the eigendecomposition only while the
  // stiffness moves and caches it otherwise.
  static const Matrix6d &criticalShapingDamping(CartesianShapingState &shaping);
 
  WaitFreeTripleBuffer<JointImpedanceGains> gains_handle_;
  WaitFreeTripleBuffer<CartesianImpedanceGains> cartesian_gains_handle_;
  double gains_time_constant_;
  Vector7d current_stiffness_;
  Vector7d current_damping_;
  std::optional<CartesianShapingState> cartesian_shaping_;
};
 
}  // namespace franky