arm_system.h

Go to the documentation of this file.

#pragma once

// Html table writer
#include <aerial_autonomy/common/html_utils.h>
// Base robot system
#include <aerial_autonomy/robot_systems/base_robot_system.h>
// Arm hardware
#include <arm_parsers/arm_parser.h>

#include <iomanip>
#include <sstream>

class ArmSystem : public virtual BaseRobotSystem {

private:
  ArmParser &arm_hardware_;

public:
  ArmSystem(ArmParser &arm_hardware)
      : BaseRobotSystem(), arm_hardware_(arm_hardware) {}

  Eigen::Matrix4d getEndEffectorPose() {
    return arm_hardware_.getEndEffectorTransform();
  }

  bool grip(bool grip_action) {
    if (grip_action) {
      //\todo Gowtham Change arm plugins to update gripping strategy
      int grip_position = 1000;
      return arm_hardware_.grip(grip_position);
    } else {
      //\todo Gowtham Change arm plugins to update gripping strategy
      int grip_position = 2000;
      return arm_hardware_.grip(grip_position);
    }
  }

  void power(bool state) {
    if (state) {
      arm_hardware_.sendCmd(ArmParser::POWER_ON);
    } else {
      arm_hardware_.sendCmd(ArmParser::POWER_OFF);
    }
  }

  void foldArm() { arm_hardware_.sendCmd(ArmParser::FOLD_ARM); }

  void rightArm() { arm_hardware_.sendCmd(ArmParser::RIGHT_ARM); }

  std::string getSystemStatus() const {
    HtmlTableWriter table_writer;
    table_writer.beginRow();
    table_writer.addHeader("Arm Status:", Colors::blue, 4);
    table_writer.beginRow();
    table_writer.addCell("Joint Angles: ");
    for (double q : arm_hardware_.getJointAngles()) {
      table_writer.addCell(q);
    }
    table_writer.beginRow();
    table_writer.addCell("Joint Velocities: ");
    for (double q : arm_hardware_.getJointVelocities()) {
      table_writer.addCell(q);
    }
    Eigen::Matrix4d ee_transform = arm_hardware_.getEndEffectorTransform();
    table_writer.beginRow();
    table_writer.addCell("End effector translation: ");
    for (int i = 0; i < 3; ++i) {
      table_writer.addCell(ee_transform(i, 3));
    }
    Eigen::Matrix3d ee_rotation = ee_transform.topLeftCorner(3, 3);
    Eigen::Vector3d euler_angles = ee_rotation.eulerAngles(2, 1, 0);
    table_writer.beginRow();
    table_writer.addCell("End effector RPY: ");
    for (int i = 0; i < 3; ++i) {
      table_writer.addCell(euler_angles[i]);
    }
    table_writer.beginRow();
    std::string command_status = (getCommandStatus() ? "True" : "False");
    std::string command_status_color =
        (getCommandStatus() ? Colors::green : Colors::red);
    table_writer.addCell(command_status, "CommandStatus", command_status_color,
                         2);
    table_writer.beginRow();
    std::string arm_enabled = (enabled() ? "True" : "False");
    std::string enabled_color = (enabled() ? Colors::green : Colors::red);
    table_writer.addCell(arm_enabled, "Enabled", enabled_color, 2);
    return table_writer.getTableString();
  }

  bool getCommandStatus() const { return arm_hardware_.getCommandStatus(); }

  bool enabled() const {
    return arm_hardware_.state == ArmParser::ENABLED ? true : false;
  }
};