cartesian_impedance_base.hpp

Go to the documentation of this file.

#pragma once
 
#include <Eigen/Core>
#include <Eigen/Eigenvalues>
#include <array>
#include <cmath>
#include <memory>
#include <optional>
#include <variant>
#include <vector>
 
#include "franky/motion/motion.hpp"
#include "franky/motion/torque_control_utils.hpp"
#include "franky/twist.hpp"
#include "franky/twist_acceleration.hpp"
#include "franky/wait_free_triple_buffer.hpp"
 
namespace franky {
 
struct CartesianReference {
  Affine target{Affine::Identity()};
 
  std::optional<Twist> target_twist{};
 
  std::optional<TwistAcceleration> target_acceleration{};
 
  void validate() const {
    validateFinite(target.matrix(), "target");
    if (target_twist.has_value()) validateFinite(target_twist->vector_repr(), "target_twist");
    if (target_acceleration.has_value()) validateFinite(target_acceleration->vector_repr(), "target_acceleration");
  }
};
 
inline Matrix6d cartesianGainBlocks(double translational, double rotational) {
  Matrix6d gains = Matrix6d::Zero();
  gains.topLeftCorner<3, 3>() = translational * Eigen::Matrix3d::Identity();
  gains.bottomRightCorner<3, 3>() = rotational * Eigen::Matrix3d::Identity();
  return gains;
}
 
inline Matrix6d defaultCartesianImpedanceStiffness() { return cartesianGainBlocks(500.0, 50.0); }
 
inline Matrix6d defaultCartesianImpedanceDamping(const Matrix6d &stiffness) {
  Eigen::SelfAdjointEigenSolver<Matrix6d> solver(stiffness);
  return 2.0 * solver.operatorSqrt();
}
 
struct CartesianImpedanceGains {
  CartesianImpedanceGains() = default;
 
  explicit CartesianImpedanceGains(Matrix6d stiffness, std::optional<Matrix6d> damping = std::nullopt)
      : stiffness(std::move(stiffness)), damping(std::move(damping)) {
    validate();
  }
 
  static CartesianImpedanceGains isotropic(
      double translational_stiffness, double rotational_stiffness,
      std::optional<double> translational_damping = std::nullopt,
      std::optional<double> rotational_damping = std::nullopt) {
    CartesianImpedanceGains gains;
    gains.stiffness = cartesianGainBlocks(translational_stiffness, rotational_stiffness);
    if (translational_damping.has_value() || rotational_damping.has_value()) {
      gains.damping = cartesianGainBlocks(
          translational_damping.value_or(2.0 * std::sqrt(translational_stiffness)),
          rotational_damping.value_or(2.0 * std::sqrt(rotational_stiffness)));
    }
    gains.validate();
    return gains;
  }
 
  static CartesianImpedanceGains diagonal(const Vector6d &stiffness, std::optional<Vector6d> damping = std::nullopt) {
    CartesianImpedanceGains gains;
    gains.stiffness = stiffness.asDiagonal();
    if (damping.has_value()) gains.damping = damping->asDiagonal();
    gains.validate();
    return gains;
  }
 
  Matrix6d stiffness{defaultCartesianImpedanceStiffness()};
  std::optional<Matrix6d> damping{std::nullopt};
 
  void validate() const {
    validateFinite(stiffness, "stiffness");
    if (damping.has_value()) validateFinite(*damping, "damping");
  }
};
 
struct PostureTask {
  PostureTask() = default;
 
  PostureTask(
      const Vector7d &target, const Vector7d &stiffness, std::optional<Vector7d> damping = std::nullopt,
      std::optional<double> max_torque = std::nullopt)
      : target(target), stiffness(stiffness), damping(std::move(damping)), max_torque(max_torque) {}
 
  PostureTask(
      const Vector7d &target, double stiffness, std::optional<double> damping = std::nullopt,
      std::optional<double> max_torque = std::nullopt)
      : target(target), stiffness(Vector7d::Constant(stiffness)), max_torque(max_torque) {
    if (damping.has_value()) this->damping = Vector7d::Constant(*damping);
  }
 
  Vector7d target{Vector7d::Zero()};
 
  Vector7d stiffness{Vector7d::Zero()};
 
  std::optional<Vector7d> damping{std::nullopt};
 
  std::optional<double> max_torque{std::nullopt};
};
 
struct ManipulabilityTask {
  ManipulabilityTask() = default;
 
  ManipulabilityTask(double gain, double damping = 0.0, std::optional<double> max_torque = std::nullopt)
      : gain(gain), damping(damping), max_torque(max_torque) {}
 
  double gain{0.0};
 
  double damping{0.0};
 
  std::optional<double> max_torque{std::nullopt};
};
 
using NullspaceTask = std::variant<PostureTask, ManipulabilityTask>;
 
struct NullspaceGains {
  Vector7d posture_stiffness{Vector7d::Zero()};
  std::optional<Vector7d> posture_damping{std::nullopt};
  std::optional<double> posture_max_torque{std::nullopt};
 
  double manipulability_gain{0.0};
  double manipulability_damping{0.0};
  std::optional<double> manipulability_max_torque{std::nullopt};
};
 
class CartesianImpedanceBase : public Motion<franka::Torques> {
 public:
  struct Params {
    Matrix6d stiffness{defaultCartesianImpedanceStiffness()};
 
    std::optional<Matrix6d> damping{std::nullopt};
 
    Eigen::Vector3d translational_error_clip{Eigen::Vector3d::Constant(0.10)};
 
    Eigen::Vector3d rotational_error_clip{Eigen::Vector3d::Constant(0.25)};
 
    std::array<std::optional<double>, 6> force_constraints{};
 
    std::vector<NullspaceTask> nullspace_tasks{};
 
    TorqueSafetyParams safety{};
 
    FrictionCompensationParams friction{};
 
    void validate() const {
      validateFinite(stiffness, "stiffness");
      if (damping.has_value()) validateFinite(*damping, "damping");
      friction.validate();
    }
  };
 
  [[nodiscard]] const Affine &target() const { return target_; }
 
  void setGains(const CartesianImpedanceGains &gains) {
    gains.validate();
    gains_handle_.set(gains);
  }
  [[nodiscard]] CartesianImpedanceGains getGains() const { return gains_handle_.getLastWritten(); }
 
  void setNullspaceGains(const NullspaceGains &gains) { nullspace_gains_handle_.set(gains); }
  [[nodiscard]] NullspaceGains getNullspaceGains() const { return nullspace_gains_handle_.getLastWritten(); }
 
 protected:
  explicit CartesianImpedanceBase(Affine target, const Params &params, double gains_time_constant = 0.1);
 
  [[nodiscard]] franka::Torques computeCommand(
      const RobotState &robot_state, const CartesianReference &reference, double dt);
 
  [[nodiscard]] inline const Params &base_params() const { return params_; }
 
  Affine target_;
 
 private:
  const Matrix6d &criticalDamping();
 
  Params params_;
 
  WaitFreeTripleBuffer<CartesianImpedanceGains> gains_handle_;
  WaitFreeTripleBuffer<NullspaceGains> nullspace_gains_handle_;
  double gains_time_constant_;
  Matrix6d current_stiffness_;
  Matrix6d current_damping_;
  NullspaceGains current_nullspace_gains_;
 
  Matrix6d critical_damping_;
 
  std::optional<Matrix6d> critical_damping_stiffness_;
};
 
using ImpedanceMotion = CartesianImpedanceBase;
 
}  // namespace franky