<commit_before>#include "riptide_controllers/thruster_controller.h"

#undef debug
#undef report
#undef progress

tf::Matrix3x3 rotation_matrix;
tf::Vector3 ang_v;

// Thrust limits (N):
double MIN_THRUST = -8.0;
double MAX_THRUST = 8.0;

// Vehicle mass (kg):
double MASS = 34.47940950;

// Moments of inertia (kg*m^2)
double Ixx = 0.50862680;
double Iyy = 1.70892348;
double Izz = 1.77586420;

// Acceleration commands (m/s^):
double cmdSurge = 0.0;
double cmdSway = 0.0;
double cmdHeave = 0.0;
double cmdRoll = 0.0;
double cmdPitch = 0.0;
double cmdYaw = 0.0;

struct vector
{
  double x;
  double y;
  double z;
};

void get_transform(vector *v, tf::StampedTransform *tform)
{
  v->x = tform->getOrigin().x();
  v->y = tform->getOrigin().y();
  v->z = tform->getOrigin().z();
  return;
}

/*** Thruster Positions ***/
// Positions are in meters relative to the center of mass.
vector pos_surge_stbd_hi;
vector pos_surge_port_hi;
vector pos_surge_port_lo;
vector pos_surge_stbd_lo;
vector pos_sway_fwd;
vector pos_sway_aft;
vector pos_heave_port_aft;
vector pos_heave_stbd_aft;
vector pos_heave_stbd_fwd;
vector pos_heave_port_fwd;

/*** EQUATIONS ***/
// These equations solve for linear/angular acceleration in all axes

// Linear Equations
struct surge
{
  template <typename T>
  bool operator()(const T *const surge_port_hi, const T *const surge_stbd_hi, const T *const surge_port_lo,
                  const T *const surge_stbd_lo, const T *const sway_fwd, const T *const sway_aft,
                  const T *const heave_port_fwd, const T *const heave_stbd_fwd, const T *const heave_port_aft,
                  const T *const heave_stbd_aft, T *residual) const
  {
    residual[0] =
        (rotation_matrix.getRow(0).x() * (/*surge_port_hi[0] + surge_stbd_hi[0] + */surge_port_lo[0] + surge_stbd_lo[0]) +
         rotation_matrix.getRow(0).y() * (sway_fwd[0] + sway_aft[0]) +
         rotation_matrix.getRow(0).z() *
             (heave_port_fwd[0] + heave_stbd_fwd[0] + heave_port_aft[0] + heave_stbd_aft[0])) /
            T(MASS) -
        T(cmdSurge);
    return true;
  }
};

struct sway
{
  template <typename T>
  bool operator()(const T *const surge_port_hi, const T *const surge_stbd_hi, const T *const surge_port_lo,
                  const T *const surge_stbd_lo, const T *const sway_fwd, const T *const sway_aft,
                  const T *const heave_port_fwd, const T *const heave_stbd_fwd, const T *const heave_port_aft,
                  const T *const heave_stbd_aft, T *residual) const
  {
    residual[0] =
        (rotation_matrix.getRow(1).x() * (/*surge_port_hi[0] + surge_stbd_hi[0] +*/ surge_port_lo[0] + surge_stbd_lo[0]) +
         rotation_matrix.getRow(1).y() * (sway_fwd[0] + sway_aft[0]) +
         rotation_matrix.getRow(1).z() *
             (heave_port_fwd[0] + heave_stbd_fwd[0] + heave_stbd_aft[0] + heave_port_aft[0])) /
            T(MASS) -
        T(cmdSway);
    return true;
  }
};

struct heave
{
  template <typename T>
  bool operator()(const T *const surge_port_hi, const T *const surge_stbd_hi, const T *const surge_port_lo,
                  const T *const surge_stbd_lo, const T *const sway_fwd, const T *const sway_aft,
                  const T *const heave_port_fwd, const T *const heave_stbd_fwd, const T *const heave_port_aft,
                  const T *const heave_stbd_aft, T *residual) const
  {
    residual[0] =
        (rotation_matrix.getRow(2).x() * (surge_port_hi[0] + surge_stbd_hi[0] + surge_port_lo[0] + surge_stbd_lo[0]) +
         rotation_matrix.getRow(2).y() * (sway_fwd[0] + sway_aft[0]) +
         rotation_matrix.getRow(2).z() *
             (heave_port_fwd[0] + heave_stbd_fwd[0] + heave_port_aft[0] + heave_stbd_aft[0])) /
            T(MASS) -
        T(cmdHeave);
    return true;
  }
};<commit_msg>Added buoyancy to thruster controller<commit_after>#include "riptide_controllers/thruster_controller.h"

#undef debug
#undef report
#undef progress

tf::Matrix3x3 rotation_matrix;
tf::Vector3 ang_v;

// Thrust limits (N):
double MIN_THRUST = -8.0;
double MAX_THRUST = 8.0;

// Vehicle mass (kg):
// TODO: Get this value from model
double MASS = 34.47940950;

// Vehcile volume (m^3)
// TODO: Get this value from model
double VOLUME = 0.0334;

// Gravity (m/s^2)
double GRAVITY = 9.81;

