Update SdfGenerator to save joint data to file

src/SdfGenerator.cc

@@ -29,26 +29,33 @@
 #include "ignition/gazebo/components/AirPressureSensor.hh"
 #include "ignition/gazebo/components/Altimeter.hh"
 #include "ignition/gazebo/components/Camera.hh"
+#include "ignition/gazebo/components/ChildLinkName.hh"
 #include "ignition/gazebo/components/ContactSensor.hh"
 #include "ignition/gazebo/components/DepthCamera.hh"
 #include "ignition/gazebo/components/ForceTorque.hh"
 #include "ignition/gazebo/components/GpuLidar.hh"
 #include "ignition/gazebo/components/Imu.hh"
 #include "ignition/gazebo/components/Inertial.hh"
+#include "ignition/gazebo/components/Joint.hh"
+#include "ignition/gazebo/components/JointAxis.hh"
+#include "ignition/gazebo/components/JointType.hh"
 #include "ignition/gazebo/components/Light.hh"
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/LogicalCamera.hh"
 #include "ignition/gazebo/components/Magnetometer.hh"
 #include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
+#include "ignition/gazebo/components/ParentLinkName.hh"
 #include "ignition/gazebo/components/Pose.hh"
 #include "ignition/gazebo/components/RgbdCamera.hh"
 #include "ignition/gazebo/components/SelfCollide.hh"
+#include "ignition/gazebo/components/Sensor.hh"
 #include "ignition/gazebo/components/SourceFilePath.hh"
 #include "ignition/gazebo/components/SegmentationCamera.hh"
 #include "ignition/gazebo/components/Static.hh"
 #include "ignition/gazebo/components/ThermalCamera.hh"
+#include "ignition/gazebo/components/ThreadPitch.hh"
 #include "ignition/gazebo/components/WindMode.hh"
 #include "ignition/gazebo/components/World.hh"
 
@@ -502,6 +509,21 @@ namespace sdf_generator
       }
     }
 
+    if (_elem->HasElement("joint"))
+    {
+      // update joints
+      sdf::ElementPtr jointElem = _elem->GetElement("joint");
+      while (jointElem)
+      {
+        std::string jointName = jointElem->Get<std::string>("name");
+        auto jointEnt = _ecm.EntityByComponents(
+            components::ParentEntity(_entity), components::Name(jointName));
+        if (jointEnt != kNullEntity)
+          updateJointElement(jointElem, _ecm, jointEnt);
+        jointElem = jointElem->GetNextElement("joint");
+      }
+    }
+
     return true;
   }
 
@@ -603,7 +625,6 @@ namespace sdf_generator
     // Update sdf based on current components.
     // This list is to be updated as other components become updateable during
     // simulation
-
     auto updateSensorNameAndPose = [&]
     {
       // override name and pose sdf element using values from ECM
@@ -771,6 +792,144 @@ namespace sdf_generator
     return true;
   }
 
