robot_state.hpp

Go to the documentation of this file.

// Adapted from
// https://github.com/frankaemika/libfranka/blob/main/include/franka/robot_state.h
#pragma once
 
#include <franka/robot_state.h>
 
#include "franky/elbow_state.hpp"
#include "franky/twist.hpp"
#include "franky/twist_acceleration.hpp"
#include "franky/types.hpp"
#include "franky/util.hpp"
 
namespace franky {
 
struct Errors : public franka::Errors {
  Errors() noexcept : franka::Errors() {}
  Errors(const franka::Errors &other) noexcept : franka::Errors(other) {}
  Errors(const Errors &other) noexcept : franka::Errors(other) {}
  Errors &operator=(const Errors &other) noexcept {
    franka::Errors::operator=(other);
    return *this;
  }
};
 
struct RobotState {
  static RobotState from_franka(
      const franka::RobotState &robot_state, std::optional<Vector7d> q_est = std::nullopt,
      std::optional<Vector7d> dq_est = std::nullopt, std::optional<Vector7d> ddq_est = std::nullopt,
      std::optional<Twist> O_dP_EE_est = std::nullopt, std::optional<TwistAcceleration> O_ddP_EE_est = std::nullopt,
      std::optional<double> delbow_est = std::nullopt, std::optional<double> ddelbow_est = std::nullopt);
 
  static RobotState from_franka(
      const franka::RobotState &robot_state, const Jacobian &ee_jacobian, const Vector7d &q_est, const Vector7d &dq_est,
      const Vector7d &ddq_est);
 
  Affine O_T_EE{};  // NOLINT(readability-identifier-naming)
 
  Affine O_T_EE_d{};  // NOLINT(readability-identifier-naming)
 
  Affine F_T_EE{};  // NOLINT(readability-identifier-naming)
 
#ifdef FRANKA_0_8
  Affine F_T_NE{};  // NOLINT(readability-identifier-naming)
 
  Affine NE_T_EE{};  // NOLINT(readability-identifier-naming)
#endif
 
  Affine EE_T_K{};  // NOLINT(readability-identifier-naming)
 
  double m_ee{};
 
  IntertiaMatrix I_ee{};  // NOLINT(readability-identifier-naming)
 
  Eigen::Vector3d F_x_Cee{};  // NOLINT(readability-identifier-naming)
 
  double m_load{};
 
  IntertiaMatrix I_load{};  // NOLINT(readability-identifier-naming)
 
  Eigen::Vector3d F_x_Cload{};  // NOLINT(readability-identifier-naming)
 
  double m_total{};
 
  IntertiaMatrix I_total{};  // NOLINT(readability-identifier-naming)
 
  Eigen::Vector3d F_x_Ctotal{};  // NOLINT(readability-identifier-naming)
 
  ElbowState elbow{};
 
  ElbowState elbow_d{};
 
  ElbowState elbow_c{};
 
  double delbow_c{};
 
  double ddelbow_c{};
 
  Vector7d tau_J{};  // NOLINT(readability-identifier-naming)
 
  Vector7d tau_J_d{};  // NOLINT(readability-identifier-naming)
 
  Vector7d dtau_J{};  // NOLINT(readability-identifier-naming)
 
  Vector7d q{};
 
  Vector7d q_d{};
 
  Vector7d dq{};
 
  Vector7d dq_d{};
 
  Vector7d ddq_d{};
 
  Vector7d joint_contact{};
 
  Vector6d cartesian_contact{};
 
  Vector7d joint_collision{};
 
  Vector6d cartesian_collision{};
 
  Vector7d tau_ext_hat_filtered{};
 
  Vector6d O_F_ext_hat_K{};  // NOLINT(readability-identifier-naming)
 
  Vector6d K_F_ext_hat_K{};  // NOLINT(readability-identifier-naming)
 
  Twist O_dP_EE_d{};  // NOLINT(readability-identifier-naming)
 
#ifdef FRANKA_0_9
  Eigen::Vector3d O_ddP_O{};  // NOLINT(readability-identifier-naming)
#endif
 
  Affine O_T_EE_c{};  // NOLINT(readability-identifier-naming)
 
  Twist O_dP_EE_c{};  // NOLINT(readability-identifier-naming)
 
  TwistAcceleration O_ddP_EE_c{};  // NOLINT(readability-identifier-naming)
 
  Vector7d theta{};
 
  Vector7d dtheta{};
 
  Errors current_errors{};
 
  Errors last_motion_errors{};
 
  double control_command_success_rate{};
 
  franka::RobotMode robot_mode = franka::RobotMode::kUserStopped;
 
  franka::Duration time{};
 
  std::optional<Vector7d> q_est{};
 
  std::optional<Vector7d> dq_est{};
 
  std::optional<Vector7d> ddq_est{};
 
  std::optional<Twist> O_dP_EE_est{};
 
  std::optional<TwistAcceleration> O_ddP_EE_est{};
 
  std::optional<double> delbow_est{};
 
  std::optional<double> ddelbow_est{};
};
 
}  // namespace franky