From 68dc51989fea388dc2977762fa0a047bcd525b67 Mon Sep 17 00:00:00 2001 From: "Addisu Z. Taddese" Date: Tue, 21 Sep 2021 00:25:41 -0500 Subject: [PATCH] Change Wrench type, add test --- .../components/JointConstraintWrench.hh | 52 --------- .../components/JointTransmittedWrench.hh | 96 ++++++++++++++++ .../gazebo/components/Serialization.hh | 37 ------- src/systems/physics/Physics.cc | 41 ++++--- test/integration/physics_system.cc | 104 ++++++++++++++---- ...aints.sdf => joint_transmitted_wrench.sdf} | 45 ++++++-- 6 files changed, 238 insertions(+), 137 deletions(-) delete mode 100644 include/ignition/gazebo/components/JointConstraintWrench.hh create mode 100644 include/ignition/gazebo/components/JointTransmittedWrench.hh rename test/worlds/{joint_constraints.sdf => joint_transmitted_wrench.sdf} (77%) diff --git a/include/ignition/gazebo/components/JointConstraintWrench.hh b/include/ignition/gazebo/components/JointConstraintWrench.hh deleted file mode 100644 index 9452c916461..00000000000 --- a/include/ignition/gazebo/components/JointConstraintWrench.hh +++ /dev/null @@ -1,52 +0,0 @@ -/* - * Copyright (C) 2021 Open Source Robotics Foundation - * - * Licensed under the Apache License, Version 2.0 (the "License"); - * you may not use this file except in compliance with the License. - * You may obtain a copy of the License at - * - * http://www.apache.org/licenses/LICENSE-2.0 - * - * Unless required by applicable law or agreed to in writing, software - * distributed under the License is distributed on an "AS IS" BASIS, - * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - * See the License for the specific language governing permissions and - * limitations under the License. - * - */ -#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTCONSTRAINTWRENCH_HH_ -#define IGNITION_GAZEBO_COMPONENTS_JOINTCONSTRAINTWRENCH_HH_ - -#include - -#include -#include -#include -#include - -namespace ignition -{ -namespace gazebo -{ -// Inline bracket to help doxygen filtering. -inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { -namespace components -{ -/// \brief Joint Constraint wrench in SI units (Nm for revolute, N for -/// prismatic). The first set of 3 values coorespond to the torque while the -/// second set of 3 correspond to the force. The wrench is expressed in the -/// Joint frame and the force component is applied at the joint origin. -/// \note The term Wrench is used here to mean a 6D vector formed by stacking -/// torque and force vectors. This is different from the Wrench used in screw -/// theory. -using JointConstraintWrench = - Component, class JointConstraintWrenchTag, - serializers::ArrayDoubleSerializer<6>>; -IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointConstraintWrench", - JointConstraintWrench) -} -} -} -} - -#endif diff --git a/include/ignition/gazebo/components/JointTransmittedWrench.hh b/include/ignition/gazebo/components/JointTransmittedWrench.hh new file mode 100644 index 00000000000..a15586c2ac7 --- /dev/null +++ b/include/ignition/gazebo/components/JointTransmittedWrench.hh @@ -0,0 +1,96 @@ +/* + * Copyright (C) 2021 Open Source Robotics Foundation + * + * Licensed under the Apache License, Version 2.0 (the "License"); + * you may not use this file except in compliance with the License. + * You may obtain a copy of the License at + * + * http://www.apache.org/licenses/LICENSE-2.0 + * + * Unless required by applicable law or agreed to in writing, software + * distributed under the License is distributed on an "AS IS" BASIS, + * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + * See the License for the specific language governing permissions and + * limitations under the License. + * + */ +#ifndef IGNITION_GAZEBO_COMPONENTS_JOINTTRANSMITTEDWRENCH_HH_ +#define IGNITION_GAZEBO_COMPONENTS_JOINTTRANSMITTEDWRENCH_HH_ + +#include + +#include +#include +#include +#include + +namespace ignition +{ +namespace gazebo +{ +// Inline bracket to help doxygen filtering. +inline namespace IGNITION_GAZEBO_VERSION_NAMESPACE { + +/// \brief Data structure that contains torque and force. +struct Wrench +{ + math::Vector3d torque; + math::Vector3d force; + + /// \brief Equality comparison operator + /// \param[in] _other The other Wrench against which this wrench is compared. + /// \return True if both the torque and force values are equal. + bool operator==(const Wrench &_other) const + { + return (this->torque == _other.torque) && (this->force == _other.force); + } +}; + +namespace serializers +{ +class WrenchSerializer +{ + /// \brief Serialization for `Wrench`. + /// \param[in] _out Output stream. + /// \param[in] _wrench Wrench to stream + /// \return The stream. + public: static std::ostream &Serialize(std::ostream &_out, + const Wrench &_wrench) + { + _out << _wrench.torque << " " << _wrench.torque; + return _out; + } + + /// \brief Deserialization for `Wrench`. + /// \param[in] _in Input stream. + /// \param[out] _Wrench Wrench to populate + /// \return The stream. + public: static std::istream &Deserialize(std::istream &_in, Wrench &_wrench) + { + _in >> _wrench.torque; + _in >> _wrench.force; + return _in; + } +}; +} // namespace serializers + +namespace components +{ +/// \brief Joint Transmitted wrench in SI units (Nm for revolute, N for +/// prismatic). The first set of 3 values coorespond to the torque while the +/// second set of 3 correspond to the force. The wrench is expressed in the +/// Joint frame and the force component is applied at the joint origin. +/// \note The term Wrench is used here to mean a 6D vector formed by stacking +/// torque and force vectors. This is different from the Wrench used in screw +/// theory. +using JointTransmittedWrench = + Component; +IGN_GAZEBO_REGISTER_COMPONENT("ign_gazebo_components.JointTransmittedWrench", + JointTransmittedWrench) +} // namespace components +} +} +} + +#endif diff --git a/include/ignition/gazebo/components/Serialization.hh b/include/ignition/gazebo/components/Serialization.hh index ce6b6d7b86f..50f0f5a3ce7 100644 --- a/include/ignition/gazebo/components/Serialization.hh +++ b/include/ignition/gazebo/components/Serialization.hh @@ -132,43 +132,6 @@ namespace serializers return _in; } }; - /// \brief Serializer for components that hold `std::array`. - template - class ArrayDoubleSerializer - { - /// \brief Serialization - /// \param[in] _out Output stream. - /// \param[in] _arr Array to stream - /// \return The stream. - public: static std::ostream &Serialize(std::ostream &_out, - const std::array &_arr) - { - ignition::msgs::Double_V msg; - *msg.mutable_data() = {_arr.begin(), _arr.end()}; - msg.SerializeToOstream(&_out); - return _out; - } - - /// \brief Deserialization - /// \param[in] _in Input stream. - /// \param[in] _arr Array to populate - /// \return The stream. - public: static std::istream &Deserialize(std::istream &_in, - std::array &_arr) - { - ignition::msgs::Double_V msg; - msg.ParseFromIstream(&_in); - if (static_cast(msg.data_size()) > N) - { - ignerr << "Size of input array exceeds " << N << ". Cannot deserialize" - << std::endl; - return _in; - } - - std::copy(msg.data().begin(), msg.data().end(), _arr.begin()); - return _in; - } - }; /// \brief Serializer for components that hold protobuf messages. class MsgSerializer diff --git a/src/systems/physics/Physics.cc b/src/systems/physics/Physics.cc index 963de43915c..7d8d3b0c836 100644 --- a/src/systems/physics/Physics.cc +++ b/src/systems/physics/Physics.cc @@ -26,6 +26,7 @@ #include #include #include +#include #include #include #include @@ -117,7 +118,7 @@ #include "ignition/gazebo/components/ParentEntity.hh" #include "ignition/gazebo/components/ParentLinkName.hh" #include "ignition/gazebo/components/ExternalWorldWrenchCmd.hh" -#include "ignition/gazebo/components/JointConstraintWrench.hh" +#include "ignition/gazebo/components/JointTransmittedWrench.hh" #include "ignition/gazebo/components/JointForceCmd.hh" #include "ignition/gazebo/components/Physics.hh" #include "ignition/gazebo/components/PhysicsEnginePlugin.hh" @@ -421,10 +422,10 @@ class ignition::gazebo::systems::PhysicsPrivate physics::SetJointTransformFromParentFeature>{}; ////////////////////////////////////////////////// - // Joint constraint wrench - /// \brief Feature list for getting joint constraint wrenches. - public: struct JointGetConstraintWrenchFeatureList : physics::FeatureList< - physics::GetJointConstraintWrench>{}; + // Joint transmitted wrench + /// \brief Feature list for getting joint transmitted wrenches. + public: struct JointGetTransmittedWrenchFeatureList : physics::FeatureList< + physics::GetJointTransmittedWrench>{}; ////////////////////////////////////////////////// // Collisions @@ -570,7 +571,7 @@ class ignition::gazebo::systems::PhysicsPrivate JointFeatureList, DetachableJointFeatureList, JointVelocityCommandFeatureList, - JointGetConstraintWrenchFeatureList + JointGetTransmittedWrenchFeatureList >; /// \brief A map between joint entity ids in the ECM to Joint Entities in @@ -2786,21 +2787,27 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, }); IGN_PROFILE_END(); - // Update joint constraints - _ecm.Each( + // Update joint transmitteds + _ecm.Each( [&](const Entity &_entity, components::Joint *, - components::JointConstraintWrench *_wrench) -> bool + components::JointTransmittedWrench *_wrench) -> bool { auto jointPhys = this->entityJointMap - .EntityCast(_entity); + .EntityCast(_entity); if (jointPhys) { - const auto &jointWrench = jointPhys->GetConstraintWrench(); - std::copy(jointWrench.data(), jointWrench.data() + jointWrench.size(), - std::begin(_wrench->Data())); - _ecm.SetChanged(_entity, components::JointConstraintWrench::typeId, - ComponentState::PeriodicChange); + const auto &jointWrench = jointPhys->GetTransmittedWrench(); + + gazebo::Wrench wrenchData; + wrenchData.torque = math::eigen3::convert(jointWrench.torque); + wrenchData.force = math::eigen3::convert(jointWrench.force); + const auto state = + _wrench->SetData(wrenchData, std::equal_to()) + ? ComponentState::PeriodicChange + : ComponentState::NoChange; + _ecm.SetChanged(_entity, components::JointTransmittedWrench::typeId, + state); } else { @@ -2808,9 +2815,9 @@ void PhysicsPrivate::UpdateSim(EntityComponentManager &_ecm, if (!informed) { igndbg - << "Attempting to get joint constraint wrenches, but the " + << "Attempting to get joint transmitted wrenches, but the " "physics engine doesn't support this feature. Values in the " - "JointConstraintWrench component will not be meaningful." + "JointTransmittedWrench component will not be meaningful." << std::endl; informed = true; } diff --git a/test/integration/physics_system.cc b/test/integration/physics_system.cc index a27065c7773..8736ffee937 100644 --- a/test/integration/physics_system.cc +++ b/test/integration/physics_system.cc @@ -18,6 +18,7 @@ #include #include +#include #include #include @@ -36,6 +37,7 @@ #include "ignition/gazebo/Server.hh" #include "ignition/gazebo/SystemLoader.hh" #include "ignition/gazebo/Util.hh" +#include "ignition/gazebo/components/PoseCmd.hh" #include "ignition/gazebo/test_config.hh" // NOLINT(build/include) #include "ignition/gazebo/components/AxisAlignedBox.hh" @@ -44,7 +46,7 @@ #include "ignition/gazebo/components/Geometry.hh" #include "ignition/gazebo/components/Inertial.hh" #include "ignition/gazebo/components/Joint.hh" -#include "ignition/gazebo/components/JointConstraintWrench.hh" +#include "ignition/gazebo/components/JointTransmittedWrench.hh" #include "ignition/gazebo/components/JointPosition.hh" #include "ignition/gazebo/components/JointPositionReset.hh" #include "ignition/gazebo/components/JointVelocity.hh" @@ -1666,13 +1668,13 @@ TEST_F(PhysicsSystemFixture, Heightmap) ///////////////////////////////////////////////// // Joint force -TEST_F(PhysicsSystemFixture, JointConstraintWrench) +TEST_F(PhysicsSystemFixture, JointTransmittedWrench) { common::Console::SetVerbosity(4); ignition::gazebo::ServerConfig serverConfig; const auto sdfFile = std::string(PROJECT_SOURCE_PATH) + - "/test/worlds/joint_constraints.sdf"; + "/test/worlds/joint_transmitted_wrench.sdf"; serverConfig.SetSdfFile(sdfFile); @@ -1693,32 +1695,92 @@ TEST_F(PhysicsSystemFixture, JointConstraintWrench) const components::Joint *) -> bool { _ecm.CreateComponent(_entity, - components::JointConstraintWrench()); + components::JointTransmittedWrench()); return true; }); } }); + const std::size_t totalIters = 1800; + std::vector wrenches; + wrenches.reserve(totalIters); + // Simply collect joint wrenches. We check the values later. testSystem.OnPostUpdate( - [&](const gazebo::UpdateInfo &_info, + [&](const gazebo::UpdateInfo &, const gazebo::EntityComponentManager &_ecm) { - _ecm.Each( - [&](const ignition::gazebo::Entity &, const components::Name *_name, - const components::JointConstraintWrench *_force) -> bool - { - std::cout << "Joint: " << _name->Data() << " " - << _force->Data().size() << " " << _info.iterations - << ": "; - for (const auto &jf : _force->Data()) - { - std::cout << jf << " "; - } - std::cout << std::endl; - return true; - }); + const auto sensorJointEntity = _ecm.EntityByComponents( + components::Joint(), components::Name("sensor_joint")); + const auto jointWrench = + _ecm.ComponentData( + sensorJointEntity); + if (jointWrench.has_value()) + { + wrenches.push_back(*jointWrench); + } }); server.AddSystem(testSystem.systemPtr); - server.Run(true, 1800, false); - SUCCEED(); + server.Run(true, totalIters, false); + + ASSERT_EQ(totalIters, wrenches.size()); + + const double kWeightScaleContactHeight = 0.05 + 0.25; + const double kSensorMass = 0.2; + const double kWeightMass = 10; + const double kGravity = 9.8; + const double kWeightInitialHeight = 0.5; + const double dt = 0.001; + const double timeOfContact = std::sqrt( + 2 * (kWeightInitialHeight - kWeightScaleContactHeight) / kGravity); + std::size_t iterOfContact = + static_cast(std::round(timeOfContact / dt)); + + for (std::size_t i = 0; i < iterOfContact - 10; ++i) + { + const auto &wrench = wrenches[i]; + EXPECT_NEAR(0.0, wrench.force.X(), 1e-3); + EXPECT_NEAR(0.0, wrench.force.Y(), 1e-3); + EXPECT_NEAR(kGravity * kSensorMass, wrench.force.Z(), 1e-3); + EXPECT_EQ(math::Vector3d::Zero, wrench.torque); + } + + // Wait 300 (determined empirically) iterations for values to stabilize. + for (std::size_t i = iterOfContact + 300; i < wrenches.size(); ++i) + { + const auto &wrench = wrenches[i]; + EXPECT_NEAR(0.0, wrench.force.X(), 1e-3); + EXPECT_NEAR(0.0, wrench.force.Y(), 1e-3); + EXPECT_NEAR(kGravity * (kSensorMass + kWeightMass), wrench.force.Z(), 1e-3); + EXPECT_EQ(math::Vector3d::Zero, wrench.torque); + } + + // Move the weight off center so it generates torque + testSystem.OnPreUpdate( + [&](const gazebo::UpdateInfo &, gazebo::EntityComponentManager &_ecm) + { + const auto weightEntity = _ecm.EntityByComponents( + components::Model(), components::Name("weight")); + _ecm.SetComponentData( + weightEntity, math::Pose3d(0.2, 0.1, 0.5, 0, 0, 0)); + }); + + server.RunOnce(); + // Reset PreUpdate so it doesn't keep moving the weight + testSystem.OnPreUpdate({}); + wrenches.clear(); + server.Run(true, totalIters, false); + ASSERT_EQ(totalIters, wrenches.size()); + + // Wait 300 (determined empirically) iterations for values to stabilize. + for (std::size_t i = iterOfContact + 300; i < wrenches.size(); ++i) + { + const auto &wrench = wrenches[i]; + EXPECT_NEAR(0.0, wrench.force.X(), 1e-3); + EXPECT_NEAR(0.0, wrench.force.Y(), 1e-3); + EXPECT_NEAR(kGravity * (kSensorMass + kWeightMass), wrench.force.Z(), 1e-3); + + EXPECT_NEAR(0.1 * kGravity * kWeightMass, wrench.torque.X(), 1e-3); + EXPECT_NEAR(-0.2 * kGravity * kWeightMass, wrench.torque.Y(), 1e-3); + EXPECT_NEAR(0.0, wrench.torque.Z(), 1e-3); + } } diff --git a/test/worlds/joint_constraints.sdf b/test/worlds/joint_transmitted_wrench.sdf similarity index 77% rename from test/worlds/joint_constraints.sdf rename to test/worlds/joint_transmitted_wrench.sdf index 24478eb6bc6..48e43b2dbe3 100644 --- a/test/worlds/joint_constraints.sdf +++ b/test/worlds/joint_transmitted_wrench.sdf @@ -1,6 +1,6 @@ - + true @@ -61,10 +61,35 @@ + 0.5 0.5 -0.01 0 0 0 - - 1 1 0.04 - + + 0.01 + + + + + 0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 -0.5 -0.01 0 0 0 + + + 0.01 + + + + + -0.5 0.5 -0.01 0 0 0 + + + 0.01 + @@ -113,9 +138,9 @@ - - 0.5 0.5 0.5 - + + 0.25 + 0.0 0.8 0.8 1 @@ -125,9 +150,9 @@ - - 0.5 0.5 0.5 - + + 0.25 +