+  /////////////////////////////////////////////////
+  bool updateJointElement(sdf::ElementPtr _elem,
+                          const EntityComponentManager &_ecm,
+                          const Entity &_entity)
+  {
+    // Update sdf based on the joint component
+    auto updateJointNameAndPose = [&]
+    {
+      // override name and pose sdf element using values from ECM
+      auto *nameComp = _ecm.Component<components::Name>(_entity);
+      _elem->GetAttribute("name")->Set(nameComp->Data());
+
+      auto *poseComp = _ecm.Component<components::Pose>(_entity);
+      auto poseElem = _elem->GetElement("pose");
+
+      // Remove all attributes of poseElem
+      for (const auto *attrName : {"relative_to", "degrees", "rotation_format"})
+      {
+        sdf::ParamPtr attr = poseElem->GetAttribute(attrName);
+        if (nullptr != attr)
+        {
+          attr->Reset();
+        }
+      }
+      poseElem->Set(poseComp->Data());
+      return true;
+    };
+
+    // joint
+    auto jointComp = _ecm.Component<components::Joint>(_entity);
+    if (!jointComp)
+    {
+      return false;
+    }
+
+    // joint type
+    auto jointTypeComp = _ecm.Component<components::JointType>(_entity);
+    sdf::JointType jointType = jointTypeComp->Data();
+    if (jointTypeComp)
+    {
+      std::string jointTypeStr = "invalid";
+      switch (jointType)
+      {
+        case sdf::JointType::BALL:
+          jointTypeStr = "ball";
+          break;
+        case sdf::JointType::CONTINUOUS:
+          jointTypeStr = "continuous";
+          break;
+        case sdf::JointType::FIXED:
+          jointTypeStr = "fixed";
+          break;
+        case sdf::JointType::PRISMATIC:
+          jointTypeStr = "prismatic";
+          break;
+        case sdf::JointType::GEARBOX:
+          jointTypeStr = "gearbox";
+          break;
+        case sdf::JointType::REVOLUTE:
+          jointTypeStr = "revolute";
+          break;
+        case sdf::JointType::REVOLUTE2:
+          jointTypeStr = "revolute2";
+          break;
+        case sdf::JointType::SCREW:
+          jointTypeStr = "screw";
+          break;
+        case sdf::JointType::UNIVERSAL:
+          jointTypeStr = "universal";
+          break;
+        default:
+          break;
+      }
+      _elem->GetAttribute("type")->Set<std::string>(jointTypeStr);
+    }
+
+    // parent
+    auto parentLinkNameComp =
+        _ecm.Component<components::ParentLinkName>(_entity);
+    if (parentLinkNameComp)
+    {
+      _elem->GetElement("parent")->Set<std::string>(parentLinkNameComp->Data());
+    }
+    // child
+    auto childLinkNameComp = _ecm.Component<components::ChildLinkName>(_entity);
+    if (childLinkNameComp)
+    {
+      _elem->GetElement("child")->Set<std::string>(childLinkNameComp->Data());
+    }
+    // thread pitch
+    auto threadPitchComp = _ecm.Component<components::ThreadPitch>(_entity);
+    if (threadPitchComp && jointType == sdf::JointType::SCREW)
+    {
+      _elem->GetElement("thread_pitch")->Set<double>(threadPitchComp->Data());
+    }
+    // axis
+    auto jointAxisComp = _ecm.Component<components::JointAxis>(_entity);
+    if (jointAxisComp)
+    {
+      const sdf::JointAxis axis = jointAxisComp->Data();
+      _elem->GetElement("axis")->Copy(axis.ToElement());
+    }
+    // axis2
+    auto jointAxis2Comp = _ecm.Component<components::JointAxis2>(_entity);
+    if (jointAxis2Comp)
+    {
+      const sdf::JointAxis axis2 = jointAxis2Comp->Data();
+      _elem->GetElement("axis2")->Copy(axis2.ToElement(1u));
+    }
+
+    // sensors
+    // remove existing ones in sdf element and add new ones from ECM.
+    std::vector<sdf::ElementPtr> toRemove;
+    if (_elem->HasElement("sensor"))
+    {
+      for (auto sensorElem = _elem->GetElement("sensor"); sensorElem;
+           sensorElem = sensorElem->GetNextElement("sensor"))
+      {
+        toRemove.push_back(sensorElem);
+      }
+    }
+    for (const auto &e : toRemove)
+    {
+      _elem->RemoveChild(e);
+    }
+
+    auto sensorEntities = _ecm.EntitiesByComponents(
+        components::ParentEntity(_entity), components::Sensor());
+
+    for (const auto &sensorEnt : sensorEntities)
+    {
+      sdf::ElementPtr sensorElem = _elem->AddElement("sensor");
+      updateSensorElement(sensorElem, _ecm, sensorEnt);
+    }
+
+    return updateJointNameAndPose();
+  }
+
   /////////////////////////////////////////////////
   /// \brief Checks if a string is a number
   /// \param[in] _str The string to check

