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 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{};
 
  franka::Errors current_errors{};
 
  franka::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