limits.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <stdexcept>
 
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_common/utils.h>
#include <tesseract_scene_graph/joint.h>
#include <tesseract_urdf/limits.h>
#include <tesseract_urdf/utils.h>
 
namespace tesseract_urdf
{
tesseract_scene_graph::JointLimits::Ptr parseLimits(const tinyxml2::XMLElement* xml_element)
{
  auto limits = std::make_shared<tesseract_scene_graph::JointLimits>();
 
  int status = xml_element->QueryDoubleAttribute("lower", &(limits->lower));
  if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Limits: Missing or failed to parse attribute 'lower'!"));
 
  status = xml_element->QueryDoubleAttribute("upper", &(limits->upper));
  if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Limits: Missing or failed to parse attribute 'upper'!"));
 
  if (xml_element->QueryDoubleAttribute("effort", &(limits->effort)) != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Limits: Missing or failed to parse attribute 'effort'!"));
 
  if (xml_element->QueryDoubleAttribute("velocity", &(limits->velocity)) != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Limits: Missing or failed to parse attribute 'velocity'!"));
 
  status = xml_element->QueryDoubleAttribute("acceleration", &(limits->acceleration));
  if (status == tinyxml2::XML_NO_ATTRIBUTE)
    limits->acceleration = 0.5 * limits->velocity;
  else if (status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Limits: Failed to parse attribute 'acceleration'!"));
 
  status = xml_element->QueryDoubleAttribute("jerk", &(limits->jerk));
  if (status == tinyxml2::XML_NO_ATTRIBUTE)
    limits->jerk = 1000;
  else if (status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Limits: Failed to parse attribute 'jerk'!"));
 
  return limits;
}
 
tinyxml2::XMLElement* writeLimits(const std::shared_ptr<const tesseract_scene_graph::JointLimits>& limits,
                                  tinyxml2::XMLDocument& doc)
{
  if (limits == nullptr)
    std::throw_with_nested(std::runtime_error("Limits are nullptr and cannot be converted to XML"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(LIMITS_ELEMENT_NAME.data());
 
  // if upper and lower are both zero, don't write it.  This should only happen for continuous joints.
  if (!tesseract_common::almostEqualRelativeAndAbs(limits->lower, 0.0) ||
      !tesseract_common::almostEqualRelativeAndAbs(limits->upper, 0.0))
  {
    xml_element->SetAttribute("lower", toString(limits->lower).c_str());
    xml_element->SetAttribute("upper", toString(limits->upper).c_str());
  }
 
  // Always write effort & velocity
  xml_element->SetAttribute("effort", toString(limits->effort).c_str());
  xml_element->SetAttribute("velocity", toString(limits->velocity).c_str());
 
  // Write out nonzero acceleration (Tesseract-exclusive)
  if (!tesseract_common::almostEqualRelativeAndAbs(limits->acceleration, 0.0) &&
      !tesseract_common::almostEqualRelativeAndAbs(limits->acceleration, limits->velocity * 0.5))
    xml_element->SetAttribute("acceleration", toString(limits->acceleration).c_str());
 
  // Write out nonzero jerk (Tesseract-exclusive)
  if (!tesseract_common::almostEqualRelativeAndAbs(limits->jerk, 0.0) &&
      !tesseract_common::almostEqualRelativeAndAbs(limits->jerk, 1000))
    xml_element->SetAttribute("jerk", toString(limits->jerk).c_str());
 
  return xml_element;
}
 
}  // namespace tesseract_urdf