pose.cpp

Go to the documentation of this file.

#include <fanuc_post_processor/pose.hpp>

namespace fanuc_post_processor
{

FanucPose::FanucPose(const std::vector<FanucAxis> extended_axes,
                     const std::vector<FanucGroup> extra_groups)
{
  // Add first group
  FanucAxis x(FanucAxis::AxisType::LINEAR, "X");
  FanucAxis y(FanucAxis::AxisType::LINEAR, "Y");
  FanucAxis z(FanucAxis::AxisType::LINEAR, "Z");
  FanucAxis w(FanucAxis::AxisType::ROTATIVE, "W");
  FanucAxis p(FanucAxis::AxisType::ROTATIVE, "P");
  FanucAxis r(FanucAxis::AxisType::ROTATIVE, "R");

  std::vector<FanucAxis> axis;
  axis.push_back(x);
  axis.push_back(y);
  axis.push_back(z);
  axis.push_back(w);
  axis.push_back(p);
  axis.push_back(r);

  // Append extended axis
  axis.insert(std::end(axis), std::begin(extended_axes), std::end(extended_axes));

  FanucGroup gp1(axis);
  groups_.push_back(gp1);

  // Append extra groups
  groups_.insert(std::end(groups_), std::begin(extra_groups), std::end(extra_groups));
}

FanucPose::~FanucPose()
{
}

std::string FanucPose::movementTypeToString() const
{
  if (move_type_ == JOINT)
    return "J";
  else if (move_type_ == LINEAR)
    return "L";
  else
    return "MovementType not recognized!";
}

std::string FanucPose::speedTypeToString() const
{
  if (speed_type_ == MM_SEC)
    return "mm/sec";
  else if (speed_type_ == CM_SEC)
    return "cm/sec";
  else if (speed_type_ == M_SEC)
    return "m/sec";
  else if (speed_type_ == MM_MIN)
    return "mm/min";
  else if (speed_type_ == CM_MIN)
    return "cm/min";
  else if (speed_type_ == M_MIN)
    return "m/min";
  else if (speed_type_ == PERCENTAGE)
    return "%";
  else
    return "";
}

std::string FanucPose::configurationToString() const
{
  std::string configuration("");

  if (std::get<0>(configuration_) == FlipNonFlip::Flip)
    configuration.append("F ");
  else
    configuration.append("N "); // Non-flip

  if (std::get<1>(configuration_) == Elbow::Down)
    configuration.append("D ");
  else
    configuration.append("U "); // Up

  if (std::get<2>(configuration_) == BackFront::Back)
    configuration.append("B");
  else
    configuration.append("T"); // Front

  return configuration;
}

bool FanucPose::hasSameGroupsAxes(const FanucPose &other) const
                                  {
  if (other.groups_.size() != groups_.size())
    return false;

  for (unsigned i(0); i < groups_.size(); ++i)
  {
    if (other.groups_[i].axis_.size() != groups_[i].axis_.size())
      return false;

    for (unsigned j(0); j < groups_[i].axis_.size(); ++j)
    {
      if (other.groups_[i].axis_[j].name_ != groups_[i].axis_[j].name_)
        return false;
      if (other.groups_[i].axis_[j].type_ != groups_[i].axis_[j].type_)
        return false;
    } // axis for loop
  } // groups for loop

  return true;
}

bool FanucPose::isValid() const
{
  if (groups_.empty())
  {
    ROS_ERROR_STREAM("Pose has empty groups");
    return false;
  }

  if (groups_[0].axis_.size() < 6)
  {
    ROS_ERROR_STREAM("Pose does not have at least 6 axes");
    return false;
  }

  if (groups_[0].axis_[0].name_ != "X" &&
      groups_[0].axis_[0].type_ != FanucAxis::AxisType::LINEAR)
  {
    ROS_ERROR_STREAM("Pose has wrong X axis");
    return false;
  }

  if (groups_[0].axis_[1].name_ != "Y" &&
      groups_[0].axis_[1].type_ != FanucAxis::AxisType::LINEAR)
  {
    ROS_ERROR_STREAM("Pose has wrong Y axis");
    return false;
  }

  if (groups_[0].axis_[2].name_ != "Z" &&
      groups_[0].axis_[2].type_ != FanucAxis::AxisType::LINEAR)

  {
    ROS_ERROR_STREAM("Pose has wrong Z axis");
    return false;
  }

  if (groups_[0].axis_[3].name_ != "W" &&
      groups_[0].axis_[3].type_ != FanucAxis::AxisType::ROTATIVE)
  {
    ROS_ERROR_STREAM("Pose has wrong W axis");
    return false;
  }

  if (groups_[0].axis_[4].name_ != "P" &&
      groups_[0].axis_[4].type_ != FanucAxis::AxisType::ROTATIVE)
  {
    ROS_ERROR_STREAM("Pose has wrong P axis");
    return false;
  }
  if (groups_[0].axis_[5].name_ != "R" &&
      groups_[0].axis_[5].type_ != FanucAxis::AxisType::ROTATIVE)
  {
    ROS_ERROR_STREAM("Pose has wrong R axis");
    return false;
  }

  if (pose_id_ == 0)
  {
    ROS_ERROR_STREAM("Pose ID cannot be zero");
    return false;
  }

  if (move_type_ == JOINT)
  {
    if (speed_type_ != PERCENTAGE)
    {
      ROS_ERROR_STREAM("Speed type in joint mode must be PERCENTAGE");
      return false;
    }

    if (speed_ > 100)
    {
      ROS_ERROR_STREAM("Speed percentage cannot be > 100%");
      return false;
    }
  }
  else if (move_type_ == LINEAR)
  {
    if (speed_type_ == PERCENTAGE)
    {
      ROS_ERROR_STREAM("Pose is LINEAR, speed cannot be PERCENTAGE");
      return false;
    }
  }
  else
    return false;

  if (cnt_ > 100)
  {
    ROS_ERROR_STREAM("CNT cannot be > 100");
    return false;
  }

  // Speed is a register number
  if (speed_register_ && speed_ < 1)
  {
    ROS_ERROR_STREAM("Speed register specified: register ID cannot be zero!");
    return false;
  }

  for (auto group : groups_)
  {
    if (group.axis_.empty())
    {
      ROS_ERROR_STREAM("Group axes cannot be empty");
      return false;
    }

    if (group.user_frame_ > 9)
    {
      ROS_ERROR_STREAM("Group user frame cannot be > 9");
      return false;
    }

    if (group.tool_frame_ == 0)
    {
      ROS_ERROR_STREAM("Group tool frame cannot be zero");
      return false;
    }

    if (group.tool_frame_ > 9)
    {
      ROS_ERROR_STREAM("Group tool frame cannot be > 9");
      return false;
    }
  }
  return true;
}

std::string FanucPose::getMainLineString() const
{
  if (!isValid())
    return "";

  std::string line("");

  const std::string p_id(std::to_string(pose_id_));
  const std::string movement(movementTypeToString());
  const std::string speed_unit(speedTypeToString());

  std::string move_speed;
  if (speed_register_)
    move_speed = "R[" + std::to_string((unsigned)speed_) + "]";
  else
    move_speed = std::to_string(unsigned(speed_));

  line = movement + " P[" + p_id + "] " + move_speed + speed_unit;

  if (cnt_ == 0) // FINE
    line.append(" FINE " + option_ + ";");
  else
    // CNT 1-100
    line.append(" CNT" + std::to_string(cnt_) + " " + option_ + ";");

  return line;
}

std::string FanucPose::getPoseLinesString() const
{
  if (!isValid())
    return "";

  std::string pose_block;
  pose_block.append("P[" + std::to_string(pose_id_) + "]{\n");
  pose_block.append("   GP1:\n");
  pose_block.append(
                    "\tUF : " + std::to_string(groups_[0].user_frame_) + ", UT : "
                        + std::to_string(groups_[0].tool_frame_) + ",         CONFIG : '");

  pose_block.append(configurationToString());
  pose_block.append(", ");
  pose_block.append(std::to_string(flipnonflip_rotations_) + ", ");
  pose_block.append(std::to_string(elbow_rotations_) + ", ");
  pose_block.append(std::to_string(backfront_rotations_) + "',\n");

  pose_block.append(
                    "\tX = " + std::to_string(groups_[0].axis_[0].value_ * 1000.0) + " mm, Y = "
                        + std::to_string(groups_[0].axis_[1].value_ * 1000.0)
                        + " mm, Z = " + std::to_string(groups_[0].axis_[2].value_ * 1000.0) + " mm,\n");
  pose_block.append(
      "\tW = " + std::to_string(groups_[0].axis_[3].value_ * 180.0 / M_PI) + " deg, P = " + std::to_string(groups_[0].axis_[4].value_* 180.0 / M_PI)
          + " deg, R = " + std::to_string(groups_[0].axis_[5].value_ * 180.0 / M_PI) + " deg");

  // Append extended axis
  if (groups_[0].axis_.size() > 6)
  {
    pose_block.append(",\n");

    for (unsigned i(6); i < groups_[0].axis_.size(); ++i)
    {
      double value(groups_[0].axis_[i].value_);
      if (groups_[0].axis_[i].type_ == FanucAxis::AxisType::ROTATIVE)
        value *= 180.0 / M_PI; // rad2deg
      else
        value *= 1000.0; // meters to millimeters

      pose_block.append("\t");
      pose_block.append(groups_[0].axis_[i].name_ + " = " + std::to_string(value));

      if (groups_[0].axis_[i].type_ == FanucAxis::AxisType::LINEAR)
        pose_block.append(" mm");
      else
        pose_block.append(" deg");

      if (i == groups_[0].axis_.size() - 1)
        pose_block.append("\n");
      else
        pose_block.append(",\n");
    }
  }
  else
    pose_block.append("\n");

  // Append other groups
  if (groups_.size() != 1)
  {
    for (unsigned group_number(1); group_number < groups_.size(); ++group_number)
    {
      unsigned axis_per_line(0);

      pose_block.append("   GP" + std::to_string(group_number + 1) + ":\n");
      pose_block.append(
                        "\tUF : " + std::to_string(groups_[group_number].user_frame_) + ", UT : "
                            + std::to_string(groups_[group_number].tool_frame_) + ",\n");

      for (auto &axis : groups_[group_number].axis_)
      {
        if (axis_per_line == 0)
          pose_block.append("\t");
        else
          pose_block.append(" ");
        pose_block.append(axis.name_ + "= ");

        double value(axis.value_);
        if (axis.type_ == FanucAxis::AxisType::ROTATIVE)
          value *= 180.0 / M_PI; // rad2deg
        else
          value *= 1000.0; // meters to millimeters

        pose_block.append(std::to_string(value));

        if (axis.type_ == FanucAxis::AxisType::LINEAR)
          pose_block.append(" mm");
        else
          pose_block.append(" deg");

        if (&axis != &groups_[group_number].axis_.back())
          pose_block.append(",");

        if (axis_per_line == 2)
        {
          pose_block.append("\n");
          axis_per_line = 0;
          continue;
        }
        ++axis_per_line;
      }

      if (axis_per_line != 0)
        pose_block.append("\n");
    }

  }

  pose_block.append("};\n");
  return pose_block;
}

void FanucPose::setRobotPose(const Eigen::Isometry3d &pose)
{
  if (!isValid())
    return;

  const Eigen::Vector3d euler_angles(pose.linear().eulerAngles(0, 1, 2));
  groups_[0].axis_[0].value_ = pose.translation().x();
  groups_[0].axis_[1].value_ = pose.translation().y();
  groups_[0].axis_[2].value_ = pose.translation().z();

  // W
  groups_[0].axis_[3].value_ = euler_angles[0];
  // P
  groups_[0].axis_[4].value_ = euler_angles[1];
  // R
  groups_[0].axis_[5].value_ = euler_angles[2];
}

}