src/SdfGenerator.hh

@@ -131,6 +131,17 @@ namespace sdf_generator
   bool updateLightElement(sdf::ElementPtr _elem,
                            const EntityComponentManager &_ecm,
                            const Entity &_entity);
+
+  /// \brief Update an sdf::Element of a joint.
+  /// Intended for internal use.
+  /// \input[in, out] _elem sdf::Element to update
+  /// \input[in] _ecm Immutable reference to the Entity Component Manager
+  /// \input[in] _entity joint entity
+  /// \returns true if update succeeded.
+  IGNITION_GAZEBO_VISIBLE
+  bool updateJointElement(sdf::ElementPtr _elem,
+                           const EntityComponentManager &_ecm,
+                           const Entity &_entity);
 }  // namespace sdf_generator
 }  // namespace IGNITION_GAZEBO_VERSION_NAMESPACE
 }  // namespace gazebo

test/integration/save_world.cc

@@ -26,6 +26,8 @@
 #include <sdf/Collision.hh>
 #include <sdf/ForceTorque.hh>
 #include <sdf/Imu.hh>
+#include <sdf/Joint.hh>
+#include <sdf/JointAxis.hh>
 #include <sdf/Magnetometer.hh>
 #include <sdf/Model.hh>
 #include <sdf/Lidar.hh>
@@ -855,6 +857,105 @@ TEST_F(SdfGeneratorFixture, WorldWithLights)
   ASSERT_NE(nullptr, genWorld);
 }
 