// Water density (kg/m^3)
double WATER_DENSITY = 1000.0;
double BUOYANCY = VOLUME * WATER_DENSITY * GRAVITY;

// Moments of inertia (kg*m^2)
double Ixx = 0.50862680;
double Iyy = 1.70892348;
double Izz = 1.77586420;

// Acceleration commands (m/s^):
double cmdSurge = 0.0;
double cmdSway = 0.0;
double cmdHeave = 0.0;
double cmdRoll = 0.0;
double cmdPitch = 0.0;
double cmdYaw = 0.0;

struct vector
{
  double x;
  double y;
  double z;
};

void get_transform(vector *v, tf::StampedTransform *tform)
{
  v->x = tform->getOrigin().x();
  v->y = tform->getOrigin().y();
  v->z = tform->getOrigin().z();
  return;
}

/*** Thruster Positions ***/
// Positions are in meters relative to the center of mass.
vector pos_surge_stbd_hi;
vector pos_surge_port_hi;
vector pos_surge_port_lo;
vector pos_surge_stbd_lo;
vector pos_sway_fwd;
vector pos_sway_aft;
vector pos_heave_port_aft;
vector pos_heave_stbd_aft;
vector pos_heave_stbd_fwd;
vector pos_heave_port_fwd;

/*** EQUATIONS ***/
// These equations solve for linear/angular acceleration in all axes

// Linear Equations
struct surge
{
  template <typename T>
  bool operator()(const T *const surge_port_hi, const T *const surge_stbd_hi, const T *const surge_port_lo,
                  const T *const surge_stbd_lo, const T *const sway_fwd, const T *const sway_aft,
                  const T *const heave_port_fwd, const T *const heave_stbd_fwd, const T *const heave_port_aft,
                  const T *const heave_stbd_aft, T *residual) const
  {
    residual[0] =
        (rotation_matrix.getRow(0).x() * (/*surge_port_hi[0] + surge_stbd_hi[0] + */surge_port_lo[0] + surge_stbd_lo[0]) +
         rotation_matrix.getRow(0).y() * (sway_fwd[0] + sway_aft[0]) +
         rotation_matrix.getRow(0).z() *
             (heave_port_fwd[0] + heave_stbd_fwd[0] + heave_port_aft[0] + heave_stbd_aft[0])) /
            T(MASS) -
        T(cmdSurge);
    return true;
  }
};

struct sway
{
  template <typename T>
  bool operator()(const T *const surge_port_hi, const T *const surge_stbd_hi, const T *const surge_port_lo,
                  const T *const surge_stbd_lo, const T *const sway_fwd, const T *const sway_aft,
                  const T *const heave_port_fwd, const T *const heave_stbd_fwd, const T *const heave_port_aft,
                  const T *const heave_stbd_aft, T *residual) const
  {
    residual[0] =
        (rotation_matrix.getRow(1).x() * (/*surge_port_hi[0] + surge_stbd_hi[0] +*/ surge_port_lo[0] + surge_stbd_lo[0]) +
         rotation_matrix.getRow(1).y() * (sway_fwd[0] + sway_aft[0]) +
         rotation_matrix.getRow(1).z() *
             (heave_port_fwd[0] + heave_stbd_fwd[0] + heave_stbd_aft[0] + heave_port_aft[0])) /
            T(MASS) -
        T(cmdSway);
    return true;
  }
};

struct heave
{
  template <typename T>
  bool operator()(const T *const surge_port_hi, const T *const surge_stbd_hi, const T *const surge_port_lo,
                  const T *const surge_stbd_lo, const T *const sway_fwd, const T *const sway_aft,
                  const T *const heave_port_fwd, const T *const heave_stbd_fwd, const T *const heave_port_aft,
                  const T *const heave_stbd_aft, T *residual) const
  {
    residual[0] =
        (rotation_matrix.getRow(2).x() * (/*surge_port_hi[0] + surge_stbd_hi[0]*/ + surge_port_lo[0] + surge_stbd_lo[0]) +
         rotation_matrix.getRow(2).y() * (sway_fwd[0] + sway_aft[0]) +
         rotation_matrix.getRow(2).z() * (heave_port_fwd[0] + heave_stbd_fwd[0] + heave_port_aft[0] + heave_stbd_aft[0]) +
         T(BUOYANCY)) /
         T(MASS) -
         T(cmdHeave);
    return true;
  }
};
{'fraction_non_alphanumeric': 0.1250634840020315, 'fraction_numerical': 0.02717115286947689, 'mean_word_length': 3.2258583690987126, 'pattern_counts': {'":': 0, '<': 9, '<?xml version=': 0, '>': 21, 'https://': 0, 'lorem ipsum': 0, 'www.': 0, 'xml': 0}, 'pii_count': 0, 'substrings_counts': 0, 'word_list_counts': {'cursed_substrings.json': 0, 'profanity_word_list.json': 0, 'sexual_word_list.json': 0, 'zh_pornsignals.json': 0}}
{'dir': 'git-commits-cleaned', 'id': '7312909', 'n_tokens_mistral': 3170, 'n_tokens_neox': 3052, 'n_words': 691}