dynamics.cpp

Go to the documentation of this file.

 
#include <tesseract_common/macros.h>
TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
#include <stdexcept>
 
#include <console_bridge/console.h>
#include <tinyxml2.h>
TESSERACT_COMMON_IGNORE_WARNINGS_POP
 
#include <tesseract_scene_graph/joint.h>
#include <tesseract_urdf/dynamics.h>
#include <tesseract_urdf/utils.h>
 
namespace tesseract_urdf
{
tesseract_scene_graph::JointDynamics::Ptr parseDynamics(const tinyxml2::XMLElement* xml_element)
{
  if (xml_element->Attribute("damping") == nullptr && xml_element->Attribute("friction") == nullptr)
    std::throw_with_nested(std::runtime_error("Dynamics: Missing both attributes 'damping' and 'friction', remove tag "
                                              "or add attributes and values!"));
 
  auto dynamics = std::make_shared<tesseract_scene_graph::JointDynamics>();
 
  int status = xml_element->QueryDoubleAttribute("damping", &(dynamics->damping));
  if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Dynamics: Error parsing attribute 'damping'!"));
 
  if (status == tinyxml2::XML_NO_ATTRIBUTE)
    CONSOLE_BRIDGE_logDebug("Dynamics: Missing attribute 'damping', using default value 0!");
 
  status = xml_element->QueryDoubleAttribute("friction", &(dynamics->friction));
  if (status != tinyxml2::XML_NO_ATTRIBUTE && status != tinyxml2::XML_SUCCESS)
    std::throw_with_nested(std::runtime_error("Dynamics: Error parsing attribute 'friction'!"));
 
  if (status == tinyxml2::XML_NO_ATTRIBUTE)
    CONSOLE_BRIDGE_logDebug("Dynamics: Missing attribute 'friction', using default value 0!");
 
  return dynamics;
}
 
tinyxml2::XMLElement* writeDynamics(const std::shared_ptr<const tesseract_scene_graph::JointDynamics>& dynamics,
                                    tinyxml2::XMLDocument& doc)
{
  if (dynamics == nullptr)
    std::throw_with_nested(std::runtime_error("Dynamics is nullptr and cannot be converted to XML"));
  tinyxml2::XMLElement* xml_element = doc.NewElement(DYNAMICS_ELEMENT_NAME.data());
 
  xml_element->SetAttribute("damping", toString(dynamics->damping).c_str());
  xml_element->SetAttribute("friction", toString(dynamics->damping).c_str());
 
  return xml_element;
}
 
}  // namespace tesseract_urdf