+/////////////////////////////////////////////////
+TEST_F(SdfGeneratorFixture, ModelWithJoints)
+{
+  this->LoadWorld(ignition::common::joinPaths("test", "worlds",
+      "joint_sensor.sdf"));
+
+  const std::string worldGenSdfRes =
+      this->RequestGeneratedSdf("joint_sensor");
+
+  sdf::Root root;
+  sdf::Errors err = root.LoadSdfString(worldGenSdfRes);
+  EXPECT_TRUE(err.empty());
+  auto *world = root.WorldByIndex(0);
+  ASSERT_NE(nullptr, world);
+  EXPECT_EQ(1u, world->ModelCount());
+
+  EXPECT_TRUE(world->ModelNameExists("model"));
+  auto *model = world->ModelByName("model");
+  ASSERT_NE(nullptr, model);
+  EXPECT_EQ(2u, model->LinkCount());
+  auto *link1 = model->LinkByName("link1");
+  ASSERT_NE(nullptr, link1);
+  auto *link2 = model->LinkByName("link2");
+  ASSERT_NE(nullptr, link2);
+  EXPECT_EQ(1u, model->JointCount());
+  auto *joint = model->JointByName("joint");
+  ASSERT_NE(nullptr, joint);
+
+  EXPECT_EQ("link1", joint->ParentLinkName());
+  EXPECT_EQ("link2", joint->ChildLinkName());
+  EXPECT_EQ(sdf::JointType::REVOLUTE2, joint->Type());
+
+  // Get the first axis
+  const sdf::JointAxis *axis = joint->Axis();
+  ASSERT_NE(nullptr, axis);
+  ASSERT_NE(nullptr, axis->Element());
+
+  // Get the second axis
+  const sdf::JointAxis *axis2 = joint->Axis(1);
+  ASSERT_NE(nullptr, axis2);
+
+  EXPECT_EQ(ignition::math::Vector3d::UnitZ, axis->Xyz());
+  EXPECT_EQ(ignition::math::Vector3d::UnitY, axis2->Xyz());
+
+  EXPECT_EQ("__model__", axis->XyzExpressedIn());
+  EXPECT_TRUE(axis2->XyzExpressedIn().empty());
+
+  EXPECT_DOUBLE_EQ(-0.5, axis->Lower());
+  EXPECT_DOUBLE_EQ(0.5, axis->Upper());
+  EXPECT_DOUBLE_EQ(-1.0, axis2->Lower());
+  EXPECT_DOUBLE_EQ(1.0, axis2->Upper());
+
+  EXPECT_DOUBLE_EQ(123.4, axis->Effort());
+  EXPECT_DOUBLE_EQ(0.5, axis2->Effort());
+
+  EXPECT_DOUBLE_EQ(12.0, axis->MaxVelocity());
+  EXPECT_DOUBLE_EQ(200.0, axis2->MaxVelocity());
+
+  EXPECT_DOUBLE_EQ(0.1, axis->Damping());
+  EXPECT_DOUBLE_EQ(0.0, axis2->Damping());
+
+  EXPECT_DOUBLE_EQ(0.2, axis->Friction());
+  EXPECT_DOUBLE_EQ(0.0, axis2->Friction());
+
+  EXPECT_DOUBLE_EQ(1.3, axis->SpringReference());
+  EXPECT_DOUBLE_EQ(0.0, axis2->SpringReference());
+
+  EXPECT_DOUBLE_EQ(10.6, axis->SpringStiffness());
+  EXPECT_DOUBLE_EQ(0.0, axis2->SpringStiffness());
+
+  // sensor
+  const sdf::Sensor *forceTorqueSensor =
+    joint->SensorByName("force_torque_sensor");
+  ASSERT_NE(nullptr, forceTorqueSensor);
+  EXPECT_EQ("force_torque_sensor", forceTorqueSensor->Name());
+  EXPECT_EQ(sdf::SensorType::FORCE_TORQUE, forceTorqueSensor->Type());
+  EXPECT_EQ(ignition::math::Pose3d(10, 11, 12, 0, 0, 0),
+      forceTorqueSensor->RawPose());
+  auto forceTorqueSensorObj = forceTorqueSensor->ForceTorqueSensor();
+  ASSERT_NE(nullptr, forceTorqueSensorObj);
+  EXPECT_EQ(sdf::ForceTorqueFrame::PARENT, forceTorqueSensorObj->Frame());
+  EXPECT_EQ(sdf::ForceTorqueMeasureDirection::PARENT_TO_CHILD,
+      forceTorqueSensorObj->MeasureDirection());
+
+  EXPECT_DOUBLE_EQ(0.0, forceTorqueSensorObj->ForceXNoise().Mean());
+  EXPECT_DOUBLE_EQ(0.1, forceTorqueSensorObj->ForceXNoise().StdDev());
+  EXPECT_DOUBLE_EQ(1.0, forceTorqueSensorObj->ForceYNoise().Mean());
+  EXPECT_DOUBLE_EQ(1.1, forceTorqueSensorObj->ForceYNoise().StdDev());
+  EXPECT_DOUBLE_EQ(2.0, forceTorqueSensorObj->ForceZNoise().Mean());
+  EXPECT_DOUBLE_EQ(2.1, forceTorqueSensorObj->ForceZNoise().StdDev());
+
+  EXPECT_DOUBLE_EQ(3.0, forceTorqueSensorObj->TorqueXNoise().Mean());
+  EXPECT_DOUBLE_EQ(3.1, forceTorqueSensorObj->TorqueXNoise().StdDev());
+  EXPECT_DOUBLE_EQ(4.0, forceTorqueSensorObj->TorqueYNoise().Mean());
+  EXPECT_DOUBLE_EQ(4.1, forceTorqueSensorObj->TorqueYNoise().StdDev());
+  EXPECT_DOUBLE_EQ(5.0, forceTorqueSensorObj->TorqueZNoise().Mean());
+  EXPECT_DOUBLE_EQ(5.1, forceTorqueSensorObj->TorqueZNoise().StdDev());
+}
+
 /////////////////////////////////////////////////
 /// Main
 int main(int _argc, char **_argv)