From 0d4a0c630776e797d7d855c2a147ceb9165f1361 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Fri, 19 Nov 2021 20:35:21 -0800 Subject: [PATCH 1/3] save joints Signed-off-by: Ian Chen --- src/SdfGenerator.cc | 161 ++++++++++++++++++++++++++++++++- src/SdfGenerator.hh | 11 +++ test/integration/save_world.cc | 100 ++++++++++++++++++++ test/worlds/joint_sensor.sdf | 107 ++++++++++++++++++++++ 4 files changed, 378 insertions(+), 1 deletion(-) create mode 100644 test/worlds/joint_sensor.sdf diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index cb27899313..7a8b2b1193 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -29,12 +29,16 @@ #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" @@ -42,13 +46,16 @@ #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("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(_entity); + _elem->GetAttribute("name")->Set(nameComp->Data()); + + auto *poseComp = _ecm.Component(_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(_entity); + if (!jointComp) + { + return false; + } + + // joint type + auto jointTypeComp = _ecm.Component(_entity); + if (jointTypeComp) + { + sdf::JointType jointType = jointTypeComp->Data(); + 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(jointTypeStr); + } + + // parent + auto parentLinkNameComp = + _ecm.Component(_entity); + if (parentLinkNameComp) + { + _elem->GetElement("parent")->Set(parentLinkNameComp->Data()); + } + // child + auto childLinkNameComp = _ecm.Component(_entity); + if (childLinkNameComp) + { + _elem->GetElement("child")->Set(childLinkNameComp->Data()); + } + // thread pitch + auto threadPitchComp = _ecm.Component(_entity); + if (threadPitchComp) + { + _elem->GetElement("thread_pitch")->Set(threadPitchComp->Data()); + } + // axis + auto jointAxisComp = _ecm.Component(_entity); + if (jointAxisComp) + { + const sdf::JointAxis axis = jointAxisComp->Data(); + _elem->GetElement("axis")->Copy(axis.ToElement()); + } + // axis2 + auto jointAxis2Comp = _ecm.Component(_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 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 diff --git a/src/SdfGenerator.hh b/src/SdfGenerator.hh index aa7798d449..83a7844c29 100644 --- a/src/SdfGenerator.hh +++ b/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 diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index c951eab0ea..9d438a3037 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -26,6 +26,8 @@ #include #include #include +#include +#include #include #include #include @@ -855,6 +857,104 @@ TEST_F(SdfGeneratorFixture, WorldWithLights) ASSERT_NE(nullptr, genWorld); } +///////////////////////////////////////////////// +TEST_F(SdfGeneratorFixture, ModelWithJoints) +{ + this->LoadWorld("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) diff --git a/test/worlds/joint_sensor.sdf b/test/worlds/joint_sensor.sdf new file mode 100644 index 0000000000..f96cdeefe0 --- /dev/null +++ b/test/worlds/joint_sensor.sdf @@ -0,0 +1,107 @@ + + + + + + + + + + + + + link2 + link1 + + 0 0 1 + 0.5 + true + + -0.5 + 0.5 + 123.4 + 12 + + + 0.1 + 0.2 + 1.3 + 10.6 + + + + 0 1 0 + 1.5 + false + + -1 + 1 + 0.5 + 200 + + + + + 1 + + 0 + 0.2 + + + + + 10 11 12 0 0 0 + + parent + parent_to_child + + + + 0 + 0.1 + + + + + 1 + 1.1 + + + + + 2 + 2.1 + + + + + + + 3 + 3.1 + + + + + 4 + 4.1 + + + + + 5 + 5.1 + + + + + + + + + + From ceafa41c527e43b597ef0e75fe95da722e7d9916 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Mon, 29 Nov 2021 13:47:36 -0800 Subject: [PATCH 2/3] use joinPaths Signed-off-by: Ian Chen --- test/integration/save_world.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/save_world.cc b/test/integration/save_world.cc index 9d438a3037..0e3964b052 100644 --- a/test/integration/save_world.cc +++ b/test/integration/save_world.cc @@ -860,7 +860,8 @@ TEST_F(SdfGeneratorFixture, WorldWithLights) ///////////////////////////////////////////////// TEST_F(SdfGeneratorFixture, ModelWithJoints) { - this->LoadWorld("test/worlds/joint_sensor.sdf"); + this->LoadWorld(ignition::common::joinPaths("test", "worlds", + "joint_sensor.sdf")); const std::string worldGenSdfRes = this->RequestGeneratedSdf("joint_sensor"); From a29039e5ecbf067643f1a2ff9525c7da5ef1f56b Mon Sep 17 00:00:00 2001 From: Nate Koenig Date: Tue, 30 Nov 2021 09:14:25 -0800 Subject: [PATCH 3/3] Only output thread_pitch for screw joints Signed-off-by: Nate Koenig --- src/SdfGenerator.cc | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/SdfGenerator.cc b/src/SdfGenerator.cc index 7a8b2b1193..9f8b9286c2 100644 --- a/src/SdfGenerator.cc +++ b/src/SdfGenerator.cc @@ -829,9 +829,9 @@ namespace sdf_generator // joint type auto jointTypeComp = _ecm.Component(_entity); + sdf::JointType jointType = jointTypeComp->Data(); if (jointTypeComp) { - sdf::JointType jointType = jointTypeComp->Data(); std::string jointTypeStr = "invalid"; switch (jointType) { @@ -883,7 +883,7 @@ namespace sdf_generator } // thread pitch auto threadPitchComp = _ecm.Component(_entity); - if (threadPitchComp) + if (threadPitchComp && jointType == sdf::JointType::SCREW) { _elem->GetElement("thread_pitch")->Set(threadPitchComp->Data()); }