impedance_motion.hpp

Go to the documentation of this file.

#pragma once
 
#include <map>
#include <optional>
 
#include <Eigen/Core>
#include <Eigen/Geometry>
 
#include "franky/robot_pose.hpp"
#include "franky/motion/motion.hpp"
#include "franky/motion/reference_type.hpp"
 
namespace franky {
 
class ImpedanceMotion : public Motion<franka::Torques> {
 public:
  struct Params {
    ReferenceType target_type{ReferenceType::kAbsolute};
 
    double translational_stiffness{2000};
 
    double rotational_stiffness{200};
 
    Eigen::Vector<double, 6> force_constraints;
 
    Eigen::Vector<bool, 6> force_constraints_active{Eigen::Vector<bool, 6>::Zero()};
  };
 
  explicit ImpedanceMotion(Affine target, const Params &params);
 
 protected:
  void initImpl(const RobotState &robot_state, const std::optional<franka::Torques> &previous_command) override;
 
  franka::Torques
  nextCommandImpl(
      const RobotState &robot_state,
      franka::Duration time_step,
      franka::Duration rel_time,
      franka::Duration abs_time,
      const std::optional<franka::Torques> &previous_command) override;
 
  [[nodiscard]] inline Affine intermediate_target() const {
    return intermediate_target_;
  }
 
  [[nodiscard]] inline Affine target() const {
    return absolute_target_;
  }
 
  virtual std::tuple<Affine, bool>
  update(const RobotState &robot_state, franka::Duration time_step, franka::Duration time) = 0;
 
 private:
  Affine absolute_target_;
  Affine target_;
  Params params_;
 
  Eigen::Matrix<double, 6, 6> stiffness, damping;
  Affine intermediate_target_;
 
  std::unique_ptr<franka::Model> model_;
};
 
}  // namespace franky