2019-11-14-sdformat-semantic-pose-idea.cc

template <typename Class, typename... Args>
math::RigidTransformd ResolveRigidTransform(
    const Class& element, Args... args) {
  ignition::math::Pose3d pose;
  ThrowAnyErrors(element.ResolvePose(args..., pose));
  return ToRigidTransform(pose);
}

ResolveRigidTransform(link, model_frame)

---

math::RigidTransform ResolveRigidTransform(const SemanticPose& pose, std::string relative_to = "") {
  Pose3d value;
  ThrowAnyErrors(pose.Resolve(relative_to, value));
  return ToRigidTransform(pose);
}

ResolveRigidTransform(link.SemanticPose(), model_frame)

gistfile1.txt

class Link {
  
  PoseElement* GetPoseElement();
  ...
  
  ElementPtr xml;
}

class PoseElement {

  void SetRelativeTo(...)
  string GetRelativeTo(...)
  
  Pose3d GetRawValue()
  Pose3d SetRawValue(...)
  
  Pose3d Resolve(...)

  ...

  ElementPtr xml;
};


class AxisXyzElement {
  
  string GetExpressedIn(...);
  ...
  
  ...
  ElementPtr xml;
};